前言

国赛篇的第一期我们实现了最基础的多点导航，并利用rviz标定分段目标点进行导航。但是官方补充完规则之后，这个方案就无法用来进行完成比赛了。因为比赛明确规定了分段点的标定要由程序计算得出，不能由人为决定。本期内容就提供了几个可行的方案，并对其中最好的方案进行实现。 下面贴上国赛需要注意的细则：

同样，由于是赛前，不做技术方案讲解，也不公开算法，仅仅提供几种可行的思路。

正文

对于规则的改变，我们需要添加一些必要的算法，当然这也是在能够实现多点导航的基础之上，由算法来进行分段点的标记。 我们选择了其中我认为算是目前我想到的方案中最好的一种进行实现，大家可以看效果，如果有更好的方法，也欢迎交流。 下面开始进入正题~

方案一

直接开门见山吧，我就着重讲我们用的方案了。首先对地图进行分析，十一个区域每相邻两个区域之间的路径是唯一的，所以我们的整体思路就是把导航划分到每个相邻区域的导航，也就是一步一步到达最后终点。每次只走相邻区域，这种时候只需要去除你上一个位置区域，就能够保证路径的规划永远不会在车子的后方。比如：下面图中，当你从7到达10的时候，你的规划可以选择的相邻点有8，2，11，7，但是如果你去除上一个位置的区域，也就是7，那么就剩下8，2，11，按照这个思路，规划的路径永远不会在车后方。将整个导航划分成相邻点的分段导航，在车模抵达第一个分段点的一定范围内，再发布第二个分段点。依次来完成整个导航。
上面是大体的框架，而路径的检索，则需要靠遍历每个点，在此之前，我们需要将每个点的相邻区域存在代码中，因为比赛中区域不会变化，因此可以用这个来作为遍历的方法。 从当前点遍历相邻的区域，去除上一个区域，不断遍历直到满足两个条件，一是到达终点区域，二是已经走完了需要经过的所有区域，才算是一条满足条件的路径。在遍历的过程中还需要记录步长，最后从满足条件的路径中选出步长最短的路径即可。（如果仅仅是以到达终点为结束的标志的话，则会出现中途无法经过终点的情况。并且如果起点终点均为同一个区域，那么就无法进行路径规划了）下面给大家举一个例子：当起点在11，车头朝向10的时候，我们终点也设置为11，必经区域为3，2，1。那么规划出来的路径就如下图。

可以看到，我们的方案每一步都只会规划出相邻的区域，这个时候只需要把下一个需要到达的区域作为当前目标发布即可，到达指定区域后再发布下一个目标点，按顺序依次到达最终目标。

不过我们的算法还额外实现了一个功能，就是能够按照参数来排除一些转向不方便的路径。 下面我会详细说明： 由于实际车模的转向能力比较差，转向半径也很大，因此会出现一些弯道很难转过去的情况，比如下图中红色箭头的那些转向方式，如果搭配上障碍物，那么对车模的转向能力则是很大的考验。
我们的算法中，通过上一个区域，当前区域，以及下一个区域三个点，以当前区域为角点绘制夹角，计算角度，当转向所需要的角度比较大的时候，那么这条路径即便满足经过区域的要求最终到达终点，那么也会将其剔除掉。这个过程将用来优化路径的选择。

下面，我放上我在仿真中测试该算法的效果：

关于输入区域号如何正确发布目的地的坐标信息这个问题，我就不多解释了，上一期内容实现的rviz publish point标定分段目标点的功能，完全可以改写成用publish point来标定区域点。 为了不用每次开启时都存储区域坐标，我们将标记的区域依次存入工程目录下的一个TXT文本文件中，让另一个路径规划的节点来读取该文件数据，这样只需要在得到地图后，做一次存储的操作即可。 具体步骤可参照下面的视频：

OK~这就是我们的方案实现，经过了解，很多人遇到了路径规划的时候会规划在车后方，或者规划途中会出写变成其他路径的情况，这两种情况都是由于路径规划的步长太大，比如从1到11，中间有两条较短的路径可选，那么在行驶过程中，就很有可能让路径规划在两条路线中来回跳动，犹豫不决。这种现象可以参照中间锥桶的情况，路径同样是在锥桶左右两边来回跳动。 当然如果步长写成每次导航都到相邻区域并排除上一个到达的区域，那么就能很完美的解决上面两个问题。

这是目前我们对已有方案的分析改良后选出的最好的方案了，既能够根据参数调节路径的难易程度，又能保证路径的唯一性，可行性。并且经过测试并不会耗费大量资源，计算过程甚至不足0.5秒，而且计算只会在导航开始前，也仅计算一次，所以并不会有任何影响。 如果有更好的方案，或者能改进的想法，非常欢迎在评论区留言~

方案二

这个方案是比较容易实现并且思路比较简单的方案，这个方案的思路来源于古月前辈的《ROS机器人开发实践》这本书中。 简单来说就是随机导航。可以沿用方案一的相邻点导航，在此基础上，以一个完全随机方式进行导航。 也就是说，每到达一个点，随机发布相邻的下一个点，每次到达终点区域的时候进行一个判断，如果已经经过全部需要经过的区域则停止，如果没有，就继续随机导航。这个功能的实现在古月前辈的书中有详细说明和实现教程。 除此之外的暴力检索算法等如果不细分为每个相邻区域的规划的话则会很容易出现路径不唯一的情况。

总结

以上便是完赛的两种方案，别看只有两个，方案一可是我们从多个方案里集思广益得出的合成版，可谓是去糟粕取精华！ 用方案一算法规划出来的路径不管使用DWA规划器还是TEB规划器都能很高效稳定地完赛。 今年竞赛ROS组别的难度大大提升，并且时间也相当紧张，所以大家也必须打起十二分精神来，努力争取更好的成绩！ 最后祝大家在比赛中取得优异的成绩啦~~

最后的最后，点赞过一百或者浏览超两千，赛后公开源码~~