融合改进RRT和Dijkstra算法的机器人动态路径规划*

马新国，马希青

(河北工程大学机械与装备工程学院，邯郸 056038)

0 引言

近些年，移动机器人作为智能系统提高了许多行业的生产力，为人类生活提供了很多便利，因此受到越来越多的关注。路径规划是研究移动机器人的一个重要内容。路径规划旨在通过规划算法，在有障碍物的工作环境内，规划出从起点到目标区域的自身约束条件的无碰撞路径[1]。该方向迄今已有大量的研究成果。如基于网格划分的路径规划算法，需要对周围环境划分区域，扩展遍历到目标点[2-3]，该类算法在地图环境较大时过于耗费资源；群智能算法虽然适合整体环境优化，但容易陷入局部最优，且不适合高维环境下的路径规划。

RRT算法是一种简单快速的基于采样的路径规划算法，它在工作空间中随机采样，由起始点向采样点扩展，逐步生成类似于树的路径，最终搜索到目标点。但RRT算法的缺陷也非常明显：①搜索过于盲目，无方向性；②节点利用率低，搜索过程中会产生许多冗余节点；③得到的路径只是可行而并非最优，有非常大的优化空间。针对以上RRT算法的缺点和不足，国内外学者提出了许多优化改进RRT算法的方法，马小陆等[5]提出将跳点思想融入到RRT*算法中，缩小搜索节点数量，提高搜查效率，并且在ROS中构建仿真环境，验证了算法高效性。王坤等[6]在RRT-Connect算法的基础上进行改进，动态设置步长并引入目标偏置策略，提高了搜索效率。KARAMAN等[7]提出了RRT*算法，对RRT算法产生的路径进行重新选择父节点和重新布线，不断重复该操作从而改进和优化路径。GAMMELL等[8]提出了Informed-RRT*算法，在RRT*算法的基础上，在起点和终点之间形成的椭圆区域内采样，大大减少了搜索时间。董敏等[9]使用双向RRT思想，并对改进算法生成的路径的路径设置评价函数中选出最优路径，并对选出的路径平滑调整，通过仿真，验证算法有效性。刘恩海等[10]针对随机树生长的随机性进行改进，引导向目标点生长且能够根据周围环境做出调整，算法搜索具有很强的目标性和偏向性。李金良等[11]对搜索方向和路径节点角度进行了约束，优化了路径平滑度，对改进后规划的节点使用B样条优化。

针对RRT算法在路径规划中无方向性，路径并非最优且不适合机器人移动的问题，首先对RRT算法进行改进，再与Dijkstra算法融合规划出优化路径，最后与动态窗口算法融合。全局路径规划中，设置目标节点采样率和动态步长，提高搜索速度和效率。扩展改进后的RRT算法的每个节点得到可行区域，采用Dijkstra算法搜索可行区域内的最优路径，减少路径长度。局部路径规划中，通过融合RRT和Dijkstra算法得到的路径节点引导动态窗口算法，防止陷入局部最优。

1 改进的RRT算法

类似于树不断生长，RRT算法不断地向外扩展分支以找到一条由起始点到目标点的唯一路径。其在寻找分支的过程中是随机采样，无需对周围环境进行几何划分，搜索空间的覆盖率高，搜索的范围广。在时间允许的情况下，只要存在可行路径就一定能找到。但该特性也决定了RRT算法采样效率不高，路径中有大量的冗余节点，且找到的路径不一定是最优的，有比较大的优化空间。针对这些问题，提出以下两点改进。

1.1 设置目标节点采样率

传统RRT算法的扩展是随机的，在寻找路径时具有一定的盲目性。因此对扩展方向进行一定概率的导向。采样情况可用式(1)表示。

(1)

式中，s为随机函数生成的随机数；sg为目标节点采样率；Pgoal为目标点；Prand为在地图范围内的随机采样点；Pnew为扩展树的新节点。

在无障碍物的环境中，当起始点与目标点相同，设置目标节点采样率为80%时，该RRT算法能够很快的接近并搜索到目标点，而传统RRT算法导向性一般，耗时且产生了非常多的冗余节点。两种算法搜索出的路径对照如图1所示。

(a) 无障碍物传统RRT算法 (b) 无障碍物改进RRT算法图1 无障碍下改进RRT算法与传统RRT算法对比

1.2 动态设置步长策略

在RRT算法中，步长是该算法寻找路径的一个重要因素，在无障碍物且步长允许的情况下，算法拥有搜索时间随步长的增大而减小的特性。

但如果周围环境有障碍物，该特性会大打折扣。此时步长选取太大容易陷入障碍物的狭窄范围，难以搜索到目标点和可行路径；步长选取太小会产生过多的冗余节点，降低搜索效率。较好的办法是根据周围环境和障碍物的分布状况动态设置步长，避免上述两种极端情况，更快地遍历障碍物较少的空间，减少搜索时间，快速寻找到可行路径。

本文所采用的步长调整策略：

(1)给定初始步长step，采样函数随机产生采样点，判断该采样点与最近的树节点之间障碍物的数量。

