# 固定场地条件下的赛道级导航需求说明 ## 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。