浙江大学 Hello World 战队 2026 赛季轮腿哨兵导航开源

仓库地址：https://github.com/Polyacetone/HWSentryNav26

全国赛精彩回顾里面截的图

主要亮点：

基于因子图优化的鲁棒里程计。本项目在开源 SLAM 框架 GLIM 的基础上进行了定制化开发。针对平衡轮腿哨兵剧烈撞击、翻车等极端工况，采用因子图滑窗优化能在平滑窗口内联合优化历史位姿，显著提升里程计的鲁棒性。

针对跨地形场景的路径规划策略。针对平衡轮腿机器人跨地形时的速度和方向约束，按“从粗到细”四段推进。首先用空间 A* 决定地形通道。基于空间路径提取走廊，用曲率弧长运动基元做 Kinodynamic A* 动力学搜索。然后对粗动力学可行路径进行分段五次多项式 MINCO 连续优化。最后在弧长域求解速度剖面。

LPV 隐状态模型 + FDDP 模型预测控制。轮腿底盘纵向响应是非最小相位系统，普通 PID 或者简单模型 MPC 控制效果差。本项目基于实车录制的命令-响应数据离线辨识以底盘高度为调度变量的 LPV 模型，不可直接测量的隐状态由观测器在线估计，配合管式辅助环抑制模型误差。MPC 求解器为自研 FDDP，性能较好。

自主设计的导航状态机。导航状态机采用扁平化有限状态结构，便于管理和维护。状态机为本项目从零设计，未采用 Navigation2 行为树等现成方案，灵活度较高。

聚类动态障碍物跟踪与多步预测代价图。点云差分提取动态点后，经形态学闭运算与连通域聚类为独立目标，匈牙利匹配完成帧间关联，卡尔曼恒速滤波维护航迹，并以局部足迹栅格保留目标形状。确认航迹按衰减速度模型外推，生成逐帧代价图序列。预测帧按时间对齐进入 MPC 预测代价，实现动态障碍物的预测避让。

因子图离线建图优化与动态物体去除。在线里程计受实时性限制仅做固定滞后平滑，全局一致性可能不足。离线优化器将关键帧组织为两阶段因子图。局部子图内帧间 VGICP 配准优化，全局以子图为节点再做 VGICP + 回环检测，获得全局一致的位姿轨迹。随后用 3D-DDA 光线投射自动去除动态物体，产出干净的全局点云供定位与动态检测使用。可以在适应性训练中边打边建图，保存原始关键帧后离线优化，效果也还算不错。

最终成果：我们在临港的完整场地进行测试的时候，导航上下单级/二级台阶已经比较稳定，跳上高地、飞坡和反飞坡也能初步实现（可惜还是达到不了上场的稳定性要求）。由于人手不足（导航只有我一个人在负责，且电控要同时调两辆轮腿车）、缺乏调试场地（校内没有好用的场地地形，7月下旬到临港后才测试飞坡/反飞坡/跳高地）、轮腿调试经验不足（我们队在这个赛季才调出来能用的轮腿车）、以及其他各种问题（比如浙大本赛季其他开源中也提到的管理问题），所以今年国赛中哨兵的表现不算亮眼~~（其实是区完了）~~，也导致项目本身存在很多不足之处，所以还请各位大佬理解，也欢迎大家提出建议或改进意见。

临港测试的时候一次比较成功的飞坡与一次不那么成功的反飞坡：

国赛场上录包示意图：

原始点云（左）与离线优化后点云（右）对比图：

系统辨识中指令-响应-预测曲线对比图：

参考资料&友情链接：