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

add 梯形速度规划

Browse Source
This commit is contained in:
Robofish 2026-03-21 02:36:56 +08:00
parent 5139f4ad6a
commit 29d9185b7c
2 changed files with 17 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
User/module/balance_chassis.c
View File

@ -110,6 +110,7 @@ static void Chassis_ResetControllers(Chassis_t *c) {
c->chassis_state.last_velocity_x = 0.0f;
c->chassis_state.target_x = 0.0f;
c->chassis_state.target_velocity_x = 0.0f;
c->chassis_state.ramped_vx = 0.0f;
c->chassis_state.last_target_velocity_x = 0.0f;
}
@ -1136,13 +1137,25 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
c->chassis_state.last_target_velocity_x = c->chassis_state.target_velocity_x;
}
/* ==================== 梯形速度规划 ==================== */
/* ramped_vx 按 max_acceleration 限制变化率，防止目标速度突变 */
float max_delta = c->param->motion.max_acceleration * c->dt;
float vx_diff = c->chassis_state.target_velocity_x - c->chassis_state.ramped_vx;
if (vx_diff > max_delta) {
c->chassis_state.ramped_vx += max_delta;
} else if (vx_diff < -max_delta) {
c->chassis_state.ramped_vx -= max_delta;
} else {
c->chassis_state.ramped_vx = c->chassis_state.target_velocity_x;
}
/* 有速度指令时：纯速度控制，目标位移跟随当前位移（消除位移误差）
* 无速度指令时：位移+速度双环控制，积分保持位置锁定
* 位移误差钳位到±2m，防止积分漂移过大 */
if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) {
if (fabsf(c->chassis_state.ramped_vx) > 0.01f) {
c->chassis_state.target_x = c->chassis_state.position_x+0.8f; /* 直接锁定在当前位置，消除位移误差 */
} else {
c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
c->chassis_state.target_x += c->chassis_state.ramped_vx * c->dt;
}
float pos_err = c->chassis_state.target_x - c->chassis_state.position_x;
if (pos_err > 2.0f) {
@ -1186,7 +1199,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
.phi = 0.0f+c->param->lqr_offset.phi,
.d_phi = 0.0f,
.x = c->chassis_state.target_x-c->param->lqr_offset.x + leg_x_comp,
.d_x = c->chassis_state.target_velocity_x,
.d_x = c->chassis_state.ramped_vx,
};
/* ==================== Yaw轴控制 ==================== */

1
User/module/balance_chassis.h
View File

@ -239,6 +239,7 @@ typedef struct {
float last_velocity_x; /* 上一次速度用于数值微分 */
float target_x; /* 目标x位置 */
float target_velocity_x; /* 目标速度 */
float ramped_vx; /* 梯形规划后的实际目标速度 */
float last_target_velocity_x; /* 上一次目标速度 */
} chassis_state;