ASER/Doc/混合导航方案.md

混合导航方案

1. 文档目的

本文档用于明确本项目后续正式比赛版导航应采用的总体方案。

目标不是重写当前全部导航代码，而是：

1. 保留现有“垄沟内局部闭环控制”能力
2. 在其上补齐赛道级状态机与段间动作编排
3. 让机器人能够按照固定地图完成 6 条垄沟的 S 型遍历
4. 最终从唯一出口驶离并停回启动区

本文档强调的是“混合导航”：

• 上层使用固定地图和拓扑状态机决定现在该去哪
• 中层使用动作执行器完成转向、连接段推进、再入沟
• 下层使用现有局部传感器闭环完成沟内稳定行驶

它不是纯局部反应式导航，也不是通用 SLAM。

2. 已知场地理解

根据 Doc/map.md：

• 场地净尺寸约为 300cm x 390cm
• 内部有 5 条田垄
• 因围栏与田垄、田垄与田垄之间均有通道，所以可通行垄沟实际为 6 条
• 启动区位于场地下侧靠左，外接唯一入口
• 各条垄沟是横向分布的长通道
• 垄沟间通过左右两端的短连接段串起来

因此，比赛中的真实轨迹不是“在端部横移搜索下一条沟”，而是：

1. 从启动区进入场地
2. 沿入口直线段前进
3. 到第 1 条垄沟入口附近
4. 原地转 90° 入沟
5. 沿垄沟通过
6. 到端后原地转 90°
7. 走一小段连接直线
8. 再原地转 90° 入下一条垄沟
9. 重复以上动作，形成 S 型遍历
10. 全部垄沟完成后离场并回停启动区

3. 为什么要做混合导航

当前项目已经具备较强的局部能力：

• 4 路侧向 VL53 做走廊观测
• IMU 提供 wz 和 yaw_continuous
• EKF / Filter 输出 e_y、e_th、conf
• corridor_ctrl 输出沟内控制指令
• segment_fsm 负责安全裁剪
• nav_script 能做单段脚本验证

但这套能力本质上仍然偏向：

• “单条垄沟怎么跑稳”
• 而不是
• “整张赛道下一步该去哪里”

正式比赛需要解决的核心问题有：

1. 当前正在第几条垄沟
2. 下一条应该进入哪条垄沟
3. 当前应该左转还是右转
4. 什么时候从沟内控制切到端部动作
5. 什么时候从端部动作切回沟内控制
6. 什么时候结束全部遍历并离场
7. 离场后如何回停到启动区

这些问题无法只靠局部测距瞬时值回答，必须引入上层任务状态。

4. 混合导航的核心思想

本项目推荐采用：

固定地图 + 赛道级状态机 + 局部闭环控制

其中：

• 固定地图负责描述赛道结构
• 状态机负责描述任务推进
• 局部闭环负责把当前这一小段走稳

整体思路是：

• 用地图回答“接下来去哪”
• 用状态机回答“现在该做什么动作”
• 用传感器闭环回答“这一段怎么安全稳定地过去”

5. 三层架构

5.1 上层：赛道级导航层

职责：

• 记录当前 corridor_id
• 决定下一个目标 target_corridor_id
• 决定当前阶段
• 决定下一步是左转还是右转
• 在所有阶段之间推进任务

这层不直接控制车轮，只输出“当前应该执行哪种段动作”。

5.2 中层：段动作执行层

职责：

• 入场直线推进
• 90° 原地转向
• 连接段直线推进
• 再次 90° 入沟
• 出场段动作
• 回停启动区动作

这层输出当前周期的期望 v/w，但仍需经过安全层裁剪。

5.3 下层：局部闭环控制层

职责：

• 在垄沟内保持居中
• 控制 e_y 和 e_th
• 提供局部重捕获判据
• 在当前段可观测时给出稳定闭环

这一层尽量复用现有实现，不重复发明轮子。

6. 当前代码与未来架构的对应关系

现有代码可保留并复用的部分：

1. App/preproc/

• 继续负责传感器清洗与观测构造

2. App/est/

• 继续负责 e_y / e_th / conf 估计

3. App/nav/corridor_ctrl.c

• 继续负责沟内局部控制

4. App/Contract/robot_blackboard.*

• 继续作为全局传感器快照中心

5. App/Contract/robot_cmd_slot.*

• 继续作为导航输出到 CAN 的命令槽

6. App/nav/segment_fsm.*

• 保留为安全层，但后续必须增加“动作语义感知”

当前不应再承担最终比赛全局职责的部分：

1. App/nav/nav_script.c

• 当前更像“单垄沟验证脚本”
• 不适合继续膨胀成完整赛道导航总控

