基于改进RRT*的移动机器人运动规划算法

2017-05-25潘思宇徐向荣

山西大学学报（自然科学版） 2017年2期

关键词：最优性移动机器人障碍物

潘思宇,徐向荣

(安徽工业大学机械工程学院,安徽马鞍山 243002)

基于改进RRT*的移动机器人运动规划算法

潘思宇,徐向荣*

(安徽工业大学机械工程学院,安徽马鞍山 243002)

RRT*(快速搜索随机树)算法在以往研究中存在收敛速度慢、结果不稳定的缺点。针对此问题,文章在现有RRT*基础之上提出一种新型改进算法。该改进算法结合环境约束、车辆自身约束和运动学约束,舍弃原算法贪心思想并引入启发式采样节点插入算法,提高路径规划的速度和质量;接着对改进算法进行理论分析,证明算法具有概率完整性、渐近最优性,从理论上保证算法能快速收敛到最优路径。通过各种仿真环境的测试,验证改进算法的有效性、稳定性和正确性,也验证理论分析的正确性。

快速搜索随机树;路径规划;移动机器人;机器人操作系统

0 引言

近年来智能移动机器人在工业、农业、航空航天及空间探索等方面起到了重要的作用,因而成为学术界研究和关注的热点问题。移动机器人在任务空间自由活动的过程中,首先需要解决的问题,就是路径规划问题。路径规划是指移动机器人按照某些性质指标(如最短路径,时间最短或者代价最少等)的要求,规划出一条从起始点到目标点位置的最优或是次优的无碰撞路径,并指示移动机器人按照该路径行驶[1]。而解决这些问题需要考虑的因素有:任务空间的复杂性和不确定性,规划算法的有效性、最优性和实时性以及满足移动机器人本体的运动学和动力学特性。

在过去的一段时间里,为了解决这些问题,国内外学者进行了大量的研究,不断地进行探索,提出了很多路径规划的理论和方法。传统的路径规划算法如蚁群算法、遗传算法、人工势场算法等,这些算法在处理简单的规划问题有一定的优越性,但是在复杂环境下和高维空间中算法的复杂的会急剧增加,导致收敛时间长、求解困难[2]。此外,基于势场或启发函数的算法,如 A*、D*和人工势场法等,在处理规划问题时虽然能满足最优性和实时性的要求,但是因其并未考虑移动机器人本体的运动学和动力学限制,使得规划的路径不一定能被移动机器人所执行[3]。

其他改进算法需要在一个确定性空间中对障碍物进行确定的建模,在高维空间中容易引发维度灾难。为了解决高维规划问题,基于采样的算法因此被引入[4],相比于其他先进算法，其主要优势就是避免构建显式的任务空间,且其已被证明是有效解决路径规划问题的方案[5]，其中,最著名的基于采样的算法有PRM和RRT,但当存在未知障碍物时PRM算法的效率是极其低下的[6]。RRT算法收敛速度快,同时能应用在存在未知障碍物的环境中[7],但是,RRT算法中仍然有一些缺点[8-10]:(1)因其采用的是全局均匀随机策略,导致其路径不稳定,规划出的路径未必是最优路径,而且无谓地消耗大量资源,收敛速度缓慢;(2)最近节点选择算法在解决复杂问题时可能会导致算法失效;(3)因算法的随机性生成的路径过于粗糙,不便于移动机器人实施;(4)因算法缺少学习能力,在扩展过程中选择的节点可能会产生使路径发生碰撞导致扩展失败的情况,然而在后续的扩展过程中,当遇到同一区域选择随机目标点是,这些会发生碰撞的节点和处在障碍物区域无法实化的节点将再次被选取,产生重复而不必要的运算,影响整体算法效率。

