21 KiB
固定场地条件下的赛道级导航需求说明
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 条垄沟在车体一侧（例如右侧）
- 原地右转
90°对准垄沟后入沟 - 沿垄沟通过
- 到端后原地左转
90°，进入端部直线连接段 - 再前进一小段到下一条垄沟入口附近
- 再原地左转
90°入下一条垄沟 - 之后重复形成 S 型遍历
也就是说：
- 下一条垄沟并不是完全未知目标
- 它通常位于“端部连接直线段之后的固定相邻位置”
- 问题核心不是远距离搜索，而是 按固定几何完成 90° 转向 + 短直线推进 + 再次 90° 入沟
换句话说：
VL53适合做局部跟墙/居中- 不适合独立承担赛道级换沟导航
3. 当前项目的能力边界
HANDOFF.md 已经明确指出：
- 当前项目更像“单垄沟闭环验证系统”
- 还不是完整的
6垄沟遍历赛道导航系统
当前已经具备的能力主要是：
- 垄沟内定位与控制
- 到端检测
- 原地转向
- 安全停车
当前还缺失的关键能力是：
- 多垄沟拓扑状态管理
- 端部换沟策略
- 下一目标垄沟的判定逻辑
- 出场与停回启动区的赛道级动作编排
所以，这不是某一个传感器量程的小补丁问题，而是系统层次已经需要从“局部控制”提升到“地图引导下的赛道导航”。
4. 为什么仅靠局部传感器闭环不够
4.1 局部闭环擅长的任务
局部闭环擅长的是：
- 在
40cm垄沟里保持居中 - 在局部几何约束下控制横向误差和航向误差
- 在接近端部时停车或转向
这些任务的共同特征是：
- 机器人周围可观测到明确的近场结构
- 当前目标由附近环境直接决定
4.2 换沟问题的本质不同
从第一条垄沟换到第二条垄沟，不只是“继续沿墙走”，而是要回答以下更高层的问题：
- 我当前已经完成了第几条垄沟
- 下一条目标垄沟是哪一条
- 这次应该左移还是右移
- 端部
90°转向后需要前进多少连接距离 - 下一次
90°转向应朝哪一侧入沟 - 什么时候说明我已经对准并进入了下一条垄沟，可以重新切回局部走廊跟踪模式
这些问题都不是单次近场测距能直接回答的。
它们需要：
- 赛道拓扑信息
- 段落状态
- 距离累计
- 转向后姿态保持
- 已完成进度记忆
所以，换沟本质上是“任务级导航问题”，不是“单纯传感器观测问题”。
5. 是否需要全局导航
5.1 需要，但不一定是 SLAM 式全局导航
你提出“地图是固定的，是否要改成全局式导航”，我的判断是：
- 需要“全局导航思想”
- 但不需要上“通用移动机器人 SLAM”那种全局导航系统
原因是这个比赛场地有几个重要特征：
- 地图是固定结构
- 场地拓扑不变
- 垄沟数量固定
- 宽度和相对排列固定
- 尺寸有误差但不是完全未知
- 规则允许
+-5% - 说明不能死信纯几何标称值
- 但也不代表你需要从零建图
- 导航目标是离散段落式的
- 进入第 1 条垄沟
- 沿垄沟前进
- 到端
- 换到相邻垄沟
- 重复
- 最后出场并停回启动区
这类问题更适合：
- 固定地图
- 拓扑状态机
- 局部感知闭环
- 少量里程/姿态积分
而不是：
- 实时全局建图
- 通用路径规划
- 自由空间导航
5.2 更准确的说法：需要“赛道级固定地图导航”
如果用更工程化的语言描述，真正需要的不是传统 SLAM，而是：
- 固定地图下的赛道级导航
- 或者：拓扑-度量混合导航
它的含义是：
- 上层知道整张赛道的结构
- 下层在当前局部段里做高频闭环
- 段与段之间的切换靠预定义几何、里程推进和事件触发完成
这比“纯局部反应式导航”强很多，但比“完整 SLAM”简单得多，也更符合比赛实际。
6. 为什么不建议直接走完整 SLAM / 全局自由导航
6.1 赛场结构太规则，SLAM 的收益不高
比赛场地不是开放未知环境，而是高度规则的固定赛道。
如果直接上完整 SLAM，会遇到两个问题：
- 复杂度远大于实际收益
- 还要处理对称结构导致的重定位歧义
因为 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 动作脚本法
在端部动作中执行一段有结构的换沟脚本，例如：
- 到端停车
- 原地转
90°到端部连接段朝向 - 用 IMU 保持该朝向直行一小段
- 用里程计累计连接段推进距离
- 到达下一条垄沟入口附近后再原地转
90° - 一旦两侧 VL53 重新形成新垄沟的局部几何特征，则确认入沟成功
- 切回局部走廊跟踪
如果按你描述的实际拓扑，更准确的理解是：
- 换沟动作不是“横向平移到另一条沟”
- 而是“端部出沟后走一段连接直线，再 90° 入下一沟”
- 整体轨迹更接近规则的 S 型遍历
8.3 重新捕获法
下一条垄沟入口的确认，最可靠的时刻往往不是“远处看见”，而是“进入附近后重新捕获到局部走廊结构”。
也就是说：
- 上层负责把车带到“应该接近下一条垄沟入口”的区域
- 下层负责在近场把入口真正锁住
这比远距离直接识别入口更稳。
9. 哪些传感器在赛道级导航里应该扮演什么角色
9.1 左右 VL53L0X
适合：
- 垄沟内横向定位
- 局部几何重捕获
- 判断自己是否已经重新进入某条垄沟
不适合：
- 远距离寻找下一条通道口
- 独立承担赛道级换沟导航
9.2 IMU
适合：
- 转向控制
- 端部横移/换沟时的航向保持
- 跨局部无墙约束阶段的短时姿态维持
9.3 编码器 / 里程计
适合：
- 换沟距离推进
- 已走段长估计
- 作为脚本动作的度量输入
缺点：
- 地毯和打滑会导致累计误差
所以它应作为：
- 主推进量
- 但不是唯一最终确认依据
9.4 前后长距测距
当前硬件里前后测距量程远大于 VL53，理论上更适合：
- 到端检测
- 判断前方是否接近围栏
- 在开阔区提供安全保护
- 对某些段落提供事件辅助
它不一定直接告诉你“第二通道口在这”，但可以帮助判断：
- 是否还在端部开阔区
- 是否已经逼近另一侧围栏或终点边界
- 是否应减速/停止/切换动作阶段
10. 推荐的总体方案
10.1 推荐方向：固定地图 + 拓扑状态机 + 局部闭环
这是我最推荐的方向。
其核心思想是：
- 地图不是在线建出来的，而是事先已知
- 机器人不需要理解“任意世界坐标”
- 机器人只需要知道自己当前位于哪个赛道段、下一步要切到哪个段
可以把整张赛道抽象成若干固定段：
- 启动区
- 入口对准段
- 垄沟 1
- 端部连接段 1
- 垄沟 2
- 端部连接段 2
- ...
- 垄沟 6
- 出场段
- 启动区停车段
如果按运动学动作再细分，每一个“端部连接段”内部实际上可以拆成：
- 到端停车
- 第一次
90°转向 - 连接直线推进
- 第二次
90°转向 - 入沟重捕获
每个段有自己的：
- 目标朝向
- 目标推进方向
- 退出条件
- 允许使用的观测
- 安全策略
10.2 这不是“纯全局”，而是“全局指导下的局部控制”
这种方案的优势在于：
- 不会把所有问题都压给近场传感器
- 不会过度依赖里程计的绝对精度
- 保留你当前局部走廊控制代码的大部分价值
- 更贴合固定场地赛事的工程实际
11. 不推荐的方案
11.1 不推荐：继续纯局部反应式地扩当前逻辑
如果只是继续把当前“单垄沟往返”逻辑往外补，仍然不引入上层赛道状态，那么很容易遇到：
- 转完向不知道该去第几条沟
- 明明应该换沟，却又回到原来那条沟附近
- 因地毯或打滑导致脚本距离错位
- 无法稳定完成最终出场与回停
11.2 不推荐：直接上完整 SLAM / 通用全局路径规划
这对当前赛题来说过重，收益不一定匹配复杂度。
12. 对当前项目的直接结论
结合当前代码与比赛规则，可以得出以下结论：
- 你的顾虑成立
- 左右 VL53 无法独立解决“数米外下一条垄沟入口识别”问题
- 当前项目确实缺少赛道级导航层
- 当前更偏向单垄沟局部验证系统
- 后续必须补上固定地图下的全局段落管理
- 否则无法可靠完成正式比赛要求的
6垄沟遍历
- 但不建议走完整 SLAM 路线
- 更合适的是固定地图 + 拓扑状态机 + 局部感知闭环
13. 后续实施建议
如果后续开始真正改造导航系统，建议优先级如下：
- 先定义完整赛道拓扑和段落状态机
- 明确每一段的进入条件、退出条件、目标朝向和目标距离
- 让局部走廊控制只负责“在某条已知垄沟里跑稳”
- 让换沟段由 IMU + 里程计 + 长距测距辅助来完成
- 用左右 VL53 做“重新捕获下一条垄沟”确认，而不是远距离搜寻入口
- 最后再考虑是否需要引入更强的全局定位增强手段
13.1 建议分 5 步走
结合当前项目现状，建议不要一口气重写整套导航，而是按“先把单段动作语义补完整，再把赛道级层叠上去”的顺序推进。
推荐拆成 5 步。
第 1 步：先把现有局部导航链路补到“可复用”
这一步不是做全局导航，而是先把现有单垄沟能力整理成后续可调用的稳定基础模块。
优先要补或改的模块：
App/nav/segment_fsm.c/.h
- 增加“动作语义”或“模式感知”输入
- 区分：走廊前进、原地转向、横向换沟、退出直线
- 解决当前
TURN_AT_END可能被安全层整段按死的问题
App/nav/nav_script.c/.h
- 不再把它视为最终比赛脚本
- 先把它收敛成“单段动作编排器”或“局部验证脚本”
- 明确哪些能力未来会上移到赛道级状态机
App/preproc/corridor_msgs.h
- 补充动作模式、段类型、重捕获结果等跨模块消息定义
- 避免后面继续把语义塞进零散布尔量里
App/app_tasks.c
- 补齐导航启动前的 ready 判定
- 明确 navTask 每周期里：局部控制输出、赛道级输出、安全层输出的优先级
这一阶段的目标不是“遍历 6 条垄沟”，而是：
- 让“走廊跟踪 / 到端 / 原地转向 / 安全仲裁”这几个局部动作变成可被上层稳定调用的基础能力
第 2 步：新增赛道级拓扑状态机模块
这一步开始真正引入“全局导航思想”，但仍然基于固定地图，不做 SLAM。
建议新增模块：
App/nav/global_nav_fsm.c/.h
- 负责赛道级阶段推进
- 记录当前是第几条垄沟
- 决定下一条目标垄沟编号
- 决定下一次是左转入沟还是右转入沟
- 决定当前处于：启动、入沟、走沟、到端、换沟、再入沟、出场、回停 的哪一阶段
App/nav/track_map.c/.h
- 保存固定赛道的拓扑和名义几何
- 例如：垄沟数量、编号顺序、相邻关系、换沟方向、出口所在侧、启动区相对位置
- 不追求通用地图系统，只做当前赛题所需的固定地图描述
App/preproc/corridor_msgs.h
- 增加赛道级状态输出结构
- 例如：当前 corridor_id、target_corridor_id、segment_type、progress_state
为什么这一步要单独拆出来：
- 当前项目最大缺口不是局部控制，而是“不知道自己在整张赛道里进行到哪一步”
- 这个职责不能继续堆在
nav_script.c里，否则会越改越像一份超长 if-else 脚本
第 3 步：新增换沟动作层模块
这一层负责跨出当前垄沟、去相邻垄沟入口附近、再把局部控制权交还给走廊跟踪层。
建议新增模块：
App/nav/lane_change_executor.c/.h
- 输入：当前赛道级目标、目标换沟方向、目标段参数
- 输出：本周期期望动作
v/w - 内部实现典型动作序列：
- 到端停车
- 第一次
90°转向到连接段朝向 - 维持航向直线推进
- 按里程推进到预计下一沟入口区域
- 第二次
90°转向入沟 - 减速搜索并等待局部结构重捕获
App/nav/heading_hold.c/.h或并入lane_change_executor
- 用 IMU 做连接直线阶段短时航向保持
- 与走廊控制器解耦，避免把“无侧墙阶段”继续硬塞进
corridor_ctrl.c
App/nav/reacquire_detector.c/.h
- 负责判断是否已重新进入一条有效垄沟
- 典型判据：左右 VL53 重新同时形成合理几何、置信度恢复、持续若干拍成立
这一阶段的核心目标是：
- 不依赖“远距离看到下一条垄沟入口”
- 而是“按固定地图推进到预计区域，再由局部传感器完成重捕获确认”
第 4 步：补出场与回停模块
前 3 步完成后，系统已经具备：
- 走一条垄沟
- 到端
- 换到相邻垄沟
但正式比赛还需要最后的：
- 从最后一条垄沟驶离
- 回到唯一出口
- 停回启动区
建议新增模块：
App/nav/exit_planner.c/.h
- 定义从最后有效垄沟切到出场段的固定动作逻辑
- 决定离场时的目标朝向、推进距离、退出条件
App/nav/start_zone_dock.c/.h
- 负责最终回停启动区
- 可以做成简化版固定脚本，不需要复杂路径规划
这一阶段不要追求炫技，重点是：
- 流程完整
- 可解释
- 可调参
- 能在赛场误差下稳定完成收尾动作
第 5 步：最后再做统一调参与验证支撑
当前项目已经有局部参数，但赛道级导航落地后，还需要把“段参数”和“地图参数”系统化管理。
建议补的模块或整理项：
App/robot_params.h
- 补充赛道级参数
- 例如：换沟名义距离、减速搜索距离、重捕获持续拍数、回停距离等
App/nav/global_nav_debug.c/.h或临时调试结构
- 给 CubeMonitor / 日志暴露关键内部量
- 例如：当前 corridor_id、stage、target_heading、reacquire_flag、lane_change_progress
HANDOFF.md/GLOBAL_NAV_REQUIREMENT.md
- 随代码同步更新
- 保证后续调试时文档和实现一致
这一阶段的目标是：
- 把“能跑”变成“能调、能解释、能复现”
13.2 各步的交付标准
为了避免后续开发一直停留在“看起来写了很多模块”，建议每一步都设一个明确交付标准。
第 1 步交付标准：
- 原地转向不再被安全层错误清零
- 走廊前进 / 原地转向 / 退出直行三类动作有清晰安全语义
- 局部导航链路在架空测试中行为稳定可解释
第 2 步交付标准：
- 系统能够明确输出“当前第几条垄沟、下一条目标是哪条、当前赛道阶段是什么”
- 不再依赖单个脚本文件隐式记录全局进度
第 3 步交付标准：
- 能从一条垄沟末端稳定切换到相邻垄沟入口附近
- 能通过 VL53 重捕获确认重新入沟
第 4 步交付标准：
- 能在完成全部目标垄沟后可靠离场
- 能完成最终回停启动区
第 5 步交付标准：
- 参数、日志、状态可观测性完整
- 可以支持正式场地反复调参与问题定位
13.3 为什么是这个顺序
这个顺序的核心原则是：
- 先补“动作语义一致性”，再补“赛道级状态记忆”
- 先解决端部动作可用性，再扩展多垄沟遍历
- 先做固定地图和拓扑状态机，再考虑任何更重的全局定位增强
如果顺序反过来，例如一开始就写完整 6 垄沟状态机，但底层转向和换沟动作还不稳定，那么上层状态再完整也只会变成“会卡在某一步的复杂脚本”。
因此，最合理的开发路径不是“先把全局状态机写满”，而是：
- 第 1 步：把局部动作做成稳固积木
- 第 2 步：加赛道级任务管理
- 第 3 步：补跨段动作
- 第 4 步：补最终出场与回停
- 第 5 步：统一调参与调试支撑
14. 一句话结论
你的问题本质上说明：当前系统已经不能只靠“局部走廊导航”来思考了。
真正需要补的是“固定地图下的赛道级导航层”，而不是盲目把 VL53 当成远距离入口探测器，也不是直接上复杂 SLAM。