ASER/Doc/GLOBAL_NAV_REQUIREMENT.md

# 固定场地条件下的赛道级导航需求说明

## 1. 问题背景

当前项目已经具备较完整的“单条垄沟内局部导航”能力，包括：

- 侧向测距支撑的走廊横向定位
- IMU / EKF 支撑的航向估计
- 前向距离触发的到端检测
- 原地转向
- 走廊内闭环控制

但正式比赛要求并不是“在一条走廊里走稳”这么简单，而是：

- 遍历全部 `6` 条垄沟
- 在端部完成换沟
- 最终从唯一出入口驶离场地
- 再停回启动区

因此，真正的问题已经从“局部走廊控制”升级成了“赛道级导航”。

本文件针对一个新的核心顾虑做说明：

- 左右 `VL53L0X` 是近场侧向传感器，实际有效距离有限
- 当小车走完第一条通道、完成转向、准备去第二条通道口时，下一条通道口可能仍在数米之外
- 此时无法指望左右侧向 VL53 直接识别远处的下一条通道入口

由此引出新的设计问题：

- 是否需要把当前导航从“局部走廊式”升级为“固定地图下的全局式导航”

本文件只做需求分析与方案建议，不修改现有代码。

## 2. 比赛场地与问题是否成立

根据比赛规则文件 `附件6：B类“马铃薯捡拾机器人竞技”比赛及评审规则.md`：

- 场地尺寸：`390cm x 300cm`
- 共有 `5` 条田垄
- 每条田垄：长 `220cm`、宽 `30cm`
- 围栏与田垄之间、相邻田垄之间均为 `40cm` 垄沟
- 实际需要遍历的是 `6` 条垄沟
- 场地尺寸允许误差：`+-5%`

结合 `HANDOFF.md` 的赛道理解：

- 当前赛道本质上是 `6` 段平行窄走廊
- 其间通过端部动作完成换沟
- 最终还要完成出场和停回启动区

因此，你的顾虑是成立的：

- 左右侧向 VL53 的作用域主要是在“贴着垄沟/墙体附近”时建立局部几何约束
- 一旦小车离开当前垄沟、进入端部开阔区或横向换沟区，下一条垄沟入口往往不在 VL53 的可靠观测范围内
- 所以不能把“下一条通道入口识别”这个任务建立在左右 VL53 的远距离探测能力上

这里需要特别澄清一个容易说错的点：

- 赛道级运动并不一定是“在端部开阔区横移很长一段，再去远距离搜索下一条垄沟入口”
- 更符合当前场地理解的运动方式，是一种 **S 型串行入沟**：
  1. 从出发区直行到第 1 条垄沟入口附近
  2. 第 1 条垄沟在车体一侧（例如右侧）
  3. 原地右转 `90°` 对准垄沟后入沟
  4. 沿垄沟通过
  5. 到端后原地左转 `90°`，进入端部直线连接段
  6. 再前进一小段到下一条垄沟入口附近
  7. 再原地左转 `90°` 入下一条垄沟
  8. 之后重复形成 S 型遍历

也就是说：

- 下一条垄沟并不是完全未知目标
- 它通常位于“端部连接直线段之后的固定相邻位置”
- 问题核心不是远距离搜索，而是 **按固定几何完成 90° 转向 + 短直线推进 + 再次 90° 入沟**

换句话说：

- `VL53` 适合做局部跟墙/居中
- 不适合独立承担赛道级换沟导航

## 3. 当前项目的能力边界

`HANDOFF.md` 已经明确指出：

- 当前项目更像“单垄沟闭环验证系统”
- 还不是完整的 `6` 垄沟遍历赛道导航系统

当前已经具备的能力主要是：

1. 垄沟内定位与控制
2. 到端检测
3. 原地转向
4. 安全停车

当前还缺失的关键能力是：

1. 多垄沟拓扑状态管理
2. 端部换沟策略
3. 下一目标垄沟的判定逻辑
4. 出场与停回启动区的赛道级动作编排

所以，这不是某一个传感器量程的小补丁问题，而是系统层次已经需要从“局部控制”提升到“地图引导下的赛道导航”。

## 4. 为什么仅靠局部传感器闭环不够

### 4.1 局部闭环擅长的任务

局部闭环擅长的是：

- 在 `40cm` 垄沟里保持居中
- 在局部几何约束下控制横向误差和航向误差
- 在接近端部时停车或转向

这些任务的共同特征是：

- 机器人周围可观测到明确的近场结构
- 当前目标由附近环境直接决定

