1.0

.idea/claudeCodeTabState.xml

@@ -7,9 +7,10 @@
           <value>
             <TabSessionState>
               <option name="provider" value="claude" />
+              <option name="sessionId" value="53161578-6c23-4a4d-b8fb-a45fa428ed7b" />
               <option name="cwd" value="$PROJECT_DIR$" />
-              <option name="model" value="claude-sonnet-4-6" />
+              <option name="model" value="claude-opus-4-6" />
-              <option name="permissionMode" value="bypassPermissions" />
+              <option name="permissionMode" value="plan" />
               <option name="reasoningEffort" value="medium" />
             </TabSessionState>
           </value>

App/VL53L1X_API/platform/vl53_calibration_config.h

@@ -17,13 +17,205 @@ typedef struct {
  * 当前先提供空白占位，未标定时保持 calibrated = 0，驱动将跳过加载。
  */
 static const Vl53L1RuntimeCalibration_t k_vl53l1_left_calibration[2] = {
-    { .calibrated = 0u, .data = {0} },
+    {
-    { .calibrated = 0u, .data = {0} },
+        .calibrated = 0u,
+        .data = {
+            .struct_version = 3970629922u,
+            .customer = {
+                .global_config__spad_enables_ref_0 = 223u,
+                .global_config__spad_enables_ref_1 = 247u,
+                .global_config__spad_enables_ref_2 = 251u,
+                .global_config__spad_enables_ref_3 = 254u,
+                .global_config__spad_enables_ref_4 = 255u,
+                .global_config__spad_enables_ref_5 = 7u,
+                .global_config__ref_en_start_select = 0u,
+                .ref_spad_man__num_requested_ref_spads = 11u,
+                .ref_spad_man__ref_location = 1u,
+                .algo__crosstalk_compensation_plane_offset_kcps = 0u,
+                .algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
+                .algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
+                .ref_spad_char__total_rate_target_mcps = 2560u,
+                .algo__part_to_part_range_offset_mm = 0,
+                .mm_config__inner_offset_mm = 35,
+                .mm_config__outer_offset_mm = 9,
+            },
+            .add_off_cal_data = {
+                .result__mm_inner_actual_effective_spads = 280u,
+                .result__mm_outer_actual_effective_spads = 1344u,
+                .result__mm_inner_peak_signal_count_rtn_mcps = 397u,
+                .result__mm_outer_peak_signal_count_rtn_mcps = 1305u,
+            },
+            .optical_centre = {
+                .x_centre = 144u,
+                .y_centre = 112u,
+            },
+            .gain_cal = {
+                .standard_ranging_gain_factor = 2011u,
+            },
+            .cal_peak_rate_map = {
+                .cal_distance_mm = 0,
+                .max_samples = 0u,
+                .width = 0u,
+                .height = 0u,
+                .peak_rate_mcps = {
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u
+                }
+            }
+        }
+    },
+    {
+        .calibrated = 0u,
+        .data = {
+            .struct_version = 3970629922u,
+            .customer = {
+                .global_config__spad_enables_ref_0 = 255u,
+                .global_config__spad_enables_ref_1 = 189u,
+                .global_config__spad_enables_ref_2 = 255u,
+                .global_config__spad_enables_ref_3 = 255u,
+                .global_config__spad_enables_ref_4 = 255u,
+                .global_config__spad_enables_ref_5 = 15u,
+                .global_config__ref_en_start_select = 0u,
+                .ref_spad_man__num_requested_ref_spads = 14u,
+                .ref_spad_man__ref_location = 1u,
+                .algo__crosstalk_compensation_plane_offset_kcps = 0u,
+                .algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
+                .algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
+                .ref_spad_char__total_rate_target_mcps = 2560u,
+                .algo__part_to_part_range_offset_mm = 0,
+                .mm_config__inner_offset_mm = 37,
+                .mm_config__outer_offset_mm = 9,
+            },
+            .add_off_cal_data = {
+                .result__mm_inner_actual_effective_spads = 280u,
+                .result__mm_outer_actual_effective_spads = 1288u,
+                .result__mm_inner_peak_signal_count_rtn_mcps = 470u,
+                .result__mm_outer_peak_signal_count_rtn_mcps = 1456u,
+            },
+            .optical_centre = {
+                .x_centre = 112u,
+                .y_centre = 128u,
+            },
+            .gain_cal = {
+                .standard_ranging_gain_factor = 2011u,
+            },
+            .cal_peak_rate_map = {
+                .cal_distance_mm = 0,
+                .max_samples = 0u,
+                .width = 0u,
+                .height = 0u,
+                .peak_rate_mcps = {
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u
+                }
+            }
+        }
+    },
 };
 
 static const Vl53L1RuntimeCalibration_t k_vl53l1_right_calibration[2] = {
-    { .calibrated = 0u, .data = {0} },
+    {
-    { .calibrated = 0u, .data = {0} },
+        .calibrated = 0u,
+        .data = {
+            .struct_version = 3970629922u,
+            .customer = {
+                .global_config__spad_enables_ref_0 = 159u,
+                .global_config__spad_enables_ref_1 = 254u,
+                .global_config__spad_enables_ref_2 = 255u,
+                .global_config__spad_enables_ref_3 = 255u,
+                .global_config__spad_enables_ref_4 = 239u,
+                .global_config__spad_enables_ref_5 = 15u,
+                .global_config__ref_en_start_select = 0u,
+                .ref_spad_man__num_requested_ref_spads = 6u,
+                .ref_spad_man__ref_location = 1u,
+                .algo__crosstalk_compensation_plane_offset_kcps = 0u,
+                .algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
+                .algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
+                .ref_spad_char__total_rate_target_mcps = 2560u,
+                .algo__part_to_part_range_offset_mm = 0,
+                .mm_config__inner_offset_mm = 54,
+                .mm_config__outer_offset_mm = 31,
+            },
+            .add_off_cal_data = {
+                .result__mm_inner_actual_effective_spads = 224u,
+                .result__mm_outer_actual_effective_spads = 1456u,
+                .result__mm_inner_peak_signal_count_rtn_mcps = 336u,
+                .result__mm_outer_peak_signal_count_rtn_mcps = 1382u,
+            },
+            .optical_centre = {
+                .x_centre = 112u,
+                .y_centre = 112u,
+            },
+            .gain_cal = {
+                .standard_ranging_gain_factor = 2011u,
+            },
+            .cal_peak_rate_map = {
+                .cal_distance_mm = 0,
+                .max_samples = 0u,
+                .width = 0u,
+                .height = 0u,
+                .peak_rate_mcps = {
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u
+                }
+            }
+        }
+    },
+    {
+        .calibrated = 0u,
+        .data = {
+            .struct_version = 3970629922u,
+            .customer = {
+                .global_config__spad_enables_ref_0 = 255u,
+                .global_config__spad_enables_ref_1 = 255u,
+                .global_config__spad_enables_ref_2 = 255u,
+                .global_config__spad_enables_ref_3 = 191u,
+                .global_config__spad_enables_ref_4 = 191u,
+                .global_config__spad_enables_ref_5 = 11u,
+                .global_config__ref_en_start_select = 0u,
+                .ref_spad_man__num_requested_ref_spads = 11u,
+                .ref_spad_man__ref_location = 2u,
+                .algo__crosstalk_compensation_plane_offset_kcps = 0u,
+                .algo__crosstalk_compensation_x_plane_gradient_kcps = 0,
+                .algo__crosstalk_compensation_y_plane_gradient_kcps = 0,
+                .ref_spad_char__total_rate_target_mcps = 2560u,
+                .algo__part_to_part_range_offset_mm = 0,
+                .mm_config__inner_offset_mm = 30,
+                .mm_config__outer_offset_mm = 5,
+            },
+            .add_off_cal_data = {
+                .result__mm_inner_actual_effective_spads = 224u,
+                .result__mm_outer_actual_effective_spads = 1456u,
+                .result__mm_inner_peak_signal_count_rtn_mcps = 334u,
+                .result__mm_outer_peak_signal_count_rtn_mcps = 1524u,
+            },
+            .optical_centre = {
+                .x_centre = 112u,
+                .y_centre = 96u,
+            },
+            .gain_cal = {
+                .standard_ranging_gain_factor = 2011u,
+            },
+            .cal_peak_rate_map = {
+                .cal_distance_mm = 0,
+                .max_samples = 0u,
+                .width = 0u,
+                .height = 0u,
+                .peak_rate_mcps = {
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u, 0u, 0u, 0u, 0u, 0u, 0u, 0u,
+                    0u
+                }
+            }
+        }
+    },
 };
 
 #endif /* VL53L1_CALIBRATION_CONFIG_H */

App/app_tasks.c

@@ -440,6 +440,10 @@ void AppTasks_Init(void)
     .w_max             = PARAM_CTRL_W_MAX,           /* 角速度限幅 */
     .v_max             = PARAM_CTRL_V_MAX,           /* 线速度限幅 */
     .speed_reduction_k = PARAM_CTRL_SPEED_REDUCTION, /* 调优：弯道减速系数 */
+    .exit_front_dist   = PARAM_CTRL_EXIT_FRONT_DIST, /* 调优：出沟检测距离 */
+    .wall_escape_dist  = PARAM_CTRL_WALL_ESCAPE_DIST,
+    .wall_escape_kp    = PARAM_CTRL_WALL_ESCAPE_KP,
+    .wall_escape_w_max = PARAM_CTRL_WALL_ESCAPE_WMAX,
   };
   CorridorCtrl_Init(&ctrl_cfg);
 
