实时最佳路径规划算法,路径规划算法发展史

图6 顶点可见性判断

可视图法能求得最短路径，但搜索时间长，并且缺乏灵活性，即一旦机器人的起始点和目标点发生改变，就要重新构造可视图，比较麻烦。可视图法适用于多边形障碍物，对于圆形障碍物失效。切线图法和Voronoi图法对可视图法进行了改进。切线图法用障碍物的切线表示弧，因此是从起始点到目标点的最短路径的图，移动机器人必须几乎接近障碍物行走。其缺点是如果控制过程中产生位置误差，机器人碰撞障碍物的可能性会很高。Voronoi图法用尽可能远离障碍物和墙壁的路径表示弧。因此，从起始点到目标点的路径将会增长，但采用这种控制方式时，即使产生位置误差，移动机器人也不会碰到障碍物。

Dijkstra算法由荷兰计算机科学家艾兹赫尔·戴克斯特拉（Edsger Wybe Dijkstra）发明，通过计算初始点到自由空间内任何一点的最短距离可以得到全局最优路径。算法从初始点开始计算周围4个或者8个点与初始点的距离，再将新计算距离的点作为计算点计算其周围点与初始点的距离，这样计算像波阵面一样在自由空间内传播，直到到达目标点。这样就可以计算得到机器人的最短路径。

Dijkstra算法是一种经典的广度优先的状态空间搜索算法，即算法会从初始点开始一层一层地搜索整个自由空间直到到达目标点。这样会大大增加计算时间和数据量。而且搜索得到的大量对于机器人运动是无用的。

为了解决Dijkstra算法效率低的问题，A*算法作为一种启发式算法被提出。该算法在广度优先的基础上加入了一个估价函数。

快速搜索随机树（RRT）算法是一种增量式采样的搜索方法，该方法在应用中不需要任何参数整定，具备良好的使用性能。它利用增量式方法构建搜索树，以逐渐提高分辨能力，而无须设置任何分辨率参数。在极限情况，该搜索树将稠密的布满整个空间，此时搜索树由很多较短曲线或路经构成，以实现充满整个空间的目的。增量式方法构建的搜索树其导向取决于稠密采样序列，当该序列为随机序列时，该搜索树称为快速搜索随机树（Rapidly Exploring Random Tree，RRT），而不论该序列为随机还是确定性序列，都被称为快速搜索稠密树（Rapidly Exploring Dense Trees，RDTs），这种规划方法可处理微分等多种约束。

算法步骤

图7 随机树构建过程

由于在搜索过程中考虑了机器人的动力学约束，因此生成的路径的可行性很好。但是算法的随机性导致其只具备概率完备性。

LaValle等人的工作奠定了RRT方法的基础。在采样策略方面，RRTGoalBiaS方法在控制机器人随机运动的同时，以一定概率向最终目标运动；RRTGoalZoom方法分别在整个空间和目标点周围的空间进行采样；RRTCon方法则通过加大随机步长改进规划速度。双向规划思想也被采用，衍生出RRTExtExt，RRTExtCon，RRTConCon等多种算法。

基本RRT算法收敛到终点位姿的速度可能比较慢。为了提高算法的效率和性能，需不断对该算法进行改进。如为了提高搜索效率采用双向随机搜索树(Bi~RRT)，从起始点和目标点并行生成两棵RRT，直至两棵树相遇，算法收敛。由于这个算法相比于原始RRT有更好的收敛性，因此在目前路径规划中是很常见的。NikAMelchior提出的粒子RRT算法，考虑了地形的不确定性，保证了在不确定性环境下搜索树的扩展。

Kuffner和Lavane又提出RRT-connectlv，使得节点的扩展效率大大提高。运动规划中，距离的定义非常复杂，Pengcheng研究了在RRT生长过程中距离函数不断学习的算法以降低距离函数对环境的敏感性。考虑到基本RRT规划器得到的路径长度一般是最优路径的1.3~1.5倍，英国的J.desmithl研究了变分法技术使其达到最优。Amna A引入KD树作为二级数据结构加速查找距离从环境中取出的随机点最近的叶节点，降低了搜索成本。该算法在动态障碍物、高维状态空间和存在运动学、动力学等微分约束的环境中的运动规划已经得到广泛的应用。

基本RRT算法倾向于遍历整个自由空间直到获得可行路径，这使其不可能用于未知或动态环境中的机器人在线运动规划。利用滚动规划的思想可以将RRT算法进行改进，使其具备在线规划能力。

滚动规划

机器人在未知或动态环境中运动时，只能探知其传感器范围内有限区域内的环境信息。机器人利用局部信息进行局部运动规划，并根据一定的评价准则得到局部目标。机器人到达局部目标后再次进行新的局部规划。如此反复进行直到到达全局目标。

滚动规划算法的基本原理：

1. 环境信息预测：在滚动的每一步，机器人根据探测到的视野内的信息、或所有已知的环境信息，建立环境模型，包括设置已知区域内的节点类型信息等；
2. 局部滚动优化：将上述环境信息模型看成一个优化的窗口，在此基础上，根据目标点的位置和特定的优化策略计算出下一步的最优子目标，然后根据子目标和环境信息模型，选择局部规划算法，确定向子目标行进的局部路径，并实施当前策略，即依所规划的局部路径行进若干步，窗口相应向前滚动；
3. 反馈信息校正：根据局部最优路径，驱动机器人行走一段路径后，机器人会探测到新的未知信息，此时可以根据机器人在行走过程探测到的新信息补充或校正原来的环境模型，用于滚动后下一步的局部规划。

其中，局部子目标是在滚动窗口中寻找一个全局目标的映射，它必须避开障碍物，且满足某种优化指标。子目标的选择方法反映了全局优化的要求与局部有限信息约束的折衷，是在给定信息环境下企图实现全局优化的自然选择。

基于滚动窗口的路径规划算法依靠实时探测到的局部环境信息，以滚动方式进行在线规划。在滚动的每一步，根据探测到的局部信息，用启发式方法生成优化子目标，在当前滚动窗口内进行局部路径规划，然后实施当前策略(依局部规划路径移动一步)，随滚动窗口推进，不断取得新的环境信息，从而在滚动中实现优化与反馈的结合。由于规划问题压缩到滚动窗口内，与全局规划相比其计算量大大下降。

基于滚动窗口的路径规划算法的具体步骤如下：

• 步骤0：对起点、终点、工作环境、机器人的视野半径、步长进行初始化；
• 步骤1：如果终点到达，规划中止；
• 步骤2：对当前滚动窗口内的环境信息进行刷新；
• 步骤3：产生局部子目标；
• 步骤4：根据子目标及已知环境信息，在当前滚动窗口内规划一条优化的局部可行路径；
• 步骤5：依规划的局部路径行进一步，步长小于视野半径；
• 步骤6：返回步骤1。

在一个滚动窗口内，随机树以当前位置为起始点，构建传感器范围内的随机树。构建方法与基本RRT算法一致。为了使全局环境中随机树具有向目标方向生长的趋势，在运动规划时引入启发信息，减少随机树的随机性，提高搜索效率。