前言

这是我在古月居的第一篇文章，我想介绍一下我们组去年的参赛经历，以及整个做比赛的过程。内含干货，包括我们控制算法的设计思路等~~结尾我也会回答一下有些同学私聊我的问题。

正文

视频效果

话不多说，直接上视频。

第一个视频是拿到车模组装完成之后的效果，第二个就是我们在之后几个月里应用的最终方案，经过改善之后，能从肉眼看到差距之大了。包括电机的声音对吧，我感觉大部分男生应该都很喜欢高速运转的电机声吧，当然我肯定是非常喜欢的。下面我会开始介绍我们整个项目的经历以及制作过程。

制作流程

梦开始的地方~~（ps：箱子上是有湿，冒雨拿的快递，当时还是很激动的，终于等到了车模）第十四届智能车竞赛ROS组别参赛心得起初刚拿到车，组装的时候，大家应该都和我一样，大大的懵字写在脸上，按官方的教程组装好车模，配置好各种功能之后，问题就随之而来了，这不是开箱就能跑了吗？那我们要做什么工作呢？ …… 但其实问题远远没有这么简单，那时候我就在想，既然大家都能完成比赛了，那比的是什么呢，那可不就是速度了嘛。

接下来就是干货了，之后大部分参赛同学都陆续组装完了车，开始研究了起来，第一个遇到的问题就是定位不准。只要一提速，车模的位置就飘出去了，很是诡异，所以大家的速度都被这个问题限的死死的。定位漂移，导致的建图不准，导航撞墙等等，都是很严重的问题。而我们当时也并没有找到很好的方法克服，也是因为对ROS的不够了解，这样才没有找到比较正确的方法。当时我和学长讨论的是，因为速度提高后，里程计漂移，那应该就是加速度的二次积分出来的距离有问题。于是，我们第一个方案就出来了。因为之间有做普通组别的经验，那么第一个想到的就是用编码器积分出的距离来做里程计，当然实现起来也不是那么容易的，整个车模的传动根本不适合加编码器，甚至光码盘。不过后来就想到了个比较合适的方法，既然没有适合的轮子，那再加一个轮子不就完了嘛。

第十四届智能车竞赛ROS组别参赛心得这样我们的方案当晚就上了卓大大的公众号，也获得了许可。编码器的数据由单片机正交解码获得，并通过串口发送到了上位机，然后我就在上位机构建了两个话题，一个用于发布实时速度，另一个则是实时的速度积分。经过测试发现，确实不管速度多快，积分出来的距离，就仅和编码器轮子转的圈数相关，只要这个轮子不打滑，距离相当精准。接下来就是利用这个距离来构建里程计并校准激光雷达里程计了，这才是难点。因为对ROS工程不是很了解，所以我直接改起了RF2O的源码，我把最后决定里程计位置的代码，全部换成了利用距离和IMU数据解算的xy轴的位置。最后确实改善了一些，但效果并不明显，因为漂移的问题仍然存在。这就更诡异了，直接推翻了我们的想法。然后我便停下了这种“治标不治本”的调试行为，买了两本ROS的书学习。工欲善其事必先利其器不是没道理的。在了解了rqt工具箱之后，确实对我有了很大的帮助，而且换了roboware，不用再打开一堆gedit看代码了。

rqt_graph

在看完节点话题的关系图之后，我们发现，决定小车最后坐标的，并不是rf2o输出的里程计信息。而是将rf2o输入ekf之后获得的坐标信息。那我们的目标就标定了这个扩展卡尔曼滤波。

'''[ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
numeric_limits
  
   ::max() if unspecified. It is strongly recommended that these parameters be removed if not
required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
the thresholds. '''
odom0_pose_rejection_threshold: 8
odom0_twist_rejection_threshold: 0.75

在看了ekf_params.yaml文件之后，发现这两个参数的注释有这么一句话It is strongly recommended that these parameters be removed if not required.如果不需要，强烈建议删除这俩参数。从参数名字来看，一个是对pose的限制，一个是对twist的限制。我们直接把这两个参数注释了，然后发现速度瞬间就提上来了，并且达到了我们使用编码器的预期效果。我们用遥控器加速，快到车头都要抬起来了，定位都是实时的。做到这步，我们已经备受鼓舞了，我依旧记得当时是某个没课的日子，早上七点，对，没错，是一宿没睡。哈哈哈哈哈哈哈，发现了这个问题之后，当时和队友都困意全无，如果不是怕身体出问题，肯定会一直研究下去的。之后去门口吃了碗拉面当早饭，回宿舍睡了会儿就又来实验室工作了。ヾ(•ω•`)o 到此，我们便把定位的问题解决了。

接下来的问题，便是如何驾驭这个速度，因为你的速度极限高，你发挥不了，那还是等于零。因为我是物联网专业的，学长是电信专业的，两个人对自控地了解都仅限于自学，所以确实也尝试了很多方案，好在学长也经验丰富，有很多点子能尝试。首先我们尝试了DWA规划器发布的cmd_vel的控制指令，包括速度转向。但是发现这个指令输出的速度并不极限，尤其是直道，可以大幅度加速的，但规划器并没有规划出高速，在对DWA规划器几番调参之后并不满意，于是我们便决定自己写控制算法。一开始的试用方案就是，转向仍然使用DWA规划器的数据，速度由我们自己写，当转角小的时候，我们给一个大的速度，转向大的时候就给一个小的速度。这相当于是速度依靠转角数据的P控制。当然测试效果也不佳，问题就在，每次速度都是在入弯之后才减下来的，所以入弯并没有达到要求速度，会让车失控撞墙。但是如果要让入弯达到要求速度，那么直道也得跟着减速，也就是直道不快，弯道也就能减下来了。但是这样还不够极限。于是我们就又放弃了这种想法。转向必须也得自己来写。然后依靠曲率转角，并提前减速的最终方案就出来了。之前在竞赛群里讨论时，有很多人问我 “你rviz上的三个点是干什么用的？” 第十四届智能车竞赛ROS组别参赛心得

这个其实就是我们的转向方案，在车前方取三个有间隔的点，计算以这三个点构成的三角形的外接圆半径，当然这不是严格意义上的曲率，但已经足够。以此来作为转角的依据。当时使用的DWA规划器没有TEB这种规划器好，因为TEB可以用于阿克曼模型，并用于竞速，TEB有全程时间优先的参数，可以让路径尽可能贴内，而当时用DWA的时候，只有尽可能让膨胀系数小一些，好让路径能够贴内道，当然这也是有风险的，那就是撞障碍。但是调的好就能避免。在得到前方的曲率数据后，我们的速度就是由曲率来控制的，这样就有了一定的前瞻性，在曲率大也就是半径小的地方，我们会有一个提前减速，这样车就能达到入弯的极限，并且直道可以有很高的速度。当然缺点就是这样写参数会比较多，包括三个点之间的距离，以及前瞻长度，减速的系数，等等。不过好在经过大量调试下，这些参数都调整的差不多了。

到此，我们的车已经很接近最终版本了，不过目前还有一个问题，那就是舵机的响应时间，因为我们想尽可能把一切都发挥的很极限。由于车模的重量大，以及这种越野车模轮胎的摩擦力大，因此导致舵机的机械响应就变慢了。我们就在已经做好的主控上额外加了针对舵机的恒压恒流的一个升压输出模块，电压高一些让舵机扭矩能输出大一些。第十四届智能车竞赛ROS组别参赛心得也就是图上路由器前面的一块板子。到此最终版本就做出来了，这个时候也已经是六月份了，剩下的一个多月时间，就是在室外搭建场地，不断进行路跑，调试，不断测试，喂蚊子。/(ㄒoㄒ)/~~

探讨

这部分就放一些有的同学私聊问我的问题，本来想取名问答的，不过感觉我应该也不太够格解答所有问题，哈哈哈哈，所以就叫探讨了。我会说我的理解和思路。因为我也是初学小白，很多东西理解的不是很深刻，所以回答难免会有出问题的地方，不过我知道的东西我都会尽可能回答同学，有问题的欢迎指正，交流。谢谢啦~

问：车模导航的时候转向会一直左右抖动。答：如果使用的是cmd_vel的数据，那这个应该是规划器参数不合适导致的，可以考虑限制一下转向的速度，以及加速度。

问：ekf的参数怎么调节，怎么配置。答：这次仿真其实我们是没有用到ekf的，ekf本身就是对里程计信息进行一定程度上的优化，不过因为rf2o出来的定位信息我用着已经足够了，因此没有再加ekf。

问：我的车在过弯的时候会卡着不动，然后还会倒车，怎么办。答：这是因为规划器根据你的参数无法规划出抵达目标点的决策，需要看一下你的teb参数是否设置合理，包括最小转向半径，车模尺寸，膨胀系数。如果不想让车模倒车的话，可以考虑修改这个参数weight_kinematics_forward_drive 这个参数的意思是迫使车模选择前进方向的权重，数值越大，倒车的惩罚越大，所以按需设置就好了，teb是不能禁止倒车的，只能把优先级降的非常低，如果想完全禁止倒车，可以自己重写速度的控制算法。