From 9b5d806e5f8bdbe5bfe32501efc255b8a3b15afc Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 02:09:43 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9k?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/virtual_chassis.c | 4 +-- User/module/balance_chassis.c | 22 ++++++++-------- User/module/config.c | 25 ++++++++++--------- .../balance/series_legs/get_k_length.m | 4 +-- 4 files changed, 29 insertions(+), 26 deletions(-) diff --git a/User/device/virtual_chassis.c b/User/device/virtual_chassis.c index 7ff105d..c2e3e0c 100644 --- a/User/device/virtual_chassis.c +++ b/User/device/virtual_chassis.c @@ -177,7 +177,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id, if (y_int & 0x800000) { y_int |= 0xFF000000; } - chassis->data.imu.gyro.y = (float)y_int / 1000.0f; // 直接除以1000，保持rad/s + chassis->data.imu.gyro.y = -(float)y_int / 1000.0f; // 直接除以1000，保持rad/s // z: 16位有符号整数 (字节6-7) - 精度0.01 rad/s (不再转换单位) int16_t z_int; @@ -206,7 +206,7 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id, if (pit_int & 0x800000) { pit_int |= 0xFF000000; } - chassis->data.imu.euler.pit = (float)pit_int / 10000.0f; // 直接除以10000，保持rad + chassis->data.imu.euler.pit = -(float)pit_int / 10000.0f; // 直接除以10000，保持rad // roll: 16位有符号整数 (字节6-7) - 精度0.001 rad (不再转换单位) int16_t rol_int; diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 39ff6d6..c0f2c6b 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -217,18 +217,20 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) { break; case CHASSIS_MODE_RECOVER: { - float fn; - fn = -25.0f; + float fn, tp; + fn = -20.0f; + tp = 0.0f; - VMC_InverseSolve(&c->vmc_[0], fn, 0.0f); + VMC_InverseSolve(&c->vmc_[0], fn, tp); VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]); - VMC_InverseSolve(&c->vmc_[1], fn, 0.0f); + VMC_InverseSolve(&c->vmc_[1], fn, tp); VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]); // Chassis_MotorEnable(c); - c->output.wheel[0] = 0.0f; - c->output.wheel[1] = 0.0f; + c->output.wheel[0] = c_cmd->move_vec.vx *0.2f; + + c->output.wheel[1] = c_cmd->move_vec.vx *0.2f; Chassis_Output(c); // 统一输出 } break; @@ -269,6 +271,7 @@ void Chassis_Output(Chassis_t *c) { // out[3] = 0.0f; Virtual_Chassis_SendJointTorque(&virtual_chassis, out); Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]); + // Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f); // for (int i = 0; i < 2; i++) { } @@ -316,7 +319,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_State_t target_state = {0}; target_state.theta = 0.0f; // 目标摆杆角度 target_state.d_theta = 0.0f; // 目标摆杆角速度 - target_state.phi = -0.1f; // 目标俯仰角 + target_state.phi = -0.2f; // 目标俯仰角 target_state.d_phi = 0.0f; // 目标俯仰角速度 target_state.x = c->chassis_state.target_x; // 目标位置 target_state.d_x = target_dot_x; // 目标速度 @@ -373,7 +376,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->vmc_[0].leg.d_theta, c->dt); // 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反） Delta_Tp[1] = -Delta_Tp[0]; - // 左腿控制 { @@ -382,7 +384,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->vmc_[0].leg.L0, leg_d_length[0], c->dt); // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力） - virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 20.0f ; + virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 10.0f ; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0], @@ -403,7 +405,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { c->vmc_[1].leg.L0, leg_d_length[1], c->dt); // 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力） - virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 20.0f ; + virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 10.0f ; // VMC逆解算（包含摆角补偿） if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1], diff --git a/User/module/config.c b/User/module/config.c index 6dee08c..3de3599 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -113,19 +113,20 @@ Config_RobotParam_t robot_config = { } }, .lqr_gains = { - .k11_coeff = { -1.971875171647865e+02f, 2.333085109680034e+02f, -1.246153590631013e+02f, -1.174312882115276e+00f }, // theta - .k12_coeff = { 1.013831636839851e+01f, -9.401172931240684e+00f, -7.093019361028483e+00f, -1.965070661986029e-01f }, // d_theta - .k13_coeff = { -8.071626496047382e+01f, 8.488840381053994e+01f, -3.185529183755884e+01f, 8.124359207371032e-01f }, // x - .k14_coeff = { -9.146471594077464e+01f, 9.719345618895781e+01f, -3.842805305561099e+01f, 7.255654659220748e-01f }, // d_x - .k15_coeff = { 6.971426904628133e+00f, 1.184154915473761e+00f, -5.491870448330107e+00f, 1.440694962464955e+00f }, // phi - .k16_coeff = { -1.435148366342453e+00f, 3.514683607111185e+00f, -2.799267599341074e+00f, 7.265018312372623e-01f }, // d_phi - .k21_coeff = { 2.190542850560747e+02f, -1.777611537012371e+02f, 2.829510877414540e+01f, 1.179295840423852e+01f }, // theta - .k22_coeff = { 1.499600560463937e+01f, -1.645293844666804e+01f, 6.159388841822305e+00f, 1.123989134168646e+00f }, // d_theta - .k23_coeff = { -2.101291454047308e+00f, 2.194459164295155e+01f, -2.170715123911771e+01f, 7.721879713384771e+00f }, // x - .k24_coeff = { -8.999340091522836e+00f, 3.152322448910924e+01f, -2.733558805289098e+01f, 9.654200348790059e+00f }, // d_x - .k25_coeff = { 2.294462014541718e+01f, -2.777789794229430e+01f, 1.160794075992378e+01f, 2.826138680231716e+00f }, // phi - .k26_coeff = { 1.624214174273862e+01f, -1.723094946447953e+01f, 6.402650924792812e+00f, 8.755827436652930e-02f }, // d_phi + .k11_coeff = { -2.018567892252635e+02f, 2.443886947748371e+02f, -1.170514252719224e+02f, 1.756967283355012e+00f }, // theta + .k12_coeff = { 5.755541993064226e+00f, -2.330435203830081e+00f, -5.839389482510768e+00f, 3.102177559173085e-01f }, // d_theta + .k13_coeff = { -4.626648375801587e+01f, 4.957163915561897e+01f, -1.892978626686148e+01f, 1.163361730642856e+00f }, // x + .k14_coeff = { -6.133244696547617e+01f, 6.581150547694307e+01f, -2.562467010643359e+01f, 1.546359441777505e+00f }, // d_x + .k15_coeff = { -9.787512606327831e-01f, 8.581307530201126e+00f, -7.593138673503293e+00f, 1.797419370480695e+00f }, // phi + .k16_coeff = { 2.873430467178664e+00f, -8.356289550331172e-01f, -1.159458299556627e+00f, 5.473036343381187e-01f }, // d_phi + .k21_coeff = { 4.048940262085570e+02f, -3.365968455386673e+02f, 6.490322687755379e+01f, 1.009032151585340e+01f }, // theta + .k22_coeff = { 3.731821338634553e+01f, -3.863396990944306e+01f, 1.309004485801674e+01f, -8.491362495876140e-02f }, // d_theta + .k23_coeff = { 3.197848557628166e+01f, -1.613497787175245e+01f, -5.139546674944345e+00f, 4.089512188250406e+00f }, // x + .k24_coeff = { 4.042762833518352e+01f, -2.015511550072881e+01f, -6.901438674565010e+00f, 5.459259288213627e+00f }, // d_x + .k25_coeff = { 9.671666382924865e+01f, -1.007923006268712e+02f, 3.656525147129298e+01f, -7.438566326110798e-01f }, // phi + .k26_coeff = { 2.324856410306975e+01f, -2.529653580362136e+01f, 9.749203865654970e+00f, -5.110687534172851e-01f }, // d_phi }, + .virtual_chassis_param = { .motors = { .can = BSP_CAN_1, diff --git a/utils/Simulation-master/balance/series_legs/get_k_length.m b/utils/Simulation-master/balance/series_legs/get_k_length.m index ad6fd84..98c489f 100644 --- a/utils/Simulation-master/balance/series_legs/get_k_length.m +++ b/utils/Simulation-master/balance/series_legs/get_k_length.m @@ -48,8 +48,8 @@ function K = get_k_length(leg_length) B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]); B=double(B); - Q=diag([700 100 2000 1500 2300 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 - R=[140 0;0 50]; %T Tp + Q=diag([2000 1 600 200 2000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1 + R=[240 0;0 50]; %T Tp K=lqr(A,B,Q,R);