ASER/Doc/实施方案.md

# ARES 混合导航实施方案

> **版本**: v1.0  
> **日期**: 2025 年  
> **依据**: `HANDOFF.md`、`混合导航方案.md`、`map.md`、现有代码分析  
> **本文目标**: 将混合导航方案从概念设计转化为可逐步实施的工程计划，明确每一个文件、每一个结构体、每一个状态转移

---

## 目录

1. [赛道几何精确推演](#1-赛道几何精确推演)
2. [S 型遍历轨迹精确描述](#2-s-型遍历轨迹精确描述)
3. [总体架构设计](#3-总体架构设计)
4. [完整状态机定义](#4-完整状态机定义)
5. [地图数据模型](#5-地图数据模型)
6. [新增模块详细设计](#6-新增模块详细设计)
7. [现有模块改造](#7-现有模块改造)
8. [数据流水线改造](#8-数据流水线改造)
9. [参数清单](#9-参数清单)
10. [文件清单汇总](#10-文件清单汇总)
11. [分步实施计划](#11-分步实施计划)
12. [风险与降级策略](#12-风险与降级策略)

---

## 1. 赛道几何精确推演

### 1.1 从 `map.md` 提取的关键尺寸

根据地图 (1 字符 = 10cm)，精确测量如下：

```
场地净尺寸:  X = 300cm (30格)    Y = 390cm (39格，不含启动区)

围栏:        1格 = 10cm 厚

田垄:        宽 30cm (3格)    长 220cm (22格)
             左端距左围栏内壁 40cm (4格)
             右端距右围栏内壁 40cm (4格)

垄沟 (南北向通道):
             每条宽 40cm (4格)
             长度 = 垄背长度 = 220cm (被垄背两侧约束)

端部开放区 (垄背两端到围栏):
             左端: 40cm (4格)
             右端: 40cm (4格)

启动区:      宽 40cm × 深 100cm，位于场地最下方 (Y=39)，紧邻左侧通道
入口:         宽 40cm (4格)，位于左下角，从Y=39到围栏外
```

### 1.2 关键几何参数表

| 参数 | 值 | 来源 |
|------|-----|------|
| 垄沟长度 (沟内可行驶) | **220 cm** | 垄背长度 = 22格 |
| 垄沟宽度 | **40 cm** | 围栏/垄背间距 |
| 端部连接通道宽度 (左端) | **40 cm** | 左围栏到垄背左端 |
| 端部连接通道宽度 (右端) | **40 cm** | 右围栏到垄背右端 |
| 连接段推进距离 (沟间距) | **70 cm** | 垄背宽 30cm + 垄沟宽 40cm = 70cm，中心到中心 |
| 入口段长度 (启动区到场内第一个垄沟中心) | **~65 cm** | 从入口边缘到 corridor 1 中心线的距离 |
| 场地总深度 (Y方向) | **390 cm** | 围栏内净尺寸 |
| 场地总宽度 (X方向) | **300 cm** | 围栏内净尺寸 |

### 1.3 6 条垄沟的 Y 坐标 (中心线，从下往上编号)

> 坐标系: 原点在左下围栏内壁角，X 向右，Y 向上

从地图分析，自下而上（从入口端开始）：

| 垄沟编号 | 两侧约束 | 中心线 Y 坐标 (约) |
|----------|----------|-------------------|
| Corridor 1 | 下围栏 ↔ 垄背5 | Y ≈ 20 cm |
| Corridor 2 | 垄背5 ↔ 垄背4 | Y ≈ 90 cm |
| Corridor 3 | 垄背4 ↔ 垄背3 | Y ≈ 160 cm |
| Corridor 4 | 垄背3 ↔ 垄背2 | Y ≈ 230 cm |
| Corridor 5 | 垄背2 ↔ 垄背1 | Y ≈ 300 cm |
| Corridor 6 | 垄背1 ↔ 上围栏 | Y ≈ 370 cm |

> **注**: 具体 Y 坐标值在实施中不重要，因为我们不做全局定位。关键参数是 **沟间距 70cm** 和 **沟长 220cm**。

---

## 2. S 型遍历轨迹精确描述

### 2.1 S 型路径定义

```
启动区 → 入口 → [左端通道向上] → 右转90° → Corridor 1 (→) → 左转90°
→ [右端通道向上 70cm] → 左转90° → Corridor 2 (←) → 右转90°
→ [左端通道向上 70cm] → 右转90° → Corridor 3 (→) → 左转90°
→ [右端通道向上 70cm] → 左转90° → Corridor 4 (←) → 右转90°
→ [左端通道向上 70cm] → 右转90° → Corridor 5 (→) → 左转90°
→ [右端通道向上 70cm] → 左转90° → Corridor 6 (←) → [到左端]
→ 左转90° → [左端通道向下 回到入口] → 出场 → 回停启动区
```

其中 `(→)` 表示从左端到右端行驶，`(←)` 表示从右端到左端行驶。

### 2.2 动作序列表 (完整遍历)

| 步骤 | 动作类型 | 传感器依赖 | 终止条件 | 方向参考 |
|------|---------|-----------|----------|---------|
| 0 | **启动区等待** | 无 | 外部启动信号 | — |
| 1 | **入场直线** | IMU航向 + 前激光 | 到达第一个垄沟入口附近（里程计 + 侧向VL53开始有效） | 向上(+Y) |
| 2 | **右转 90°** | IMU yaw_continuous | 转角达到 90° | 原地转向 |
| 3 | **重捕获入沟** | VL53 双侧有效 + conf | 锁定走廊结构 | — |
| 4 | **Corridor 1 跟踪 (→)** | VL53 + IMU + 前激光 | 前激光到端 | 向右(+X) |
| 5 | **左转 90° (出沟)** | IMU | 转角达到 90° | 原地转向 |
| 6 | **连接段直行 70cm** | IMU航向 + 里程计 | 里程到 70cm 或侧向开始看到下一沟壁 | 向上(+Y) |
| 7 | **左转 90° (入沟)** | IMU | 转角达到 90° | 原地转向 |
| 8 | **重捕获入沟** | VL53 | 锁定 | — |
| 9 | **Corridor 2 跟踪 (←)** | VL53 + IMU + 前激光 | 前激光到端 | 向左(-X) |
| 10 | **右转 90° (出沟)** | IMU | 转角达到 90° | 原地转向 |
| 11 | **连接段直行 70cm** | IMU + 里程计 | 同步骤 6 | 向上(+Y) |
| 12 | **右转 90° (入沟)** | IMU | 转角达到 90° | 原地转向 |
| 13 | **重捕获入沟** | VL53 | 锁定 | — |
| 14 | **Corridor 3 跟踪 (→)** | ... | ... | 向右 |
| ... | *重复 S 型模式* | ... | ... | ... |
| 29 | **Corridor 6 跟踪 (←)** | VL53 + IMU + 前激光 | 前激光到端 (左端围栏) | 向左 |
| 30 | **左转 90° (朝出口)** | IMU | 转角达到 90° | 原地转向 |
| 31 | **出场直线** | IMU + 里程计 + VL53丢失检测 | 侧向VL53全丢 + 里程足够 | 向下(-Y) |
| 32 | **回停启动区** | 里程计 + 超时 | 到达启动区范围内 | 减速停车 |

### 2.3 转向方向规律

关键规律（从垄沟 1 开始，假设 1→2→3→4→5→6 的 S 型）：

| 从 → 到 | 行驶方向 | 到端后出沟转向 | 连接段朝向 | 入沟转向 |
|---------|---------|-------------|-----------|---------|
| C1(→) → C2 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) |
| C2(←) → C3 | 右→左 | 右转(CW) | 向上 | 右转(CW) |
| C3(→) → C4 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) |
| C4(←) → C5 | 右→左 | 右转(CW) | 向上 | 右转(CW) |
| C5(→) → C6 | 左→右 | 左转(CCW) | 向上 | 左转(CCW) |

**规律**: 奇数沟(→)到端后两次左转；偶数沟(←)到端后两次右转。

可用公式表达:
```c
// 当前沟结束后的转向方向
if (corridor_id % 2 == 1)  // 奇数沟(→)，在右端出
    turn_direction = TURN_LEFT;   // CCW, +90°
else                              // 偶数沟(←)，在左端出
    turn_direction = TURN_RIGHT;  // CW, -90°
```

---

## 3. 总体架构设计

### 3.1 三层架构

```
┌───────────────────────────────────────────────────────────────────────┐
│                        上层: 赛道级导航层                               │
│                    global_nav.c/.h + track_map.c/.h                   │
│                                                                       │
│  职责: 知道"当前第几沟、下一沟是谁、该往哪转"                              │
│  输出: ActionRequest_t { action_type, turn_dir, ... }                 │
└──────────────────────────────┬────────────────────────────────────────┘
                               │
┌──────────────────────────────▼────────────────────────────────────────┐
│                        中层: 动作执行层                                 │
│              action_executor.c/.h (统一调度器)                          │
│              内部调用: turn_action / straight_action / reacquire       │
│                                                                       │
│  职责: 把一个"动作请求"变成实际的 v/w 指令序列                             │
│  输出: RawCmd_t { v, w }                                              │
└──────────────────────────────┬────────────────────────────────────────┘
                               │
┌──────────────────────────────▼────────────────────────────────────────┐
│                        下层: 局部闭环层 (已有)                           │
│              corridor_ctrl.c + corridor_ekf + corridor_preproc        │
│                                                                       │
│  职责: 沟内精确居中跟踪                                                 │
│  输出: RawCmd_t { v, w }                                              │
└──────────────────────────────┬────────────────────────────────────────┘
                               │
┌──────────────────────────────▼────────────────────────────────────────┐
│                     安全仲裁层 (已有，需改造)                             │
│                        segment_fsm.c                                  │
│                                                                       │
│  职责: 根据动作模式做上下文感知的安全裁剪                                   │
│  输出: SegFsmOutput_t { safe_v, safe_w }                              │
└──────────────────────────────┬────────────────────────────────────────┘
                               │
                          CmdSlot → CAN TX
```

### 3.2 调用关系 (navTask 内一次循环)

```
AppTasks_RunNavTask_Impl() {
    // Step 1-3: 不变 (黑板快照 → 预处理 → EKF)
    Blackboard_GetSnapshot(&board);
    CorridorPreproc_ExtractObs(&board, now, &obs);
    CorridorFilter_Update(&obs, wz, vx, dt, yaw, yaw_ok, &state);

    // Step 4: 【改造】赛道级导航 + 动作执行（替代原 NavScript）
    GlobalNav_Update(&obs, &state, &board, &nav_output);
    //   nav_output 包含:
    //     - action_type: 当前在执行什么动作
    //     - cmd: { v, w }       (中层输出)
    //     - request_corridor: bool  (是否要求走廊控制器接管)
    //     - safety_mode: enum   (当前动作对安全层的模式要求)

    // Step 5: 控制律（仅在 request_corridor 时运行）
    if (nav_output.request_corridor)
        CorridorCtrl_Compute(&state, &obs, imu_wz, &raw_cmd);
    else
        raw_cmd = nav_output.cmd;

    // Step 6: 【改造】安全层（感知当前动作模式）
    SegFsm_Update(&raw_cmd, &obs, &state, nav_output.safety_mode, &fsm_out);

    // Step 7: 不变
    CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0);
}
```

---

## 4. 完整状态机定义

### 4.1 赛道级状态机 (GlobalNavStage)

```c
typedef enum {
    GNAV_IDLE,                  // 启动区等待
    GNAV_ENTRY_STRAIGHT,        // 入场直线 (从启动区进入场地，向上行驶到第一条沟入口)
    GNAV_TURN_INTO_CORRIDOR,    // 转向进入垄沟 (原地转 90°)
    GNAV_REACQUIRE,             // 重捕获走廊 (低速前进，等VL53锁定)
    GNAV_CORRIDOR_TRACK,        // 沟内闭环跟踪 (现有 corridor_ctrl)
    GNAV_TURN_OUT_OF_CORRIDOR,  // 到端后出沟转向 (原地转 90°)
    GNAV_LINK_STRAIGHT,         // 连接段直行 (IMU航向保持 + 里程计推进)
    GNAV_TURN_INTO_NEXT,        // 转向进入下一条沟 (原地转 90°)
    GNAV_EXIT_STRAIGHT,         // 出场直行 (向下回到入口)
    GNAV_DOCK,                  // 回停启动区
    GNAV_FINISHED,              // 终态
    GNAV_ERROR                  // 异常态 (超时兜底)
} GlobalNavStage_t;
```

### 4.2 完整状态转移图

```
IDLE
  │ Start()
  ▼
ENTRY_STRAIGHT ──────────────────────── [侧向VL53开始有效 或 里程到达]
  │
  ▼
TURN_INTO_CORRIDOR ──────────────────── [IMU 转角 ≥ 90° - tolerance]
  │
  ▼
REACQUIRE ───────────────────────────── [VL53双侧有效 + conf≥阈值 + 持续N拍]
  │
  ▼
CORRIDOR_TRACK ──────────────────────── [前激光 d_front ≤ 到端阈值]
  │
  ▼
TURN_OUT_OF_CORRIDOR ────────────────── [IMU 转角 ≥ 90° - tolerance]
  │
  ├─── [还有未遍历的沟? → 是]
  │         │
  │         ▼
  │    LINK_STRAIGHT ────────────────── [里程 ≥ 连接段距离 或 侧向VL53探到壁]
  │         │
  │         ▼
  │    TURN_INTO_NEXT ───────────────── [IMU 转角 ≥ 90° - tolerance]
  │         │
  │         ▼
  │    REACQUIRE → CORRIDOR_TRACK (循环)
  │
  ├─── [最后一条沟完成? → 是]
  │         │
  │         ▼
  │    EXIT_STRAIGHT ────────────────── [侧向VL53全丢 + 里程足够 或 到达出口]
  │         │
  │         ▼
  │    DOCK ─────────────────────────── [里程到 或 超时]
  │         │
  │         ▼
  │    FINISHED
  │
  └─── [超时保护 → 任何阶段超时]
            │
            ▼
         ERROR → FINISHED (安全停车)
```

### 4.3 各状态详细行为

#### GNAV_IDLE
```
行为:     v=0, w=0
安全模式: SAFETY_MODE_IDLE
进入时:   系统启动默认态
退出条件: GlobalNav_Start() 被调用
退出动作: 记录 IMU yaw_ref，初始化里程计基准
```

#### GNAV_ENTRY_STRAIGHT
```
行为:     v=PARAM_ENTRY_V (0.08 m/s), w=IMU航向保持PD
          w = kp_heading * (yaw_ref - yaw_current)  // 简单P控制保直
安全模式: SAFETY_MODE_STRAIGHT (前后激光防撞生效)
进入时:   记录起始里程
退出条件: 以下任一:
          (1) 侧向VL53某侧开始有效 (探到第一条沟的壁)
          (2) 里程 ≥ PARAM_ENTRY_DISTANCE
          (3) 超时 PARAM_ENTRY_TIMEOUT
退出动作: 记录当前 yaw 为沟口参考
```

#### GNAV_TURN_INTO_CORRIDOR
```
行为:     v=0, w=turn_dir * PARAM_TURN_OMEGA (±1.0 rad/s)
          接近目标角度时减速: 剩余 < 0.5rad 时线性减速至 0.3 rad/s
安全模式: SAFETY_MODE_TURN (允许v=0时保留w, 前后激光不触发全停)
进入时:   记录 turn_start_yaw = IMU yaw_continuous
          从 track_map 查询本次转向方向
退出条件: |yaw_current - turn_start_yaw| ≥ 90° - PARAM_TURN_TOLERANCE (默认 5°)
退出动作: EKF 重置; 记录新航向参考
```

#### GNAV_REACQUIRE
```
行为:     v=PARAM_REACQUIRE_V (0.05 m/s), w=IMU航向保持PD
安全模式: SAFETY_MODE_STRAIGHT
进入时:   EKF 重置; 清零确认计数器; 记录起始里程
退出条件: 以下全部持续 PARAM_REACQUIRE_CONFIRM_TICKS 拍 (默认5拍=100ms):
          (1) VL53 双侧(至少3个)有效
          (2) 左右距离和 ≈ 走廊宽度 (±容差 5cm)
          (3) EKF conf ≥ PARAM_REACQUIRE_CONF_THRESH (0.6)
超时保护: PARAM_REACQUIRE_TIMEOUT (5000ms), 超时 → ERROR
退出动作: corridor_ctrl 初始化完成，切换到沟内跟踪
```

#### GNAV_CORRIDOR_TRACK
```
行为:     request_corridor = true (交给现有 corridor_ctrl)
安全模式: SAFETY_MODE_CORRIDOR (现有 SegFsm 逻辑: 前向减速/停车/E-STOP)
进入时:   记录起始里程
退出条件: 以下任一:
          (1) 前激光有效 且 d_front ≤ PARAM_END_DETECT_DIST (0.10m)
          (2) 里程 ≥ PARAM_CORRIDOR_LENGTH_MAX (2.5m, 超长保护)
退出动作: 停车; corridor_id 对应的遍历标记置位
```

#### GNAV_TURN_OUT_OF_CORRIDOR
```
行为:     v=0, w=turn_dir * PARAM_TURN_OMEGA
          (转向方向由 track_map 决定)
安全模式: SAFETY_MODE_TURN
进入时:   记录 turn_start_yaw
退出条件: |yaw_current - turn_start_yaw| ≥ 90° - PARAM_TURN_TOLERANCE
退出动作: 判断是否为最后一条沟: 是 → EXIT_STRAIGHT; 否 → LINK_STRAIGHT
```

#### GNAV_LINK_STRAIGHT
```
行为:     v=PARAM_LINK_V (0.10 m/s), w=IMU航向保持PD
安全模式: SAFETY_MODE_STRAIGHT
进入时:   记录起始里程; 记录航向参考
退出条件: 以下任一:
          (1) 里程 ≥ PARAM_LINK_DISTANCE (0.70m, 即沟间距)
          (2) 侧向VL53某侧探到新沟壁 (距离在合理范围内)
          (3) 超时 PARAM_LINK_TIMEOUT (8000ms)
退出动作: 准备进入下一条沟的转向
```

#### GNAV_TURN_INTO_NEXT
```
行为:     同 GNAV_TURN_INTO_CORRIDOR (v=0, w=±turn_omega)
安全模式: SAFETY_MODE_TURN
进入时:   记录 turn_start_yaw; 从 track_map 查询方向 (同出沟方向)
退出条件: |delta_yaw| ≥ 90° - PARAM_TURN_TOLERANCE
退出动作: EKF 重置 → REACQUIRE
```

#### GNAV_EXIT_STRAIGHT
```
行为:     v=PARAM_EXIT_V (0.15 m/s), w=IMU航向保持PD
          航向目标: 朝向出口方向（向下）
安全模式: SAFETY_MODE_STRAIGHT (但前向停车距离放宽)
进入时:   记录起始里程
退出条件: 以下任一:
          (1) 侧向VL53全部丢失 + 里程 ≥ PARAM_EXIT_RUNOUT (1.5m)
          (2) 里程 ≥ PARAM_EXIT_MAX_DIST (4.0m, 保护)
          (3) 超时 PARAM_EXIT_TIMEOUT (20000ms)
退出动作: 进入回停
```

#### GNAV_DOCK
```
行为:     v=PARAM_DOCK_V (0.05 m/s), w=0
          推进一段短距离后停车
安全模式: SAFETY_MODE_STRAIGHT
进入时:   记录起始里程
退出条件: 里程 ≥ PARAM_DOCK_DISTANCE (0.5m) 或超时
退出动作: 强制零速 → FINISHED
```

#### GNAV_FINISHED
```
行为:     v=0, w=0 (永久停车)
安全模式: SAFETY_MODE_IDLE
终态:     不再转移
```

#### GNAV_ERROR
```
行为:     v=0, w=0 (紧急停车)
安全模式: SAFETY_MODE_IDLE
说明:     任何阶段超时保护触发后进入
          等待 PARAM_ERROR_HOLD_TIME 后 → FINISHED
```

---

## 5. 地图数据模型

### 5.1 设计原则

地图不做全局坐标定位。地图只回答三个问题：
1. 从第 N 条沟完成后，下一条是第几条？
2. 这次该往哪转？（左/右）
3. 当前是不是最后一条沟？

### 5.2 数据结构

```c
/* ---- App/nav/track_map.h ---- */

#define TRACK_MAP_CORRIDOR_COUNT    6
#define TRACK_MAP_LINK_DISTANCE_M   0.70f   /* 沟间距 (中心到中心) */
#define TRACK_MAP_CORRIDOR_LENGTH_M 2.20f   /* 垄沟标称长度 */

typedef enum {
    TRAVEL_DIR_POSITIVE,   /* 从左端到右端 (→) */
    TRAVEL_DIR_NEGATIVE    /* 从右端到左端 (←) */
} TravelDirection_t;

typedef enum {
    TURN_DIR_LEFT  = +1,   /* 逆时针 (CCW), w > 0 */
    TURN_DIR_RIGHT = -1    /* 顺时针 (CW),  w < 0 */
} TurnDirection_t;

/* 单条垄沟描述 */
typedef struct {
    uint8_t            id;              /* 0-5 */
    TravelDirection_t  travel_dir;      /* 本沟行驶方向 */
    TurnDirection_t    exit_turn_dir;   /* 出沟时的转向方向 */
    TurnDirection_t    entry_turn_dir;  /* 入沟时的转向方向 (= exit_turn_dir) */
    bool               is_last;         /* 是否为最后一条沟 */
} CorridorDescriptor_t;

/* 地图查询接口 */
typedef struct {
    CorridorDescriptor_t corridors[TRACK_MAP_CORRIDOR_COUNT];
    uint8_t              entry_corridor_id;   /* 入场后第一条沟 = 0 */
    float                link_distance_m;     /* 连接段标称距离 */
    float                corridor_length_m;   /* 垄沟标称长度 */
} TrackMap_t;
```

### 5.3 地图初始化 (硬编码 S 型遍历表)

```c
/* ---- App/nav/track_map.c ---- */

static const TrackMap_t s_map = {
    .corridors = {
        /* id  travel_dir          exit_turn    entry_turn   is_last */
        { 0,  TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT,  TURN_DIR_LEFT,  false },  // C1: →, 到右端后左转
        { 1,  TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false },  // C2: ←, 到左端后右转
        { 2,  TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT,  TURN_DIR_LEFT,  false },  // C3: →
        { 3,  TRAVEL_DIR_NEGATIVE, TURN_DIR_RIGHT, TURN_DIR_RIGHT, false },  // C4: ←
        { 4,  TRAVEL_DIR_POSITIVE, TURN_DIR_LEFT,  TURN_DIR_LEFT,  false },  // C5: →
        { 5,  TRAVEL_DIR_NEGATIVE, TURN_DIR_LEFT,  TURN_DIR_LEFT,  true  },  // C6: ←, 最后一条
    },
    .entry_corridor_id = 0,
    .link_distance_m   = 0.70f,
    .corridor_length_m = 2.20f,
};

/* 查询 API */
void               TrackMap_Init(void);
const TrackMap_t*   TrackMap_Get(void);
const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id);
uint8_t             TrackMap_GetNextCorridorId(uint8_t current_id);
bool                TrackMap_IsLastCorridor(uint8_t id);
TurnDirection_t     TrackMap_GetExitTurnDir(uint8_t id);
TurnDirection_t     TrackMap_GetEntryTurnDir(uint8_t id);
```

> **为什么 C6 出沟后是左转？**
> C6(←) 行驶到左端完成后，需要朝向出口（向下）方向离场。从向左(-X)行驶的状态左转90°后，正好朝向下方(-Y)，对准出口。

---

## 6. 新增模块详细设计

### 6.1 `App/nav/global_nav.c/.h` — 赛道级总控

这是整个混合导航的核心模块，替代现有 `nav_script.c` 的赛道编排职能。

#### 头文件接口

```c
/* ---- App/nav/global_nav.h ---- */

#include "corridor_msgs.h"
#include "track_map.h"

/* 安全模式 (传递给 segment_fsm) */
typedef enum {
    SAFETY_MODE_IDLE,       /* 零速，不做任何裁剪 */
    SAFETY_MODE_CORRIDOR,   /* 沟内: 前向减速/停车/E-STOP 全开 */
    SAFETY_MODE_TURN,       /* 转向: 允许v=0+w≠0, 前向不全停 */
    SAFETY_MODE_STRAIGHT    /* 直行段: 前后激光防撞, 无侧向约束 */
} SafetyMode_t;

/* 赛道级导航配置 */
typedef struct {
    float  entry_v;               /* 入场速度 m/s */
    float  entry_distance;        /* 入场里程上限 m */
    uint32_t entry_timeout_ms;    /* 入场超时 ms */

    float  turn_omega;            /* 转向角速度 rad/s */
    float  turn_tolerance_rad;    /* 转向完成容差 rad */
    float  turn_decel_zone_rad;   /* 接近目标角时的减速区 rad */
    float  turn_min_omega;        /* 减速区最低角速度 rad/s */
    uint32_t turn_timeout_ms;     /* 单次转向超时 ms */

    float  reacquire_v;           /* 重捕获速度 m/s */
    float  reacquire_conf_thresh; /* 重捕获置信度阈值 */
    float  reacquire_width_tol;   /* 走廊宽度容差 m */
    uint8_t reacquire_confirm_ticks; /* 连续确认拍数 */
    uint32_t reacquire_timeout_ms;

    float  corridor_end_detect_dist; /* 到端检测距离 m */
    float  corridor_length_max;      /* 沟内里程保护上限 m */

    float  link_v;                /* 连接段速度 m/s */
    float  link_distance;         /* 连接段标称距离 m */
    uint32_t link_timeout_ms;

    float  exit_v;                /* 出场速度 m/s */
    float  exit_runout;           /* 出场冲刺距离 m */
    float  exit_max_dist;         /* 出场里程保护 m */
    uint32_t exit_timeout_ms;

    float  dock_v;                /* 回停速度 m/s */
    float  dock_distance;         /* 回停距离 m */

    float  heading_kp;            /* 航向保持P增益 */
} GlobalNavConfig_t;

/* 赛道级导航输出 */
typedef struct {
    GlobalNavStage_t  stage;          /* 当前阶段 */
    const char*       stage_name;     /* 阶段名字符串 (调试) */
    uint8_t           corridor_id;    /* 当前/目标垄沟 ID */
    uint8_t           corridors_done; /* 已完成垄沟数 */

    bool              request_corridor; /* 是否请求沟内闭环 */
    bool              use_override;     /* 是否用 override 指令 */
    float             override_v;
    float             override_w;

    SafetyMode_t      safety_mode;     /* 当前安全模式 */
    bool              active;          /* 导航是否在运行 */
} GlobalNavOutput_t;

/* API */
void GlobalNav_Init(const GlobalNavConfig_t* cfg);
void GlobalNav_Start(void);
void GlobalNav_Stop(void);
void GlobalNav_Reset(void);
void GlobalNav_Update(
    const CorridorObs_t*   obs,
    const CorridorState_t* state,
    const RobotBlackboard_t* board,   /* 需要读 IMU yaw_continuous, odom */
    GlobalNavOutput_t*     out
);
GlobalNavStage_t GlobalNav_GetStage(void);
```

#### 内部状态

```c
/* ---- global_nav.c 内部 ---- */

static struct {
    GlobalNavStage_t stage;
    bool             running;

    /* 赛道级 */
    uint8_t  current_corridor_id;
    uint8_t  corridors_completed;

    /* 转向 */
    float    turn_start_yaw_deg;      /* IMU yaw_continuous at turn start */
    float    turn_target_delta_deg;   /* 90° = ±90° */
    int8_t   turn_sign;               /* +1 or -1 */

    /* 里程 */
    float    stage_start_odom_s;      /* EKF s 或 里程计 at stage entry */
    float    stage_start_vx_accum;    /* 里程计累积距离 */

    /* 超时 */
    uint32_t stage_start_ms;

    /* 重捕获 */
    uint8_t  reacquire_ok_count;

    /* 出场 */
    bool     exit_vl53_lost;
    float    exit_lost_odom_s;

    /* 航向保持 */
    float    heading_ref_deg;         /* 直行段航向参考 */

    /* 配置引用 */
    GlobalNavConfig_t cfg;
} s_nav;
```

#### 核心 Update 逻辑 (伪代码)

```c
void GlobalNav_Update(..., GlobalNavOutput_t* out) {
    uint32_t now = HAL_GetTick();
    float imu_yaw_deg  = board->imu_yaw_continuous.value;
    float odom_vx      = board->odom_vx.value;
    float elapsed_ms   = now - s_nav.stage_start_ms;

    // 默认输出
    out->use_override = true;
    out->request_corridor = false;
    out->override_v = 0;
    out->override_w = 0;
    out->safety_mode = SAFETY_MODE_IDLE;
    out->stage = s_nav.stage;
    out->corridor_id = s_nav.current_corridor_id;
    out->corridors_done = s_nav.corridors_completed;
    out->active = s_nav.running;

    if (!s_nav.running) return;

    switch (s_nav.stage) {

    case GNAV_ENTRY_STRAIGHT:
        out->override_v = s_nav.cfg.entry_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;

        // 退出: 侧向探到壁 或 里程到 或 超时
        if (side_walls_detected(obs) || odom_advanced_enough() || elapsed_ms > s_nav.cfg.entry_timeout_ms)
            transition_to(GNAV_TURN_INTO_CORRIDOR);
        break;

    case GNAV_TURN_INTO_CORRIDOR:
    case GNAV_TURN_OUT_OF_CORRIDOR:
    case GNAV_TURN_INTO_NEXT:
        execute_turn(obs, state, board, out);
        break;

    case GNAV_REACQUIRE:
        out->override_v = s_nav.cfg.reacquire_v;
        out->override_w = heading_hold_pd(...);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        if (check_reacquire_conditions(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);
        }
        if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms)
            transition_to(GNAV_ERROR);
        break;

    case GNAV_CORRIDOR_TRACK:
        out->use_override = false;
        out->request_corridor = true;
        out->safety_mode = SAFETY_MODE_CORRIDOR;

        // 到端检测
        bool front_valid = obs->valid_mask & CORRIDOR_OBS_MASK_FRONT;
        if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
            s_nav.corridors_completed++;
            transition_to(GNAV_TURN_OUT_OF_CORRIDOR);
        }
        // 里程保护
        if (odom_since_entry() > s_nav.cfg.corridor_length_max)
            transition_to(GNAV_TURN_OUT_OF_CORRIDOR);
        break;

    case GNAV_LINK_STRAIGHT:
        out->override_v = s_nav.cfg.link_v;
        out->override_w = heading_hold_pd(...);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        if (odom_since_entry() >= s_nav.cfg.link_distance || side_walls_detected(obs))
            transition_to(GNAV_TURN_INTO_NEXT);
        if (elapsed_ms > s_nav.cfg.link_timeout_ms)
            transition_to(GNAV_ERROR);
        break;

    case GNAV_EXIT_STRAIGHT:
        out->override_v = s_nav.cfg.exit_v;
        out->override_w = heading_hold_pd(...);
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        // 检测侧向全丢
        if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
            s_nav.exit_vl53_lost = true;
            s_nav.exit_lost_odom_s = current_odom();
        }
        if (s_nav.exit_vl53_lost && (current_odom() - s_nav.exit_lost_odom_s) >= s_nav.cfg.exit_runout)
            transition_to(GNAV_DOCK);
        if (elapsed_ms > s_nav.cfg.exit_timeout_ms)
            transition_to(GNAV_DOCK);
        break;

    case GNAV_DOCK:
        out->override_v = s_nav.cfg.dock_v;
        out->override_w = 0;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        if (odom_since_entry() >= s_nav.cfg.dock_distance || elapsed_ms > 5000)
            transition_to(GNAV_FINISHED);
        break;

    case GNAV_FINISHED:
        out->override_v = 0;
        out->override_w = 0;
        out->active = false;
        break;

    case GNAV_ERROR:
        out->override_v = 0;
        out->override_w = 0;
        out->safety_mode = SAFETY_MODE_IDLE;
        if (elapsed_ms > 2000)
            transition_to(GNAV_FINISHED);
        break;
    }
}
```

#### transition_to 函数

```c
static void transition_to(GlobalNavStage_t next) {
    // 通用: 记录进入时间和里程
    s_nav.stage_start_ms     = HAL_GetTick();
    s_nav.stage_start_odom_s = /* 当前里程 */;
    s_nav.reacquire_ok_count = 0;

    switch (next) {
    case GNAV_TURN_INTO_CORRIDOR: {
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
        s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
        s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */;
        s_nav.turn_target_delta_deg = 90.0f;
        break;
    }
    case GNAV_TURN_OUT_OF_CORRIDOR: {
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
        s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
        s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */;
        s_nav.turn_target_delta_deg = 90.0f;
        break;
    }
    case GNAV_TURN_INTO_NEXT: {
        uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id);
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id);
        s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
        s_nav.turn_start_yaw_deg = /* IMU yaw_continuous */;
        s_nav.turn_target_delta_deg = 90.0f;
        s_nav.current_corridor_id = next_id;
        break;
    }
    case GNAV_REACQUIRE:
        CorridorFilter_Reset();  // EKF 重置
        s_nav.heading_ref_deg = /* 当前 IMU yaw */;
        break;

    case GNAV_CORRIDOR_TRACK:
        break;

    case GNAV_LINK_STRAIGHT: {
        s_nav.heading_ref_deg = /* 当前 IMU yaw */;
        break;
    }
    case GNAV_EXIT_STRAIGHT: {
        // 最后一条沟出来后，转向应该已经朝向出口
        s_nav.heading_ref_deg = /* 当前 IMU yaw */;
        s_nav.exit_vl53_lost = false;
        break;
    }
    default:
        break;
    }

    s_nav.stage = next;
}
```

#### 转向执行 (统一的 90° 转向逻辑)

```c
static void execute_turn(const CorridorObs_t* obs, const CorridorState_t* state,
                         const RobotBlackboard_t* board, GlobalNavOutput_t* out) {
    float imu_yaw = board->imu_yaw_continuous.value;
    float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
    float target = s_nav.turn_target_delta_deg;
    float remaining_deg = target - delta;
    float remaining_rad = remaining_deg * PARAM_DEG2RAD_FACTOR;

    float omega = s_nav.cfg.turn_omega;

    // 接近目标时减速
    if (remaining_rad < s_nav.cfg.turn_decel_zone_rad) {
        float ratio = remaining_rad / s_nav.cfg.turn_decel_zone_rad;
        omega = s_nav.cfg.turn_min_omega + ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega);
    }

    out->override_v = 0;
    out->override_w = s_nav.turn_sign * omega;
    out->use_override = true;
    out->safety_mode = SAFETY_MODE_TURN;

    float tolerance_deg = s_nav.cfg.turn_tolerance_rad * PARAM_RAD2DEG_FACTOR;

    if (delta >= target - tolerance_deg) {
        // 转向完成
        switch (s_nav.stage) {
        case GNAV_TURN_INTO_CORRIDOR:
        case GNAV_TURN_INTO_NEXT:
            transition_to(GNAV_REACQUIRE);
            break;
        case GNAV_TURN_OUT_OF_CORRIDOR:
            if (TrackMap_IsLastCorridor(s_nav.current_corridor_id))
                transition_to(GNAV_EXIT_STRAIGHT);
            else
                transition_to(GNAV_LINK_STRAIGHT);
            break;
        default:
            break;
        }
    }

    // 超时保护
    if (HAL_GetTick() - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms)
        transition_to(GNAV_ERROR);
}
```

### 6.2 `App/nav/track_map.c/.h` — 赛道地图

见第 5 节数据结构。实现很简单，就是一个 const 表 + 查询函数。

```c
/* track_map.c */
#include "track_map.h"

static const TrackMap_t s_map = { /* 见 5.3 */ };
static bool s_initialized = false;

void TrackMap_Init(void) {
    s_initialized = true;
}

const TrackMap_t* TrackMap_Get(void) {
    return &s_map;
}

const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id) {
    if (id >= TRACK_MAP_CORRIDOR_COUNT) return &s_map.corridors[0];
    return &s_map.corridors[id];
}

uint8_t TrackMap_GetNextCorridorId(uint8_t current_id) {
    if (current_id + 1 >= TRACK_MAP_CORRIDOR_COUNT) return current_id;
    return current_id + 1;
}

bool TrackMap_IsLastCorridor(uint8_t id) {
    return s_map.corridors[id].is_last;
}

TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id) {
    return s_map.corridors[id].exit_turn_dir;
}

TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id) {
    return s_map.corridors[id].entry_turn_dir;
}
```

---

## 7. 现有模块改造

### 7.1 `segment_fsm.c/.h` — 安全层增加动作模式感知

#### 改动要点

1. 新增 `SafetyMode_t` 参数输入
2. `SegFsm_Update()` 签名变更: 增加 `SafetyMode_t mode` 参数
3. 根据 mode 决定安全策略

#### 改动后的行为表

| SafetyMode | 前向减速/停车 | E-STOP (conf低) | w 通过 |
|------------|-------------|-----------------|--------|
| IDLE | 不触发 | 不触发 | 清零 |
| CORRIDOR | ✅ 正常触发 | ✅ 正常触发 | 正常 |
| TURN | ❌ 不触发 (v=0时不检查前距) | ❌ 不触发 (转向时conf低是正常的) | ✅ 直接通过 |
| STRAIGHT | ✅ 正常触发 | ❌ 不触发 | 正常 |

#### 改动后的函数签名

```c
/* segment_fsm.h 改动 */

// 删除旧的:
// void SegFsm_Update(const RawCmd_t* raw_cmd, const CorridorObs_t* obs,
//                    const CorridorState_t* state, SegFsmOutput_t* out);

// 新增:
void SegFsm_Update(const RawCmd_t*        raw_cmd,
                   const CorridorObs_t*   obs,
                   const CorridorState_t* state,
                   SafetyMode_t           mode,    /* 新增 */
                   SegFsmOutput_t*        out);
```

#### 关键代码改动 (`segment_fsm.c`)

```c
void SegFsm_Update(const RawCmd_t* raw_cmd, const CorridorObs_t* obs,
                   const CorridorState_t* state, SafetyMode_t mode,
                   SegFsmOutput_t* out) {

    if (mode == SAFETY_MODE_IDLE) {
        out->state  = SEG_STATE_IDLE;
        out->safe_v = 0;
        out->safe_w = 0;
        return;
    }

    if (mode == SAFETY_MODE_TURN) {
        // 转向模式: 直接放行，不做前向检查
        out->state  = SEG_STATE_CORRIDOR;  // 复用 CORRIDOR 表示放行
        out->safe_v = raw_cmd->v;
        out->safe_w = raw_cmd->w;
        // 可选: w 的绝对值做上限裁剪
        return;
    }

    // CORRIDOR / STRAIGHT 模式: 执行原有逻辑
    // CORRIDOR 额外检查 conf → E-STOP
    // STRAIGHT 不检查 conf

    if (mode == SAFETY_MODE_CORRIDOR) {
        // 原有 E-STOP 逻辑 (conf 检查)
        if (state->conf < s_cfg.conf_estop_thresh) {
            s_state = SEG_STATE_ESTOP;
        }
        if (s_state == SEG_STATE_ESTOP && state->conf >= 0.5f) {
            s_state = SEG_STATE_CORRIDOR;
        }
    }

    // 前向距离检查 (CORRIDOR 和 STRAIGHT 都做)
    // ... 现有逻辑不变 ...
}
```

### 7.2 `nav_script.c/.h` — 降级为测试脚本

保留现有代码不删除，但不再在正式比赛流程中使用。

改动：
- 在 `app_tasks.c` 中通过编译开关选择使用 `NavScript` 还是 `GlobalNav`
- `nav_script.c` 保持现状，用于单垄沟测试验证

```c
/* app_tasks.c 中增加编译开关 */
#define USE_GLOBAL_NAV   1    /* 1=正式比赛模式  0=单沟测试模式 */
```

### 7.3 `corridor_msgs.h` — 补充枚举和结构

新增以下定义：

```c
/* corridor_msgs.h 新增 */

/* 赛道级阶段枚举 (已在 global_nav.h 定义，这里仅前向声明) */
/* #include "global_nav.h" 或直接在 corridor_msgs.h 中定义 */

/* 安全模式枚举 (可在此处定义，供 segment_fsm 和 global_nav 共享) */
typedef enum {
    SAFETY_MODE_IDLE,
    SAFETY_MODE_CORRIDOR,
    SAFETY_MODE_TURN,
    SAFETY_MODE_STRAIGHT
} SafetyMode_t;
```

> 注意: `SafetyMode_t` 定义应放在 `corridor_msgs.h` 中（数据契约），而非 `global_nav.h` 中，以便 `segment_fsm` 不需要反向依赖 `global_nav`。

### 7.4 `corridor_filter.c/.h` — 增加外部 Reset 接口

当前 `CorridorFilter_Reset()` 可能不存在或功能不完整。需要确保：

```c
/* corridor_filter.h 确认/新增 */
void CorridorFilter_Reset(void);
```

该函数应：
1. 调用 `CorridorEKF_Reset()`
2. 重置 yaw_ref 状态
3. 重置 yaw_ref_locked 标志

这在每次进入新垄沟时必须调用，否则 EKF 会用旧沟的参考值去更新新沟的状态。

### 7.5 `app_tasks.c` — navTask 流水线改造

详见第 8 节。

---

## 8. 数据流水线改造

### 8.1 改造后的 navTask 流水线

```c
/* app_tasks.c - AppTasks_RunNavTask_Impl() */

void AppTasks_RunNavTask_Impl(void) {
    static uint32_t last_ms = 0;
    uint32_t now = HAL_GetTick();
    float dt = (now - last_ms) * 0.001f;
    if (dt <= 0.0f || dt > 0.1f) dt = 0.02f;
    last_ms = now;

    /* ========== Step 1: 传感器快照 (不变) ========== */
    RobotBlackboard_t board;
    Blackboard_GetSnapshot(&board);

    /* ========== Step 2: 预处理 (不变) ========== */
    CorridorObs_t obs;
    CorridorPreproc_ExtractObs(&board, now, &obs);

    /* ========== Step 3: EKF (不变) ========== */
    float imu_wz_rad = PARAM_DEG2RAD(board.imu_wz.value);
    float odom_vx    = board.odom_vx.value;
    float imu_yaw_rad = PARAM_DEG2RAD(board.imu_yaw_continuous.value);
    bool  imu_yaw_ok  = board.imu_yaw_continuous.is_valid;

    CorridorState_t corridor_state;
    CorridorFilter_Update(&obs, imu_wz_rad, odom_vx, dt,
                          imu_yaw_rad, imu_yaw_ok, &corridor_state);

#if USE_GLOBAL_NAV
    /* ========== Step 4: 赛道级导航 (新) ========== */
    GlobalNavOutput_t nav_out;
    GlobalNav_Update(&obs, &corridor_state, &board, &nav_out);

    /* ========== Step 5: 控制律 ========== */
    RawCmd_t raw_cmd = {0};
    if (nav_out.request_corridor) {
        CorridorCtrl_Compute(&corridor_state, &obs, imu_wz_rad, &raw_cmd);
    } else if (nav_out.use_override) {
        raw_cmd.v = nav_out.override_v;
        raw_cmd.w = nav_out.override_w;
    }

    /* ========== Step 6: 安全仲裁 (改造) ========== */
    SegFsmOutput_t fsm_out;
    SegFsm_Update(&raw_cmd, &obs, &corridor_state, nav_out.safety_mode, &fsm_out);

#else
    /* ---------- 旧版: 单沟测试模式 ---------- */
    NavScriptOutput_t script_out;
    float imu_yaw_deg = board.imu_yaw_continuous.value;
    NavScript_Update(&obs, &corridor_state, imu_yaw_deg, &script_out);

    RawCmd_t raw_cmd = {0};
    if (script_out.use_override) {
        raw_cmd.v = script_out.override_v;
        raw_cmd.w = script_out.override_w;
    } else if (script_out.request_corridor) {
        CorridorCtrl_Compute(&corridor_state, &obs, imu_wz_rad, &raw_cmd);
    }

    SegFsmOutput_t fsm_out;
    SegFsm_Update(&raw_cmd, &obs, &corridor_state, SAFETY_MODE_CORRIDOR, &fsm_out);
#endif

    /* ========== Step 7: 推送指令 (不变) ========== */
    CmdSlot_Push(fsm_out.safe_v, fsm_out.safe_w, 0);
}
```

### 8.2 初始化改造

```c
void AppTasks_Init(void) {
    // ... 现有初始化不变 ...

    // 新增
    TrackMap_Init();

    GlobalNavConfig_t gnav_cfg = {
        .entry_v             = PARAM_GNAV_ENTRY_V,
        .entry_distance      = PARAM_GNAV_ENTRY_DISTANCE,
        .entry_timeout_ms    = PARAM_GNAV_ENTRY_TIMEOUT,
        .turn_omega          = PARAM_GNAV_TURN_OMEGA,
        .turn_tolerance_rad  = PARAM_GNAV_TURN_TOLERANCE,
        .turn_decel_zone_rad = PARAM_GNAV_TURN_DECEL_ZONE,
        .turn_min_omega      = PARAM_GNAV_TURN_MIN_OMEGA,
        .turn_timeout_ms     = PARAM_GNAV_TURN_TIMEOUT,
        .reacquire_v             = PARAM_GNAV_REACQUIRE_V,
        .reacquire_conf_thresh   = PARAM_GNAV_REACQUIRE_CONF,
        .reacquire_width_tol     = PARAM_GNAV_REACQUIRE_WIDTH_TOL,
        .reacquire_confirm_ticks = PARAM_GNAV_REACQUIRE_TICKS,
        .reacquire_timeout_ms    = PARAM_GNAV_REACQUIRE_TIMEOUT,
        .corridor_end_detect_dist = PARAM_SAFE_D_FRONT_STOP,
        .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,
        .exit_v              = PARAM_GNAV_EXIT_V,
        .exit_runout         = PARAM_GNAV_EXIT_RUNOUT,
        .exit_max_dist       = PARAM_GNAV_EXIT_MAX_DIST,
        .exit_timeout_ms     = PARAM_GNAV_EXIT_TIMEOUT,
        .dock_v              = PARAM_GNAV_DOCK_V,
        .dock_distance       = PARAM_GNAV_DOCK_DISTANCE,
        .heading_kp          = PARAM_GNAV_HEADING_KP,
    };
    GlobalNav_Init(&gnav_cfg);

    // ... 延迟启动 ...
    // 在 navTask 中延迟后调用 GlobalNav_Start()
}
```

---

## 9. 参数清单

### 9.1 新增参数 (`robot_params.h`)

```c
/* ======== P6 — 赛道级导航参数 ======== */

/* 入场段 */
#define PARAM_GNAV_ENTRY_V            0.08f   /* m/s — 入场速度 */
#define PARAM_GNAV_ENTRY_DISTANCE     0.65f   /* m   — 入场里程上限 */
#define PARAM_GNAV_ENTRY_TIMEOUT      10000   /* ms  — 入场超时 */

/* 转向 */
#define PARAM_GNAV_TURN_OMEGA         1.0f    /* rad/s — 转向角速度 */
#define PARAM_GNAV_TURN_TOLERANCE     0.087f  /* rad   — 转向完成容差 (~5°) */
#define PARAM_GNAV_TURN_DECEL_ZONE    0.5f    /* rad   — 接近目标时减速区 (~28°) */
#define PARAM_GNAV_TURN_MIN_OMEGA     0.3f    /* rad/s — 减速区最低角速度 */
#define PARAM_GNAV_TURN_TIMEOUT       8000    /* ms    — 单次转向超时 */

/* 重捕获 */
#define PARAM_GNAV_REACQUIRE_V        0.05f   /* m/s  — 重捕获入沟速度 */
#define PARAM_GNAV_REACQUIRE_CONF     0.6f    /* —    — 重捕获置信度阈值 */
#define PARAM_GNAV_REACQUIRE_WIDTH_TOL 0.05f  /* m    — 走廊宽度容差 */
#define PARAM_GNAV_REACQUIRE_TICKS    5       /* 拍   — 连续确认次数 (5×20ms=100ms) */
#define PARAM_GNAV_REACQUIRE_TIMEOUT  5000    /* ms   — 重捕获超时 */

/* 沟内 */
#define PARAM_GNAV_CORRIDOR_MAX_LEN   2.50f   /* m    — 沟内里程保护上限 */

/* 连接段 */
#define PARAM_GNAV_LINK_V             0.10f   /* m/s  — 连接段速度 */
#define PARAM_GNAV_LINK_DISTANCE      0.70f   /* m    — 连接段标称距离 (沟间距) */
#define PARAM_GNAV_LINK_TIMEOUT       8000    /* ms   — 连接段超时 */

/* 出场 */
#define PARAM_GNAV_EXIT_V             0.15f   /* m/s  — 出场速度 */
#define PARAM_GNAV_EXIT_RUNOUT        1.50f   /* m    — 出场侧向丢失后冲刺距离 */
#define PARAM_GNAV_EXIT_MAX_DIST      4.00f   /* m    — 出场里程保护 */
#define PARAM_GNAV_EXIT_TIMEOUT       20000   /* ms   — 出场超时 */

/* 回停 */
#define PARAM_GNAV_DOCK_V             0.05f   /* m/s  — 回停速度 */
#define PARAM_GNAV_DOCK_DISTANCE      0.50f   /* m    — 回停推进距离 */

/* 航向保持 */
#define PARAM_GNAV_HEADING_KP         0.03f   /* — — 航向保持P增益 (输入°，输出rad/s) */

/* 编译开关 */
#define USE_GLOBAL_NAV                1       /* 1=赛道模式  0=单沟测试 */
```

### 9.2 现有参数复用

以下现有参数直接复用，无需修改：

| 参数 | 用途 |
|------|------|
| `PARAM_SAFE_D_FRONT_STOP` | 到端检测距离 (沟内) |
| `PARAM_SAFE_D_FRONT_APPROACH` | 前向减速预警 |
| `PARAM_SAFE_CONF_ESTOP` | 沟内 E-STOP 阈值 |
| `PARAM_CTRL_*` | 沟内控制器增益 |
| `PARAM_EKF_*` | EKF 滤波参数 |
| `PARAM_CORRIDOR_WIDTH` | 走廊宽度 |

---

## 10. 文件清单汇总

### 10.1 新增文件

| 文件 | 位置 | 职责 | 代码量估计 |
|------|------|------|-----------|
| `global_nav.h` | `App/nav/` | 赛道级导航头文件 | ~120 行 |
| `global_nav.c` | `App/nav/` | 赛道级导航实现 | ~500 行 |
| `track_map.h` | `App/nav/` | 地图数据结构 | ~60 行 |
| `track_map.c` | `App/nav/` | 地图查询实现 | ~80 行 |

**总计新增: 4 个文件，约 760 行代码**

### 10.2 需要修改的文件

| 文件 | 改动内容 | 改动量 |
|------|---------|--------|
| `App/nav/segment_fsm.h` | `SegFsm_Update()` 签名增加 `SafetyMode_t` 参数 | ~10 行 |
| `App/nav/segment_fsm.c` | 根据 SafetyMode 分支安全策略 | ~50 行 |
| `App/preproc/corridor_msgs.h` | 新增 `SafetyMode_t` 枚举定义 | ~10 行 |
| `App/est/corridor_filter.h` | 确认 `CorridorFilter_Reset()` 接口 | ~5 行 |
| `App/est/corridor_filter.c` | 实现完整 Reset (含 yaw_ref 重置) | ~15 行 |
| `App/app_tasks.c` | navTask 流水线替换 + 初始化改造 | ~80 行 |
| `App/robot_params.h` | 新增 P6 参数组 | ~30 行 |
| `CMakeLists.txt` | 新增源文件编译 | ~3 行 |

**总计修改: 8 个文件，约 200 行改动**

### 10.3 不需要改动的文件

| 文件 | 说明 |
|------|------|
| `App/Can/snc_can_app.c/.h` | 已冻结，勿动 |
| `App/Contract/robot_blackboard.c/.h` | 接口已足够 |
| `App/Contract/robot_odom.c/.h` | 接口已足够 |
| `App/Contract/robot_cmd_slot.c/.h` | 接口已足够 |
| `App/est/corridor_ekf.c/.h` | 数学核心不变 |
| `App/preproc/corridor_preproc.c/.h` | 预处理逻辑不变 |
| `App/nav/corridor_ctrl.c/.h` | 控制律不变 |
| `App/nav/nav_script.c/.h` | 保留不删，通过编译开关切换 |
| `App/IMU/hwt101.c/.h` | 驱动不变 |
| `App/laser/laser_manager.c/.h` | 驱动不变 |
| `App/VL53L0X_API/**` | 驱动不变 |

---

## 11. 分步实施计划

### Phase 0: 准备工作 (半天)

**目标**: 为新模块搭建脚手架，不影响现有功能

1. 在 `robot_params.h` 中添加 P6 参数组和 `USE_GLOBAL_NAV` 开关（先设为 0）
2. 在 `corridor_msgs.h` 中添加 `SafetyMode_t` 枚举
3. 创建 `track_map.h/.c` 空壳文件
4. 创建 `global_nav.h/.c` 空壳文件（Init/Start/Stop/Update 都是空实现）
5. 修改 `CMakeLists.txt` 添加新源文件
6. 确认编译通过

**验证**: `cmake --build build/Debug` 无错误

---

### Phase 1: 安全层改造 (1天)

**目标**: 让安全层支持动作模式，解决 RISK-1

1. 修改 `segment_fsm.h`: `SegFsm_Update()` 增加 `SafetyMode_t mode` 参数
2. 修改 `segment_fsm.c`: 根据 mode 分支安全策略
3. 修改 `app_tasks.c` 中调用 `SegFsm_Update()` 的地方，旧模式传 `SAFETY_MODE_CORRIDOR`

**验证**: 
- `USE_GLOBAL_NAV=0` 编译通过
- 现有单沟测试行为不变 (回归测试)
- 手动测试 `SAFETY_MODE_TURN` 模式下 v=0+w≠0 能通过安全层

---

### Phase 2: 地图模块 (半天)

**目标**: 完成 `track_map` 并验证查询逻辑

1. 实现 `track_map.c` 的全部查询函数
2. 在 `global_nav.c` 的 Init 中调用 `TrackMap_Init()`

**验证**: 在初始化日志中打印 6 条沟的遍历顺序和转向方向，确认 S 型正确

---

### Phase 3: 赛道级状态机核心 (2-3天)

**目标**: 实现 `global_nav.c` 的完整状态机

分步：
1. 先实现 IDLE → ENTRY_STRAIGHT → TURN_INTO_CORRIDOR → REACQUIRE → CORRIDOR_TRACK 链路
2. 测试单沟入沟流程
3. 再实现 CORRIDOR_TRACK → TURN_OUT → LINK_STRAIGHT → TURN_INTO_NEXT → REACQUIRE 链路
4. 测试双沟 S 型过渡
5. 最后实现 EXIT_STRAIGHT → DOCK → FINISHED 链路
6. 测试完整流程

**验证**:
- 用 printf 打印状态转移日志
- 先在桌面空转 (无传感器) 验证状态机逻辑
- 再实车单沟测试
- 再实车双沟测试
- 最后实车全 6 沟测试

---

### Phase 4: EKF 重置流程 (半天)

**目标**: 确保每次进入新垄沟时 EKF 状态干净

1. 确认 `CorridorFilter_Reset()` 完整重置 EKF + yaw_ref
2. 在 REACQUIRE 进入时调用 Reset
3. 验证重捕获判据在各种入沟角度下的鲁棒性

**验证**: 连续跑 2-3 条沟，观察每条沟入沟后 EKF conf 的收敛曲线

---

### Phase 5: 流水线集成 (1天)

**目标**: `USE_GLOBAL_NAV=1` 时完整流水线打通

1. 修改 `app_tasks.c` 的 `AppTasks_RunNavTask_Impl()`
2. 修改 `AppTasks_Init()` 的初始化流程
3. 用 `#if USE_GLOBAL_NAV` 保留旧路径

**验证**: 全编译 + 实车全流程测试

---

### Phase 6: 参数调优与鲁棒化 (2-3天)

**目标**: 在实车上反复测试并调优

重点调优项：
1. 入场段里程和速度
2. 转向角速度和容差
3. 重捕获判据阈值和确认拍数
4. 连接段里程和超时
5. 出场检测逻辑
6. 各阶段超时值

---

### 总工期估计

| 阶段 | 工期 | 累计 |
|------|------|------|
| Phase 0: 准备 | 0.5 天 | 0.5 天 |
| Phase 1: 安全层 | 1 天 | 1.5 天 |
| Phase 2: 地图 | 0.5 天 | 2 天 |
| Phase 3: 状态机 | 2-3 天 | 4-5 天 |
| Phase 4: EKF 重置 | 0.5 天 | 4.5-5.5 天 |
| Phase 5: 集成 | 1 天 | 5.5-6.5 天 |
| Phase 6: 调优 | 2-3 天 | 7.5-9.5 天 |
| **总计** | **~8-10 天** | |

---

## 12. 风险与降级策略

### 12.1 已知风险

| 风险 | 严重度 | 说明 | 缓解措施 |
|------|--------|------|---------|
| **转向精度不足** | 高 | IMU 漂移导致 90° 转角不准 | 转向后用 VL53 重捕获作为闭环验证；容差设 5° |
| **连接段偏航** | 高 | IMU 短时漂移 + 轮子打滑导致连接段走歪 | 里程上限保护 + 超时保护 + 侧向VL53早期探测 |
| **重捕获失败** | 中 | 入沟角度过大导致 VL53 无法同时看到两侧壁 | 重捕获阶段允许单侧先有效，慢速蠕行直到双侧 |
| **到端检测假阳性** | 中 | 沟内障碍物被误判为端部围栏 | 可加里程下限保护 (已走 > 1.5m 才允许到端) |
| **出场方向错误** | 低 | 最后一条沟出来后航向偏差过大 | 出场段用 IMU 保持航向 + 里程超限保护 |

### 12.2 降级策略

| 场景 | 降级方案 |
|------|---------|
| 重捕获超时 | → ERROR → 停车 (保得分，不撞) |
| 连接段超时 | → ERROR → 停车 |
| 任何阶段 IMU 数据异常 | → ERROR → 停车 |
| 只能跑 3 条沟 | 修改 `track_map` 的 `is_last` 标志，提前结束 |
| 来不及实现完整出场 | 最后一条沟结束后直接停车 (放弃回停得分) |

### 12.3 不做什么 (明确的非目标)

1. **不做 SLAM / 全局定位**: 不需要知道 (x, y) 绝对坐标
2. **不做路径规划**: S 型路径是硬编码的，不需要搜索
3. **不做目标物检测/抓取**: 本方案只管导航，不管捡拾
4. **不重写 EKF**: 现有 EKF 对沟内跟踪已经够用
5. **不重写驱动层**: 传感器驱动、CAN 协议、里程计全部保留
6. **不做动态避障**: 沟内假设无动态障碍物

---

## 附录 A: 完整 S 型遍历动作序列表

| # | 状态 | 行为 | v (m/s) | w (rad/s) | 主传感器 | 退出条件 |
|---|------|------|---------|-----------|---------|---------|
| 0 | IDLE | 等待 | 0 | 0 | — | Start() |
| 1 | ENTRY_STRAIGHT | 慢速前进 | 0.08 | PD保直 | IMU | 侧向VL53有效/里程/超时 |
| 2 | TURN_INTO_CORRIDOR (右转) | 原地转90° | 0 | -1.0 | IMU yaw | 转角≥85° |
| 3 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效+conf≥0.6 |
| 4 | CORRIDOR_TRACK (C1→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
| 5 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 6 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
| 7 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 8 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
| 9 | CORRIDOR_TRACK (C2←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
| 10 | TURN_OUT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
| 11 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
| 12 | TURN_INTO_NEXT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
| 13 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
| 14 | CORRIDOR_TRACK (C3→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
| 15 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 16 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
| 17 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 18 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
| 19 | CORRIDOR_TRACK (C4←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
| 20 | TURN_OUT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
| 21 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
| 22 | TURN_INTO_NEXT (右转) | 原地转90° | 0 | -1.0 | IMU | 转角≥85° |
| 23 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
| 24 | CORRIDOR_TRACK (C5→) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
| 25 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 26 | LINK_STRAIGHT | 连接段 | 0.10 | PD保直 | IMU+odom | 里程≥0.70m |
| 27 | TURN_INTO_NEXT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 28 | REACQUIRE | 低速入沟 | 0.05 | PD保直 | VL53 | 双侧有效 |
| 29 | CORRIDOR_TRACK (C6←) | 沟内跟踪 | 0.15 | PD | VL53+IMU | d_front≤0.10m |
| 30 | TURN_OUT (左转) | 原地转90° | 0 | +1.0 | IMU | 转角≥85° |
| 31 | EXIT_STRAIGHT | 朝出口直行 | 0.15 | PD保直 | IMU+odom | VL53全丢+里程 |
| 32 | DOCK | 回停 | 0.05 | 0 | odom | 里程≥0.5m |
| 33 | FINISHED | 停车 | 0 | 0 | — | 终态 |

> **全程共**: 6 次沟内跟踪 + 11 次 90° 转向 + 5 次连接段 + 6 次重捕获 + 1 次入场 + 1 次出场 + 1 次回停 = **33 步动作**

---

## 附录 B: 关键设计决策记录

| 决策 | 选择 | 理由 |
|------|------|------|
| 地图用静态表还是动态生成 | **静态表** | 场地固定，运行时不变 |
| 转向用 90° 还是 180° | **90°** | S 型遍历天然是 90° 转向 |
| 是否需要全局坐标 (x, y) | **不需要** | 拓扑+里程足够，全局定位反而引入累积误差 |
| 重捕获判据用 conf 还是 raw VL53 | **两者都用** | conf 依赖 EKF 收敛时间，raw VL53 提供即时几何校验 |
| EKF 每次入沟是否 Reset | **Reset** | 不同沟的 e_y 参考不同，必须重建 |
| 航向保持用 PD 还是纯 P | **纯 P** | 连接段短 (<1s)，微分项贡献小，简化设计 |
| 出场方向如何确定 | **IMU 保持最后转出方向** | 最后一条沟出来后已经朝向出口 |
| 混合导航模块合几个文件 | **global_nav 1个 + track_map 1个** | 原方案拆 6 个文件过碎，实际耦合度高，合并更清晰 |
| 是否保留 nav_script | **保留，编译开关切换** | 作为单沟验证脚本仍有价值 |

---

## 附录 C: 与混合导航方案.md 的差异说明

| 混合导航方案.md 建议 | 本方案处理 | 原因 |
|---------------------|----------|------|
| 新增 6 个文件 (global_nav_fsm, track_map, lane_transition, reacquire_detector, heading_hold, exit_dock) | **合并为 2 个文件** (global_nav, track_map) | lane_transition / reacquire / heading_hold / exit_dock 的逻辑都嵌入 global_nav 的状态处理中，单独成文件会增加模块间调用复杂度，且每个文件代码量不足 100 行 |
| 航向不再使用 VL53 差分 | **保留 EKF 中的 VL53 航向观测，但降权** | 完全移除 VL53 航向观测需要改 EKF 数学，风险大；EKF 已有 chi2 拒绝机制，让它自然退化即可 |
| 赛道级核心状态量列了 10 个变量 | **精简为必要字段** | `stage_progress` 等概念已被 odom_s + elapsed_ms 覆盖 |
| REACQUIRE_CORRIDOR 作为独立阶段 | **确认保留** | 这是连接沟内闭环与段间动作的关键过渡，必须显式建模 |
| segment_fsm 区分 4 种动作模式 | **确认采纳，但简化为 SafetyMode_t 枚举** | 不需要传复杂结构体，一个枚举足够 |

---

> **文档结束**  
> 本方案设计完成后，实施第一步应是 Phase 0（脚手架搭建），确保不破坏现有功能的前提下开始增量开发。