移动机器人RRT算法改进研究

符秀辉,刘 然

(沈阳化工大学 信息工程学院,辽宁 沈阳 110142)

快速搜索随机树(Rapidly-Exploring Random Trees，RRT)算法由美国爱荷华州立大学Steven M.LaValle教授在1998年提出，RRT的思想是快速扩张一群像树一样的路径以探索(填充)空间的大部分区域，伺机找到可行的路径。RRT算法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。对RRT算法的改进也有很多，施杨洋等[1]使用双树RRT算法在障碍物周围生成了若干节点，并在这些节点中引入了斥力，生成多棵局部随机树，使局部随机树向远离障碍物方向扩展，减少了局部随机树对障碍物的检测次数和扩展过程中对障碍物的检测时间，加快了算法的收敛速度，改善了算法的偏差性；刘恩海等[2]使用自适应策略改变引力系数的大小从而改变步长，引导随机树朝目标点方向高质量地生长，并解决了目标振荡和随机树在障碍物附近聚集的问题，使RRT算法具有一定的可行性和有效性，有效缩短了路径规划时间；成怡等[3]引入了变概率目标偏向思想，通过设置算法认定目标点是局部目标点的概率阈值方法，在每次生成随机点之前随机生成概率值，与概率阈值做比较，如果高于阈值，则将生成的随机点作为扩展树子节点的生长目标点，反之则采用基本RRT算法中随机生成的节点作为扩展树子节点的生长目标点，克服了随机扩展树生长时采样的随机性和收敛速度慢的问题；徐秉超等[4]采用预生长方式选择步长，基于Metropolis接受准则的随机点筛选机制，对受时间影响而形成的不同生长可能性区域进行划分，在一定程度上解决了随机搜索树的重复生长问题，令重要障碍物产生斥力，使机器人避开障碍物，减小了势场计算规模，提高了路径规划的避障效率；乔慧芬等[5]利用人工势场法在初始环境中预规划一条初始路径，在新出现的障碍物周围确定合适的规划空间，利用RRT算法规划局部路径。最后，合并初始路径与局部路径得出最终路径，减小了路径长度，缩短了规划时间。本文针对RRT算法存在随机性强、偏差大、路径不一定最优等缺点，对RRT算法进行改进研究，借助人工势场法，加入自适应策略，不断改变步长大小，最终使机器人加速到达目标点，减小耗时，提高RRT算法的执行效率。

1 RRT算法

RRT算法是以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树。当随机树中的叶子节点包含了目标点或机器人进入了目标区域后，则在随机树中找到一条从初始点到目标点的路径，并在状态空间中对采样点进行碰撞检测，使机器人能够避开障碍物。

在二维空间中，以初始点作为根节点，生长随机树，并建立随机节点列表。如果随机节点在空闲空间上，则通过欧氏距离公式计算找到随机节点距离随机节点最近的树节点，再通过满足步长限制的中间节点随机扩展一定步长并生成新节点，期间与中间点进行碰撞检测。如果与中间点存在障碍物，则重新寻找新节点；如果不存在，则将这个新节点加入到随机树中。当随机树中的子节点包含目标点时，则在随机树中生成一条由起始点到目标点的路径；如果发生碰撞，则放弃这次扩展，其生长方式如图1所示。

图1 RRT随机生长方式

2 自适应RRT算法

针对RRT算法的随机性强、搜索没有目的性、路径不一定最优的缺点，提出借助人工势场引力函数思想，改进节点的生成方式，对随机树中的每个节点xn都增加一个目标引力函数G(n)，此处的节点xn表示由起点xstart(或终点xgoal)向外扩展的第n个节点xnew。其公式可表示为

F(n)=R(n)+G(n)

(1)

式中：F(n)为从节点n到目标点的生长指导函数；R(n)为从初始节点到节点n的随机生长函数。

胡兵等[6]和成浩浩等[7]对RRT算法中的节点进行了改进。在改进RRT算法过程中产生的新节点xnew由随机树节点xrand和目标节点xgoal共同决定。如图3所示，在二维空间下，定义沿随机点xrand扩展方向的点为x1，步长为ρ，沿目标点xgoal扩展方向的点为x2，步长为kρ，k为系数，目标点方向则由x1、x2共同决定，通过调节系数k使新节点不断向目标点偏移，最终到达目标点。