@@ -470,13 +474,26 @@ void AppTasks_Init(void)
     .reacquire_v            = PARAM_GNAV_REACQUIRE_V,
     .reacquire_conf_thresh  = PARAM_GNAV_REACQUIRE_CONF,
     .reacquire_width_tol    = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
+    .reacquire_min_odom     = PARAM_GNAV_REACQUIRE_MIN_ODOM,
     .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
     .reacquire_timeout_ms   = PARAM_GNAV_REACQUIRE_TIMEOUT,
+    .align_kp_th            = PARAM_GNAV_ALIGN_KP_TH,
+    .align_kp_y             = PARAM_GNAV_ALIGN_KP_Y,
+    .align_th_tol_rad       = PARAM_GNAV_ALIGN_TH_TOL,
+    .align_y_tol_m          = PARAM_GNAV_ALIGN_Y_TOL,
+    .align_confirm_ticks    = PARAM_GNAV_ALIGN_TICKS,
+    .align_timeout_ms       = PARAM_GNAV_ALIGN_TIMEOUT,
+    .reacquire_min_back_dist = PARAM_GNAV_REACQUIRE_MIN_BACK,
     .corridor_end_detect_dist = PARAM_GNAV_CORRIDOR_END_DIST,
     .corridor_length_max    = PARAM_GNAV_CORRIDOR_MAX_LEN,
     .link_v                 = PARAM_GNAV_LINK_V,
     .link_distance          = PARAM_GNAV_LINK_DISTANCE,
     .link_timeout_ms        = PARAM_GNAV_LINK_TIMEOUT,
+    .link_gap_runout        = PARAM_GNAV_LINK_GAP_RUNOUT,
+    .link_wall_target       = PARAM_GNAV_LINK_WALL_TARGET,
+    .link_wall_kp           = PARAM_GNAV_LINK_WALL_KP,
+    .link_wall_heading_kp   = PARAM_GNAV_LINK_WALL_HEADING_KP,
+    .link_wall_blend        = PARAM_GNAV_LINK_WALL_BLEND,
     .exit_v                 = PARAM_GNAV_EXIT_V,
     .exit_runout            = PARAM_GNAV_EXIT_RUNOUT,
     .exit_max_dist          = PARAM_GNAV_EXIT_MAX_DIST,

App/est/corridor_filter.c

@@ -146,3 +146,22 @@ void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad)
     s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
     s_imu_yaw_ref_set = true;
 }
+
+void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad)
+{
+    if (!s_initialized) return;
+
+    CorridorEKF_ResetHeading();
+    s_imu_yaw_ref_rad = imu_yaw_continuous_rad;
+    s_imu_yaw_ref_set = true;
+}
+
+void CorridorFilter_CorrectIMUReference(float heading_correction_rad)
+{
+    if (!s_initialized || !s_imu_yaw_ref_set) return;
+
+    /* 修正IMU参考值，用于消除转向系统性偏差
+     * 例如：VL53检测到车头偏右2度，heading_correction_rad = -0.035 rad
+     * 修正后，EKF会认为当前IMU方向才是正确的走廊方向 */
+    s_imu_yaw_ref_rad += heading_correction_rad;
+}

App/est/corridor_filter.h

@@ -61,6 +61,24 @@ extern "C" {
    */
   void CorridorFilter_RebaseAfterTurnaround(float imu_yaw_continuous_rad);
 
+  /**
+   * @brief 仅重建航向参考，不改横向位置
+   *
+   * 用于进入新垄沟后，已经通过侧墙把车头摆正，
+   * 这时把当前 IMU yaw 设为新的走廊参考，同时仅清零 e_th。
+   */
+  void CorridorFilter_RebaseHeading(float imu_yaw_continuous_rad);
+
+  /**
+   * @brief 修正IMU航向参考值（用于消除转向系统性偏差）
+   *
+   * 用于赛道模式转向后，用VL53检测到的墙壁航向误差修正IMU参考值
+   * 只修正参考值，不改变当前状态估计
+   * 
+   * @param heading_correction_rad 航向修正量 (rad)，正值表示车头需要左转
+   */
+  void CorridorFilter_CorrectIMUReference(float heading_correction_rad);
+
 #ifdef __cplusplus
 }
 #endif

App/nav/corridor_ctrl.c

@@ -32,6 +32,17 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
         return;
     }
 
+    /* ========================================================
+     * 出沟保护: 当前激光检测到接近出口时，停止使用左右激光控制
+     * 避免出沟时左右激光数据突变导致车身大幅度转向
+     * ======================================================== */
+    bool near_exit = false;
+    if ((obs->valid_mask & (1U << 4)) != 0U) {  /* 前激光有效 (bit 4) */
+        if (obs->d_front <= s_cfg.exit_front_dist) {
+            near_exit = true;
+        }
+    }
+
     /* ========================================================
      * 核心控制律:
      *   w_cmd = kp_theta * e_th  +  kd_theta * (-imu_wz)  +  kp_y * e_y
@@ -40,11 +51,79 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
      * - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
      *   用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
      * - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
+     *
+     * 出沟保护: 接近出口时，仅使用航向保持，不使用横向和角度纠偏
      * ======================================================== */
 
-    float w_cmd = -(s_cfg.kp_theta * state->e_th
+    float w_cmd;
+    bool escape_active = false;
+    if (near_exit) {
+        /* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
+        w_cmd = -(s_cfg.kd_theta * imu_wz);
+    } else {
+        /*
+         * 简化原则：
+         * - 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);
+
+        /* ========================================================
+         * 近墙脱离保护:
+         *   当某一侧平均距离已经明显过小，说明车身已经在擦壁或即将擦壁。
+         *   此时不能只等 EKF 慢慢回中，直接叠加一个远离墙面的转向保护项。
+         * ======================================================== */
+        {
+            bool left_front_ok  = ((obs->valid_mask & (1U << 0)) != 0U);
+            bool left_rear_ok   = ((obs->valid_mask & (1U << 1)) != 0U);
+            bool right_front_ok = ((obs->valid_mask & (1U << 2)) != 0U);
+            bool right_rear_ok  = ((obs->valid_mask & (1U << 3)) != 0U);
+            float w_escape = 0.0f;
+            float left_min = 10.0f;
+            float right_min = 10.0f;
+
+            if (left_front_ok && obs->d_lf < left_min) left_min = obs->d_lf;
+            if (left_rear_ok  && obs->d_lr < left_min) left_min = obs->d_lr;
+            if (right_front_ok && obs->d_rf < right_min) right_min = obs->d_rf;
+            if (right_rear_ok  && obs->d_rr < right_min) right_min = obs->d_rr;
+
+            if (left_min < s_cfg.wall_escape_dist) {
+                float err = s_cfg.wall_escape_dist - left_min;
+                w_escape -= s_cfg.wall_escape_kp * err;  /* 左侧很近 -> 轻微右转 */
+                escape_active = true;
+            }
+
+            if (right_min < s_cfg.wall_escape_dist) {
+                float err = s_cfg.wall_escape_dist - right_min;
+                w_escape += s_cfg.wall_escape_kp * err;  /* 右侧很近 -> 轻微左转 */
+                escape_active = true;
+            }
+
+            w_escape = clampf(w_escape, -s_cfg.wall_escape_w_max, s_cfg.wall_escape_w_max);
+            w_cmd += w_escape;
+        }
+    }
 
     /* 角速度限幅：防止 PD 溢出导致原地打转 */
     w_cmd = clampf(w_cmd, -s_cfg.w_max, s_cfg.w_max);
@@ -59,6 +138,11 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
     float speed_reduction = s_cfg.speed_reduction_k * fabsf(w_cmd) / s_cfg.w_max;
     float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);
 
