赛场机器人导航与规划系统全解读：从因子图到 FDDP 的完整闭环

标签：机器人、自动驾驶、路径规划、MPC、SLAM、因子图、轨迹优化

这篇文章不是在堆算法名，而是在拆一套真正能跑赛场的系统：定位建图、跨地形规划、模型预测控制、导航状态机、动态障碍物预测，以及离线重建。

如果把整套系统看成一条流水线，它大概是这样的：

这套架构的核心思想很清楚：
把“看得准、走得稳、转得动、躲得开、撞不坏”拆成多个层次分别解决。

---

1. 基于因子图优化的里程计

赛场里最难受的不是“慢”，而是强冲击、翻车、急停、打滑。普通里程计一旦把连续积分当成唯一真理，误差会很快滚雪球。

因子图优化的思路不一样：它不是只盯着上一帧和下一帧，而是把一段时间内的关键帧、IMU、轮速、激光匹配、回环约束一起放进一个图里做全局优化。
这样做的好处是：

1. 单帧观测坏了，不会直接把轨迹带偏。
2. 发生碰撞或姿态突变时，图优化还能靠其他约束把轨迹拉回来。
3. 适合“边跑边记”，后面还能做离线重优化。

图 1：因子图里程计中的多源约束关系，视觉因子、IMU 因子和边缘化因子共同约束位姿、速度与偏置。

这类方法的工程价值很直接：
短时依赖局部观测，长时依赖全局约束。
所以在赛道里，它通常比纯积分式里程计更抗撞、更抗翻。

工程上还要注意一点：因子图不是“把所有数据都塞进去就好”。
高频 IMU、轮速和点云匹配如果全部无脑入图，计算量会很快膨胀。实际系统通常会做关键帧筛选、滑动窗口优化和异常因子剔除，让它在鲁棒性和实时性之间保持平衡。

---

2. 跨地形场景的路径规划：空间 A* + Kinodynamic A* + MINCO + 速度剖面

跨地形不是“能找到路”就结束了，真正麻烦的是：
路找到了，车却过不去。

所以这条规划链路一般会分成四层：

2.1 空间 A*

先在栅格或代价地图上做全局搜索，拿到一条可达的拓扑路径。
它解决的是“从哪儿去到哪儿”的问题。

图 2：A* 在栅格地图中的路径寻优过程，启发函数引导搜索朝目标方向扩展。

2.2 Kinodynamic A*

普通 A* 只看几何可达，Kinodynamic A* 会把速度、加速度、转向和动力学约束一起考虑进去。
它解决的是“这条路车到底能不能按自己的物理限制走过去”的问题。

图 3：栅格地图中的规划结果。蓝色区域可理解为搜索扩展或安全走廊，黄色折线是最终路径。

例如普通 A* 可能给出一条贴着障碍物急转的折线，但真实车辆有最小转弯半径、最大加速度和姿态稳定限制。Kinodynamic A* 会把这些约束提前放进搜索节点扩展里，避免后端优化阶段才发现“这条路物理上不可执行”。

2.3 MINCO 优化

MINCO 的价值在于：它不是只给你一条折线，而是给你一条平滑、连续、可执行的轨迹。
通常会把位置、速度、加速度甚至 jerk 一起纳入优化，让轨迹更像真实车辆能跑出来的东西。

2.4 速度剖面优化

最后再做速度规划，把“几何轨迹”变成“时间轨迹”。
跨地形时，坡面、台阶、碎石、草地等场景会让轮胎附着和稳定性变化很大，速度剖面就是把这些风险压到可控范围内。

这一层的本质是：
先找路，再把路变成能跑的路，最后把能跑的路变成安全的速度曲线。

这也是为什么跨地形规划不能只靠一个算法完成。空间 A* 负责全局连通性，Kinodynamic A* 负责动力学可达性，MINCO 负责轨迹平滑和约束优化，速度剖面负责把地形风险转化为时间尺度上的安全余量。

---

3. LPV 隐状态模型 + FDDP 模型预测控制

这部分是整套系统里最“控制味儿”的地方。

3.1 为什么要 LPV

车辆底盘不是一个固定参数模型。
在不同车高、载荷、姿态、地形条件下，动力学会变。

所以这里用的是 LPV（Linear Parameter-Varying） 思路：
把某个可测的调度变量引入模型，比如底盘高度，让系统参数随着工况变化而变化。
这比硬塞一个固定线性模型更贴近实车。

这里还有一个关键点：模型不是纯手写出来的，而是基于实车录制的“命令-响应”数据离线辨识。也就是说，系统会记录控制命令和车辆实际响应，再用这些数据拟合不同底盘高度下的模型参数。这样得到的模型通常比纯理论模型更贴近真实底盘。

3.2 隐状态怎么处理