(2)若未存在障碍物，步长取初始值step来延伸新的树节点；否则取式(2)来快速延伸新的树节点。

step=step+(1-obs/S)*step

(2)

式中，step为初始步长；S为以距离随机点最近的树节点和目标点为对角线的矩形面积；obs为矩形中障碍物的数量。在搜索过程中，步长是随障碍物占矩形面积比而变化的，比值越大，代表矩形面积内障碍物越多，步长越小。相反则步长取值较大。能够快速搜索到目标点。在无障碍环境中，固定步长和动态步长的搜索路径对照如图2所示。

(a) 有障碍物传统RRT算法 (b) 动态设置步长后RRT算法图2 有障碍下改进RRT算法与传统RRT算法对比

可以清楚地看出在环境相同的情况下，动态设置步长后的RRT算法在搜索路径中产生的分支和冗余节点更少。

2 Dijkstra算法

Dijkstra算法经常使用在网络路由和地理信息系统中，用于解决非负权重图的单元最短路径问题。它是一种典型的基于贪心策略的最短路径规划算法，用于计算一个节点到其他所有节点的最短路径。

Dijkstra算法的核心思想是以起始点为原点,根据距起始点最短路程长度递增的次序对路径进行迭代,得到从起始点至目标点间的最短路径。Dijkstra算法的主要特点在于从开始点为中心向外层层扩展，直至延伸到达终点为止。

如图3所示，是Dijkstra算规划产生的路径。S代表起点，G代表终点，黑色物块表示障碍物。浅灰色区域表示搜索路径时经过的区域。深灰色区域表示搜索到的最优路径区域。Dijkstra在搜索最优路径时，需要对地图环境进行几何划分，从起点开始向周围所有节点扩展，直到扩展到目标点为止。Dijkstra所寻路径一定是最优的，但在搜索路径时需要扩展许多无用的节点。因此非常耗费资源，尤其在地图环境较大时，其搜索时间较RRT算法多出许多，搜索效率会变的非常低。

图3 Dijkstra算法产生路径

3 融合Dijkstra与RRT路径规划算法

基于上述两种算法的优缺点，本文将Dijstra算法和改进优化后的RRT算法进行融合，优势互取，从而能够寻找出一条可行的、距离更优的路径。为方便叙述，下文将该方法称为RRT-Dijkstra算法。

在使用改进后的RRT算法进行搜索后，形成一条初始可行路径。由RRT特性可知该路径并非最短，其周围区域存在比初始路径更短的路径。将初始路径的每个节点周围进行栅格化，形成路径的可行区域。在可行区域中使用Dijkstra算法搜索，重新规划出一条比初始路径更近的搜索路径。优化步骤如图4所示。

(a) 改进RRT算法 (b) 向周围扩展可行区域 (c) 优化后路径

表1 改进RRT算法与融合Dijkstra算法后路径对比

从表1中可得，两种算法融合后，路径从28.127 1减少到了23.970 6，优化了14.778%，路径拐点从15个减少到3个，优化了80%。既利用了改进后RRT算法搜索到可行路径的快速性，提升了搜索效率；也避免了Dijkstra扩展多余节点造成的资源浪费，优化了搜索路径。

虽然该路径长度上得到了优化，但是得到的搜索路径不够平滑，且移动机器人在拐点处无法迅速改变速度大小和方向，融合算法得到的路径不符合移动机器人的运动规律，也不能根据环境变化实现动态避障。因此，将融合后的算法与动态窗口法相结合，实现全局最优。

4 改进动态窗口算法

动态窗口算法是一种局部路径规划算法，该算法在速度空间内采样线速度和角速度，根据机器人的运动学模型预测下一时间间隔的轨迹。对预测内的轨迹进行方位角、与障碍物距离、速度大小等因素进行评分，从而获得更加安全平滑的局部路径。传统的动态窗口算法评价方式单一，评价权重固定不变，不能使机器人快速接近目标点，所以对动态窗口算法的评价函数的权重做动态调整。

4.1 机器人运动模型

机器人运动模型选择全向移动的，即可以向前和左右移动，也可以转动。机器人相邻时刻内(ms级)，运动距离短，可将相邻两点之间的运动轨迹看成直线。设机器人向前移动速度为vx，向左移动速度为vy，角速度为ω，机器人在Δt内的运动轨迹如式(3)所示。

(3)

根据该运动模型在速度空间重采集线速度和角速度，预测下一个时间段内机器人的运动轨迹。

4.2 机器人速度采样

考虑到机器人本身速度的范围和周围环境对机器人的限制，在速度采样时，对机器人的角速度和线速度进行限制。如式(4)所示。

