ASER/Doc/HYBRID_NAVIGATION_GUIDE.md

# 固定地图赛道的混合导航说明

## 1. 文档目的

本文档面向后续接手项目的人，说明当前比赛场景下为什么推荐采用一种“混合导航”方案，而不是单纯依赖局部传感器闭环，也不是直接上通用 SLAM / 全局路径规划。

这里的“混合导航”特指：

- 上层使用固定地图和拓扑状态机
- 中层使用段落动作与事件切换
- 下层使用局部传感器闭环稳定控制

这套思路适用于当前这类：

- 场地结构固定
- 任务流程固定
- 局部几何约束很强
- 但局部传感器看不到全局目标

的比赛型机器人系统。

## 2. 场地与任务本质

根据比赛规则与 `HANDOFF.md`：

- 场地大小为 `390cm x 300cm`
- 内部有 `5` 条田垄
- 实际需要遍历的是 `6` 条垄沟
- 每条垄沟宽 `40cm`
- 每条田垄长 `220cm`
- 只有 `1` 个出入口
- 出入口外有启动区

从导航角度，这不是自由环境中的随机移动问题，而是一个非常明确的固定任务：

1. 从启动区进入场地
2. 进入某一条垄沟
3. 沿垄沟稳定前进并作业
4. 到达端部后完成转向与换沟
5. 依次遍历全部 `6` 条垄沟
6. 最终从唯一出口驶离场地
7. 自主停回启动区

如果把整个任务抽象出来，本质上就是一个 **S 形遍历任务**。

示意如下：

```text
入口 -> 沟1 ↑
         ↓ 沟2
         ↑ 沟3
         ↓ 沟4
         ↑ 沟5
         ↓ 沟6 -> 出口
```

这类问题最重要的不是“任意时刻知道自己在全局坐标中的绝对位置”，而是：

- 知道自己当前在第几条沟
- 知道下一条目标沟是哪一条
- 知道当前处于入沟、走沟、到端、换沟、出场还是回停阶段
- 在每个阶段里，用合适的传感器和控制策略去完成当前动作

## 3. 为什么不能只靠局部导航

当前项目已经具备很强的局部导航能力，尤其是在单条垄沟内：

- 侧向测距可以支撑居中或偏置行驶
- IMU 可以提供航向与转角信息
- 前向测距可以做端部触发
- 里程计可以做短时推进量估计

但只靠局部导航会遇到明显边界：

### 3.1 局部导航能解决什么

局部导航适合解决：

- 在窄沟内居中
- 在窄沟内保持姿态稳定
- 接近端部时减速或停车
- 在局部可观测条件下闭环修正误差

### 3.2 局部导航解决不了什么

局部导航不擅长解决：

- 我现在完成了第几条沟
- 下一条目标沟应该是左边还是右边
- 转完向后该走多远才能接近下一条沟入口
- 什么时候该结束整场遍历并朝唯一出口离场

这些问题需要：

- 全局任务记忆
- 地图拓扑信息
- 阶段状态机
- 跨局部观测空窗的动作脚本

所以，仅靠“看到什么就跟什么”的局部反应式导航，不足以稳定跑完整场比赛。

## 4. 为什么也不建议直接上通用 SLAM

有些人看到“局部导航不够”，第一反应会是“那就做全局 SLAM”。

但当前赛题并不适合直接走这条路线。

### 4.1 地图不是未知的

赛场结构高度固定：

- 垄沟数固定
- 相对排列固定
- 出入口固定
- 启动区固定

这意味着没有必要像服务机器人那样，在未知环境中一边探索一边建图。

### 4.2 场地高度对称

`6` 条垄沟在局部上非常相似。

这会让很多通用全局定位方法遇到典型问题：

- 局部观测相似
- 重定位歧义大
- 小误差可能让系统把“第 2 沟”认成“第 3 沟”附近

### 4.3 比赛更看重稳定完赛，而不是地图美观

比赛规则更关心：

- 能不能完整遍历
- 能不能不撞边
- 能不能顺利出场
- 能不能停回启动区

不是在考察一套通用 SLAM 系统的建图效果。

### 4.4 通用 SLAM 工程负担大