因此后续应新增赛道级模块，而不是把全部逻辑继续堆进 nav_script.c。

7. 推荐状态机建模

建议把赛道任务拆成以下大阶段：

1. START_ZONE

• 启动区待发

2. ENTRY_STRAIGHT

• 从启动区经唯一入口进入场地
• 沿左侧入口直线段前进

3. TURN_INTO_CORRIDOR

• 到目标垄沟入口后原地转 90°
• 对准目标垄沟

4. CORRIDOR_TRACK

• 沟内闭环跟踪
• 使用现有 corridor_ctrl

5. TURN_OUT_AT_END

• 到达当前垄沟末端
• 原地转 90° 转向连接段

6. LINK_STRAIGHT

• 沿端部连接段直行一小段
• 用 IMU 保持航向
• 用里程计或事件触发控制推进

7. TURN_INTO_NEXT_CORRIDOR

• 原地转 90°
• 对准下一条垄沟

8. REACQUIRE_CORRIDOR

• 低速确认两侧 VL53 是否重新形成合理走廊结构
• 成功后切回 CORRIDOR_TRACK

9. EXIT_FIELD

• 全部垄沟完成后，朝唯一出口离场

10. DOCK_START_ZONE

• 回到启动区并停车

11. FINISHED

• 比赛结束

8. 赛道级核心状态量

建议赛道级层显式维护以下变量：

• current_corridor_id
• target_corridor_id
• total_corridor_count = 6
• travel_direction
• turn_side
• stage
• stage_progress
• next_turn_is_left
• is_final_exit_phase
• reacquire_confirm_count

其中最关键的是：

• 当前在第几条沟
• 下一条是哪条沟
• 这次入沟应该左转还是右转
• 当前处于哪个动作阶段

9. 传感器参数与角色分工

9.1 左右 VL53L0X

已知参数：

• 每侧 2 个，共 4 个
• 主要用于侧向测距
• 精确测量距离按当前工程经验取 1.2m 以内
• 当前已由人工完成标定，但单点测距仍存在约 ±1cm 的偏差

适合：

• 沟内居中
• 入沟重捕获确认

不适合：

• 作为 yaw / e_th 的主观测来源
• 远距离搜索下一条沟入口
• 独立完成赛道级导航

设计含义：

• VL53 是近场几何约束传感器
• 只能在“已经接近某条沟”时帮你锁住这条沟
• 不能把“下一条沟在哪里”这个问题压给它
• 由于单点误差量级约为 ±1cm，同侧前后差分法对噪声非常敏感
• 因此不推荐继续用 VL53 前后差分直接计算 yaw，航向应主要依赖 IMU

9.2 前后 STP-23L

已知参数：

• 前后各 1 个
• 有效测距范围 7cm ~ 7.5m

适合：

• 到端检测
• 前后安全边界监测
• 开阔区边界辅助判定
• 某些段落的事件触发

不适合：

• 独立判断当前位于哪条垄沟

设计含义：

• STP 是远距离边界感知传感器
• 它适合回答“前面/后面还有多远”“是否接近端部或围栏”
• 不适合承担精细入沟定位

9.3 前后 ATK-MS53L1M

已知参数：

• 前后各 1 个
• 有效测距范围 4cm ~ 3.9m

适合：

• 近距离补盲
• 填补 STP 在近端盲区的不足
• 近场防撞保护

设计含义：

• ATK 不是主导航传感器
• 它的核心价值是让前后边界感知在近距离不断层
• 在转向、再入沟、靠近围栏时很重要

9.4 IMU

适合：

• 原地转 90°
• 连接段航向保持
• 无侧墙阶段的短时姿态约束

9.5 编码器 / 里程计

适合：

• 连接段推进量估计
• 段落推进计量
• 动作超时和距离上限保护

注意：

• 地毯和打滑会影响绝对精度
• 不能单独作为最终入沟确认依据

10. 各传感器在混合导航中的分工原则

建议按下面的分工使用传感器：

1. 沟内阶段

• 主用：左右 VL53 做横向约束，IMU 做航向约束
• 辅助：前后激光仅做安全和到端检测

2. 转向阶段

• 主用：IMU yaw_continuous
• 辅助：前后激光做安全保护

3. 连接段阶段

• 主用：IMU + 里程计
• 辅助：前后 STP/ATK 做边界与防撞

4. 再入沟阶段

• 主用：左右 VL53
• 辅助：IMU 做姿态稳定，前后激光做安全兜底

一句话总结：

• VL53 负责“锁住局部走廊”
• IMU 负责“航向约束和跨过无墙约束阶段”
• 里程计 负责“推进量”
• STP/ATK 负责“边界和安全”