+    /* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */
+    if (escape_active) {
+        v_cmd *= 0.80f;
+    }
+
     /* 线速度限幅：不允许倒车，不允许超速 */
     v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);
 
@@ -66,11 +150,13 @@ void CorridorCtrl_Compute(const CorridorState_t *state,
      * 置信度降级保护:
      *   当滤波器健康度 conf 过低（两边雷达全瞎），
      *   说明走廊参照完全丢失，降低线速度防止盲飞
+     *
+     *   注意：阈值不宜过高，否则会过度降级导致控制器失效
      * ======================================================== */
-    if (state->conf < 0.3f) {
+    if (state->conf < 0.2f) {
         /* 健康度极低：速度打三折，保持航向惯性滑行 */
         v_cmd *= 0.3f;
-    } else if (state->conf < 0.6f) {
+    } else if (state->conf < 0.4f) {
         /* 健康度较低（单侧退化）：速度打七折 */
         v_cmd *= 0.7f;
     }

App/nav/corridor_ctrl.h

@@ -16,6 +16,11 @@ typedef struct {
     float w_max;              // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
     float v_max;              // 线速度输出硬限幅 (m/s)
     float speed_reduction_k;  // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
+    
+    float exit_front_dist;    // 出沟检测距离 (m)，前激光小于此值时禁用左右激光控制
+    float wall_escape_dist;   // 近墙脱离阈值 (m)，小于此值触发直接远离墙面
+    float wall_escape_kp;     // 近墙脱离增益 (rad/s per m)
+    float wall_escape_w_max;  // 近墙脱离角速度限幅 (rad/s)
 } CorridorCtrlConfig_t;
 
 #ifdef __cplusplus

App/nav/global_nav.c

@@ -38,6 +38,7 @@ static struct {
     float    turn_start_yaw_deg;        /* IMU yaw_continuous at turn start */
     float    turn_target_delta_deg;     /* 90° */
     int8_t   turn_sign;                 /* +1 (CCW) or -1 (CW) */
+    float    turn_goal_yaw_deg;         /* 本次转向完成后的名义目标航向 */
 
     /* 里程 (用里程计积分距离) */
     float    stage_entry_odom_vx_accum; /* 进入阶段时的里程计累计距离 */
@@ -49,6 +50,12 @@ static struct {
     /* 重捕获 */
     uint8_t  reacquire_ok_count;
 
+    /* 对齐 */
+    uint8_t  align_ok_count;
+    uint8_t  wall_heading_stable_count;
+    float    wall_heading_prev_rad;
+    bool     wall_heading_prev_valid;
+
     /* 出场 */
     bool     exit_vl53_lost;
     float    exit_lost_distance;
@@ -60,6 +67,8 @@ static struct {
     float    link_d_front_start;    /* 进入连接段时前激光读数 (m) */
     bool     link_d_front_valid;    /* 进入时前激光是否有效 */
     uint8_t  link_gap_count;        /* 非围栏侧 VL53 连续丢失计数 (沟口确认) */
+    bool     link_gap_seen;         /* 是否已经确认看到下一个沟口 */
+    float    link_gap_seen_odom;    /* 看到沟口时的累计里程 */
 
     /* EKF 进度保存 */
     float    corridor_entry_s;
@@ -87,6 +96,21 @@ static inline float gnav_fabsf(float x)
     return x < 0.0f ? -x : x;
 }
 
+typedef struct {
+    bool  sides_complete;
+    bool  near_sat;
+    bool  width_ok;
+    bool  heading_valid;
+    bool  diagonal_conflict;
+    bool  low_yaw_rate;
+    bool  safe_for_align;
+    bool  need_align;
+    float d_left_avg;
+    float d_right_avg;
+    float e_y_m;
+    float heading_rad;
+} CorridorPoseEval_t;
+
 /** 简单 P 控制航向保持，输入偏差 (deg)，输出角速度 (rad/s) */
 static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
 {
@@ -127,19 +151,20 @@ static bool gap_detected_on_open_side(const CorridorObs_t* obs,
                                        TravelDirection_t prev_travel_dir)
 {
     if (prev_travel_dir == TRAVEL_DIR_EAST) {
-        /* 在右端通道，右侧贴围栏 → 检查左侧 VL53 */
+        /* 在右端通道，右侧贴围栏 → 检查左侧 VL53 
+         * 修改为"与"逻辑：前后都丢失才算沟口，避免单个传感器失效导致误判 */
         bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
                        || (obs->d_lf > 0.5f);  /* >50cm 视为沟口 (正常贴壁约10cm) */
         bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
                        || (obs->d_lr > 0.5f);
-        return lf_lost || lr_lost;
+        return lf_lost && lr_lost;  /* 前后都丢失才算沟口 */
     } else {
         /* 在左端通道，左侧贴围栏 → 检查右侧 VL53 */
         bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF)
                        || (obs->d_rf > 0.5f);
         bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR)
                        || (obs->d_rr > 0.5f);
-        return rf_lost || rr_lost;
+        return rf_lost && rr_lost;  /* 前后都丢失才算沟口 */
     }
 }
 
@@ -151,26 +176,145 @@ static bool all_side_lost(const CorridorObs_t* obs)
     return (obs->valid_mask & side_mask) == 0U;
 }
 
+/** 计算墙壁航向误差（用于转向后微调） */
+static bool compute_wall_heading_error(const CorridorObs_t* obs, float* out_heading_rad)
+{
+    const float sensor_base = PARAM_SENSOR_BASE_LENGTH;
+
+    bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
+                   ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
+    bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
+                    ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
+
+    /* 赛道模式这里要求双侧都有效，单侧/退化数据不允许触发航向摆正。 */
+    if (!left_ok || !right_ok) {
+        return false;
+    }
+
+    float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base);
+    float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base);
+
+    /* 双侧估计必须基本一致，避免沟口边缘/单边异常导致突然大打角。 */
+    if (gnav_fabsf(left_heading - right_heading) > PARAM_DEG2RAD(8.0f)) {
+        return false;
+    }
+
+    *out_heading_rad = 0.5f * (left_heading + right_heading);
+    return true;
+}
+
+static void evaluate_corridor_pose(const CorridorObs_t* obs,
+                                   const RobotBlackboard_t* board,
+                                   CorridorPoseEval_t* out_eval)
+{
+    memset(out_eval, 0, sizeof(*out_eval));
+
+    bool left_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U) &&
+                   ((obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U);
+    bool right_ok = ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U) &&
+                    ((obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U);
+
+    out_eval->sides_complete = left_ok && right_ok;
+    if (!out_eval->sides_complete) {
+        return;
+    }
+
+    out_eval->d_left_avg = (obs->d_lf + obs->d_lr) * 0.5f;
+    out_eval->d_right_avg = (obs->d_rf + obs->d_rr) * 0.5f;
+    out_eval->e_y_m = 0.5f * (out_eval->d_right_avg - out_eval->d_left_avg);
+
+    {
+        float total_width = out_eval->d_left_avg + out_eval->d_right_avg + PARAM_ROBOT_WIDTH;
+        float width_err = gnav_fabsf(total_width - s_nav.cfg.corridor_width);
+        out_eval->width_ok = (width_err <= s_nav.cfg.reacquire_width_tol);
+    }
+
+    {
+        float sat_eps = 0.002f;
+        out_eval->near_sat = (obs->d_lf <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) ||
+                             (obs->d_lr <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) ||
+                             (obs->d_rf <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps)) ||
+                             (obs->d_rr <= (PARAM_VL53_SIDE_SAT_NEAR_M + sat_eps));
+    }
+
+    {
+        float near_thresh = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH) - 0.015f;
+        bool lf_near = obs->d_lf < near_thresh;
+        bool lr_near = obs->d_lr < near_thresh;
+        bool rf_near = obs->d_rf < near_thresh;
+        bool rr_near = obs->d_rr < near_thresh;
+
+        /* 对角贴墙模式：大角度进沟时常见，此时 wall heading 几何容易失真。 */
+        out_eval->diagonal_conflict = (lf_near && rr_near) || (rf_near && lr_near);
+    }
+
+    {
+        float sensor_base = PARAM_SENSOR_BASE_LENGTH;
+        float left_heading = atan2f(obs->d_lr - obs->d_lf, sensor_base);
+        float right_heading = atan2f(obs->d_rf - obs->d_rr, sensor_base);
+
+        if (gnav_fabsf(left_heading - right_heading) <= PARAM_DEG2RAD(8.0f)) {
+            out_eval->heading_rad = 0.5f * (left_heading + right_heading);
+            out_eval->heading_valid = out_eval->width_ok && !out_eval->near_sat;
+        }
+    }
+
+    if (board != NULL && board->imu_wz.is_valid) {
+        out_eval->low_yaw_rate = gnav_fabsf(board->imu_wz.value) < 25.0f;
+    }
+
+    {
+        float half_gap = 0.5f * (s_nav.cfg.corridor_width - PARAM_ROBOT_WIDTH);
+        float min_side = (out_eval->d_left_avg < out_eval->d_right_avg)
+                         ? out_eval->d_left_avg : out_eval->d_right_avg;
+        bool heading_bad = out_eval->heading_valid &&
+                           (gnav_fabsf(out_eval->heading_rad) > s_nav.cfg.align_th_tol_rad);
+        bool near_wall = min_side < (half_gap - s_nav.cfg.align_y_tol_m);
+        bool off_center = gnav_fabsf(out_eval->e_y_m) > s_nav.cfg.align_y_tol_m;
+
+        out_eval->need_align = heading_bad || near_wall || off_center || out_eval->diagonal_conflict;
+    }
+
+    out_eval->safe_for_align = out_eval->heading_valid &&
+                               !out_eval->diagonal_conflict &&
+                               out_eval->low_yaw_rate;
+}
+
+static void update_wall_heading_stability(bool valid, float heading_rad)
+{
+    if (!valid) {
+        s_nav.wall_heading_stable_count = 0;
+        s_nav.wall_heading_prev_valid = false;
+        return;
+    }
+
+    if (s_nav.wall_heading_prev_valid &&
+        gnav_fabsf(heading_rad - s_nav.wall_heading_prev_rad) <= PARAM_DEG2RAD(4.0f)) {
+        if (s_nav.wall_heading_stable_count < 255U) {
+            s_nav.wall_heading_stable_count++;
+        }
+    } else {
+        s_nav.wall_heading_stable_count = 1;
+    }
+
+    s_nav.wall_heading_prev_rad = heading_rad;
+    s_nav.wall_heading_prev_valid = true;
+}
+
 /** 检查重捕获条件 */
 static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
 {
-    /* 条件 1: 至少 3 个侧向传感器有效 */
+    /* 条件 1: 四个侧向传感器都有效
-    uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
+     * 赛道模式的重捕获要更稳，不能接受 3/4 这种退化几何。 */
-                        CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
-    uint8_t active = obs->valid_mask & side_mask;
-    int count = 0;
-    for (int i = 0; i < 4; i++) {
-        if (active & (1U << i)) count++;
-    }
-    if (count < 3) return false;
-
-    /* 条件 2: 左右距离和 ≈ 走廊宽度 */
     bool left_ok  = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
                     (obs->valid_mask & CORRIDOR_OBS_MASK_LR);
     bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
                     (obs->valid_mask & CORRIDOR_OBS_MASK_RR);
 
-    if (left_ok && right_ok) {
+    if (!left_ok || !right_ok) return false;
+
+    /* 条件 2: 左右距离和 ≈ 走廊宽度，且这里是必检项 */
+    {
         float d_left  = (obs->d_lf + obs->d_lr) * 0.5f;
         float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
         float total_width = d_left + d_right + PARAM_ROBOT_WIDTH;
@@ -201,6 +345,7 @@ static const char* const s_stage_names[] = {
     "ENTRY_STRAIGHT",
     "TURN_INTO_CORRIDOR",
     "REACQUIRE",
+    "ALIGN",
     "CORRIDOR_TRACK",
     "TURN_OUT",
     "LINK_STRAIGHT",
@@ -294,6 +439,10 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
     s_nav.stage_start_ms = s_last_update_ms;
     s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum;
     s_nav.reacquire_ok_count = 0;
+    s_nav.align_ok_count = 0;
+    s_nav.wall_heading_stable_count = 0;
+    s_nav.wall_heading_prev_rad = 0.0f;
+    s_nav.wall_heading_prev_valid = false;
 
     switch (next) {
     case GNAV_ENTRY_STRAIGHT:
@@ -306,6 +455,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
         s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
         s_nav.turn_start_yaw_deg = imu_yaw;
         s_nav.turn_target_delta_deg = 90.0f;
+        s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f;
         break;
     }
     case GNAV_TURN_OUT_OF_CORRIDOR: {
@@ -313,6 +463,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
         s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
         s_nav.turn_start_yaw_deg = imu_yaw;
         s_nav.turn_target_delta_deg = 90.0f;
+        s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f;
         break;
     }
     case GNAV_TURN_INTO_NEXT: {
@@ -321,6 +472,7 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
         s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
         s_nav.turn_start_yaw_deg = imu_yaw;
         s_nav.turn_target_delta_deg = 90.0f;
+        s_nav.turn_goal_yaw_deg = s_nav.turn_start_yaw_deg + (float)s_nav.turn_sign * 90.0f;
         s_nav.current_corridor_id = next_id;
         break;
     }
@@ -339,6 +491,8 @@ static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
         s_nav.link_d_front_start = 0.0f;
         s_nav.link_d_front_valid = false;  /* 首拍再记录 */
         s_nav.link_gap_count = 0;
+        s_nav.link_gap_seen = false;
+        s_nav.link_gap_seen_odom = 0.0f;
         break;
 
     case GNAV_EXIT_STRAIGHT:
@@ -501,7 +655,7 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
         break;
 
     /* ============================================================
-     * 三种转向状态统一处理
+     * 转向进入下一条沟 (原地转 90°)
      * ============================================================ */
     case GNAV_TURN_INTO_CORRIDOR:
     case GNAV_TURN_OUT_OF_CORRIDOR:
@@ -511,27 +665,122 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
 
     /* ============================================================
      * 重捕获走廊
+     * 
+     * 问题背景：
+     *   转弯刚完成时，车身可能还在沟口外（端部通道内），
+     *   但此时两侧VL53同样能测到两侧垄背端面（~10cm），
+     *   宽度和 = 10+10+20 = 40cm，与真正在沟内完全一致。
+     *   这导致尚未入沟就满足重捕获条件，切到ALIGN，
+     *   EKF的e_y可能是大偏差，车卡死在入口。
+     *
+     * 修复方案：
+     *   引入后部激光距离检测：只有后激光距离 > 40cm，
+     *   说明车身已完全进入沟内，才允许开始确认计数。
+     *   这确保四颗VL53全部脱离沟口边缘进入稳定侧壁区域。
      * ============================================================ */
     case GNAV_REACQUIRE:
         out->override_v  = s_nav.cfg.reacquire_v;
-        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
+        out->override_w  = 0.0f;  /* 不做航向控制，让车自然进沟，ALIGN阶段再摆正 */
-                                            s_nav.cfg.heading_kp);
         out->safety_mode = SAFETY_MODE_STRAIGHT;
 
-        if (check_reacquire(obs, state)) {
+        {
+            /* 进沟深度守卫：后激光 + 最小入沟里程 双保险。
+             * 尤其对 C1，不能只靠后激光，否则在入口区就可能过早假成功。 */
+            bool back_ok = false;
+            bool odom_ok = (odom_since_entry() >= s_nav.cfg.reacquire_min_odom);
+            if ((obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U) {
+                if (obs->d_back > s_nav.cfg.reacquire_min_back_dist) {
+                    back_ok = true;
+                }
+            }
+
+            if (back_ok && odom_ok && check_reacquire(obs, state)) {
                 s_nav.reacquire_ok_count++;
             } else {
+                /* 深度不足 / 里程不足 / 条件未满足 → 计数严格清零，
+                 * 防止在沟口处积累部分计数后误触发。 */
                 s_nav.reacquire_ok_count = 0;
             }
+        }
 
         if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
-            transition_to(GNAV_CORRIDOR_TRACK, board);
+            /* 简化：重捕获成功后统一停车摆正航向。
+             * 不再在这里用墙姿态决定“摆不摆正”，避免不同沟表现不一致。 */
+            transition_to(GNAV_ALIGN, board);
         }
         if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
-            transition_to(GNAV_ERROR, board);
+            /* 取消重捕获失败态：超时后不进 ERROR，
+             * 直接转入短暂停车摆正阶段，让车停下来继续自恢复。 */
+            transition_to(GNAV_ALIGN, board);
         }
         break;
 
+    /* ============================================================
+     * 短暂停车摆正航向 (ALIGN)
+     *
+     * 只用 side VL53 的前后差来摆正车头，不做横向居中。
+     * 目的不是把车居中，而是避免“斜着进沟时前轮先蹭墙，
+     * 控制器还继续朝墙边打”的情况。
+     * ============================================================ */
+    case GNAV_ALIGN: {
+        float imu_yaw_err_rad = 0.0f;
+        bool imu_align_ok = false;
+
+        if (board->imu_yaw_continuous.is_valid) {
+            imu_yaw_err_rad = PARAM_DEG2RAD(s_nav.turn_goal_yaw_deg - board->imu_yaw_continuous.value);
+            while (imu_yaw_err_rad > 3.14159265f) imu_yaw_err_rad -= 6.2831853f;
+            while (imu_yaw_err_rad < -3.14159265f) imu_yaw_err_rad += 6.2831853f;
+
+            if (gnav_fabsf(imu_yaw_err_rad) < s_nav.cfg.align_th_tol_rad &&
+                (!board->imu_wz.is_valid || gnav_fabsf(board->imu_wz.value) < 15.0f)) {
+                imu_align_ok = true;
+            }
+        }
+
+        out->override_v  = 0.0f;
+        out->use_override = true;
+        out->request_corridor = false;
+        out->safety_mode = SAFETY_MODE_STRAIGHT;
+
+        if (board->imu_yaw_continuous.is_valid) {
+            /* imu_yaw_err_rad = target - current
+             * 误差为正时，需要正角速度(左转/逆时针)去追目标，不能取反。 */
+            float w_align_imu = 1.2f * imu_yaw_err_rad;
+            out->override_w = gnav_clampf(w_align_imu, -0.18f, 0.18f);
+        } else {
+            out->override_w = 0.0f;
+        }
+
+        if (imu_align_ok) {
+            s_nav.align_ok_count++;
+        } else {
+            s_nav.align_ok_count = 0;
+        }
+
+        if (s_nav.align_ok_count >= s_nav.cfg.align_confirm_ticks) {
+            if (board->imu_yaw_continuous.is_valid) {
+                CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
+                s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
+            }
+            transition_to(GNAV_CORRIDOR_TRACK, board);
+        }
+        if (elapsed_ms > 400U) {
+            if (board->imu_yaw_continuous.is_valid && imu_align_ok) {
+                CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
+                s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
+                transition_to(GNAV_CORRIDOR_TRACK, board);
+            }
+        }
+        if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
+            if (board->imu_yaw_continuous.is_valid) {
+                CorridorFilter_RebaseHeading(board->imu_yaw_continuous.value * 0.01745329252f);
+                s_nav.heading_ref_deg = board->imu_yaw_continuous.value;
+            }
+            transition_to(GNAV_CORRIDOR_TRACK, board);
+        }
+        break;
+    }
+
     /* ============================================================
      * 沟内闭环跟踪 (交给 corridor_ctrl)
      * ============================================================ */
@@ -540,11 +789,9 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
         out->request_corridor = true;
         out->safety_mode      = SAFETY_MODE_CORRIDOR;
 
-        /* 到端检测 */
         {
             bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
             if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
-                /* 里程下限保护: 至少走了 1.0m 才允许认定到端，避免假阳性 */
                 float corridor_odom = odom_since_entry();
                 if (corridor_odom > 1.0f) {
                     s_nav.corridors_completed++;
@@ -552,7 +799,7 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
                 }
             }
         }
-        /* 里程超长保护 */
+
         if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
             s_nav.corridors_completed++;
             transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
@@ -560,91 +807,152 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
         break;
 
     /* ============================================================
-     * 连接段直行 (纵向端部通道北行, 方案2: 三信号联合判定)
+     * 连接段直行 (端部通道内，从一条沟到下一条沟)
      *
-     * 传感器情况（转向完成后面朝北）：
+     * 控制策略：IMU 决定主航向；单边 VL53 只做离墙保底。
-     *   - 前激光（朝北）→ 上围栏，d_front 随北行递减      [精度高]
+     * 也就是说：
-     *   - 后激光（朝南）→ 下围栏，d_back 随北行递增       [精度高]
+     *   - 默认纯航向保持
-     *   - 围栏侧 VL53 → 始终有效 (~10cm)                [不用于判定]
+     *   - 只有当贴围栏侧距离小于阈值时，才附加一个远离墙面的修正
-     *   - 非围栏侧 VL53 → 贴垄背端面时有效，到垄沟开口时丢失  [沟口标志]
+     *   - 不做墙跟随融合，避免单边 VL53 把主方向带偏
-     *
-     * 信号定义：
-     *   A: 里程计     odom >= link_distance * 0.85  (打滑衰减, 权重低)
-     *   B: 前激光变化  d_front缩小 >= link_distance * 0.85 (权重高)
-     *   C: 非围栏侧VL53 丢失/跳到>50cm，连续2拍确认  (直接探测沟口, 权重中)
-     *
-     * 触发逻辑: B || (A && C)
-     *   - 前激光变化量足够 → 直接触发（最可靠的单一信号）
-     *   - 里程计到位 + VL53探到沟口 → 联合触发（互相校验）
      * ============================================================ */
-    case GNAV_LINK_STRAIGHT:
+    case GNAV_LINK_STRAIGHT: {
         out->override_v  = s_nav.cfg.link_v;
-        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
-                                            s_nav.cfg.heading_kp);
         out->safety_mode = SAFETY_MODE_STRAIGHT;
 
+        float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
+                                          s_nav.cfg.heading_kp);
+        float w_wall_guard = 0.0f;
+        float w_wall_parallel = 0.0f;
+        const CorridorDescriptor_t* corridor = TrackMap_GetCorridor(s_nav.current_corridor_id);
+
+        if (corridor != NULL && corridor->travel_dir == TRAVEL_DIR_EAST) {
+            if ((obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
+                (obs->valid_mask & CORRIDOR_OBS_MASK_RR)) {
+                float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
+                if (d_right < s_nav.cfg.link_wall_target) {
+                    float error = s_nav.cfg.link_wall_target - d_right;
+                    w_wall_guard = s_nav.cfg.link_wall_kp * error;
+                }
+
+                /* 右端通道：右侧是围栏。
+                 * 用右前/右后距离差把车身摆成与围栏平行，
+                 * 避免“不蹭墙但车身是歪的”。 */
+                {
+                    float heading_right = atan2f(obs->d_rf - obs->d_rr, PARAM_SENSOR_BASE_LENGTH);
+                    w_wall_parallel = -s_nav.cfg.link_wall_heading_kp * heading_right;
+                }
+            }
+        } else if (corridor != NULL) {
+            if ((obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
+                (obs->valid_mask & CORRIDOR_OBS_MASK_LR)) {
+                float d_left = (obs->d_lf + obs->d_lr) * 0.5f;
+                if (d_left < s_nav.cfg.link_wall_target) {
+                    float error = s_nav.cfg.link_wall_target - d_left;
+                    w_wall_guard = -(s_nav.cfg.link_wall_kp * error);
+                }
+
+                /* 左端通道：左侧是围栏。 */
+                {
+                    float heading_left = atan2f(obs->d_lr - obs->d_lf, PARAM_SENSOR_BASE_LENGTH);
+                    w_wall_parallel = -s_nav.cfg.link_wall_heading_kp * heading_left;
+                }
+            }
+        }
+
+        w_wall_parallel = gnav_clampf(w_wall_parallel, -0.20f, 0.20f);
+        out->override_w = gnav_clampf(w_heading + w_wall_guard + w_wall_parallel, -1.0f, 1.0f);
+
         {
             bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
 
-            /* 首拍记录前激光基准值 */
             if (!s_nav.link_d_front_valid && front_valid) {
                 s_nav.link_d_front_start = obs->d_front;
                 s_nav.link_d_front_valid = true;
             }
 
-            /* ---- 信号 A: 里程计 (考虑打滑, 用 85% 容差) ---- */
+            /* 连接段入口判定原则：
-            bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;
+             *   用“看到开口”的那一刻作为唯一锚点，然后固定前冲一小段再转向。
+             *   这样每次到沟入口的位置更一致，不再混用多套主触发条件。
+             */
+            bool odom_guard_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.55f;
 
-            /* ---- 信号 B: 前激光变化量 (高精度) ---- */
+            /* 前激光仅保留为兜底，不再参与主触发。 */
             bool laser_ok = false;
             if (s_nav.link_d_front_valid && front_valid) {
                 float d_front_delta = s_nav.link_d_front_start - obs->d_front;
-                /* 车身中心到前激光有偏置(FRONT_LASER_OFFSET)，但这里用的是差值，偏置抵消 */
+                laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 1.05f);
-                laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
             }
 
-            /* ---- 信号 C: 非围栏侧 VL53 沟口检测 (需连续2拍确认) ----
+            bool gap_confirmed = false;
-             *
+            if (corridor != NULL) {
-             * 判定阈值 0.5m 的来源:
+                bool gap_now = gap_detected_on_open_side(obs, corridor->travel_dir);
-             *   正常贴垄背端面时 VL53 读数 ≈ (通道宽/2 - 车宽/2 - VL53内缩)
-             *                               = (0.40/2 - 0.20/2 - 0.0)
-             *                               = 0.10m
-             *   到垄沟开口时 VL53 读数 > 1.2m (超出有效距离) 或无效
-             *   阈值 0.5m 在两者之间，足够区分
-             */
-            const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
-            /* 在 LINK_STRAIGHT 阶段，current_corridor_id 仍是刚走完的沟 (尚未更新)
-               所以 cd->travel_dir 就是刚走完那条沟的方向，直接用来判断当前在哪个端部通道 */
-            bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir);
-
                 if (gap_now) {
-                if (s_nav.link_gap_count < 255) s_nav.link_gap_count++;
+                    if (s_nav.link_gap_count < 255U) s_nav.link_gap_count++;
                 } else {
                     s_nav.link_gap_count = 0;
                 }
-            bool gap_confirmed = (s_nav.link_gap_count >= 2);  /* 连续2拍 (40ms @ 20ms周期) */
+                gap_confirmed = (s_nav.link_gap_count >= 5U);
 
-            /* ---- 联合判定: B || (A && C) ---- */
+                if (odom_guard_ok && gap_confirmed && !s_nav.link_gap_seen) {
-            if (laser_ok || (odom_ok && gap_confirmed))
+                    s_nav.link_gap_seen = true;
-            {
+                    s_nav.link_gap_seen_odom = s_nav.odom_distance_accum;
+                }
+            }
+
+            bool gap_runout_ok = false;
+            if (s_nav.link_gap_seen) {
+                gap_runout_ok = (s_nav.odom_distance_accum - s_nav.link_gap_seen_odom)
+                                >= s_nav.cfg.link_gap_runout;
+            }
+
+            if (s_nav.link_gap_seen && gap_runout_ok) {
+                transition_to(GNAV_TURN_INTO_NEXT, board);
+            } else if (!s_nav.link_gap_seen && laser_ok) {
+                /* 极端兜底：如果开口检测失效，才允许前激光变化量接管。 */
                 transition_to(GNAV_TURN_INTO_NEXT, board);
             }
         }