### 4.2 换沟问题的本质不同

从第一条垄沟换到第二条垄沟，不只是“继续沿墙走”，而是要回答以下更高层的问题：

1. 我当前已经完成了第几条垄沟
2. 下一条目标垄沟是哪一条
3. 这次应该左移还是右移
4. 端部 `90°` 转向后需要前进多少连接距离
5. 下一次 `90°` 转向应朝哪一侧入沟
6. 什么时候说明我已经对准并进入了下一条垄沟，可以重新切回局部走廊跟踪模式

这些问题都不是单次近场测距能直接回答的。

它们需要：

- 赛道拓扑信息
- 段落状态
- 距离累计
- 转向后姿态保持
- 已完成进度记忆

所以，换沟本质上是“任务级导航问题”，不是“单纯传感器观测问题”。

## 5. 是否需要全局导航

### 5.1 需要，但不一定是 SLAM 式全局导航

你提出“地图是固定的，是否要改成全局式导航”，我的判断是：

- 需要“全局导航思想”
- 但不需要上“通用移动机器人 SLAM”那种全局导航系统

原因是这个比赛场地有几个重要特征：

1. 地图是固定结构
- 场地拓扑不变
- 垄沟数量固定
- 宽度和相对排列固定

2. 尺寸有误差但不是完全未知
- 规则允许 `+-5%`
- 说明不能死信纯几何标称值
- 但也不代表你需要从零建图

3. 导航目标是离散段落式的
- 进入第 1 条垄沟
- 沿垄沟前进
- 到端
- 换到相邻垄沟
- 重复
- 最后出场并停回启动区

这类问题更适合：

- 固定地图
- 拓扑状态机
- 局部感知闭环
- 少量里程/姿态积分

而不是：

- 实时全局建图
- 通用路径规划
- 自由空间导航

### 5.2 更准确的说法：需要“赛道级固定地图导航”

如果用更工程化的语言描述，真正需要的不是传统 SLAM，而是：

- 固定地图下的赛道级导航
- 或者：拓扑-度量混合导航

它的含义是：

1. 上层知道整张赛道的结构
2. 下层在当前局部段里做高频闭环
3. 段与段之间的切换靠预定义几何、里程推进和事件触发完成

这比“纯局部反应式导航”强很多，但比“完整 SLAM”简单得多，也更符合比赛实际。

## 6. 为什么不建议直接走完整 SLAM / 全局自由导航

### 6.1 赛场结构太规则，SLAM 的收益不高

比赛场地不是开放未知环境，而是高度规则的固定赛道。

如果直接上完整 SLAM，会遇到两个问题：

1. 复杂度远大于实际收益
2. 还要处理对称结构导致的重定位歧义

因为 `6` 条垄沟本身非常相似：

- 走廊宽度相同
- 垄长相同
- 多个局部观测在不同位置上可能长得很像

这对通用 SLAM 来说并不天然友好。

### 6.2 规则里存在地毯与尺寸误差，纯度量导航也不能硬信

比赛规则明确说明：

- 随机会在 `2` 条垄沟铺设地毯模拟松软路面
- 场地尺寸允许 `+-5%` 误差

这意味着：

- 纯里程计距离不能被绝对相信
- 纯固定距离脚本也不能完全相信

所以最合理的方案不是“只靠地图”，而是：

- 地图负责告诉你要去哪一段
- 传感器负责告诉你你是否已经贴近那一段并进入局部闭环条件

## 7. 推荐的导航架构

我建议把系统分成三层。

### 7.1 第 1 层：局部走廊控制层

职责：

- 在垄沟内保持稳定行驶
- 利用侧向 VL53 和 IMU 控制 `e_y`、`e_th`
- 处理贴墙、居中、偏置行走等局部任务

这层继续使用你现在已经有的能力即可。

### 7.2 第 2 层：端部与换沟动作层

职责：

- 识别到端
- 完成 `90°` 转向
- 保持连接段朝向直线推进
- 在下一条垄沟入口处再执行一次 `90°` 转向入沟
- 在接近下一条垄沟入口时重新捕获局部走廊结构

这一层的主要依赖不应再是“直接看到很远处的下一条通道口”，而应是：

- IMU 航向保持
- 编码器里程推进
- 前向/后向长距测距作安全与事件辅助
- 重新捕获两侧结构时切回走廊模式

### 7.3 第 3 层：赛道级拓扑/地图层

职责：