由于这些不足的存在,国内外的学者对此展开了研究,许多RRT的改进算法被提出,以适应不同的应用场景。为了提高节点的扩展效率,2000年Kuffner和LaValle提出了RRT-connect[11],次年,他们提出了双向搜索树(Bidirectional-RRT),从起始点和目标点同时出发并行生成两棵RRT,直至两棵树相遇,加速算法收敛[12]。C.Urmson和R.Simmons(2003)提出了一种启发式的偏向算法,该算法引导RRT树向目标区域生长,使得RRT算法能得到一个低时间成本的解决方案[13]。 D.Ferguson和A.Stentz(2006)考虑多次运行RRT算法以逐步提高解决方案的质量,即使不能保证收敛到最优的解决方案,也能保证算法每次运行的结果是一个比较小的路径代价的路径[14]。Karaman和Frazzoli(2010)首次提出RRT*算法,用来改进由基本RRT算法产生的并非概率最优解的问题[15]。M.Jordan和A.Perez(2013)提出了B-RRT*,用一个轻微变异的贪心RRT-connect作为启发函数来连接两颗随机树[16]。 A.H. Qureshi和 S. Mumtaz(2014)提出了TG-RRT*,利用三角几何来选取节点,以降低得到最优解所需要的迭代次数,从而使得算法快速收敛[17]。Ahmed Hussain Qureshi, Yasar Ayaz(2015)提出了IB-RRT*,利用双向树的方法,通过智能样本插入的启发函数使得算法快速收敛到最优路径[5],C. Wouter Bac和Tim Roorda(2016)提出利用RRT算法在ROS平台上做移动机器人的路径规划研究,并应用于在密集障碍物环境中运动规划问题中[18]。

本文在这些问题的基础上提出了一个新的算法,采用智能随机抽样的方法以降低得到最优解所需要的迭代次数,从而使得算法快速收敛,同时引入了惩罚函数来记忆所选取过的节点以提高算法的效率,同时考虑移动机器人所受到的运动学和动力学限制,使得生成的轨迹可以直接被实际机器人所直接实现。通过仿真实验,证实了该算法的有效性、最优性和实时性。

1 移动机器人的非完整性约束模型

Fig.1 Geometrics of mobile robot图1 移动机器人几何示意图

非完整性约束是指含有系统广义坐标导数且不可积分的约束[19],移动机器人是一个典型的非完整约束系统。而本文要研究的移动机器人模型,是一种典型的非完整性约束的系统。

移动机器人在任务空间中状态变量为(x,y,θ,v,φ),其中(x,y)为移动机器人后轮轴的中心在系统坐标系下的坐标,θ为移动机器人前进方向与x轴的夹角,φ为移动机器人前轮方向与前进方向之间的夹角,v为移动机器人的速度,由于受到非完整性约束,所以车轮与地面是点接触且在接触点只有纯滚动没有相对的滑动。由分析可知:

(1)

从而可得移动机器人所受到的约束方程:

(2)

其中,移动机器人的控制变量为u0(加速度)和u1(车轮的角速度)。移动机器人的最小转弯半径(最大曲率):

(3)

除了上面的约束条件之外,还有:

(4)

由此可知,没有考虑机器人的非完整性约束的规划算法,它们不考虑车辆约束,规划出来的路径也不能用于实际的路径规划当中[20]。

2 规划算法的描述与实现

2.1 RRT*算法

RRT*通过在新的节点附近建立周围节点分布图来改进现有搜索树,然后遍历周围的节点检查是否存在新的路径比现有的路径的代价更低,如果存在则将现有的路径换掉。通过这种方式RRT*算法可以保证最终所确认的路径是渐进最优解。下面是算法的基本流程。

ALGORITHM:RRT*

1.V←{xinit};E←φ;T←(V,E);

2.fori←0toNdo

3.xrand←Sample(i)

4.Xnear←NearVertices(xrand,T)

5.ifXnear=φthen

6.Xnear←NearVertex(xrand,T)

7.Ls←GetSortedList(xrand,Xnear)

8.xmin←ChooseBestParent(Ls)

9.ifxmin≠φthen

10.T←InsertVertex(xrand,xmin,T)

11.T←REWIREVERTICES(xrand,Ls,E)

12.returnT=(V,E)

