From 55403f1f87bcf2b990e692acd4cc199cb9857c17 Mon Sep 17 00:00:00 2001
From: Robofish <1683502971@qq.com>
Date: Tue, 17 Mar 2026 08:41:22 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=9D=E6=8A=A4=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 | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c
index 07cbcc1..cbab103 100644
--- a/User/module/balance_chassis.c
+++ b/User/module/balance_chassis.c
@@ -1047,12 +1047,19 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
   }
 
   /* 有速度指令时：纯速度控制，目标位移跟随当前位移（消除位移误差）
-   * 无速度指令时：位移+速度双环控制，积分保持位置锁定 */
+   * 无速度指令时：位移+速度双环控制，积分保持位置锁定
+   * 位移误差钳位到±2m，防止积分漂移过大 */
   if (fabsf(c->chassis_state.target_velocity_x) > 0.01f) {
     c->chassis_state.target_x = c->chassis_state.position_x;
   } else {
     c->chassis_state.target_x += c->chassis_state.target_velocity_x * c->dt;
   }
+  float pos_err = c->chassis_state.target_x - c->chassis_state.position_x;
+  if (pos_err > 2.0f) {
+    c->chassis_state.target_x = c->chassis_state.position_x + 2.0f;
+  } else if (pos_err < -2.0f) {
+    c->chassis_state.target_x = c->chassis_state.position_x - 2.0f;
+  }
 
 
   /* ==================== 状态更新 ==================== */