- 记录当前是第几条垄沟
- 决定下一条目标垄沟编号
- 决定换沟方向
- 决定当前所处阶段：入场、走廊、端部、换沟、再入沟、退出、回停
- 在最终阶段规划如何从最后一条垄沟回到唯一出口并停回启动区

这层就是“全局导航思想”的承载层。

## 8. 具体到你的顾虑：第二通道口怎么判定

这个问题不应该被设计成：

- “我在端部一转身，然后用 VL53 去远距离搜索第二通道口在哪里”

更合理的设计应该是：

### 8.1 已知目标法

系统在上层已经知道：

- 当前完成的是第 `i` 条垄沟
- 下一个目标是第 `i+1` 条或第 `i-1` 条垄沟

也就是说，目标不是“搜索未知入口”，而是“按已知地图去下一个入口”。

### 8.2 动作脚本法

在端部动作中执行一段有结构的换沟脚本，例如：

1. 到端停车
2. 原地转 `90°` 到端部连接段朝向
3. 用 IMU 保持该朝向直行一小段
4. 用里程计累计连接段推进距离
5. 到达下一条垄沟入口附近后再原地转 `90°`
6. 一旦两侧 VL53 重新形成新垄沟的局部几何特征，则确认入沟成功
7. 切回局部走廊跟踪

如果按你描述的实际拓扑，更准确的理解是：

- 换沟动作不是“横向平移到另一条沟”
- 而是“端部出沟后走一段连接直线，再 90° 入下一沟”
- 整体轨迹更接近规则的 S 型遍历

### 8.3 重新捕获法

下一条垄沟入口的确认，最可靠的时刻往往不是“远处看见”，而是“进入附近后重新捕获到局部走廊结构”。

也就是说：

- 上层负责把车带到“应该接近下一条垄沟入口”的区域
- 下层负责在近场把入口真正锁住

这比远距离直接识别入口更稳。

## 9. 哪些传感器在赛道级导航里应该扮演什么角色

### 9.1 左右 VL53L0X

适合：

- 垄沟内横向定位
- 局部几何重捕获
- 判断自己是否已经重新进入某条垄沟

不适合：

- 远距离寻找下一条通道口
- 独立承担赛道级换沟导航

### 9.2 IMU

适合：

- 转向控制
- 端部横移/换沟时的航向保持
- 跨局部无墙约束阶段的短时姿态维持

### 9.3 编码器 / 里程计

适合：

- 换沟距离推进
- 已走段长估计
- 作为脚本动作的度量输入

缺点：

- 地毯和打滑会导致累计误差

所以它应作为：

- 主推进量
- 但不是唯一最终确认依据

### 9.4 前后长距测距

当前硬件里前后测距量程远大于 VL53，理论上更适合：

- 到端检测
- 判断前方是否接近围栏
- 在开阔区提供安全保护
- 对某些段落提供事件辅助

它不一定直接告诉你“第二通道口在这”，但可以帮助判断：

- 是否还在端部开阔区
- 是否已经逼近另一侧围栏或终点边界
- 是否应减速/停止/切换动作阶段

## 10. 推荐的总体方案

### 10.1 推荐方向：固定地图 + 拓扑状态机 + 局部闭环

这是我最推荐的方向。

其核心思想是：

- 地图不是在线建出来的，而是事先已知
- 机器人不需要理解“任意世界坐标”
- 机器人只需要知道自己当前位于哪个赛道段、下一步要切到哪个段

可以把整张赛道抽象成若干固定段：

1. 启动区
2. 入口对准段
3. 垄沟 1
4. 端部连接段 1
5. 垄沟 2
6. 端部连接段 2
7. ...
8. 垄沟 6
9. 出场段
10. 启动区停车段

如果按运动学动作再细分，每一个“端部连接段”内部实际上可以拆成：

1. 到端停车
2. 第一次 `90°` 转向
3. 连接直线推进
4. 第二次 `90°` 转向
5. 入沟重捕获

每个段有自己的：

- 目标朝向
- 目标推进方向
- 退出条件
- 允许使用的观测
- 安全策略

### 10.2 这不是“纯全局”，而是“全局指导下的局部控制”

这种方案的优势在于：

- 不会把所有问题都压给近场传感器
- 不会过度依赖里程计的绝对精度
- 保留你当前局部走廊控制代码的大部分价值
- 更贴合固定场地赛事的工程实际

## 11. 不推荐的方案

### 11.1 不推荐：继续纯局部反应式地扩当前逻辑

如果只是继续把当前“单垄沟往返”逻辑往外补，仍然不引入上层赛道状态，那么很容易遇到：