有些状态不能直接测，比如某些内部动态、等效侧偏、姿态耦合项等。
这时就要靠观测器在线估计，把“看不见的状态”补出来。

图 4：观测器的核心思想是把模型预测与噪声测量融合起来，得到更可信的状态估计。

3.3 为什么加管式辅助环

现实里模型总有误差，尤其是赛车场景。
管式辅助环的作用，就是给名义轨迹外面再套一层“安全管道”，用额外反馈把偏差压住。

3.4 FDDP 做 MPC 求解器

FDDP 可以理解成一种更注重可行性的 DDP 求解方式。
它的优势在于：

1. 对非线性问题适应性强。
2. 每个控制周期都能滚动优化。
3. 能把模型约束、代价函数和轨迹跟踪统一到一个框架里。

图 5：MPC 的滚动优化框架。FDDP 可以作为其中的高性能非线性优化求解器。

所以这一整段可以概括成一句话：
LPV 负责“像实车”，观测器负责“补状态”，FDDP 负责“把控制算出来”。

从控制闭环看，它大概是这样的：

---

4. 自主设计的导航状态机

很多人一看到状态机就会觉得“老派”，但在赛场里它其实非常好用。

这里采用的是扁平化有限状态机，而不是一套很重的行为树。
优点很实际：

1. 状态和转移关系一眼能看懂。
2. 调试时很容易定位是哪个状态切走的。
3. 对比赛现场这种高频异常切换场景，维护成本低。

图 6：有限状态机示意。扁平化 FSM 的状态和转移条件更容易在比赛现场调试。

常见状态通常包括：

• 起步 / 搜索
• 全局规划跟踪
• 局部避障
• 失效恢复
• 重新定位
• 停车或任务结束

它不一定最“学术”，但通常很“工程”。

为什么没有直接采用 Navigation2 行为树？
行为树适合复杂任务组合，比如导航、恢复、重规划、等待、清障等行为需要高度复用时，它的优势很明显。但赛场系统往往更强调低延迟、强可控和现场调试效率。扁平状态机虽然表达能力没那么“花”，但每个状态、每条转移都非常明确，比赛现场改起来也更直接。

---

5. 聚类动态障碍物跟踪与多步预测代价图

这部分解决的是：
不要等障碍物挡到眼前才开始躲。

典型流程是：

1. 对点云或目标检测结果做聚类。
2. 用匈牙利算法做跨帧匹配。
3. 用卡尔曼滤波估计当前状态并预测未来几步。
4. 把预测结果写进代价图。
5. MPC 根据“未来会不会撞”来提前修正轨迹。

图 7：动态障碍物跟踪中，卡尔曼滤波可以根据历史观测估计当前状态并预测短时运动趋势。

这一步最关键的不是“识别到了障碍物”，而是让控制器看到障碍物的未来位置。
这样局部避障就从“反应式躲闪”变成了“预测式绕行”。

如果再把预测结果灌进局部 costmap，就能形成一个更自然的动态避让闭环。

这一模块的难点主要在数据关联。
同一个障碍物跨帧以后位置变了、形状变了，甚至短暂遮挡了，系统仍然要判断“它是不是上一帧那个目标”。匈牙利匹配负责做全局最优分配，卡尔曼滤波负责平滑估计和短时预测，两者配合起来，才能让动态障碍物轨迹稳定下来。

---

6. 因子图离线建图优化与动态物体去除

这个部分很适合赛场和训练场景：
前期可以先“边跑边记”，把原始关键帧完整保存下来；后期再离线做因子图重优化，并把动态物体剔除掉。

为什么这招有效？

1. 在线阶段先保证系统跑起来。
2. 离线阶段再做更重的优化，质量更高。
3. 动态人车、摆件、临时障碍会被清理掉，地图更干净。

图 8：点云地图效果示意。离线优化和动态物体去除的目标，就是得到更干净、更稳定的地图。

这类思路很像“先完成任务，再修图”。
在真实比赛里，先把系统稳定送到终点，再去追求最优地图质量，通常是更合理的工程取舍。

动态物体去除的常见思路包括：多次观测一致性检查、语义分割、点云占据概率统计，以及基于轨迹残差的异常点剔除。对于训练场景来说，哪怕在线地图有一点脏，只要原始关键帧保存完整，后期仍然可以通过离线优化把地图质量拉回来。

---

总结

如果把这套系统压缩成一句话，我会这么说：

前端用因子图保证定位鲁棒，中间用分层规划把路径变成可执行轨迹，后端用 LPV + FDDP 做高性能控制，再用状态机和动态障碍预测补上赛场里的各种意外。

它厉害的地方不在于某个单独算法多新，而在于每一层都选了一个“够强、够稳、够好维护”的方案，然后把它们拼成了一个闭环。

---