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