+
         if (elapsed_ms > s_nav.cfg.link_timeout_ms) {
             transition_to(GNAV_ERROR, board);
         }
         break;
+    }
 
     /* ============================================================
-     * 出场直行
+     * 出场直行 (左端通道向南返回)
      * ============================================================ */
-    case GNAV_EXIT_STRAIGHT:
+    case GNAV_EXIT_STRAIGHT: {
         out->override_v  = s_nav.cfg.exit_v;
-        out->override_w  = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
-                                            s_nav.cfg.heading_kp);
         out->safety_mode = SAFETY_MODE_STRAIGHT;
 
-        /* 检测侧向全丢 */
+        float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
+                                          s_nav.cfg.heading_kp);
+        float w_wall = 0.0f;
+        bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
+        bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
+
+        if (rf_ok || rr_ok) {
+            float d_right;
+
+            if (rf_ok && rr_ok) {
+                d_right = (obs->d_rf + obs->d_rr) * 0.5f;
+            } else if (rf_ok) {
+                d_right = obs->d_rf;
+            } else {
+                d_right = obs->d_rr;
+            }
+
+            /* 出场阶段在左端通道向南走，右侧仍然是围栏。
+             * 用右侧 VL53 直接叠加一个更硬的保墙约束，避免一路蹭墙。 */
+            {
+                float error = s_nav.cfg.link_wall_target - d_right;
+                w_wall = 1.5f * s_nav.cfg.link_wall_kp * error;
+            }
+        }
+
+        out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f);
+
         if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
             s_nav.exit_vl53_lost = true;
             s_nav.exit_lost_distance = s_nav.odom_distance_accum;
