融合改进RRT和动态窗口法的路径规划算法

辛鹏，马希青

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

0 前言

移动机器人可以完成自主移动，自行从起始点到目标点完成相应工作，能够为工业生产和人们的生活提供较大的便利，因此，移动机器人具有较为广泛的研发前景。路径规划在开发中占有主导方向。如何高效地找到从起始点到终点的安全路径是路径规划的重点[1-3]。常见的规划算法有：遗传算法[4]、A*算法[5]、人工鱼群算法[6]、蚁群算法[7]等。

快速搜索随机树(Rapid-exploration Random Tree，RRT)是较为常见的一种路径规划算法，适用于二维和三维空间。但是由于搜索过程中随机树的扩展方向是全方位的，因此不能很好地向目标点靠近，搜索效率较低。目前，对于RRT算法已经有了较多的研发和探索。文献[8]提出将跳点思想融入到RRT*算法中，缩小搜索节点数量，提高搜查效率，并且在ROS中构建仿真环境，验证算法高效性。文献[9]使用双向RRT思想，并对改进算法生成的路径设置评价函数，从中选出最优路径，并对选出的路径平滑调整，通过仿真验证算法有效性。文献[10]针对随机树生长的随机性进行改进，引导它向目标点生长且能够根据周围环境做出调整，算法搜索具有很强的目标性和偏向性。文献[11]调整了搜索方向并约束搜索角度，对改进后规划的节点使用B样条优化。

针对RRT算法低效，规划的路径并不能实现路径最优且不适合机器人移动的问题，本文作者将改进RRT算法与动态窗口法融合。在全局路径中，调整扩展方向，提取路径中关键节点，并进行重新规划，提高搜索效率，减少路径长度。在局部路径中，通过改进RRT算法引导动态窗口法，防止陷入局部最优。

1 改进RRT算法

RRT算法可应用于二维与三维空间中。通过在地图中不断随机采样，增加树上的叶子从而逐渐扩展随机树，当树叶扩展到目标点或者树叶距离目标点在一定范围之内，可以在随机树中找到一条从起始到目标点的无碰撞路径。因为RRT算法扩展点是随机在地图上生成，因此在搜索路径中存在较大随机性，不能高效找到路径，并且路径中有大量的冗余路段和冗余节点。针对这些问题，提出以下两点改进。

1.1 增加评价函数

传统RRT算法在扩张时是随机的，会有很大的不确定性，因此，应对扩展方向进行限制。如式(1)所示：

(1)

其中：Ppoint为实际生成的随机点；S为以起点和目标点为对角线的矩形面积;Nobs为矩形中障碍物的数量;Prand为地图中的随机点；Pgoal为目标点。在搜索过程中，当起始点与目标点之间没有阻碍时能够快速搜索到目标点，无障碍时与传统RRT算法对照如图1所示。

图1 无障碍下改进RRT算法与传统RRT算法对比

当路径存在障碍物时，按照公式扩展随机点；当路径中障碍物过少时，路径偏向目标点，会存在找不到路径的情况，此时以当前点为起点，进行随机搜索；存在障碍物的情况下，改进算法与传统算法的对照如图2所示。

图2 有障碍下改进RRT算法与传统RRT算法对比

通过图2可以清晰地看出：在起点与终点相同的情况下，改进RRT算法生成的路径长度更短，随机树的分支更少，在搜索路径的过程中更有目的性。

1.2 路径优化

改进RRT算法规划的路径中仍存在大量的冗余节点及路段，在路径中提取关键节点，去除冗余节点，并重新规划路径。实现步骤为：

(1)将所有节点依次摆放进集合中{P1,P2,P3，…，Pn}。

(2)从起点开始依次直线连接路径中的节点，直到连线点Pt+1中存在障碍物，此时Pt为关键点，并以它为起始点，继续寻找下一个关键点，直到关键点与目标点连接中无障碍物。

(3)将目标点与起始点都放入关键节点中，使用关键节点重新规划出路径。优化步骤如图3所示。

图3 优化步骤

从表1中可得：优化后，路径长度从28.970 6减少到25.200 3，优化13.014%；路径拐点从15个减少到3个，优化了80%。但优化后路径不够平滑，不适合移动机器人的移动，且不能在局部路径中实现避障。因此，将全局算法与动态窗口法相结合，实现全局最优。

表1 改进RRT算法与优化后路径对比

2 改进动态窗口法

动态窗口法的思想在于对采集到的多组速度数据进行提取，并根据提取出的速度按照分辨率模拟出下一个时间节点内的所有路径，按照一定的评价体系对路径进行评分。传统动态窗口法评价函数固定且单一，不能使机器人快速向目标点移动，因此对动态窗口法的系数进行动态调整。

