robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

addx补偿

Browse Source
This commit is contained in:
Robofish 2026-03-15 13:32:15 +08:00
parent 9992ffba2d
commit bef9394790
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
User/module/balance_chassis.c
View File

@ -805,12 +805,19 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0);
/* ==================== 目标状态 ==================== */
/* 腿长-位移补偿：根据腿长多项式拟合补偿位移偏移 */
float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
float leg_x_comp = -45.406f * avg_L0 * avg_L0 * avg_L0
+ 7.06585f * avg_L0 * avg_L0
+ 5.98465f * avg_L0
- 1.14975f;
LQR_State_t target_state = {
.theta = 0.0f+c->param->lqr_offset.theta,
.d_theta = 0.0f,
.phi = 0.0f+c->param->lqr_offset.phi,
.d_phi = 0.0f,
.x = c->chassis_state.target_x+c->param->lqr_offset.x,
.x = c->chassis_state.target_x+c->param->lqr_offset.x + leg_x_comp,
.d_x = c->chassis_state.target_velocity_x,
};
@ -914,7 +921,6 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* ==================== 轮毂输出 ==================== */
/* 腿长增加时有效轮距增大，等比例缩小yaw_force防止过冲 */
float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;
float yaw_scale = c->param->leg.base_length / avg_L0; /* 以基础腿长为基准归一化 */
float scaled_yaw_force = c->yaw_control.yaw_force * yaw_scale;