@@ -655,7 +963,6 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
                 transition_to(GNAV_DOCK, board);
             }
         }
-        /* 里程上限保护 */
         if (odom_since_entry() >= s_nav.cfg.exit_max_dist) {
             transition_to(GNAV_DOCK, board);
         }
@@ -663,21 +970,48 @@ void GlobalNav_Update(const CorridorObs_t*      obs,
             transition_to(GNAV_DOCK, board);
         }
         break;
+    }
 
     /* ============================================================
      * 回停启动区
      * ============================================================ */
-    case GNAV_DOCK:
+    case GNAV_DOCK: {
         out->override_v  = s_nav.cfg.dock_v;
-        out->override_w  = 0.0f;
         out->safety_mode = SAFETY_MODE_STRAIGHT;
 
-        if (odom_since_entry() >= s_nav.cfg.dock_distance ||
+        float w_heading = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
-            elapsed_ms > 5000U)
+                                          s_nav.cfg.heading_kp);
+        float w_wall = 0.0f;
+        bool rf_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
+        bool rr_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
+
+        if (rf_ok || rr_ok) {
+            float d_right;
+
+            if (rf_ok && rr_ok) {
+                d_right = (obs->d_rf + obs->d_rr) * 0.5f;
+            } else if (rf_ok) {
+                d_right = obs->d_rf;
+            } else {
+                d_right = obs->d_rr;
+            }
+
+            /* 回停阶段右墙保持更硬：
+             * IMU 仍然给主方向，但右侧距离约束直接叠加，不再做 50% 混合。 */
             {
+                float error = s_nav.cfg.link_wall_target - d_right;
+                w_wall = 1.5f * s_nav.cfg.link_wall_kp * error;
+            }
+        }
+
+        out->override_w = gnav_clampf(w_heading + w_wall, -1.0f, 1.0f);
+
+        if (odom_since_entry() >= s_nav.cfg.dock_distance ||
+            elapsed_ms > 5000U) {
             transition_to(GNAV_FINISHED, board);
         }
         break;
