基于改进RRT与DWA融合算法的路径规划

符 强,宁永科,纪元法,孙希延

(桂林电子科技大学信息与通信学院,广西 桂林 541004)

1 引言

移动机器人技术近几年来成为了研究的热点,它广泛应用于自动驾驶、医疗、运输、农业等领域[1-3],而且在疫情期间发挥了重大作用,而路径规划算法是研究机器人的重要关键技术之一。机器人路径规划指的是在指定的工作环境空间下,机器人能够寻找出一个从起始位置到目标位置的可到达的无碰撞路径[4,5]。路径规划算法是实现移动机器人寻找路径的重要前提,近年来,国内外学者也在不断的提出相关路径规划算法。

路径规划算法主要分为全局路径规划算法和局部路径规划算法[6]。全局路径规划算法主要包括以下几种:基于图搜索法、如栅格地图法、人工势场法、A*算法、D*算法等。基于仿生学的蚁群算法、遗传算法等。局部路径规划算法主要有动态窗口算法等。基于图搜索的全局路径规划算法虽然能够找到最优路径,但是随着环境的复杂程度增加,会导致算法的计算能力下降[7];基于仿生学的智能算法在搜索路径过程中,容易陷入局部最优。为了解决以上路径规划算法存在问题,有学者提出了基于随机采样的路径规划算法,如快速搜索随机树算法(RRT),概率路标算法(PRM)。

快速搜索随机树算法(RRT)指的是在工作环境空间内,通过对环境进行不断采样,得到采样节点[8],直到搜索到目标位置为止,该算法通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效的解决高维空间和复杂约束的路径规划问题,与其它算法相比,RRT算法可以应用在完整系统,也可以应用在非完整系统,解决了在复杂环境下的移动机器人路径规划难题。但是传统RRT算法存在随机采样特性,在狭小空间算法不容易收敛,且生成路径并不是最优[9]。针对以上问题,基于RRT的改进算法不断被提出。文献[10]提出了RRT-Connect算法,相比传统RRT算法,很大程度上提高了算法搜索效率,解决了狭窄环境问题,且降低了大量无用搜索节点。文献[11]提出了具有目标导向的RRT算法,该算法改变了RRT算法的节点扩展方式,使得新扩展节点向着目标位置前进,并对最终路径进行了优化,使得路径更加平滑。文献[12]提出了DRRT-Connect算法,该算法引入了自适应步长函数,加快了算法的搜索速度。文献[13]提出了RP-RRT*算法,该算法将人工势场思想引入到RRT算法的随机采样过程中,减少了算法的随机采样特性。文献[14]提出了一种改进RRT算法,主要是优化了随机采样节点方向的角度。

本文提出了一种改进融合算法,首先对RRT算法的随机采样进行优化,将随机采样变为智能采样,减少RRT算法的随机采样特性,其次利用可见图思想对生成路径进行优化,减少路径的无用节点,进一步缩短路径,最后融合动态窗口算法解决了RRT算法不能应用于动态环境问题。对改进前和改进后的RRT算法进行了仿真和实际环境测试。

2 改进RRT算法

2.1 传统RRT算法

RRT算法是Lavalle于1998年提出的一种基于快速搜索随机树路径规划方法[15],该算法主要利用在环境空间内进行随机采样,并进行无碰撞检查,直到搜索到目标点。RRT算法扩展节点过程如图1所示。

图1 RRT算法扩展节点过程示意图

若某个采样点达到设定目标点范围,就认为已经找到了目标位置,搜索过程结束,以此通过不断迭代找到可行路径。RRT算法整体流程如图2所示。

图2 RRT算法整体流程图

2.2 改进RRT算法

2.2.1 智能采样

为了进一步优化RRT算法,将RRT算法的随机采样改变为智能采样,即通过在障碍物顶点附近生成尽可能多的节点,达到优化算法随机采样的目的。

智能采样前一阶段与RRT算法采样完全一致,在后一阶段有所改进,具体思想为:在RRT算法随机采样基础上,一旦找到一条可行路径,智能采样将在障碍物附近寻找特殊的节点,然后以这些特殊节点设定大小的圆生成更多无碰撞的有效节点。这些圆形区域标志了障碍物顶点的位置,如图3(b)所示,相比图3(a),减少了算法采样随机性,提高搜索效率。与RRT算法相比,在更少的时间和迭代次数下寻找到最优路径。

图3 智能采样和优化路径示意图

2.2.2 路径优化

通过上面智能采样搜索到所有的可到达目标的节点,通过不断回溯,它们彼此连接形成路径,因为智能采样的圆内标记了障碍物的顶点信息,因此从目标位置开始,根据三角不等式原理c

图4 三角不等式原理示意图

因为RRT算法的采样点是在环境空间内随机采样,所以算法整体效率低,改进RRT算法加入了智能采样和路径优化过程,使得算法能够快速收敛到全局最优解,减少算法迭代次数,提高算法效率。改进RRT算法的整体流程如图5所示。

图5 改进RRT算法整体流程图

3 动态窗口算法

3.1 机器人运动模型

在本文中,移动机器人采用四轮差速小车模型,只能进行前向和旋转运动,假设俩相邻点之间在Δ(t)时间内做匀速直线运动,则机器人运动模型可以表示为

(1)

3.2 速度采样

动态窗口算法通过对速度空间(v,ω)进行采样,采样速度空间如图6所示,x轴代表角速度ω,y轴代表线速度v。在速度空间(v,ω)内,由于实际机器人自身约束和实际环境限制,存在vmax和ωmax,所以实际采样空间(v,ω)可以表示为

图6 DWA算法速度空间图

Vs={(v,ω)|vmin≤v≤vmax,ωmin≤ω≤ωmax}

(2)

另外,移动机器人受自身电机约束影响,限制移动机器人实际可到达的速度为

(3)

基于对机器人安全考虑,必须在碰到障碍物前的安全距离内将速度减为0,因此Va需满足

(4)

综上所述,实际速度采样的动态窗口Vr大小为

Vr=Vs∩Va∩Vd

(5)

假设动态窗口算法在前向轨迹预测的Δ(t)时间内假设速度不变,生成前向预测轨迹,如图7和图8所示。

图7 前向轨迹预测原理示意图

图8 前向轨迹预测过程和结果示意图

3.3 评价函数

在上面得到的若干轨迹中,通过设置合理的评价函数筛选出最优的轨迹。具体评价函数表达式为

G(v,w)=k[αHeading(v,w)+βGoal(v,w)

+γPath(v,w)+σOcc(v,w)]

(6)

式中,参数k用于路径平滑;α、β、γ、σ为评价函数加权系数;Path(v,w)用于计算轨迹偏离全局路径距离;Heading(v,w)用于计算方位角;Goal(v,w)用于计算轨迹对应目标点距离;Occ(v,w)用于计算轨迹距障碍物距离。

4 融合算法

在本文中移动机器人采用四驱移动机器小车,小车自身搭载激光雷达,利用激光雷达对工作环境空间进行不断扫描,构建出需要进行路径规划的二维栅格地图。得到全局地图后,首先利用本文改进RRT算法进行随机采样,得到初始可行路径,筛选出障碍物附近特殊点的集合,以此集合的所有点得到设定半径的圆形区域,通过回溯,得到初步路径,再通过三角不等式原理对路径进行优化,减少路径的拐点个数,最后融合动态窗口算法解决了动态环境下路径规划问题,提高算法整体的实时动态性能。

在二维栅格地图中得到全局路径,机器人根据当前时刻的位置、线速度和角速度,在采样周期内进行前向轨迹预测,得到所有的轨迹集合,丢弃所有的不满足动态窗口内的速度和与障碍物发生碰撞的轨迹,利用评价函数在剩下的所有轨迹中选出最优轨迹并将轨迹对应速度信息发送给机器人移动底盘,从而控制移动机器人移动,安全到达目标位置。完整的融合算法流程如图9所示。

5 实验与分析

5.1 改进RRT算法仿真

仿真环境采用测试机器配置为intel(R) CPU2.2GHz/8G,搭载系统为Ubuntu18.04系统,实验地图环境选择矩形栅格地图,分辨率为1m*1m,整个地图大小为50m*30m,其中,给地图中设置一部分障碍物,能更好的测试算法性能。为了测试本文改进RRT算法的有效性,将传统RRT算法,RRT-Star算法,双向RRT(RRT-connect)算法与本文改进RRT算法在相同测试环境下进行对比。本文构建了三种不同的尺寸大小相同地图环境,地图环境中白色区域为无障碍物可行区域,灰色代表了障碍物所处区域。

本文将改进RRT算法与传统RRT算法,RRT-Star算法, 双向RRT(RRT-connect)算法进行仿真对比,所有算法在地图中都设置相同的起点位置坐标为(2,2),目标位置坐标为(49,24)。为了保证所有的算法都能收敛且搜索到较优路径,在本文规格为50m×30m地图中,设置迭代次数为10000次,能够满足实验需求。仿真地图大小不仅仅局限于本实验设置大小,迭代次数应该根据实验地图大小合理设置,保证算法能够搜索到较优路径。另外,由于RRT算法在搜索路径过程中存在随机采样特性,路径结果并不唯一,本文选择其中一个路径规划结果作为仿真结果进行对比,如图10-12所示。

图10 环境1 RRT、RRT-Star、RRT-connect、改进RRT算法仿真图

图11 环境2 RRT、RRT-Star、RRT-connect、改进RRT算法仿真图

图12 环境3 RRT、RRT-Star、RRT-connect、改进RRT算法仿真图

从仿真图中可以看出,三种地图环境的障碍物逐渐增多,环境复杂度逐渐增加,能更好的测试改进RRT算法相比RRT、RRT-Star和RRT-connect算法的优越性。因为RRT算法在搜索路径过程中是进行随机采样的,所以在实验过程中,对三种算法分别进行10次试验,取平均值得到表1数据。

表1 RRT、RRT-Star、RRT-connect、改进RRT算法仿真数据结果

环境算法搜索时间/s路径节点数迭代次数路径长度/m环境1RRT0.6413478066.71RRT-Star1.1747950055.32RRT-connect0.287720059.25改进RRT0.88380051.48环境2RRT0.3412348261.15RRT-Star1.2837950052.86RRT-connect0.196720051.89改进RRT0.84280051.89环境3RRT1.24160233279.65RRT-Star1.7572950061.12RRT-connect0.378840067.36改进RRT0.86980056.35

从仿真结果、表1和图13试验数据结果可以看出,三种算法均能搜索到可行路径,其中,RRT算法虽然搜索路径时间最短,但是路径结果曲折,路径包含节点数较多,且不是最优,不适合实际移动机器人执行;RRT-Star算法相比RRT算法在搜索路径方面花费时间较长,这是因为RRT-Star算法在RRT算法搜索到可行路径的基础上,加入了重新选择父节点和重布线过程,算法进行不断迭代,用来优化路径,虽然最终路径结果比较平滑,但是花费时间较长,迭代次数太多;本文改进RRT算法在RRT算法的基础上,加入了智能采样,降低了算法的随机采样性,算法在迭代次数很少的情况下就达到收敛。然后利用三角不等式思想,通过回溯提取关键点,优化路径。改进算法在搜索时间上在可接纳范围内,由于只提取关键点,路径节点数大大降低,路径长度得到进一步优化,从而路径更加平滑,有利于实际移动机器人执行。

图13 不同环境下算法数据测试结果比较

5.2 改进RRT算法实际环境测试

在本文中实际测试机器选择移动机器小车,装载有激光雷达,用来获取实际环境二维栅格地图,如图14所示。

图14 环境地图

上位机选择Jetson Nano,系统为Ubuntu18.04,路径规划结果显示工具选择ROS中的RVIZ可视化工具,小车控制系统选择STM32开发控制板,实际测试小车如图15所示。在本文中,测试环境选择室内办公室环境,得到全局地图后,根据改进RRT算法获得全局规划路径,然后融合动态窗口算法,解决了动态环境下路径规划问题。

图15 实验测试小车

在实际测试过程中,所有算法都采用相同的参数,移动小车最大线速度和角速度分别为0.2m/s和0.5rad/s,采样个数设置为10个,采样间隔T=0.2s,评价函数加权系数α=0.1、β=10.0、γ=0.1、σ=0.2。图中绿色线为路径规划结果。

首先在无障碍物环境下对RRT和改进RRT算法进行测试,测试结果如图16所示。

图16 无障碍物环境测试结果

然后在全局地图中加入部分障碍物,融合DWA算法对RRT和改进RRT算法进行测试,测试结果如图17-20所示。

图17 有障碍物环境1测试结果

图18 有障碍物环境1小车状态输出结果

图19 有障碍物环境2测试结果

图20 有障碍物环境2小车状态输出结果

由于在测试过程中,使用的建图算法并不是很精确,存在建图误差,此外,受机器人自身电机控制偏差等因素,使得实际测试结果与实验仿真结果往往不能保持一致。从实际测试结果可以看出,在无障碍物环境下改进RRT算法相比RRT算法路径笔直,达到最优。随着障碍物的不断增多,环境复杂度增大,虽然路径结果有所曲折,但从最终路径规划结果来看,本文改进融合算法效果较好,能够解决实际动态环境下路径规划问题,可以满足实际需求。

6 总结

本文针对传统的RRT算法搜索效率低、算法的随机采样特性导致规划路径时间长、路径曲折且不适用于动态环境等问题,提出了一种改进融合算法,具体方式是对传统的RRT算法加入智能采样和路径优化。首先根据传统RRT算法规划出可行路径,找出路径上包含障碍物顶点附近的节点,在该节点处以一定大小的圆内进行采样,减少了算法的随机采样性,然后根据三角不等式思想,通过回溯对路径进行优化,使得路径更加平滑,距离更短,最后,结合动态窗口算法(DWA),解决了动态环境下路径规划问题。最后通过仿真和实际环境测试验证了本文改进融合算法的可行性。