由ALGORITHM:RRT*所示,RRT*算法以起始点xinit作为随机搜索树的根节点,不断的从给定无障碍空间Xfree中通过随机采样Sample函数得到xrand,然后利用NearVertices函数得出邻近节点集合Xnear。邻近节点集合的定义是:给定一个样本节点,搜索树T=(V,E),一个球型空间Βxrand,r以xrand为球心以r为半径:

Near(xrand,T,r):={v∈V:v∈Βxrand,r}|→Xnear⊆V，

如果邻近节点集合Xnear是空集,则由NearestVertex函数计算出最近点xnear补充到邻近节点集合Xnear中,即邻近节点集合Xnear中只有一个元素xnear。邻近节点集合Xnear中的元素在GetSortedList函数的作用下按照代价函数c(σ)升序排列生成一个列表Ls,列表Ls每一个元素都是由三个变量参数(x′,c(σ),σ′)所组成,ChooseBestParent函数遍历列表Ls,从中选出最优父节点xmin∈Xnear,使在不碰障碍物的前提下,xinit通过xmin到xrand路径代价最小。InsertVertex函数将xmin插入搜索树T中,REWIREVERTICES函数进行随机搜索树T重组,检查每一个节点,如存在x′∈Xnear从起始点xinit通过x′到xrand的路径代价小于现存的路径代价,且是无障碍路径,就将现存路径换成新路径,如条件不满足,遍历搜索树T检查Xnear中的每一个节点。依次重复上述过程直到搜到一条从起始点到目标点的无碰撞路径。

RRT*算法虽然提供了渐近最优性的保证,但是也存在着一些缺点:(1)虽然能实现最优解收敛,但实现最优解收敛速度缓慢;(2)由于使用大量的迭代在计算最优路径,所以需要大量的资源来求解;(3)RRT*算法舍去的随即采样点,虽然无法直接连接到搜索树现存的节点上,但是其可能在目标点附近,并且能加快实现最优解,而这样的节点不应该被舍去。

2.2 改进的RRT*算法(即IB-RRT*)

ALGORITHM:IB-RRT*

3.σf←∞;E←φ;

4.Connection←True

5.fori←0toNdo

6.xrand←Sample(i)

10.Connection←False

14.if(flag)then

15.Ta←InsertVertex(xrand,xmin,Ta)

16.Ta←RewireVertex(xrand,xmin,Ta)

17.else

18.Tb←InsertVertex(xrand,xmin,Tb)

19.Tb←RewireVertex(xrand,xmin,Tb)

20.E←Ea∪Eb

21.V←Va∪Vb

22.return({Ta,Tb}=V,E)

(5)

(6)

3 对改进算法的理论分析

在任意的状态空间中,一个算法能收敛到可行解就称该算法具有概率完备性,能在有限次迭代中找到最优解就称该算法具有渐进最有性。因改进算法是一种新的算法,需要对其进行收敛性验证,证明其具有收敛性且能收敛到最优路径。

3.1 概率完备性

概率完备性的定义是:如果一个算法就有概率完备性,则对于任何一个存在可行解的路径规划问题(Xfree,xinit,Xgoal)就有

(7)

对IB-RRT*算法分析,可得对于任何一个存在可行解的路径规划问题(Xfree,xinit,Xgoal),始终存在一个常数a>0,n0∈Ν,且两者只取决于Xfree和Xgoal,

∀n>n0.

(8)

3.2 渐近最优性

渐近最优性的定义是:如果一个算法具有渐近最优性,那么对于任何路径规划问题(Xfree,xinit,Xgoal)都有最优解的存在且其代价函数c:∑→R≥0会等于一个有限的路径代价c*,即:

(9)

σ*表示一个最优路径,定义δ:=min{δ,4rn},其中rn为IB-RRT*算法的链接半径。

对于n∈N,构造一个序列{Bn}n∈N的球覆盖住σn记为Bn={Bn,1,Bn,2,…,Bn,Mn}:=coveringBalls(σn,rn,2rn),其中

(10)

(θn)>n}).

(11)

(12)

(13)

这个概率可以按照如下的方式来计算,从均匀分布的次序统计来说,最小αlogn采样点均匀分且独立的分布在[0,1],其概率分布函数为:

(14)

其中,Beta(·,·)是Beta函数,最大αlogn采样点均匀分且独立的分布在[0,1],其累积分布函数是:Fmax(x)=xαlogn.

(15)

其中Gamma(·)是gamma函数,整合上式可得:

(16)

(17)

然后,

(18)

那么事件An发生的概率:

(19)

Bn中含有球的个数可以表示为:

(20)

其中,结合上面的不等式可以得到:

(21)

3.3 快速收敛到最优路径

4 实验与仿真

4.1 算法仿真实验

将进行仿真实验来验证算法的有效性,并作为上一节中提出的概率完备性,渐近最优性提供一个实验的验证。通过对典型障碍物的规划实验,对比原有的RRT[21]、RRT*[15]、B-RRT*[16]算法和本文提出的IB-RRT*的运算速度、运算效率、迭代次数、路径代价和路径规划的成功率。每一个算法对应于每一个障碍物地图时都将运行程序50次,并记录以上参数,所有实验均在一个有着4 GB内存的Corei5的处理器,主频为2.4 MHz的电脑上运行得出的。

4.1.1 二维环境中的仿真实验

在二维环境的仿真中,选用一些比较有代表性的能够验证算法性能的障碍物地图(图2)。

Fig.2 ALG performance in 2-D environment图2 算法在二维环境中的规划

其中,图2(a)-(d)为四种规划算法在三种不同的规划问题中求解的结果。

4.1.2 三维环境中的仿真实验

在三维环境模型的仿真试验中,采用了三种不同的障碍物环境对算法进行仿真实验,以探究算法的性能。

表1 计算最优路径的实验结果

Fig.3 ALG performance in 3-D environment图3 算法在三维环境中的规划(左为随机障碍物地图,中为复杂障碍物地图,右为窄道障碍物地图)

从图3和表1中可以看出来对于同一个路径规划问题,经过大量的迭代后,IB-RRT*在任务空间中搜索到的路径和RRT*、B-RRT*有许多相似的地方,这也间接的证明了这三种算法都是渐近最优的算法,最终都能收敛到最优解。但从表1中的数据可以看出IB-RRT*在搜索速度和搜索效率上均有显著提高,在三维随机障碍物地图环境中,IB-RRT*的平均迭代次数是21 290次,而B-RRT*的平均迭代次数是36 128次,是IB-RRT*的1.69倍,而RRT*的平均迭代次数是108 965次,是IB-RRT*的5.12倍,搜索用时也从RRT*的18.8 s,B-RRT*的7.4 s降低到IB-RRT*的5.3 s;在三维复杂障碍物地图环境中,IB-RRT*的平均迭代次数是111 139次,而B-RRT*的平均迭代次数是498 972次,是IB-RRT*的4.49倍,而RRT*的平均迭代次数是1 437 342次,是IB-RRT*的12.93倍,搜索用时也从RRT*的244.1 s,B-RRT*的105.1 s降低到IB-RRT*的26.5 s;在三维窄道障碍物地图环境中,IB-RRT*的平均迭代次数是134 421次,而B-RRT*的平均迭代次数是551 771次,是IB-RRT*的4.10倍,而B-RRT*的平均迭代次数是1 290 674次,是IB-RRT*的9.61倍,搜索用时也从RRT*的218.5 s,B-RRT*的115.9 s降低到IB-RRT*的31.4 s。由此我们可以得知,IB-RRT*在搜索速度方面远快于B-RRT*和RRT*,且其搜索路径成功的概率也大幅提高,算法的稳定性增强,验证了前文的理论证明是正确。同时搜索的路径也相对其他两种种算法更为平缓,并且最终生成的可执行路径曲率也是连续变化的,满足了车辆的运动约束要求。

4.2 基于ROS的移动机器人仿真实验

移动机器人模型如图4所示,底面直径为0.25 m,高度为0.3 m。环境模型如图5a所示,每个大房间墙壁长度和宽度均为5 m,高度为2.5 m,厚度为0.2 m;小房间墙壁长度和宽度均为3 m,高度为2.5 m,厚度为0.2 m;四个门洞高度为2 m,宽度为1 m。模型构建完成后,移动机器人调用ROS中的gmapping功能包完成SLAM,实现场景地图的构建,最后获得室内场景的地图(图6)。

Fig.4 Mobile robot图4 移动机器人

a) Gazebo 下的SLAM过程 b)RVIZ下的SLAM的过程Fig.5 The process of SLAM图5 SLAM过程

Fig.6 Map information in RVIZ图6 RVIZ下显示的场景地图信息

这些文件配置好了之后,便可以利用改进的RRT算法对移动机器人进行路径规划,在地图上随机选择一目标点,并选定位置和姿态,改进的RRT算法便会生成一条无障碍的路径,如图7所示，绿色的小轨迹为算法规划出来的轨迹。

Fig.7 Process of planning图7 规划过程

图7给出了IB-RRT*动态规划过程。IB-RRT*采用的是实时重规划,Laserscan-nodelet-manager节点实时感知外界地图环境消息,将移动机器人附近的障碍物信息以scan的主题发布出去,IB-RRT*算法规划器接收该消息,并检测有没有新的障碍物,如果有新的障碍物IB-RRT*则进行实时重规划,以避开障碍物,图中的彩色区域为移动机器人感知到的周围障碍物的信息,绿色的线为初始规划出来的路径,红色的线为实时重规划的路径。因此,该算法也可以适用于动态环境的路径规划问题。

5 结论

以往的规划算法中基本没有考虑到移动机器人自身的约束,可能会导致虽然已经规划出路径,但无法被机器人所执行。本文提出了一种新的RRT算法,它舍弃原算法贪心思想并引入启发式采样节点插入算法,同时从起始点和目标点开始扩展搜索树,使得算法的收敛速度远快于改进之前;并进行理论分析证明改进算法具有概率完备性、渐近最优性,且确实优于之前的算法。通过改进算法与各种算法在不同的障碍物环境中进行反复的仿真实验,仿真的结果也说明了有效快速的解决复杂环境下的路径规划问题。但是该算法只能解决低速环境下的路径规划问题,下一步将对受滑动干扰的移动机器人模型的规划问题及高速运动的规划问题,进一步的提高算法的实用性和鲁棒性。

[1] 李磊,叶涛,谭民,等.移动机器人技术研究现状与未来[J].机器人,2002,24(5):475-480.DOI:10.1397 3/j. cnki.robot.2002.05.020.

[2] 王勇,蔡自兴,周育人.肖赤心约束优化进化算法[J].软件学报,2009,20(1):11-29.DOI:10.3724/SP.J.1001.20 09. 03363.

[3] Song J Z,Dai B,Shan E Z,etal.An Improved RRT Path Planning Algorithm[J].ActaElectronicaSinica,2010,38(B02):225-228.

[4] Elbanhawi M,Simic M.Sampling-based Robot Motion Planning:A Review[J].IEEEAccess,2014,2(1):56-77.DOI:10.1109/ACCESS.2014.2302442.

[5] Qureshi A H,Ayaz Y.Intelligent Bidirectional Rapidly-exploring Random Trees for Optimal Motion Planning in Complex Cluttered Environments[J].RoboticsandAutonomousSystems,2015,68:1-11.DOI: 10.1016/j.robot. 2015.02.007.

[6] Karaman S,Frazzoli E.Sampling-based Algorithms for Optimal Motion Planning[J].TheInternationalJournalofRoboticsResearch,2011,30(7):846-894.DOI:10.1177/0278364911406761.

[7] Park J H,Tai Y W.A Simulation Based Method for Vehicle Motion Prediction[J].ComputerVisionandImageUnderstanding,2015,136:79-91.DOI:10.1016/j.cviu.2015.03.004.

[8] Vonásek V,Saska M,Winkler L,etal.High-level Motion Planning for CPG-driven Modular Robots[J].RoboticsandAutonomousSystems,2015,68:116-128.DOI:10.1016/j.robot.2015.01.006.

