优化改进RRT和人工势场法的路径规划算法

辛 鹏,王艳辉,+,刘晓立,马希青,徐 东

(1.河北工程大学 机械与装备工程学院河北省智能工业装备技术重点实验室,河北 邯郸 056038;2.河北工程大学 河北省高品质冷镦钢技术创新中心,河北 邯郸 056038)

0 引言

移动机器人可以完成自主移动,自行从当前位姿到目标位姿进行工作,在工业生产和日常生活中具有普遍的应用。路径规划是移动机器人开发的核心部分,即针对当前环境规划出从初始位姿到最终位姿的无碰撞路径[1-3]。目前,常用的路径规划算法为：A*算法[4]、动态窗口法[5]、蚁群算法[6]、遗传算法[7]等。

快速搜索随机树(Rapidly Random Tree,RRT)是非常经典的算法,其搜索的随机性同时适用于二维和三维空间。但是搜索过程中随机树向四周均匀扩散,因此随机性过大,特别是在狭窄通道寻找路径时,会消耗过多时间,搜索效率较低。目前针对RRT算法国内外已经有了大量的研究和探索。文献[8]为了提升双向RRT*算法的效率,提出将终点偏向的思想以及样条插值法融入到双向RRT*算法中,使搜索时更具有偏向性,搜索效率更高。并对规划好的路径剔除冗余节点,减少路径长度。文献[9]为了降低双向搜索随机树采样的随机性,设置了引力场引导算法向目标点搜索。为提升搜索效率,在起点和目标点之间加入扩展节点,由扩展节点分别向起点和目标点扩展,即将原来的两棵随机树扩展为4个。文献[10]为提高收敛速度并规划出更优的起始路径,增大了双向RRT*算法ChooseParent(Vnear,Vnew)和Rewire(Vnear,Vnew)中Vnew的搜索范围,并验证了算法满足概率完备性和渐进最优性。文献[11]针对RRT算法规划的路径冗余节点和路径拐点较多的问题,采取以概率P向目标点扩展随机树,并在扩展时增加角度约束,提高搜索效率。对生成的轨迹运用三次B样条曲线进行重新规划,相较于传统RRT有较大的提升。文献[12]为了满足智能车在复杂环境中的快速、平稳的行驶,提出同心圆RRT算法,该算法使用同心圆生成随机点,在选择邻近点时充分考虑到车辆的运动学情况和邻近点与目标点的距离,算法在实际环境中有很好的有效性和实用性。文献[13]针对RRT算法搜索盲目、容易陷入极小环境的缺点,在RRT算法中加入目标导向,并针对未知环境,无障碍环境,传感器能扫到边界和传感器不能扫到边界等几种情况提出不同的搜索策略,针对新环境高效地规划出路径。文献[14]为了使智能车在障碍物较多且分布不规则的环境中平稳移动,提出连续曲率RRT算法。在RRT算法扩展时考虑到周围环境和车辆自身影响,加入目标偏向和适当的度量函数及环境、车身的约束,极大地提升了算法规划效率。

以上文献对RRT算法在节点扩展和路径平滑性上做了大量优化。本文将改进RRT算法与改进动态窗口法融合。在全局中,融入人工势场法调整扩展方向,提取路径中关键节点,并进行重新规划,增加搜索效率,减少路径长度。在局部中,通过优化改进RRT算法引导动态窗口法,防止陷入局部最优。

1 改进RRT算法

RRT算法广泛用于二维和三维场景的路径规划中。以起始点为种子随机向四周进行生长,直到目标点也在随机树上或者距离随机树足够近,此时可以在随机树上找到从初始位置到目标位置的有效路径。RRT算法随机性较高,在扩展时,随机向四周扩散,搜索效率较低,且规划的路径中拐点和冗余路段较多,不利于机器人移动。因此,本文针对RRT提出以下方面的改进。

1.1 加入人工势场的思想

传统RRT算法在搜索路径中扩展方向随机,搜索时间较长,效率较低。因此,在RRT算法增加扩展点时加入人工势场分量,使得扩展时既有随机性,又考虑到障碍物以及目标点对当前节点扩展的影响。使随机树扩展时具有目的性,提高搜索效率。

人工势场的引力函数为：

(1)

式中：ε为引力系数,ρ(P,Pgoal)为当前点与目标点的间距。

斥力函数为：

(2)

式中：μ为斥力系数;ρ(P,Pobs)为当前点与障碍物点的间距;ρ0为障碍物影响范围,超过这个范围,障碍物不对当前节点产生斥力影响。

合势场为：

U=Uatt+Urep。

(3)

传统的人工势场法计算当前位置周围节点合势场,搜索范围过小,不利于对周围节点的判定。因此,采用扩展邻域思想,将搜索范围改为7×7搜索邻域[15],如图1所示。

图1 7×7搜索邻域人工势场

将人工势场思想加入到RRT算法中,在扩展时加入改进人工势场偏向。此时,扩展节点由人工势场法进行引导,在文献[16]的基础上加入障碍物的斥力势场,如图2所示。图中：P_rand为生成的随机节点,P_near为距离随机节点最近的点,P_test为向随机点扩展一个步长,P_test为改进人工势场法扩展出的节点,P_real为实际扩展节点。

图2 扩展时加入人工势场思想

将改进RRT算法与传统RRT算法分别在无障碍物以及有障碍环境下进行比对,验证改进的有效性。无障碍物环境下两种算法对比如图3所示。

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

当前环境中存在障碍物时,如果扩展点在障碍物上,则放弃该点,重新进行搜索。当有障碍物时,两种算法的对比如图4所示。

a 有障碍物传统RRT算法 b 有障碍物改进RRT算法图4 有障碍下改进RRT算法与传统RRT算法对比

在有障碍物和无障碍物环境下对两种算法进行比对,通过路径长度以及随机树扩展方向可以看出,改进RRT算法规划的路径更短且扩展时更具有偏向性。

1.2 路径优化

改进RRT算法规划的路径中依旧存在冗余路段,此时,对改进后的路径进行二次调节[17],提取关键节点,去除冗余节点,规划出新的路径。具体实现为：

(1)将所有节点按顺序放入集合{q1,q2,q3,…,qn}。

(2)从起始节点q1逐一连接集合中的节点,直至与qt+1节点之间的连线通过障碍物,qt为集合中的关键点。此时,从qt开始,依次与剩下的节点连接,直到找到所有的关键点。

(3)从起始点依次连接关键点和目标点,规划出新的路径,具体优化实现如图5所示。

a 改进RRT算法 b 提取关键转折点 c 优化后路径图5 优化步骤

a 传统动态窗口法 b 改进动态窗口法路径图6 改进动态窗口法与传统动态窗口法对比

由表1可知,优化后,路径长度减少了2.1%,路径中拐点减少了75%。改进后的算法规划的路径更加平滑,拐点更少,路径长度更短,适合全局规划,但是不适合机器人障碍物较多情况下移动且不能实现动态避障。此时应将优化改进算法加入改进动态窗口法,在局部优化机器人移动,使机器人移动实现全局最优。

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

2 改进动态窗口法

动态窗口法是模拟出速度空间中所有速度下的路径,在一定时间内对机器人的朝向、机器人移动速度以及障碍物的距离等进行评分并按照一定比例进行汇总,最终得到总分最高的路径即最佳路径。传统动态窗口法评价函数固定,因此并不能对当前位置距离目标点等情况进行很好的判断,不利于机器人快速向目标点靠近。

2.1 机器人运动学模型

假定机器人在极短时间内的移动为直线,运动中角速度为ω,线速度为v,则机器人在Δt内的运动可由公式推导出：

x=x+vxΔtcosθt-vyΔtsinθt;y=y+vxΔtsinθt+vyΔtcosθt;

θt=θt+ωΔt。

(4)

通过式(4)以及采集到的速度信息,可以预估出在下一时间段内机器人的轨迹。

2.2 速度采样

速度空间中存在大量的角速度与线速度。此时,考虑到机器人本身以及当前环境,应对速度进行约束,速度范围为：

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

(5)

电机力矩等因素会限制机器人的移动速度,在现实环境下,机器人的移动速度为：

(6)

为了使机器人在碰到障碍物之前能够及时停下,对机器人设置速度上限：

例如下面的句子中，①句是定语从句，因为that在从句中充当了句子的主语，有实际的意义，不能去掉，否则句子的结构不完整，这个句子中that与中心词news构成修饰与被修饰的关系；②句是同位语从句，因为该句子有完整的主语（The news）、谓语（is）等，所以该句子中的that从句可以去掉，不影响句子的完整性，这里的从句是主句的具体内容。

(7)

式中dist(v,ω)为在当前路径下机器人与障碍物的最短间距。

2.3 改进评价函数

速度空间中不同的速度预估出不同的轨迹,使用评价函数对预估的轨迹进行评分。传统的评价函数为：

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

(8)

式中：head(v,ω)为在当前速度下移动时移动机器人的朝向与实际期望的偏离,vel(v,ω)表示机器人当前移动的线速度和角速度,σ为平滑系数,α、β、γ为系数。

传统评价函数中,α、β和γ都是固定值,机器人在移动中不能很好地兼顾到与障碍物的距离和与目标点的距离。为了使机器人在运行中动态窗口法能够快速规划出有效路径,将固定比例改为动态比例。当机器人与目标节点和障碍物节点都间隔较大时,增加head(v,ω)比例系数,使得机器人移动时更偏向目标点。当距离障碍物和目标点较近时,增加vel(v,ω)比例,以便快速绕开障碍物到达目标点,改进后评价函数为：

(9)

