1.0

App/nav/corridor_ctrl.c

@@ -61,10 +61,33 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
         /* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
         w_cmd = -(s_cfg.kd_theta * imu_wz);
     } else {
-        /* 正常控制: 完整PD控制律 */
+        /*
+         * 简化原则：
+         * - IMU 只管航向 (e_th + wz)
+         * - 左右激光只管居中
+         *
+         * 只要左右两侧都完整有效，就直接用左右平均距离差做横向误差：
+         *   e_y_direct = (d_right - d_left)/2
+         * 车在正中时 d_left ~= d_right，因此误差应接近 0。
+         *
+         * 若当前帧双侧不完整，再退回滤波器给出的 e_y。
+         */
+        float e_y_ctrl = state->e_y;
+        bool left_ok = ((obs->valid_mask & (1U << 0)) != 0U) &&
+                       ((obs->valid_mask & (1U << 1)) != 0U);
+        bool right_ok = ((obs->valid_mask & (1U << 2)) != 0U) &&
+                        ((obs->valid_mask & (1U << 3)) != 0U);
+
+        if (left_ok && right_ok) {
+            float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
+            float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
+            e_y_ctrl = 0.5f * (d_right - d_left);
+        }
+
+        /* 正常控制: IMU 管航向, 左右激光管居中 */
         w_cmd = -(s_cfg.kp_theta * state->e_th
                 + s_cfg.kd_theta * imu_wz
-                + s_cfg.kp_y    * state->e_y);
+                + s_cfg.kp_y    * e_y_ctrl);
 
         /* ========================================================
          * 近墙脱离保护: