前言

导航和决策只适用于RMUL2026

我的设计理念就一个：冲刺！

导航

基于NAVIGATION2，ROS的版本为HUMBLE

仓库链接： https://gitee.com/codnavgation/cod_-rm2026_-navigation

建图

在没有先验pgm的情况下进行导航，nav2官方的方案是边slam边导航：slamtoolbox + nav2

只要提供了map->odom->baselink的tf，以及/map上有地图数据，nav2就可以正常运行

所以：small_pointlio提供odom->baselink以及高频里程计，slamtoolbox提供/map数据，map到odom静态发布‘0’,‘0’,'0';

由于联盟赛的环境较为简单且面积较小，定位的误差容忍度很大，所以采用了纯lio的定位方法

至此完成了建图的初期设计。在后来验证中发现，在slam 导航中，给机器人单一的航点很容易撞墙，脱困极其浪费时间，在RM分秒必争的比赛中，这是致命的

所以我们抛弃了slam 导航中的单点导航，转而采用了Navigate Through Poses, 机器人规划一条连续路径穿越所有航点，中间不停留，这样就可以让机器人非常丝滑地在无先验地图的情况下快速按照既定路线移动

适应性训练的表现：

路径点的设计

Navigate Through Poses 需要提供多个waypoints，我们使用了 waypoint_editor ,他可以在已知地图上进行waypoints的编辑，同时会保存waypoints的csv文件，决策只需解析csv即可

同时，除了到达增益区，哨兵的前压和巡逻也是使用的csv

此地图由仿真环境构建，路径点由waypoint_editor创建

感知

在调试过程中发现nav2的nav2_costmap_2d::VoxelLayer会导致成本地图的清除不够干净，总有噪点残留，会让高速移动下的哨兵频繁避障，遥遥晃晃。我们采取的是 Spatio-Temporal Voxel Layer ，他不是依靠射线去清理地图，而是时间

The Temporal in this package is the novel concept of voxel_decay whereas we have configurable functions that expire voxels and their occupation over time. Infrasture was created to store times in each voxel after which the voxel will disappear from the map.

至于地形的感知，我偏向在外部处理好后再传给costmap，这样解耦的设计，方便算法测试以及更换

上赛季对于坡道的处理，使用的是 patchwork-plusplus ，效果还凑合，调一调也能过洞

控制

本赛季采用的是MPPI Controller，避障效果一流

MPPI的调试过程及其痛苦，个人认为不能一味调试std，而是去关注temperature和gamma，可以有效的避免晃动

pathfllow 和 pathalign 的权重不宜超过10，他们的threshold_to_consider不宜超过1.0

要是想提高速度，官方的建议是：

而我实际的做法是拉高GoalCritic的权重和threshold_to_consider，邪修调参

在实际的调试中，总会遇到在车目标点附近进行圆周运动，怎么也调不好，后来干脆就不调了，让claude写了个wrapper控制器，实际的控制器还是mppi

实现的效果：在靠近目标点附近时，强制固定速度，且只用kp引导至目标点，这样做非常完美的解决了上述的问题

NOTE：一定要注意的是，不能一味去调上位机的速度，先要确保电控那边能正确把控制器的速度转换成轮速

蓝色是lio算出的车体实际速度，绿色是cmd_vel的期望速度，可见，这是明显的没有正确转换

这是我修改过的small pointlio，可以输出速度，感谢ACE战队的开源，真的非常鲁棒！

杂项

goal checker采用的是nav2_controller::PositionGoalChecker，只需设置xy的阈值而不考虑角度朝向

导航树和脱困插件是直接copy北极熊的，非常之好用[1]

fake yaw也是之前抄的北极熊24年开源[3]

还有就是不要去修改livox drvier里面的偏置！！，而是要手动去发布雷达相对底盘的tf！

展望

nav2 对于rm 的哨兵来说局限性还是太大了，个人认为，像无人机的那种快速探索导航框架才是最适合RM的，奈何本人技术力不够，只能相信后人的力量了

决策

基于behaviortree.CPP 4.8和behavior.ros2，配合使用groot2实时检测功能调试

仓库链接：https://gitee.com/codnavgation/cod_-rm2026_-behavior-tree/tree/feature%2Ffinish/

仿真

https://github.com/qza36/COD_loopback_sim

由于humble没有loopback sim，所以我手动修改了一份可以在humble上跑的，这对决策来说是非常有益的，可以在不启动gazebo的情况下就可以跑。就算没有ros2，也可以通过pixi一键部署（在macbook air上验证过）

行为

多点巡逻：使用 PubNav2Goal（Topic 发布）替代原来的 FollowWaypointsAction（Action 调用）实现多点循环巡逻

FollowWaypointsAction 偶尔会导致行为树死掉

具体的文档在： https://gitee.com/codnavgation/cod_-rm2026_-behavior-tree/blob/feature/finish/COD_Behavior/doc/PubNav2Goal_Patrol.md

由于多点巡逻需要停留一定时间，所以修改了behavior.ros2基类，增加了发布频率和等待时间的选项

同时，由于自瞄在车自身静止时命中率相对较高，所以我们想出了一套“急停”策略，当血量健康且哨兵在增益区且对面装甲板距离车身小于一定阈值时，电控强制车的速度为零

决策逻辑：开局先去占中心增益区，如果我方占领就前压，否则去中心增益地区巡逻，无论什么时候血量低于210立刻回家，血量大于350出去

决策分两类，均使用xml文件加载行为树。第一种多点决策，用于适应性训练建图使用；第二种单点决策，需要基于场地地图，单点决策比多点决策更快

Weixin Image_20260331115526_276_21.png

自瞄

https://gitee.com/ustl-cod/sentry-auto-aim/tree/tmp/

基于FYT自瞄[5]，参考同济25赛季的开源作了一些优化[2]

参考 sp_vision_25 的 aimer.cpp。对于快速旋转的目标（小陀螺），装甲板分为"迎面"和"离开"两个阶段。shooting_angle 不对称：迎面侧给 55° 宽角（装甲板正在转向最佳位置，有更多时间瞄准），离开侧仅 20° 窄角（装甲板即将消失，不值得追）。Lock 机制防止两块角度接近的装甲板之间反复切换导致云台抖动

Yaw 角优化：两阶段搜索替换 G2O 图优化

同济的 solver.cpp 以云台 yaw 为中心 ±70° 搜索最优 yaw。这种方法比 G2O BA 更鲁棒

还有一些大大小小的修改，大家可以看commit记录

感谢一下中南老大哥的开源，还有浙纺老大哥的帮助

下位机[4]

https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev/

COD战队2026年哨兵机器人，主要基于2025年哨兵的基础上进行处理。由于26年联盟赛中心增益点规则改动，需要实现哨兵快速到达中心增益点并且为队友辅助占点，因此对25年舵轮哨兵进行优化

- 实现整车质量减轻（不计算裁判系统）整车质量约12.50Kg;

- 为哨兵加入超级电容模块，在超级电容低于百分之30情况下自动关闭超级电容

- 导航方面加入原地变速小陀螺，低速小陀螺下短时间内为超级电容充电，延长超级电容的使用时间；高速小陀螺则是减少被对面自瞄瞬秒的危险，同时变速的小陀螺也可以影响自瞄的卡尔曼预测，并且通过离心力作用可以一定程度上缓解小体形哨兵装配大滑环和中心供弹引起的不下弹丸和卡弹问题

- 根据同济和中科大的热量控制，在枪口剩余热量高于100以19弹频打出（减少双发打出导致裁判系统吞伤害的问题）在100到40之间使用同济的热量控制实现一个线性减速的过程，40到30之间枪口剩余热量因和枪口回复热量相同，最后留出3发作为冗余，防止超发锁枪口，做出一版可以简陋的19弹频下基本不超热量的热量控制策略

- 功率控制，参考了西交的功率控制模型，具体可参考【RM2023-电机功率模型与功率控制开源】西交利物浦大学

首先我们读取电机的电流值与转速，将电流映射为电机的转矩，将转速转换单位为（m/s）。通过西交的模型得出具体的预测底盘功率;通过将初始化的常数力矩和角频率、输入功率求和,得总力矩，总角频率，总功率进行功率拟合，通过对计算出的功率与实际功率进行递归最小二乘拟合，将模型中的参数进行更新。舵向与行进的功率分配：通过设定底盘最大输出功率，对舵向优先分配80%的功率，使其优先提供转向，将剩余的功率分配给行进向。通过开启自整定确定参数，读取PID误差，将误差累加，根据单电机PID误差的占比和预测功率的占总输出功率的占比运用分配因子，通过平滑函数将其化为一个隶属度，最后如果拟合出的功率总值大于被分配的最大功率，则将根据上述分配的功率上限进行功率限制的求解，运用求根公式，求出限制后的力矩电流值。如若所拟合的功率功率总值并未超过我们所给定的最大功率,则直接沿用PID计算所得输出即可。在此基础上利用功率缓存做一个简单PID控制来减少最大输出功率

- 实现下位机火控代替自瞄火控，利用视觉发给下位机控制延时前的Pitch和Yaw在下位机做开火控制，减少因为数据传输产生的延迟，并且实现连续丝滑的开火控制，发挥哨兵大弹舱，高冷却的优势

参考开源-电控

