讯飞线上赛总结（二）

讯飞线上赛总结（二）——全局路径规划

在介绍全局路径规划之前先补充一些上篇博客有关成本地图的知识。

成本地图

  成本地图主要分global_costmap层和local_costmap层，两种costmap都是同一个插件costmap2D排成的实例，但是两者加载的参数不同，使用的子插件不同，最终的用途也不尽相同。
global_costmap是全局代价地图，主要服务于全局路劲规划器。一般来说，全局规划的允许范围是要远远高于局部规划器，但仍受限于global_costmap的尺寸，对于超出global_costmap的区域，全局规划器是没办法不依赖cost信息直接规划路径的。
  另外一个值得注意的参数是全局规划的膨胀半径和膨胀衰减系数，通常全局膨胀半径需要设置一个较大但仍在合理区间内的值，以使得全局规划器规划出来的路径尽量贴近路中心。

  local_costmap是局部代价地图，它只用于进行局部规划器路径规划。通常局部规划器是没有static层的，这就可以使得局部规划器检测到真实环境信息，从而规避静态地图和实际地图不匹配的情形。

路径规划

  路径规划一般都会用到move_base导航基础功能包，这个包集成了车辆导航所需的绝大多数服务，包括TF关系的处理，代价地图插件的调用，接受/move_base_simple/goal话题上发布的简单目标姿态，并自动循环调用全局规划器、局部规划器实现机器人自动导航功能。move_base由local_planner输出一个/cmd_vel话题，即最终的机器人运动控制指令，这个指令进行简单的处理后发往底层控制器即可实现简单的自动路径规划、自主运动、主动避障功能。

全局路径规划

  在搜索最优路径的算法中，全局路劲规划依据搜索算法分类，主要包括图搜索类算法、随机采样算法、动力学寻径等。

  基于图搜索的算法主要有Dijstra、A^∗、JPS（Jump Point Search）等。Dijstra算法是一种广度优先的状态空间搜索算法，即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点，最终会得到一个最短路径树。这样会大大增加计算时间和数据量。而且搜索得到的大量信息对于机器人运动是无用的，由于该算法遍历计算的节点很多，所以效率很低。A^∗算法与Dijstra算法相比，增加了启发式估计，仅关注点到点的最短路径，不但记录其到目标点的代价，还计算当前点到目标点的期望代价。它在广度优先的基础上加入了一个估价函数，减少搜索量，提高效率，同时保证路径的最优性，但环境复杂、目标点规模较大时，会带入大量重复数据和复杂的估价函数，效率仍较低。JPS算法即跳点搜索算法，在保留A^∗算法的框架同时，进一步优化了A^∗算法寻找后继节点的操作。
  基于随机采样算法主要有概率地图法（PRM）、RRT^∗、RRT、Informed RRT^∗等方法，广泛应用在动态障碍物、高维状态空间和存在运动学、动力学等微分约束的复杂环境中，但存在耗费代价大、应用实时性较差的问题。RRT算法，即快速搜索随机树算法，是一种增量式采样的搜索算法，该方法在应用中不需要任何参数整定，具有良好的使用性能。它利用增量式方法构建搜索树，以逐渐提高分辨能力，而无需设置任何分辨率参数。在极限的情况下，该搜索树将稠密的布满整个空间，此时搜索树由很多较短曲线或路径构成，以实现充满整个空间的目的。RRT^∗法的主要特征是能快速找出初始路径，之后随着采样点的增加，不断地进行优化直到找到目标点或者达到设定的最大循环次数。该算法是渐进优化的，随着迭代次数的增加，得出的路径是越来越优化的，永远不可能在有限时间中得出最优的路径。Informed RRT^∗根据RRT^∗方法首先找到路径后的一个Cbest，再根据Cbest构建一个椭圆进行采样，当Cbest不断减小，椭圆的范围也不断减小，最后收敛成一条直线（无障碍时）。大幅减小搜索范围。
  基于动力学的寻径算法有状态栅格算法、Hybrid 、Kinodynamic RRT^∗等。

A^∗算法

  A^∗算法是启发式搜索，是一种尽可能基于现有信息的搜索策略，也就是搜索全过程中尽量利用目前已知的诸如迭代步数，以及从初始状态和当前状态到目标状态估计所需的费用信息。A^∗算法可以选择下一个被检测的节点时引入了已知的全局信息，对当前节点距离终点的距离做出估计，作为评价该节点处于最优路线上的可能性的量度，这样可以先搜索可能性大的节点，从而提高了搜索过程的效率。A^∗算法的基本思想如下：引入当前节点$j$的估计函数$f^{\ast }$，当前节点$j$的估计函数定义为：

$$f^{\ast }\left(j\right)=g\left(j\right)+h^{\ast }\left(j\right)$$

  其中$g\left(j\right)$是从起点到当前节点 的实际费用的量度，$h^{\ast }\left(j\right)$是从节点$j$到终点的最小费用的估计，可以依据实际情况，选择$h^{\ast }\left(j\right)$的具体形式，$h^{\ast }\left(j\right)$要满足一个要求：不能高于节点$j$到终点的实际最小费用。从起始节点点向目的节点搜索时，每次都搜索$f^{\ast }\left(j\right)$最小的节点，直到发现目的节点。

Dijstra算法

  Dijstra算法采用的是一种贪心的策略，声明一个数组dis来保存源点到各个顶点的最短距离和一个保存已经找到了最短路径的顶点的集合：T，初始时，原点S的路径权重被赋为0（dis[s]=0）。若对于顶点S存在能直接到达的边（s，m），则把dis[m]设为W（s，m），同时把所有其他（s不能直接到达的）顶点路径长度设为无穷大。初始时，集合T只有顶点S。然后，从dis数组选择最小值，则该值就是源点S到该值对应顶点的最短路径，并且把该点加入到T中，此时完成一个顶点。接着我们需要看看新加入的顶点是否可以到达其他顶点并且看看通过该顶点到达其他点的路径长度是否比此源点直接到达短，如果是，那么就替换这些顶点在dis中的值。然后又从dis中找出最小值，重复上述动作，直到T中包含了图的所有顶点。

仿真对比

实验视频

Astra算法仿真

Dijstra

实验结果

算法	耗时（ms）	轨迹长度	迭代次数
A∗	0.4200	45.36	176
Dijstra	3.5300	44.77	1662

  通过仿真实验可以看出在最短路径搜索效率上，一般有A^∗>Dijstra。A^∗与Dijstra算法、广度优先、深度优先算法的联系：当$g\left(j\right)=0$时，该算法类似于DFS，当$h^{\ast }\left(j\right)=0$时，该算法类似于BFS。如果$h^{\ast }\left(j\right)=0$只需要求出$g\left(j\right)$，即求出起点到任意节点$j$的最短路径，则转化为单源最短路径问题，即Dijstra算法。
  写完这篇关于A^∗与Dijstra算法介绍的博客，个人觉得写的还是有些浅显了，目前正在学习全局路径规划这部分的知识，等过两周可能会对这篇博客进行补充。
  博客中有相关错误，希望大佬们指出。（感谢感谢！！）