+    }
 
     /* ============================================================
      * 终态

App/nav/global_nav.h

@@ -31,6 +31,7 @@ typedef enum {
     GNAV_ENTRY_STRAIGHT,            /* 入场直线 */
     GNAV_TURN_INTO_CORRIDOR,        /* 转向进入垄沟 (原地转 90°) */
     GNAV_REACQUIRE,                 /* 重捕获走廊 */
+    GNAV_ALIGN,                     /* 捕获成功后停车对齐航线 */
     GNAV_CORRIDOR_TRACK,            /* 沟内闭环跟踪 */
     GNAV_TURN_OUT_OF_CORRIDOR,      /* 到端后出沟转向 (原地转 90°) */
     GNAV_LINK_STRAIGHT,             /* 连接段直行 */
@@ -61,17 +62,32 @@ typedef struct {
     float    reacquire_v;               /* 重捕获速度 m/s */
     float    reacquire_conf_thresh;     /* 重捕获置信度阈值 */
     float    reacquire_width_tol;       /* 走廊宽度容差 m */
+    float    reacquire_min_odom;        /* 重捕获最小入沟里程 m */
     uint8_t  reacquire_confirm_ticks;   /* 连续确认拍数 */
     uint32_t reacquire_timeout_ms;
 
+    /* 对齐 */
+    float    align_kp_th;               /* 对齐航向P增益 (rad/s per rad) */
+    float    align_kp_y;                /* 对齐横向P增益 (rad/s per m) */
+    float    align_th_tol_rad;          /* 对齐航向容差 (rad) */
+    float    align_y_tol_m;             /* 对齐横向容差 (m) */
+    uint8_t  align_confirm_ticks;       /* 对齐确认拍数 */
+    uint32_t align_timeout_ms;          /* 对齐超时 ms */
+    float    reacquire_min_back_dist;   /* 重捕获最小后激光距离 (m)，用于判断是否真正进沟 */
+
     /* 沟内 */
     float    corridor_end_detect_dist;  /* 到端检测距离 m */
     float    corridor_length_max;       /* 沟内里程保护上限 m */
 
     /* 连接段 */
-    float    link_v;                    /* 连接段速度 m/s */
+    float    link_v;
-    float    link_distance;             /* 连接段标称距离 m */
+    float    link_distance;
     uint32_t link_timeout_ms;
+    float    link_gap_runout;          /* 检测到沟口后继续前冲距离 (m) */
+    float    link_wall_target;          /* 墙壁跟随目标距离 (m) */
+    float    link_wall_kp;              /* 墙壁跟随P增益 */
+    float    link_wall_heading_kp;      /* 单边墙平行修正增益 */
+    float    link_wall_blend;           /* 墙壁跟随权重 (0~1) */
 
     /* 出场 */
     float    exit_v;                    /* 出场速度 m/s */

App/preproc/corridor_preproc.c

@@ -19,6 +19,14 @@ static bool process_side_laser(const SensorItem_t *item, float *out_m)
         return false;
     }
 