图2 RRT改进节点生成方式

沿随机点扩展方向点x1的坐标表达式为

(2)

目标点扩展方向点x2的坐标表达式为

(3)

式中：‖xrand-xnearest‖为随机点到距离随机节点最近的树节点的距离的绝对值；‖xgoal-xnearest‖为目标点到距离随机节点最近的树节点的距离的绝对值。

因此，根据式(1)可得新节点xnew的坐标表达式：

xnew=xnearest+x1+x2

(4)

为了解决机器人耗时长的问题，王洪斌等[8]和徐飞[9]在人工势场中引入了相对速度，分别作为优化路径和排除对机器人路径规划影响很小的障碍物的指标。杨凯等[10]在人工势场法中增加了虚拟目标并考虑了阻力的影响。本文对沿目标点扩展方向的点x2做出改变，使其系数k与引力的大小有关。当机器人距离目标点远时，k增大；当机器人距离目标点近时，k减小，直至引力为零，从而避免随机树节点到达目标点时发生振荡[11]。其具体过程如下，定义引力势场函数表达式为

U(q,v,a)=εa‖q-qgoal‖m+εb‖v-vgoal‖n+εc‖a-agoal‖l

(5)

式中：εa、εb、εc为系数；m、n、l为正常数。对应引力函数表达式为

F(q,v,a)=F(q)+F(v)+F(a)

(6)

其中：

(7)

式中：Xq、Xv、Xa分别为机器人相对目标点的位置、速度和加速度的单位向量。

系数k的表达式为

k=λ‖F‖

(8)

式中：λ为正常数；‖F‖为引力的绝对值，可以进行测量，它与机器人相对目标点的位置、速度和加速度有关。

因此，得到新节点xnew的坐标表达式为

xnew=xnearest+x1+x2

(9)

3 实验结果与分析

机器人小车如图3所示，用STM32F103ZET6进行控制，外设采用激光雷达、里程计、惯性导航IMU、电机等，利用激光雷达Rplider A1进行测距，利用MPU6050测量偏航角度，进而得到小车的坐标，通过编码器对机器人运行时的车轮运行圈数计数，测量机器人运行时的速度，通过编码器对机器人速度形成闭环控制，对单位时间内的速度进行积分和微分计算，即可得到机器人运行的实际距离和加速度。移动小车实验场景按照图4所示位置进行布置，黑色区域是障碍物位置，为了使实验数据更具说服性，必须使机器人从障碍物之间的狭窄通道通过，减少其他因素的影响[11]，为此实验进行50次，将RRT算法和自适应RRT算法进行实验对比，如图4和图5所示。实验中相关参数设置如下：xstart=[2,2]，xgoal=[18,18]，εq=εv=εa=1，m=n=l=2，λ=1，小车初始速度为0.3 m/s，角速度设置为0.5 rad/s。

图3 移动小车

图4 RRT算法

图5 自适应RRT算法

在加入了自适应策略的RRT算法中，不断调整系数k，使机器人逐渐向目标点靠近，只要机器人不达到目标点，则x≠xgoal或v≠vgoal或a≠agoal总存在，因此式(9)总成立。经多次实验，RRT算法和自适应RRT算法耗时比较如图6所示。

图6 两种方法耗时比较

实验结果表明自适应RRT算法可以有效地节省时间，更具有目的性且偏差较小，其分析结果如表1所示。

表1 两种方法耗时对比

由表1可知，自适应RRT算法相比于RRT算法节省了20.45%的时间。

4 结束语

本文在RRT算法中借助人工势场法，加入了自适应策略，不断改变节点步长的大小，使机器人快速安全到达目标点。由于RRT算法本身的缺陷，会导致路径不断变化，为了减小这种影响，必须使小车行驶在同一路段，从障碍物中间通过并躲避障碍物，而不能从地图边缘到达目标点。实验结果表明，自适应RRT算法相比于RRT算法在实际应用中耗时明显减少，可以极大地提高机器人作业效率，有效地节省了时间。目前实验本身仍然存在的问题是算法只适用于比较规则的障碍物，对不规则的障碍物来说，有很多凹凸点，容易使机器人重复作业陷入死区，因此应对障碍物中凹凸点处进行研究，使机器人适用于更多场合，并对算法和路径进行优化，从而使机器人小车快速安全地到达目的地。