2.1 机器人运动学模型

机器人在地图中Δt时间内的运动一般分为圆弧和直线，为了方便计算，假定路径为直线。机器人移动中角速度为ω，线速度为v，机器人在Δt内的运行轨迹如式(2)所示：

(2)

根据在速度空间中采集到的线速度和角速度，预测在下一个时间段内机器人的运行轨迹。

2.2 速度采样

在速度空间存在大量速度数据，但考虑到机器人自身的组成以及环境对机器人的限制，因此对机器人的角速度和线速度进行限制，如式(3)：

vm={(v,ω)|v∈[vmin，vmax],

ω∈[ωmin，ωmax]}

(3)

机器人在运行中受到电机力矩等的限制，会对机器人的移动速度造成约束，因此在真实环境中的速度为

(4)

为使机器人在碰到障碍物之前停止，对机器人的移动速度设置上限：

(5)

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

2.3 改进评价函数

在采集的速度空间中存在多个数据，不同速度对应不同的轨迹，对轨迹按照标准进行评价，评价函数如式(6)所示:

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

(6)

其中：head(v,ω)表示当前移动下，移动机器人预估方向与目标方向的偏差；vel(v,ω)用来评价移动机器人当前移动速度；σ为平滑函数；α、β、γ为系数。

式(6)中α与γ和β比例值固定，机器人在移动时不能更具有目的性。此时对系数比例进行调整，使它在移动过程中动态改变:当距离目标点距离较远时，提高方向占比，快速向目标点行驶；当与障碍物接近时，提高速度占比，快速绕过阻碍。改进评价函数为式(7)：

[2-dist(v,ω)/2Rγ·vel(v,ω)+2β·dist(v,ω)]}

(7)

其中：R为障碍物半径。为了防止路径对无障碍物的判定占比过大，对dist(v,ω)进行限定，dist(v,ω)∈(0,2R)。

传统动态窗口法与改进动态窗口法的路径规划结果如图4—图5所示，对比结果如表2所示。可以看出：相较于传统算法，改进算法路径长度减少了0.462%，没有较大变化；路径规划时间缩短了19.021%，改进评价函数提升了搜索效率。

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

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

3 融合算法

传统动态窗口法在搜索过程中没有指引，容易陷进局部最优。改进的RRT算法规划的路径不够平滑，机器人在移动过程中容易与障碍物发生接触。

针对这2个问题，将改进RRT算法规划的路径分段使用动态窗口法。在整体路径中，使用改进RRT算法引导动态窗口法进行规划。在局部路径中，因为分成小段，因此很好地避免了路径陷进局部最优。融合算法的实现如图6所示。

图6 融合算法实现流程

4 仿真与分析

为了验证融合算法高效性，在MATLAB中构建栅格地图，黑色区域表示不可通行，白色区域表示可通行。起始点(4,3)，目标点(24,17)。设计2组实验进行验证。

实验一：验证优化改进RRT算法的搜索效率和融合算法的可行性，如图7—图10所示。4种算法性能对比如表3所示。

图7 传统RRT算法路径 图8 改进RRT算法路径

图9 传统动态窗口法路径 图10 融合算法路径

表3 4种算法对比

如表3所示：对比传统RRT算法，优化改进算法路径缩短了40.54%，规划时间减少59.68%，路径中拐点减少了86.36%；传统动态窗口法陷入局部最优未能找到路径，融合算法由改进RRT算法规划的路径指引，能够有效到达目标点，且融合算法规划的路径更短。

实验二：在优化改进RRT算法规划的路径中添加临时障碍物，临时障碍物为图11所示的空心栅格，以验证融合算法是否能够实现避障。

在原有路径中加入临时障碍物后，移动机器人的移动以及避障情况如图11—图13所示。移动中角速度、线速度和姿态如图14—图16。

图11 移动机器人避开第一个障碍物 图12 移动机器人避开第二个障碍物

图13 机器人整体运行轨迹

图14 机器人运行线速度 图15 机器人运行角速度

图16 机器人姿态变化

由图11—图13可知：在路径中加入临时障碍物后，移动机器人成功从起始点到达目标点，且能够很好地绕过临时障碍物，实现避障。

5 结论

在有障碍物与无障碍物的环境下分别将优化改进RRT算法与传统RRT算法进行比对，验证了改进算法的高效性。为了使算法能够实现避障，将优化改进RRT算法与改进动态窗口法相结合，经过实验验证算法的有效性，并且在规划的路径中加入临时障碍物时，移动机器人也能很好地避开。