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