+    /* 靠墙过近退化：
+     * 侧向 VL53 在最小量程附近（例如实际 3cm、模组最小可信 4cm）时，
+     * 直接判无效会让系统退化成单边观测，反而更容易贴墙。
+     * 这里改成“钳位到最小可信距离”，既保留该侧存在感，又避免把不可信的超近值直接送上去。 */
+    if (dist_m < PREPROC_SAT_NEAR_SIDE_RANGE_M) {
+        dist_m = PREPROC_SAT_NEAR_SIDE_RANGE_M;
+    }
+    
     *out_m = dist_m;
     return true;
 }

App/preproc/corridor_preproc.h

@@ -17,6 +17,9 @@
 /* VL53L0X 侧向雷达的物理有效探测区间 (m) */
 #define PREPROC_MAX_SIDE_RANGE_M  2.0f
 #define PREPROC_MIN_SIDE_RANGE_M  0.02f
+/* 侧向 VL53 靠墙过近时的退化阈值 (m)
+ * 低于此值虽然可能还有数字，但已接近最小量程区，不再作为可信几何量使用。 */
+#define PREPROC_SAT_NEAR_SIDE_RANGE_M PARAM_VL53_SIDE_SAT_NEAR_M
 
 /* 前后向雷达近战盲区阈值 (m) (STP 7cm盲区 + 1cm工程裕量) */
 #define PREPROC_BLIND_ZONE_M      0.08f

App/robot_params.h