- 转完向不知道该去第几条沟
- 明明应该换沟，却又回到原来那条沟附近
- 因地毯或打滑导致脚本距离错位
- 无法稳定完成最终出场与回停

### 11.2 不推荐：直接上完整 SLAM / 通用全局路径规划

这对当前赛题来说过重，收益不一定匹配复杂度。

## 12. 对当前项目的直接结论

结合当前代码与比赛规则，可以得出以下结论：

1. 你的顾虑成立
- 左右 VL53 无法独立解决“数米外下一条垄沟入口识别”问题

2. 当前项目确实缺少赛道级导航层
- 当前更偏向单垄沟局部验证系统

3. 后续必须补上固定地图下的全局段落管理
- 否则无法可靠完成正式比赛要求的 `6` 垄沟遍历

4. 但不建议走完整 SLAM 路线
- 更合适的是固定地图 + 拓扑状态机 + 局部感知闭环

## 13. 后续实施建议

如果后续开始真正改造导航系统，建议优先级如下：

1. 先定义完整赛道拓扑和段落状态机
2. 明确每一段的进入条件、退出条件、目标朝向和目标距离
3. 让局部走廊控制只负责“在某条已知垄沟里跑稳”
4. 让换沟段由 IMU + 里程计 + 长距测距辅助来完成
5. 用左右 VL53 做“重新捕获下一条垄沟”确认，而不是远距离搜寻入口
6. 最后再考虑是否需要引入更强的全局定位增强手段

### 13.1 建议分 5 步走

结合当前项目现状，建议不要一口气重写整套导航，而是按“先把单段动作语义补完整，再把赛道级层叠上去”的顺序推进。

推荐拆成 `5` 步。

### 第 1 步：先把现有局部导航链路补到“可复用”

这一步不是做全局导航，而是先把现有单垄沟能力整理成后续可调用的稳定基础模块。

优先要补或改的模块：

1. `App/nav/segment_fsm.c/.h`
- 增加“动作语义”或“模式感知”输入
- 区分：走廊前进、原地转向、横向换沟、退出直线
- 解决当前 `TURN_AT_END` 可能被安全层整段按死的问题

2. `App/nav/nav_script.c/.h`
- 不再把它视为最终比赛脚本
- 先把它收敛成“单段动作编排器”或“局部验证脚本”
- 明确哪些能力未来会上移到赛道级状态机

3. `App/preproc/corridor_msgs.h`
- 补充动作模式、段类型、重捕获结果等跨模块消息定义
- 避免后面继续把语义塞进零散布尔量里

4. `App/app_tasks.c`
- 补齐导航启动前的 ready 判定
- 明确 navTask 每周期里：局部控制输出、赛道级输出、安全层输出的优先级

这一阶段的目标不是“遍历 6 条垄沟”，而是：

- 让“走廊跟踪 / 到端 / 原地转向 / 安全仲裁”这几个局部动作变成可被上层稳定调用的基础能力

### 第 2 步：新增赛道级拓扑状态机模块

这一步开始真正引入“全局导航思想”，但仍然基于固定地图，不做 SLAM。

建议新增模块：

1. `App/nav/global_nav_fsm.c/.h`
- 负责赛道级阶段推进
- 记录当前是第几条垄沟
- 决定下一条目标垄沟编号
- 决定下一次是左转入沟还是右转入沟
- 决定当前处于：启动、入沟、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段

2. `App/nav/track_map.c/.h`
- 保存固定赛道的拓扑和名义几何
- 例如：垄沟数量、编号顺序、相邻关系、换沟方向、出口所在侧、启动区相对位置
- 不追求通用地图系统，只做当前赛题所需的固定地图描述

3. `App/preproc/corridor_msgs.h`
- 增加赛道级状态输出结构
- 例如：当前 corridor_id、target_corridor_id、segment_type、progress_state

为什么这一步要单独拆出来：

- 当前项目最大缺口不是局部控制，而是“不知道自己在整张赛道里进行到哪一步”
- 这个职责不能继续堆在 `nav_script.c` 里，否则会越改越像一份超长 if-else 脚本

### 第 3 步：新增换沟动作层模块

这一层负责跨出当前垄沟、去相邻垄沟入口附近、再把局部控制权交还给走廊跟踪层。

建议新增模块：

