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

添加削弱函数

Browse Source
This commit is contained in:
Robofish 2026-03-15 21:27:17 +08:00
parent 9cd6b85e93
commit 0a442e8976
1 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
User/module/balance_chassis.c
View File

@ -486,9 +486,22 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 运行LQR和VMC */ /* 运行LQR和VMC */
Chassis_LQRControl(c, &climb_cmd); Chassis_LQRControl(c, &climb_cmd);
/* 削弱LQR的摆力矩Tp，让腿撞到台阶后能自由后摆 */ /* 摆力矩Tp随theta非线性缩放：撞到台阶前保持正常，撞到后快速降低 */
c->lqr[0].control_output.Tp *= c->param->climb.tp_scale; float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
c->lqr[1].control_output.Tp *= c->param->climb.tp_scale; float theta_abs = fabsf(avg_theta);
float contact_angle = c->param->climb.theta_retract_threshold * 0.5f;
float tp_min = c->param->climb.tp_scale;
float dynamic_scale;
if (theta_abs < contact_angle) {
dynamic_scale = 1.0f; /* 未撞到，保持正常Tp */
} else {
float t = (theta_abs - contact_angle) /
(c->param->climb.theta_retract_threshold - contact_angle);
t = LIMIT(t, 0.0f, 1.0f);
dynamic_scale = 1.0f + (tp_min - 1.0f) * sqrtf(t); /* sqrt: 撞到后快速衰减 */
}
c->lqr[0].control_output.Tp *= dynamic_scale;
c->lqr[1].control_output.Tp *= dynamic_scale;
Chassis_VMCControl(c, &climb_cmd); Chassis_VMCControl(c, &climb_cmd);
Chassis_Output(c); Chassis_Output(c);
@ -496,7 +509,6 @@ static void Chassis_ClimbControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
/* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */ /* 先伸腿到位，再根据摆角触发收腿，避免刚进入模式就立刻缩回 */
bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) && bool legs_extended = (c->vmc_[0].leg.L0 > c->param->leg.max_length - 0.03f) &&
(c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f); (c->vmc_[1].leg.L0 > c->param->leg.max_length - 0.03f);
float avg_theta = (c->vmc_[0].leg.theta + c->vmc_[1].leg.theta) / 2.0f;
if (elapsed_time > 300 && legs_extended && if (elapsed_time > 300 && legs_extended &&
fabsf(avg_theta) > c->param->climb.theta_retract_threshold) { fabsf(avg_theta) > c->param->climb.theta_retract_threshold) {
c->climb.state = CLIMB_RETRACT; c->climb.state = CLIMB_RETRACT;