10.1 关于航向观测的专项说明

当前侧向 VL53L0X 虽然已经完成标定，但单点测距仍有约 ±1cm 偏差。

这个精度对于：

• 居中控制
• 左右偏移判断
• 重新捕获一条沟

通常是够用的。

但如果把它直接用于航向估计，例如用同侧前后距离差去推导 yaw / e_th，会遇到两个问题：

1. 同侧前后差分属于“小量减小量”，对噪声天然敏感
2. 当前 ±1cm 的单点误差已经足以让差分航向观测明显抖动

因此推荐原则是：

• VL53 负责横向约束和重捕获
• IMU wz + yaw_continuous 负责航向估计与转向控制
• 不再把 VL53 作为 yaw 主观测

11. 动作执行原则

11.1 沟内阶段

• 主要依赖侧向 VL53 做横向闭环，IMU 做航向闭环
• 使用 corridor_ctrl
• 安全层负责限速和急停

11.2 转向阶段

• 主要依赖 IMU yaw_continuous
• 目标是稳定完成 90°
• 安全层不能再沿用普通“前方太近则整段全停”的逻辑
• 必须允许 v=0, w!=0 的受限原地转向

11.3 连接段阶段

• 主要依赖 IMU 保持连接段朝向
• 使用里程计推进
• 接近预计入口后降速
• 前后 STP/ATK 负责边界辅助与防撞
• 进入重捕获阶段等待局部结构恢复

11.4 重捕获阶段

判据建议包括：

• 左右两侧 VL53 同时有效
• 左右几何关系符合 40cm 垄沟模型
• conf 高于阈值
• 持续若干拍成立

只有重捕获成功后，才允许切回沟内闭环。

12. 推荐新增模块

建议新增以下模块。

12.1 App/nav/global_nav_fsm.c/.h

职责：

• 维护整场比赛任务阶段
• 管理 corridor_id
• 决定下一步目标段
• 向下游发布当前动作类型

12.2 App/nav/track_map.c/.h

职责：

• 固化比赛地图拓扑
• 保存各条垄沟、连接段、入口、出口的相对关系
• 提供“当前完成哪条后下一条是谁”的规则查询

12.3 App/nav/lane_transition.c/.h

职责：

• 执行端部出沟、连接段推进、再入沟
• 内部管理两个 90° 转向和一段连接直线

12.4 App/nav/reacquire_detector.c/.h

职责：

• 负责判断是否已重新进入目标垄沟
• 对 VL53 几何结构和 conf 做持续判定

12.5 App/nav/heading_hold.c/.h

职责：

• 在无侧墙阶段提供短时航向保持
• 可独立实现，也可并入 lane_transition

12.6 App/nav/exit_dock.c/.h

职责：

• 负责最终离场与启动区停车

13. 推荐修改的现有模块

13.1 segment_fsm

必须补：

• 动作模式输入
• 区分：
  • 沟内前进
  • 原地转向
  • 连接段推进
  • 出场段直线
• 否则正式比赛阶段会在端部动作上卡死

13.2 nav_script

建议定位调整为：

• 临时验证脚本
• 单段测试脚本
• 或过渡期动作编排器

不建议继续作为最终赛道总控。

13.3 corridor_msgs

应补充：

• 赛道级阶段枚举
• 动作模式枚举
• 重捕获结果结构
• 赛道级状态输出结构

14. 推荐实施顺序

第 1 步：补底层动作语义

先修好：

• 原地转向安全逻辑
• 局部控制与安全层语义一致性
• 局部测试模式与可观测性

目标：

• 让“走沟、转向、连接直行”都能单独稳定测试

第 2 步：加入赛道级状态机

新增：

• global_nav_fsm
• track_map

目标：

• 系统明确知道“当前第几沟、下一沟是谁、这次该左转还是右转”

第 3 步：加入段间动作执行器

新增：

• lane_transition
• heading_hold
• reacquire_detector

目标：

• 从一条沟末端稳定过渡到下一条沟入口并重新入沟

第 4 步：补最终出场与回停

新增：

• exit_dock

目标：

• 让整场流程闭环，不只是在 6 条沟之间来回

第 5 步：统一参数、日志和调试接口

目标：

• 可调
• 可观测
• 可复现
• 可在实地快速定位问题

15. 一句话结论

本项目后续不应继续按“单沟脚本补丁”方式扩展。

正确方向应是：

用固定地图描述赛道，用赛道级状态机管理 S 型遍历，用动作执行器完成两次 90° 转向与连接段推进，再用现有局部闭环完成每一条垄沟内的稳定行驶。