如果直接上完整 SLAM，通常还要处理：

- 更复杂的状态与数据关联
- 更高的开发和调参成本
- 更重的算力与调度开销
- 更难解释的失败模式

这和当前项目追求的工程目标并不匹配。

## 5. 什么是“混合导航”

当前场景下推荐的“混合导航”，可以概括成一句话：

**用固定地图决定去哪，用状态机决定现在该做什么，用局部传感器闭环决定这一小段怎么稳稳地过去。**

它不是纯局部，也不是纯全局，而是分层协作。

## 6. 混合导航的三层结构

## 6.1 上层：固定地图与拓扑状态机

上层负责的是“全局任务理解”。

它需要维护的信息包括：

- 当前所在赛道段编号
- 当前已经完成了第几条垄沟
- 下一条目标垄沟是哪一条
- 当前应该左换沟还是右换沟
- 当前处于：启动、入场、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段

这一层不一定关心厘米级位置，而更关心：

- 拓扑顺序
- 阶段推进
- 事件触发

可以把整场任务拆成固定段落，例如：

1. 启动区准备
2. 入口对准
3. 垄沟 1 前进
4. 端部换沟 1
5. 垄沟 2 返回
6. 端部换沟 2
7. 垄沟 3 前进
8. ...
9. 最后一沟结束
10. 出场
11. 回停启动区

这就是“固定地图导航”的核心，不是基于任意坐标规划，而是基于已知赛道结构推进任务。

## 6.2 中层：段落动作与事件切换

中层负责把“上层目标”翻译成可执行动作。

例如在某个阶段，机器人可能执行：

- 入口慢速直行，直到捕获双侧结构
- 沿当前垄沟闭环跟踪
- 到端后原地转 `90°` 或 `180°`
- 保持某个航向横向推进一段距离
- 在接近预计位置后减速，并等待重新捕获新垄沟
- 切回垄沟跟踪模式

这一层的本质是“段脚本 + 事件触发”。

它依赖：

- IMU 姿态
- 里程计推进量
- 前向/后向安全距离
- 局部结构重新捕获结果

这层很重要，因为许多时候机器人会暂时处于“看不到完整走廊结构”的状态，比如端部换沟阶段。

## 6.3 下层：局部传感器闭环

下层负责在小范围内把车稳稳控制住。

典型任务：

- 在垄沟内居中
- 在垄沟内偏向 1/4 宽度行驶
- 根据 IMU 保持航向
- 根据前向测距做减速与停车

这一层应当追求：

- 高频
- 稳定
- 可降级
- 不依赖复杂全局推理

你现在已有的大部分控制能力，都属于这一层。

## 7. 各类传感器在混合导航中的角色

## 7.1 左右 VL53L0X

推荐职责：

- 走廊内横向定位
- 居中/偏置行驶
- 近场重捕获新垄沟

不推荐职责：

- 远距离识别下一条垄沟入口
- 独立承担换沟全流程导航

原因很简单：

- 它们是近场侧向传感器
- 强项是局部几何闭环
- 弱项是远距离赛道级感知

## 7.2 IMU

推荐职责：

- 航向估计主来源
- 转向角度判定
- 无侧墙约束阶段的短时姿态保持
- 换沟阶段的朝向控制

IMU 在混合导航里非常关键，因为它能帮助机器人跨过“局部结构暂时缺失”的区间。

## 7.3 编码器 / 里程计

推荐职责：

- 估算走过了多长距离
- 在换沟动作中提供推进量
- 与状态机结合做段落退出条件

限制：

- 遇到地毯、打滑、轮胎差异时会有误差

因此它适合作为“推进量参考”，但不适合作为唯一定位真值。

## 7.4 前后长距测距

推荐职责：

- 到端检测
- 防撞保护
- 开阔区事件辅助
- 出场或接近边界时的安全约束

这类传感器不一定直接告诉你“下一沟入口就在前方”，但能帮助你判断：

- 是否接近端部
- 是否接近围栏
- 是否该切换动作阶段

## 8. 为什么这种方案适合当前比赛

## 8.1 它符合固定地图的特点