1. `App/nav/lane_change_executor.c/.h`
- 输入：当前赛道级目标、目标换沟方向、目标段参数
- 输出：本周期期望动作 `v/w`
- 内部实现典型动作序列：
  - 到端停车
  - 第一次 `90°` 转向到连接段朝向
  - 维持航向直线推进
  - 按里程推进到预计下一沟入口区域
  - 第二次 `90°` 转向入沟
  - 减速搜索并等待局部结构重捕获

2. `App/nav/heading_hold.c/.h` 或并入 `lane_change_executor`
- 用 IMU 做连接直线阶段短时航向保持
- 与走廊控制器解耦，避免把“无侧墙阶段”继续硬塞进 `corridor_ctrl.c`

3. `App/nav/reacquire_detector.c/.h`
- 负责判断是否已重新进入一条有效垄沟
- 典型判据：左右 VL53 重新同时形成合理几何、置信度恢复、持续若干拍成立

这一阶段的核心目标是：

- 不依赖“远距离看到下一条垄沟入口”
- 而是“按固定地图推进到预计区域，再由局部传感器完成重捕获确认”

### 第 4 步：补出场与回停模块

前 3 步完成后，系统已经具备：

- 走一条垄沟
- 到端
- 换到相邻垄沟

但正式比赛还需要最后的：

- 从最后一条垄沟驶离
- 回到唯一出口
- 停回启动区

建议新增模块：

1. `App/nav/exit_planner.c/.h`
- 定义从最后有效垄沟切到出场段的固定动作逻辑
- 决定离场时的目标朝向、推进距离、退出条件

2. `App/nav/start_zone_dock.c/.h`
- 负责最终回停启动区
- 可以做成简化版固定脚本，不需要复杂路径规划

这一阶段不要追求炫技，重点是：

- 流程完整
- 可解释
- 可调参
- 能在赛场误差下稳定完成收尾动作

### 第 5 步：最后再做统一调参与验证支撑

当前项目已经有局部参数，但赛道级导航落地后，还需要把“段参数”和“地图参数”系统化管理。

建议补的模块或整理项：

1. `App/robot_params.h`
- 补充赛道级参数
- 例如：换沟名义距离、减速搜索距离、重捕获持续拍数、回停距离等

2. `App/nav/global_nav_debug.c/.h` 或临时调试结构
- 给 CubeMonitor / 日志暴露关键内部量
- 例如：当前 corridor_id、stage、target_heading、reacquire_flag、lane_change_progress

3. `HANDOFF.md` / `GLOBAL_NAV_REQUIREMENT.md`
- 随代码同步更新
- 保证后续调试时文档和实现一致

这一阶段的目标是：

- 把“能跑”变成“能调、能解释、能复现”

### 13.2 各步的交付标准

为了避免后续开发一直停留在“看起来写了很多模块”，建议每一步都设一个明确交付标准。

第 1 步交付标准：

- 原地转向不再被安全层错误清零
- 走廊前进 / 原地转向 / 退出直行三类动作有清晰安全语义
- 局部导航链路在架空测试中行为稳定可解释

第 2 步交付标准：

- 系统能够明确输出“当前第几条垄沟、下一条目标是哪条、当前赛道阶段是什么”
- 不再依赖单个脚本文件隐式记录全局进度

第 3 步交付标准：

- 能从一条垄沟末端稳定切换到相邻垄沟入口附近
- 能通过 VL53 重捕获确认重新入沟

第 4 步交付标准：

- 能在完成全部目标垄沟后可靠离场
- 能完成最终回停启动区

第 5 步交付标准：

- 参数、日志、状态可观测性完整
- 可以支持正式场地反复调参与问题定位

### 13.3 为什么是这个顺序

这个顺序的核心原则是：

1. 先补“动作语义一致性”，再补“赛道级状态记忆”
2. 先解决端部动作可用性，再扩展多垄沟遍历
3. 先做固定地图和拓扑状态机，再考虑任何更重的全局定位增强

如果顺序反过来，例如一开始就写完整 6 垄沟状态机，但底层转向和换沟动作还不稳定，那么上层状态再完整也只会变成“会卡在某一步的复杂脚本”。

因此，最合理的开发路径不是“先把全局状态机写满”，而是：

- 第 1 步：把局部动作做成稳固积木
- 第 2 步：加赛道级任务管理
- 第 3 步：补跨段动作
- 第 4 步：补最终出场与回停
- 第 5 步：统一调参与调试支撑

## 14. 一句话结论

你的问题本质上说明：当前系统已经不能只靠“局部走廊导航”来思考了。

真正需要补的是“固定地图下的赛道级导航层”，而不是盲目把 VL53 当成远距离入口探测器，也不是直接上复杂 SLAM。