2025RoboMaster辽宁科技大学COD战队电控通用控制系统(达妙MC02 STM32H723VGT6)
【RM2024-功率控制算法开源（含舵轮，轮腿，全自动调参）】香港科技大学ENTERPRIZE战队
【RM2023-电机功率模型与功率控制开源】西交利物浦大学
【RM2025-超级电容与无线充电系统开源】香港科技大学ENTERPRIZE战队
在此十分感谢以上的个人、战队、院校。参考开源

致谢-电控

此代码模板源于王芃学长。感谢我的电控学长，付增波，陈鸿翔，王芃。是他们留下来的达妙H7通用控制系统让我能在此基础上修改完善。尤其是感谢付增波学长，我许多的代码都是对他的拙劣模仿; 感谢陈鸿翔项管，对我代码不断完善提供指导和建议。
感谢导航组的代雨欣，叶少龙同学和齐志昂学长为哨兵赋予灵魂。感谢视觉组张济仁同学和韩宇学长带来的自瞄策略。感谢硬件组尹纪兴同学的超电，让哨兵体验到150w的功率。感谢王兴瑞同学的机械设计舵轮哨兵，让我能将此项目运用在实际机器人上。
感谢和我一路走来的COD成员！
感谢沈阳工业大学（辽阳分校）赫兹矩阵战队、沈阳理工大学 Ambition战队、东北大学T-DT战队、南京航空航天大学长空御风战队、大连理工大学凌Bug战队、辽宁石油化工大学TCN战队（排名不分先后）的开源或交流，使我可以更好的完善此项目。

联系方式

QQ 群：

COD 齐志昂 QQ：2746647599 github：qza36

导航-代雨欣 wechat: xx_orb354_

决策-叶少龙 QQ：1709656651

电控-缪建平 QQ：1634789078

自瞄：

韩宇 wechat：cocla009
张继仁 wechat：zjr3254781275

致谢

非常感谢代雨欣、张继仁同学日日夜夜的调试，还有叶少龙同学开发的决策，比我25年的决策好上10倍不止

感谢韩宇同学和我一起测试修改自瞄系统，没有他，哨兵会少一半的火力输出

感谢沈阳工业大学（辽阳分校）赫兹矩阵战队提供的场地支持

好的上位机要有一个鲁棒的电控支持：感谢缪建平同学的辛苦工作

最感谢的是我们新上任的指导老师，在战队最困难的时候依旧选择支持我们

最后是各位老学长的鼓励，真的很感谢你们

写在最后的话

我们奋斗过，努力过，哪怕是昙花一现，就算时间会冲淡一切，至少我们现在是光荣的

从赛季初开始，我们就寸步难行，指导老师的抛弃、学校的排挤与欺骗，让这支有着十年底蕴的队伍几近停摆。比起自己的不甘，我更心疼那些连赛场氛围都还没感受过就要被迫结束的后辈们

后面转机到来，有一位老师愿意支持我们，我们叫他A老师，这对COD来说是莫大的好消息，可是，A老师能支持的极限只有联盟赛，我们也认，毕竟UC的费用真是一笔不小的开销

寒假来临，学校关闭实验室、我们自费在村子里组了一间带大棚的花窑，当时只有一间房子有空调，调试机器人只能等太阳升起来去大棚里面调试，环境真的非常艰苦，但是大家依旧在坚持，为了证明自己，为了COD的最后一舞

邻近比赛，学校彻底剥夺了我们使用实验室的权利，我们只好回到了那个又小又挤的花窑，20多人挤在一起调车，每天下课后打车十分钟赶来，披星戴月地调完车再打车回去

整整十年的队伍底蕴，在某些人眼里似乎轻如鸿毛，说扔就扔

RMUL的季军对COD来说是历史最高，望着这块季军的牌子，心里更多的是意难平，联盟赛的舞台对COD来说真的太小了，这支在泥泞中依然能咬牙拔剑的队伍，本该在区域赛甚至国赛的赛场上，大放异彩

过去的一年，大家真的很不容易。在最难熬的寒冬里，在那个连暖气都没有的花窑，是大家搓着冻僵的手去调车、修车

感谢你们陪着COD一起咽下这些委屈，没有一个人选择当逃兵

感谢每一个在黑暗中没有放弃的你们

虽然外部的环境曾试图让我们迷失方向，但我们靠自己走出了最漂亮的一条路。作为一起扛过这些风浪的人，我想说的是：愿学弟学妹们在未来的赛场上不再受困于条件，能拥有更广阔的舞台，去弥补我们未能踏上国赛的遗憾；也愿即将各自奔赴前程的兄弟们，能带着在COD这段日子里锤炼出的坚韧，去击碎以后人生里的所有壁垒

兄弟们，辛苦了！

祝我们所有人，前路坦荡，都有光明的未来！