比赛地图结构是已知的。

这意味着：

- 目标垄沟不是未知搜索对象
- 换沟不必靠“发现远处入口”
- 可以由状态机根据当前进度直接推导“下一步该去哪”

## 8.2 它承认局部传感器的边界

混合导航没有让 VL53 去做它不擅长的事情。

它承认：

- VL53 负责局部
- IMU 负责短时姿态保持
- 里程计负责推进量
- 上层状态机负责“全局流程”

这是符合传感器物理特性的分工。

## 8.3 它能兼容尺寸误差与地毯

规则里有两个现实问题：

- 场地尺寸允许 `+-5%`
- 有 `2` 条垄沟会随机铺地毯

这意味着：

- 不能死信地图上的绝对尺寸
- 不能死信纯里程推进

混合导航的好处是：

- 上层地图只给出大方向和段结构
- 局部闭环用实时传感器做最终修正

这样既利用了先验地图，又不会被固定脚本锁死。

## 9. 一个典型的工作流程

下面给出一个典型流程，帮助理解这套导航在比赛中的运行方式。

### 9.1 启动与入场

- 上层状态机进入“入口对准”阶段
- 机器人从启动区朝入口前进
- 一旦左右侧传感器稳定捕获到垄沟结构，切换为垄沟跟踪模式

### 9.2 垄沟内前进

- 下层局部控制根据左右侧测距维持横向位置
- IMU 维持航向
- 前向测距用于到端检测
- 状态机记录当前垄沟编号与方向

### 9.3 到端

- 前向距离达到阈值
- 状态机判定当前垄沟已到端
- 进入转向阶段

### 9.4 换沟

- IMU 控制转向到目标朝向
- 编码器推进预定距离
- 长距测距做安全约束
- 接近预期位置后减速
- 直到左右 VL53 再次捕获新垄沟结构

### 9.5 再入沟

- 一旦检测到新的局部走廊结构
- 状态机确认已进入下一条垄沟
- 切换回局部走廊跟踪

### 9.6 重复直到遍历完成

- 状态机更新“当前第几沟”
- 按 S 形顺序重复以上过程

### 9.7 最终离场与回停

- 当最后一条垄沟完成后
- 状态机切换到出场段
- 利用固定地图与局部感知朝唯一出口离开
- 出场后再执行停回启动区动作

## 10. 与当前项目的关系

当前项目已经具备混合导航中的一部分基础：

- 局部走廊控制
- IMU 航向处理
- 到端检测
- 原地转向
- 段脚本雏形

但当前仍偏向：

- 单垄沟验证
- 局部闭环主导
- 缺少完整的赛道级段落管理

所以后续真正需要补的，并不是完全推翻现有控制，而是：

- 把“局部走廊能力”封装成底层能力
- 在其上补一个完整的固定地图状态机
- 把换沟和出场逻辑系统化

## 11. 一个容易犯的错误

在这类项目里，最容易犯的错误是两种极端：

### 11.1 极端一：把所有问题都交给局部传感器

这会导致：

- 换沟阶段无从判断全局目标
- 容易在端部迷失
- 难以稳定完成多垄沟遍历

### 11.2 极端二：把所有问题都交给“全局定位”

这会导致：

- 系统复杂度暴涨
- 对称环境下定位歧义严重
- 与比赛需求不匹配

混合导航的价值就在于避开这两个极端。

## 12. 推荐结论

对于当前比赛场景，推荐的总体思路是：

1. 用固定地图描述整条 S 形任务路线
2. 用拓扑状态机管理“当前在哪一段、下一步去哪一段”
3. 用 IMU 和里程计支撑跨局部观测空窗的动作执行
4. 用左右 VL53 负责局部垄沟内的高精度横向闭环与重捕获
5. 用前后长距测距做端部识别与安全保护

这就是当前场景最合适的混合导航方案。

## 13. 一句话总结

这类比赛不是“靠一套万能定位算法解决全部问题”，而是“让合适的层做合适的事”：

- 地图负责全局流程
- 状态机负责阶段切换
- 传感器负责局部闭环
- 控制器负责把每一小段稳稳跑完

这就是混合导航的核心价值。