add 梯形速度规划

User/module/balance_chassis.c

@@ -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轴控制 ==================== */

User/module/balance_chassis.h

@@ -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;