Vm={(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(4)

考虑到机器人的最大力矩，即最大线加速度和最大角加速度会对机器人的线速度和角速度造成约束，因此其速度为：

(5)

为使机器人在碰到障碍物前可以停车，对机器人预留刹车距离，对机器人的速度加以限制。

(6)

式中，dist(v,ω)为在当前运行速度移动机器人的轨迹中与障碍物最短的距离。

4.3 改进评价函数

在机器人的某一时刻的速度空间确定以后，需要对速度空间内的速度产生的路径轨迹评价，评价函数为式(7)所示。

G(v,ω)=σ(αhead(v,ω)+βdist(v,ω)+γvel(v,ω))

(7)

式中，head(v,ω)为与目标节点间的方位角大小，其值越大，代表与目标节点方向上越靠近；dist(v,ω)为机器人与周围障碍物之间的距离，其值越大，代表与周围障碍物距离越远；vel(v,ω)为机器人的速度大小，其值越大，代表下一时刻运动距离越远；α、β、γ为系数，即评价权重。

式(7)中α与γ和β比例值固定，在机器人周围环境复杂时，无法自适应调整评价权重以快速到达目标点。为了使机器人能够更加具有目的性，对评价权重进行调整。当机器人距离目标点较远切周围障碍物较少时，提高方向和速度评价权重，当靠近障碍物时，提高机器人速度占比，使其快速绕过障碍物，改进后的评价函数为式(8)所示。

(8)

式中，R为障碍物半径。对dist(v,ω)进行限定。dist(v,ω)∈(0,2R)。

图5 传统动态窗口法路径 图6 改进动态窗口法路径

由表2可看出，虽然改进权重比后的动态窗口算法所形成的路径长度减少了0.46%，并无较大变化。但路径规划时间减少了19.02%，搜索效率得到了提升。

表2 传统动态窗口法与改进动态窗口法时间对比

5 全局与局部路径规划算法融合

传统动态窗口算法容易陷入局部最优，导致无法寻找到可行路径。改进后的RRT和Dijkstra的融合算法规划出的的全局路径不够平滑，拐点处角度发生突变，不符合机器人运动学规律，且无法动态避障，容易与周围障碍物发生碰撞。针对这两个问题，将改进RRT和Dijkstra融合后的算法规划的路径分段使用动态窗口算法。使用全局路径引导动态窗口算法进行规划，有效地避免了陷入局部最优，也使路径更加平滑，适合机器人运动。融合后的算法实现如图7所示。

图7 融合算法实现流程图

6 仿真与分析

为了验证算法融合后的高效性，在MATLAB搭建仿真平台，构建地图环境。黑色区域表示障碍物，机器人无法通行，白色区域表示正常环境，机器人可以通行。设置起始点(5，3)， 目标点(17，21)。设计两组试验进行验证。

实验一：验证改进后RRT算法与RRT-Dijkstra融合算法的效率和可行性。如图8～图13所示。6种算法各自执行50次，将得到的参数值取平均值，性能对比如表3所示。

图8 传统RRT算法路径 图9 优化改进RRT算法路径

图10 Dijkstra算法产生路径 图11 RRT_Dijkstra融合算法产生路径

图12 传统动态窗口算法 图13 RRT_Dijkstra和动态窗口融合算法

以上6种算法经过仿真得到的各自的规划时间、路径长度、拐点数和迭代步数如表3所示。

表3 6种算法对比

如表3所示，优化改进后的RRT算法在搜索效率和搜索时间上有很大优化，较传统RRT算法规划时间减少了0.614 8 s，拐点数减少了60.53%，迭代次数减少了71.91%。但路径长度不减反增，所以结合Dijkstra算法优化路径。

Dijkstra算法虽然在规划的路径长度上是最优的，但过于耗费系统资源和时间，其规划时间是传统RRT算法的6倍多。

RRT-Dijkstra算法较传统RRT算法路径缩短了19.25%，拐点减少了84.21%，迭代次数减少了71.91%，规划时间增加0.207 7 s。增加的规划时间在1 s之内，基本可以忽略。该算法在时间上保持了传统RRT算法搜索路径时的快速和高效，路径长度也得到了较大优化。

单独的动态窗口算法因为无路径和节点指引，没有达到目标点。

将RRT-Dijkstra规划的路径指引改进后的动态窗口算法，机器人能够有效地到达目标点，且路径更短，更加平滑和安全，符合机器人的运动规律。

实验二：在RRT-Dijkstra算法规划的路径中添加动态障碍物，动态障碍物为图14中的深灰色栅格，以验证融合算法是否能够实现动态避障。

图14 机器人整体运行轨迹 图15 机器人运行角速度

图16 机器人运行线速度 图17 机器人姿态变化

可以看出，在路径中加入动态障碍物后，移动机器人成功从起始点到达目标点，且能够很好的绕过障碍物，实现动态避障。

7 结论

在有障碍物和无障碍物的仿真环境中，将RRT-Dijkstra融合算法与传统RRT算法分别在规划时间、迭代步数、路径距离和拐点数进行对比，验证了RRT-Dijkstra融合算法的高效性。再结合改进后的动态窗口算法，使机器人能够实现动态避障，在RRT-Dijkstra融合算法规划出的全局路径中加入障碍物，机器人可以很好地避开，验证了融合动态窗口算法后的动态避障的功能。