From 8f0f615fb1b54d8af15ef25d0e51200495dada6c Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Tue, 7 Oct 2025 23:20:04 +0800
Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E4=B8=80=E4=B8=8B=E9=80=9F=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 User/module/balance_chassis.c | 29 +++++++++++++++++++++++++----
 User/module/balance_chassis.h |  2 ++
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 0e10ae1..70cfcaa 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -58,6 +58,8 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode) {
   c->chassis_state.velocity_x = 0.0f;
   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.last_target_velocity_x = 0.0f;
 
   LQR_Reset(&c->lqr[0]);
   LQR_Reset(&c->lqr[1]);
@@ -157,6 +159,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
   c->chassis_state.velocity_x = 0.0f;
   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.last_target_velocity_x = 0.0f;
 
   /*初始化yaw控制状态*/
   c->yaw_control.target_yaw = 0.0f;
@@ -296,15 +300,32 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   /* 运动参数（参考C++版本的状态估计） */
   static float xhat = 0.0f, x_dot_hat = 0.0f;
   float target_dot_x = 0.0f;
+  float max_acceleration = 3.0f; // 最大加速度限制: 1 m/s²
 
   // 简化的速度估计（后续可以改进为C++版本的复杂滤波）
   x_dot_hat = c->chassis_state.velocity_x;
   xhat = c->chassis_state.position_x;
 
-  // 目标设定
-  target_dot_x = c_cmd->move_vec.vx * 2;
-  // target_dot_x = SpeedLimit_TargetSpeed(300.0f, c->chassis_state.velocity_x,
-  // target_dot_x, c->dt); // 速度限制器，假设最大加速度为1 m/s²
+  // 目标速度设定（原始期望速度）
+  float desired_velocity = c_cmd->move_vec.vx * 2;
+  
+  // 加速度限制处理
+  float velocity_change = desired_velocity - c->chassis_state.last_target_velocity_x;
+  float max_velocity_change = max_acceleration * c->dt; // 最大允许的速度变化
+  
+  // 限制速度变化率（加速度限制）
+  if (velocity_change > max_velocity_change) {
+    velocity_change = max_velocity_change;
+  } else if (velocity_change < -max_velocity_change) {
+    velocity_change = -max_velocity_change;
+  }
+  
+  // 应用加速度限制后的目标速度
+  target_dot_x = c->chassis_state.last_target_velocity_x + velocity_change;
+  c->chassis_state.target_velocity_x = target_dot_x;
+  c->chassis_state.last_target_velocity_x = target_dot_x;
+  
+  // 更新目标位置
   c->chassis_state.target_x += target_dot_x * c->dt;
 
   /* 分别计算左右腿的LQR控制 */
diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h
index 1160288..ad73733 100644
--- a/User/module/balance_chassis.h
+++ b/User/module/balance_chassis.h
@@ -123,6 +123,8 @@ typedef struct {
     float velocity_x;     /* 机体x速度 */
     float last_velocity_x; /* 上一次速度用于数值微分 */
     float target_x;       /* 目标x位置 */
+    float target_velocity_x; /* 目标速度 */
+    float last_target_velocity_x; /* 上一次目标速度 */
   } chassis_state;
 
   /* yaw控制相关 */