[9] Doshi A A,Postula A J,Fletcher A,etal.Development of Micro-UAV with Integrated Motion Planning for Open-cut Mining Surveillance[J].MicroprocessorsandMicrosystems,2015,39(8):829-835.DOI:10.1016/j.micpro. 2015.07.008.

[10] Park K J,Won M.People Tracking and Accompanying Algorithm for Mobile Robot Using Kinect Sensor and Extended Kalman Filter[J].TransactionsoftheKoreanSocietyofMechanicalEngineersA,2014,38(4):345-354.DOI:10.3795/KSME-A.2014.38.4.345.

[11] Kuffner J,RRT-Connect S L V.An Efficient Approach to Single-query Path Planning ieee International Conference on Robotics and Automation[J].SanFrancisco,2000,2:473-479.DOI:10.1109/ROBOT.2000.844730.

[12] LaValle S M,Kuffner J J.Randomized Kinodynamic Planning[J].TheInternationalJournalofRoboticsResearch,2001,20(5):378-400.DOI:10.1177/02783640122067453.

[13] Urmson C,Simmons R G.Approaches for Heuristically Biasing RRT Growth[C]∥IROS，2003,2:1178-1183.DOI:10.1109/IROS.2003.1248805.

[14] Ferguson D,Stentz A.Anytime Rrts[C]∥2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.IEEE,2006:5369-5375.DOI:10.1109/IROS.2006.282100.

[15] Karaman S,Frazzoli E.Incremental Sampling-based Algorithms for Optimal Motion Planning[J].RoboticsScienceandSystemsVI,2010,104.DOI:10.15607/rss.2010.vi.034.

[16] Jordan M,Perez A.Optimal Bidirectional Rapidly-exploring Random Trees,MIT-CSAIL-TR-2013-021[R].Cambridge,UK:Computer Science and Artificial Intelligence Laboratory,2013.

[17] Qureshi A H,Mumtaz S,Iqbal K F,etal.Triangular Geometry based Optimal Motion Planning using RRT*-motion Planner[C]∥2014 IEEE 13th International Workshop on Advanced Motion Control (AMC).IEEE,2014:380-385.DOI:10.1109/AMC.2014.6823312.

[18] Bac C W,Roorda T,Reshef R,etal.Analysis of a Motion Planning Problem for Sweet-pepper Harvesting in a Dense Obstacle Environment[J].BiosystemsEngineering,2015,146:85-97.DOI:10.1016/j.Biosystemseng. 2015.07.004.

[19] 徐娜,陈雄,孔庆生,等.非完整约束下的机器人运动规划算法[J].机器人,2011,33(6):666-672.DOI:10.3724/SP.J.1218.2011.00666.

[20] 杜明博,梅涛,陈佳佳,等.复杂环境下基于RRT的智能车辆运动规划算法[J].机器人,2015,37(4):443-450.DOI:10.13973/j.cnki.robot.2015.0443.

[21] LaValle S M.Rapidly-exploring Random Trees:A New Tool Path Planning[R].Ames,USA:Iowa State University,1998.

Improved RRT*-Based Motion Planning Algorithm for Mobile Robot

PAN Siyu,XU Xiangrong*

(School of Mechanical Engineering,Anhui University of Technology,Ma’anshan 243002,China)

RRT*(rapidly-exploring random tree) has the disadvantages of instability and slow convergence rate in previous studies. To solve this problem, a new RRT*-based algorithm, IB-RRT algorithm, is proposed. This algorithm combines the environmental constraints, the constraints of intelligent vehicle and kinematics constraints, discards the bias in the original algorithm and sample insertion heuristic algorithm is introduced to increase the planning speed and quality greatly.Moreover,through theoretical analysis of the improved algorithm, it is proved that the improved algorithm has probability completeness, asymptotic optimality, which ensures that the algorithm can quickly converge to the optimal path in theory. The validity, stability and correctness of this algorithm are verified by the simulation experiments and the correctness of the theoretical analysis is also vevified.

RRT(rapidly-exploring random tree);path planning;mobile robot;ROS