式中：r表示障碍物的影响领域半径,h表示机器人当前节点与目标节点的间隔,R表示起始点与目标点的距离。为避免在周围无障碍物时,dist(v,ω)比例过大,将dist(v,ω)设置在一定范围内,即dist(v,ω)∈(0,2r)。

由表2可知,对比传统RRT算法,改进动态窗口法规划的路径长度减少了0.36%,由于地图较小,路径缩短的并不明显。规划时间减少了11.28%。改进后明显提高了规划效率,缩短了路径规划时间。

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

为了验证评价函数中不同系数对实验结果的影响,加入消融实验。实验结果如表3所示。

表3 改进评价函数消融实验

通过表3可以看出,通过调整vel(v,ω)、head(v,ω)比例,能够有效减少路径长度,提高搜索效率,但规划的路径距离障碍物更近,因此需要适当提高dist(v,ω)比例,使路径与障碍物保持一定距离,进而使得机器人在移动时能够更加安全地到达目标点。

3 融合算法

传统动态窗口法在规划路径中容易陷入局部最优,需要全局的路径规划算法引导。改进后的RRT算法虽然路径平滑,但不能实现动态避障,不适合机器人移动。因此,使用优化改进RRT算法进行全局规划,将全局规划的路径按照优化后的关键节点进行分段,在相邻节点间使用改进动态窗口法规划路径,如图7所示,全局的融合算法以此类推。

图7 融合算法示意图

图8 融合算法实现流程图

4 仿真验证与实物验证

4.1 仿真验证

为检验改进RRT算法的高效性以及融合算法的有效性,在MATLAB 2017中使用矩阵建立栅格图,黑色方格表示障碍物,白色方格表示无障碍物。起始节点(4,3),目标点(29,28)。使用两组实验检测和对比。

(1)实验一 优化改进 RRT算法与传统RRT、传统A*、改进RRT算法、动态窗口法以及融合算法的路径规划效果如图9所示。5种算法规划时间、路径长度以及拐点数对比如表3所示。

如表4所示,相较于传统RRT,优化改进RRT规划的路径长度减少了16.51%,搜索时间缩短66.95%,路径中拐点数量减少92%;相较于传统A*算法,优化改进RRT算法路径规划时间更短,路径长度更短且拐点数更少,路径规划效果更好。传统动态窗口法未能找到路径,融合算法在全局由优化改进RRT算法进行引导,能够找到全局最优路径,融合算法比优化改进RRT算法路径更短。

表4 5种算法对比

(2)实验二 在优化改进RRT算法在全局规划的轨迹中加入动态障碍物如图10中红色方格所示,以检验在规划好的路径中加入障碍物,移动机器人能否避开。

图10 在优化改进RRT算法路径中加入障碍物

在全局轨迹中添加动态障碍物,机器人项目标点移动以及避障状态如图11所示。机器人在移动中的实时线速度、角速度和位姿如图12所示。

a 移动机器人避开第一个障碍物 b 移动机器人避开第二个障碍物 c 机器人整体运行轨迹图11 机器人绕过障碍物

a 机器人运行线速度图 b 机器人运行角速度图 c 机器人姿态变化图12 机器人位移过程中速度和姿态变化

通过机器人的运行轨迹以及在运行中角速度、线速度以及姿态的变化可得出机器人从初始位置平稳到达目标位置,能够实现避障。

4.2 实物验证

为了验证算法在实际环境中有效性,使用的Hawkbot智能机器人操作系统(Robot Operating System,ROS)三轮小车如图13所示,实验环境如图14所示。

图13 Hawkbot移动机器人

图14 室内实际环境

由于室内环境场景较小,采用Gmapping算法构建室内环境,实际建图效果如图15所示。

图15 SLAM建图

为了更加清晰地看到机器人运行轨迹,使用程序Showpath记录机器人移动路线,并使用蓝色线条显示。

通过图16可看出,改进融合算法相较于传统RRT融合传统动态窗口法(Dynamic Window Approach,DWA)效果更好,路径更加平滑,在绕过障碍物后规划的路径更加偏向目标点。

a 传统RRT算法和传统DWA b 优化改进RRT和改进DWA图16 传统融合算法和改进融合算法对比

5 结束语

在无障碍物条件下,改进RRT算法搜索方向更偏向目标点,搜索效率更高。在有障碍物环境下,优化改进RRT算法在路径长度、路径规划时间和路径拐点等方面都要优于传统RRT算法和传统A*算法。将全局的优化改进RRT算法与局部的优化改进动态窗口法结合成为融合算法。通过Hawkbot移动机器人验证,融合算法路径更加平滑,规划路径更短,且能够实现避开障碍物。本文算法虽然对RRT算法进行改进,但扩展时仍有一定随机性,因此更适合在障碍物较少的环境中规划有效路径。未来将研究多机器人规划算法,推进多机器人在实际生活中的协同工作。