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

FIX

Browse Source
This commit is contained in:
Robofish 2026-03-15 15:29:57 +08:00
parent c522e563af
commit 7e98e574bd
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
User/module/balance_chassis.c
View File

@ -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[0], c->vmc_[0].leg.L0);
LQR_CalculateGainMatrix(&c->lqr[1], c->vmc_[1].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; float avg_L0 = (c->vmc_[0].leg.L0 + c->vmc_[1].leg.L0) * 0.5f;