# 混合导航方案 ## 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° 转向与连接段推进，再用现有局部闭环完成每一条垄沟内的稳定行驶。**