@@ -64,7 +64,7 @@ extern "C" {
  *  典型值：根据机械安装，约 0.10~0.15m
  *  影响：航向观测精度 z_eth = atan2((d_back-d_front), L_s)
  */
-#define PARAM_SENSOR_BASE_LENGTH    0.120f
+#define PARAM_SENSOR_BASE_LENGTH    0.0755f
 
 /** @brief [实测] 比赛走廊标准宽度 [m]
  *  测量方法：卷尺测量赛场垄沟实际宽度 (规则 40cm，允许±5% 误差)
@@ -85,7 +85,7 @@ extern "C" {
  *  典型值：根据机械安装，若与车尾齐平则为 0
  *  用途：后向到端距离补偿 d_body_rear = d_sensor - offset
  */
-#define PARAM_REAR_LASER_OFFSET     0.0f
+#define PARAM_REAR_LASER_OFFSET     (-(0.018f))
 
 /** @brief [实测] 侧向 VL53L0X 传感器内缩距离 [m]
  *  测量方法：VL53L0X 发射面到车体侧面最外边缘的距离 (向内为正)
@@ -95,7 +95,7 @@ extern "C" {
  *  ⚠ 极其重要：沟道40cm - 车体20cm = 每边仅10cm，1cm的偏差就是10%的误差！
  *  注意：此值作为未分侧设置时的默认值。建议使用下面的分侧参数。
  */
-#define PARAM_VL53_SIDE_INSET       0.0f
+#define PARAM_VL53_SIDE_INSET       (-(0.018f))
 
 /** @brief [改进A][实测] 左侧 VL53L0X 传感器内缩距离 [m]
  *  测量方法：将机器人精确放在走廊正中央(卷尺确认)，
@@ -112,6 +112,13 @@ extern "C" {
  */
 #define PARAM_VL53_RIGHT_INSET      0.0f
 
+/** @brief [调优] 侧向 VL53 近墙退化阈值 [m]
+ *  含义：当侧向 VL53 读数小于该值时，认为已进入最小量程附近，数据不再可信。
+ *  处理策略：预处理层直接将该侧观测判为无效，让上层退化为主要信另一侧。
+ *  典型值：0.05~0.06
+ */
+#define PARAM_VL53_SIDE_SAT_NEAR_M  0.03f
+
 /* ------------------- 编码器参数 ------------------- */
 /** @brief [实测] 编码器每转脉冲数 (CPR)
  *  测量方法：查阅 F407 固件配置，或实车转动一圈数脉冲
@@ -128,7 +135,7 @@ extern "C" {
  *  1 = VL53L1X
  */
 #ifndef PARAM_VL53_USE_L1X
-#define PARAM_VL53_USE_L1X          0
+#define PARAM_VL53_USE_L1X          1
 #endif
 
 /* =========================================================
@@ -162,7 +169,7 @@ extern "C" {
  *  过大：EKF 认为横向很不稳定，过度依赖观测 -> 震荡
  *  过小：EKF 不信任观测，响应慢
  */
-#define PARAM_EKF_Q_EY              0.01f
+#define PARAM_EKF_Q_EY              0.02f
 
 /** @brief [调优] 航向偏差 e_th 过程噪声方差 [rad²]
  *  含义：航向状态的自然发散速度
@@ -171,7 +178,7 @@ extern "C" {
  *  过大：EKF 认为航向不稳定，IMU 作用被削弱
  *  过小：EKF 过度信任 IMU，雷达观测不起作用
  */
-#define PARAM_EKF_Q_ETH             0.001f
+#define PARAM_EKF_Q_ETH             0.002f
 
 /** @brief [调优] 沿程进度 s 过程噪声方差 [m²]
  *  含义：里程估计的不确定度
@@ -198,7 +205,7 @@ extern "C" {
  *  过大：EKF 不信任侧向雷达，横向响应慢
  *  过小：EKF 过度信任雷达，对跳变敏感
  */
-#define PARAM_EKF_R_EY              0.015f
+#define PARAM_EKF_R_EY              0.007f
 
 /** @brief [调优] 航向观测噪声方差 [rad²]
  *  含义：同侧前后雷达差分测角的可程靠度
@@ -215,8 +222,11 @@ extern "C" {
  *  典型值：0.001~0.01
  *  过大：IMU 航向观测作用弱，e_th 靠 wz 积分漂移
  *  过小：IMU 航向主导过强，短时抖动可能传入
+ *  
+ *  [修正] 如果场地本身是歪的，过度信任IMU会导致车持续擦墙
+ *  增加到0.01，让EKF更多依赖VL53传感器判断相对航向
  */
-#define PARAM_EKF_R_ETH_IMU         0.002f
+#define PARAM_EKF_R_ETH_IMU         0.01f
 
 /* --- EKF 初始状态 --- */
 /** @brief [调优] e_y 初始不确定度 [m²]
@@ -255,7 +265,7 @@ extern "C" {
  *  过大：车头左右摆动 (震荡)
  *  过小：纠偏太慢，容易撞墙
  */
-#define PARAM_CTRL_KP_THETA         2.0f
+#define PARAM_CTRL_KP_THETA         2.5f  /* 适度增加，提高航向纠偏速度 */
 
 /** @brief [调优] 航向微分增益 kd_theta [s]
  *  含义：IMU 角速度阻尼项，抑制车头转动速度
@@ -273,7 +283,7 @@ extern "C" {
  *  过大：横向纠偏过猛，引起震荡
  *  过小：偏了拉不回来
  */
-#define PARAM_CTRL_KP_Y             4.0f
+#define PARAM_CTRL_KP_Y             7.0f  /* 提高横向回中速度，适配更高巡航速度 */
 
 /** @brief [调优] 走廊巡航速度 [m/s]
  *  含义：走廊内正常行驶速度
@@ -302,6 +312,56 @@ extern "C" {
  */
 #define PARAM_CTRL_SPEED_REDUCTION  0.4f
 
+/** @brief [调优] 弯道减速死区 [0~1]
+ *  含义：当 |w|/w_max 小于该比例时，不做减速。
+ *  目的：小幅修正时保持巡航，不要长期因为轻微纠偏而慢速运行。
+ */
+#define PARAM_CTRL_SPEED_RED_DB     0.35f
+
+/** @brief [调优] 角速度变化率限幅 [rad/s^2]
+ *  含义：限制相邻控制周期之间角速度变化，防止一帧突然猛打方向。
+ */
+#define PARAM_CTRL_W_SLEW_RATE      6.0f
+
+/** @brief [调优] 入沟软启动距离 [m]
+ *  含义：进入沟内后的前一小段距离，降低回中力度，避免一入沟就猛打一把。
+ */
+#define PARAM_CTRL_STARTUP_DIST      0.15f
+
+/** @brief [调优] 入沟起始横向增益缩放 [0~1]
+ *  含义：state->s=0 时 kp_y 的缩放比例，随后随距离线性恢复到 1。
+ */
+#define PARAM_CTRL_STARTUP_KPY_SCALE 0.45f
+
+/** @brief [调优] 入沟起始角速度限幅缩放 [0~1]
+ *  含义：state->s=0 时 w_max 的缩放比例，随后随距离线性恢复到 1。
+ */
+#define PARAM_CTRL_STARTUP_W_SCALE   0.45f
+
+/** @brief [调优] 出沟检测距离 [m]
+ *  含义：前激光小于此值时禁用左右激光控制，避免出沟时数据突变导致大幅转向
+ *  典型值：0.40 (40cm)
+ *  过大：提前禁用控制，可能导致出沟前偏离
+ *  过小：禁用太晚，出沟时仍可能受左右激光突变影响
+ */
+#define PARAM_CTRL_EXIT_FRONT_DIST  0.40f
+
+/** @brief [调优] 近墙脱离阈值 [m]
+ *  含义：任一侧平均距离小于此值时，直接叠加远离墙面的保护转向
+ *  目的：不等 EKF 慢慢回中，先把车从擦墙状态拉开
+ */
+#define PARAM_CTRL_WALL_ESCAPE_DIST 0.05f
+
+/** @brief [调优] 近墙脱离增益 [rad/s per m]
+ *  含义：近墙保护的附加角速度增益
+ */
+#define PARAM_CTRL_WALL_ESCAPE_KP   6.0f
+
+/** @brief [调优] 近墙脱离角速度限幅 [rad/s]
+ *  含义：近墙保护项自身的最大角速度，避免一把打太猛
+ */
+#define PARAM_CTRL_WALL_ESCAPE_WMAX 0.25f
+
 /* =========================================================
  * 【P4】安全阈值与状态机参数
  * ========================================================= */
@@ -393,7 +453,7 @@ extern "C" {
  *  1: 使用滤波后的 range_mm_filtered
  *  0: 直接输出原始测距到 range_mm_filtered，便于做 A/B 对比
  */
-#define PARAM_VL53_USE_EMA_FILTER 1
+#define PARAM_VL53_USE_EMA_FILTER 0
 
 /** @brief [调优] VL53L0X EMA滤波平滑系数 alpha
  *  含义：新测量值的权重 (0.0~1.0)
@@ -405,7 +465,7 @@ extern "C" {
  *    0.5 - 快速响应 (延迟约40ms)
  *    0.6 - 极速响应 (延迟约25ms，抖动较大)
  */
-#define PARAM_VL53_EMA_ALPHA        0.4f
+#define PARAM_VL53_EMA_ALPHA        0.6f
 
 /* --- IMU --- */
 /** @brief [实测] HWT101 IMU 安装偏置 (航向) [rad]
@@ -428,6 +488,11 @@ extern "C" {
  */
 #define PARAM_NAV_STARTUP_DELAY_MS    5000U
 
+/** @brief 导航主循环周期 [ms]
+ *  保留为参数形式，当前使用 20ms，与 411 稳定版本一致。
+ */
+#define PARAM_NAV_TASK_PERIOD_MS      20U
+
 /* --- 入场段 --- */
 /* 启动区入口(Y=40)距第一条垄沟(Y=36~39)极近，入场距离仅约 10~40cm */
 #define PARAM_GNAV_ENTRY_V            0.08f   /* m/s  — 入场速度 (沿左端纵向通道向北) */
@@ -436,27 +501,42 @@ extern "C" {
 
 /* --- 转向 --- */
 #define PARAM_GNAV_TURN_OMEGA         1.0f    /* rad/s — 转向角速度 */
-#define PARAM_GNAV_TURN_TOLERANCE     0.087f  /* rad   — 转向完成容差 (~5°) */
+#define PARAM_GNAV_TURN_TOLERANCE     0.052f  /* rad   — 转向完成容差 (~3°) */
-#define PARAM_GNAV_TURN_DECEL_ZONE    0.5f    /* rad   — 接近目标时减速区 (~28°) */
+#define PARAM_GNAV_TURN_DECEL_ZONE    0.70f   /* rad   — 接近目标时减速区 (~40°) */
-#define PARAM_GNAV_TURN_MIN_OMEGA     0.3f    /* rad/s — 减速区最低角速度 */
+#define PARAM_GNAV_TURN_MIN_OMEGA     0.18f   /* rad/s — 减速区最低角速度 */
 #define PARAM_GNAV_TURN_TIMEOUT       8000U   /* ms    — 单次转向超时 */
 
 /* --- 重捕获 --- */
 #define PARAM_GNAV_REACQUIRE_V        0.1f   /* m/s   — 重捕获入沟速度 */
-#define PARAM_GNAV_REACQUIRE_CONF     0.6f    /*        — 重捕获置信度阈值 */
+#define PARAM_GNAV_REACQUIRE_CONF     0.4f    /*        — 重捕获置信度阈值（从0.6降到0.4，更容易成功） */
-#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f  /* m     — 走廊宽度容差 */
+#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.08f  /* m     — 走廊宽度容差（从5cm放宽到8cm） */
-#define PARAM_GNAV_REACQUIRE_TICKS    5       /* 拍    — 连续确认次数 (5×20ms=100ms) */
+#define PARAM_GNAV_REACQUIRE_MIN_ODOM 0.06f  /* m     — 最小入沟里程，快捕获后尽快停车 */
+#define PARAM_GNAV_REACQUIRE_TICKS    5      /* 拍    — 连续确认次数，取更稳的 100ms */
 #define PARAM_GNAV_REACQUIRE_TIMEOUT  5000U   /* ms    — 重捕获超时 */
 
+/* --- 对齐 --- */
+#define PARAM_GNAV_ALIGN_KP_TH        2.0f    /*        — 对齐航向P增益 (rad/s per rad) */
+#define PARAM_GNAV_ALIGN_KP_Y         4.0f    /*        — 对齐横向P增益 (rad/s per m) */
+#define PARAM_GNAV_ALIGN_TH_TOL       0.05f   /* rad    — 对齐航向容差 (~3°) */
+#define PARAM_GNAV_ALIGN_Y_TOL        0.02f   /* m      — 对齐横向容差 (2cm) */
+#define PARAM_GNAV_ALIGN_TICKS        5       /* 拍    — 对齐确认次数 (5×20ms=100ms) */
+#define PARAM_GNAV_ALIGN_TIMEOUT      3000U   /* ms    — 对齐超时 */
+#define PARAM_GNAV_REACQUIRE_MIN_BACK 0.38f   /* m     — 重捕获最小后激光距离，判断是否真正进沟 */
+
 /* --- 沟内 --- */
-#define PARAM_GNAV_CORRIDOR_MAX_LEN   2.50f   /* m     — 沟内里程保护上限 */
+#define PARAM_GNAV_CORRIDOR_MAX_LEN   2.70f   /* m     — 沟内里程保护上限 */
-#define PARAM_GNAV_CORRIDOR_END_DIST  0.05f   /* m     — 到端检测距离 */
+#define PARAM_GNAV_CORRIDOR_END_DIST  0.10f   /* m     — 到端检测距离 */
 
 /* --- 连接段 --- */
 /* 在纵向端部通道里北行，从一条垄沟入口到下一条入口，距离=垄沟宽40cm+垄背宽30cm=70cm */
 #define PARAM_GNAV_LINK_V             0.10f   /* m/s   — 连接段速度 (纵向通道内，IMU航向保持) */
-#define PARAM_GNAV_LINK_DISTANCE      0.70f   /* m     — 连接段标称距离 (沟间距) */
+#define PARAM_GNAV_LINK_DISTANCE      0.70f   /* m     — 连接段标称距离 (沟间距中心到中心) */
 #define PARAM_GNAV_LINK_TIMEOUT       8000U   /* ms    — 连接段超时 */
+#define PARAM_GNAV_LINK_GAP_RUNOUT    0.08f   /* m     — 看到下一个沟口后继续前冲距离 */
+#define PARAM_GNAV_LINK_WALL_TARGET   0.10f   /* m     — 连接段最小离墙距离（小于10cm才触发保底修正） */
+#define PARAM_GNAV_LINK_WALL_KP       3.0f    /*        — 连接段离墙保底增益 */
+#define PARAM_GNAV_LINK_WALL_HEADING_KP 1.2f  /*        — 连接段单边墙平行修正增益 */
+#define PARAM_GNAV_LINK_WALL_BLEND    0.5f    /*        — 预留给其他通道状态，连接段本身不再使用融合 */
 
 /* --- 出场 --- */
 /* C6完成后在左端通道左转朝南，沿纵向通道南行回到入口，距离约390cm */