From 29d9185b7c55db5286f94efddad2409daa1d908a Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Sat, 21 Mar 2026 02:36:56 +0800
Subject: [PATCH] =?UTF-8?q?add=20=E6=A2=AF=E5=BD=A2=E9=80=9F=E5=BA=A6?=
 =?UTF-8?q?=E8=A7=84=E5=88=92?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/balance_chassis.c | 19 ++++++++++++++++---
 User/module/balance_chassis.h |  1 +
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 9a3e91c..ad36cac 100644
--- a/User/module/balance_chassis.c
+++ b/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轴控制 ==================== */
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index eed675b..f0440ac 100644
--- a/User/module/balance_chassis.h
+++ b/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;