diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 7a2efce..fbcdc88 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -800,6 +800,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { LQR_CalculateGainMatrix(&c->lqr[0], c->vmc_[0].leg.L0); LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].leg.L0); + /* 速度误差大、高速运动或双腿离地时,禁用x位移跟踪防止轮子失控 */ + { + float vel_error = c->chassis_state.target_velocity_x - c->chassis_state.velocity_x; + bool both_off_ground = !c->vmc_[0].leg.is_ground_contact && !c->vmc_[1].leg.is_ground_contact; + if (fabsf(c->chassis_state.target_velocity_x) > 0.5f || + fabsf(vel_error) > 0.7f || + both_off_ground) { + c->lqr[0].K_matrix[0][2] = 0.0f; /* 禁用T的x位移增益 */ + c->lqr[1].K_matrix[0][2] = 0.0f; + c->chassis_state.position_x = 0.0f; + c->chassis_state.target_x = 0.0f; + } + } + /* ==================== 目标状态 ==================== */ /* 腿长-位移补偿:根据腿长多项式拟合补偿位移偏移 */ float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;