Files

2026-04-03 08:56:26 +08:00

57 KiB

Raw Blame History

ARES 混合导航实施方案

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

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)

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

可用公式表达:

// 当前沟结束后的转向方向
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)

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 设计原则

地图不做全局坐标定位。地图只回答三个问题：

从第 N 条沟完成后，下一条是第几条？
这次该往哪转？（左/右）
当前是不是最后一条沟？

5.2 数据结构

/* ---- 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 型遍历表)

/* ---- 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 的赛道编排职能。

头文件接口

/* ---- 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);

内部状态

/* ---- 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 逻辑 (伪代码)

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 函数

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° 转向逻辑)

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 表 + 查询函数。

/* 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` — 安全层增加动作模式感知

改动要点

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

改动后的行为表

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

改动后的函数签名

/* 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`)

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 保持现状，用于单垄沟测试验证

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

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

新增以下定义：

/* 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() 可能不存在或功能不完整。需要确保：

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

该函数应：

调用 CorridorEKF_Reset()
重置 yaw_ref 状态
重置 yaw_ref_locked 标志

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

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

详见第 8 节。

8. 数据流水线改造

8.1 改造后的 navTask 流水线

/* 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 初始化改造

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`)

/* ======== 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: 准备工作 (半天)

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

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

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

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

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

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

验证:

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

Phase 2: 地图模块 (半天)

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

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

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

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

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

分步：

先实现 IDLE → ENTRY_STRAIGHT → TURN_INTO_CORRIDOR → REACQUIRE → CORRIDOR_TRACK 链路
测试单沟入沟流程
再实现 CORRIDOR_TRACK → TURN_OUT → LINK_STRAIGHT → TURN_INTO_NEXT → REACQUIRE 链路
测试双沟 S 型过渡
最后实现 EXIT_STRAIGHT → DOCK → FINISHED 链路
测试完整流程

验证:

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

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

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

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

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

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

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

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

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

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

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

重点调优项：

入场段里程和速度
转向角速度和容差
重捕获判据阈值和确认拍数
连接段里程和超时
出场检测逻辑
各阶段超时值

总工期估计

阶段	工期	累计
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 不做什么 (明确的非目标)

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

附录 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（脚手架搭建），确保不破坏现有功能的前提下开始增量开发。

57 KiB Raw Blame History Unescape Escape