基于改进双向RRT算法的机器人路径规划

韩丰键，邱书波，李庆华*，刘海英

(齐鲁工业大学(山东省科学院)a. 电气工程与自动化学院；b. 电子信息工程学院(大学物理教学部)，山东 济南 250353)

随着工业生产对圆盘移动机器人要求的不断提高，路径规划已经成为一个重要研究领域，传统路径规划算法主要有A*算法[1]、蚁群算法[2]、人工势场算法[3]、遗传算法[4]等。尽管这些算法在处理低维空间路径规划问题具有一定的优越性，但是当空间维度较高时，算法在精确表达构型空间上需要占用大量的计算资源。基于采样思想的路径规划算法，如概率路线图算法(probabilistic roadmap method,PRM)[5-7]、快速扩展随机树算法(rapidly exploring random trees，RRT)[8-10]，不需要精确表达构型空间，而是通过在构型空间内获取自由构型形成构型图，以描述构型空间的连通性，这类算法在机械臂、人型机器人等高维构型空间上所体现出的优势更为明显。

Lavalle等[9]首次提出了RRT算法，基于随机采样的思想获取自由构型，用以构建一个树形网络表达自由构型空间。该算法避免了对整个环境空间建模，在高维构型空间的路径规划问题中优势更为明显。但是RRT算法采用的随机采样思想，也导致了节点的扩展无目标导向性，容易出现大量冗余节点，算法的收敛速度过慢。针对RRT算法生成构型无目标导向性、收敛速度慢、路径拐点多的缺点，Urmson等[11]提出了路径代价函数的概念以表征路径的优化程度，面向目标路径越优则路径代价越小。该算法在扩展中，不再选择距离随机构型最近的节点，而是选择k个较近的节点进行扩展，提升了已扩展RRT树内距离随机构型较近节点的搜索性能。代价函数的引入使得RRT树的扩展算法具有较好的目标导向性，可有效提升算法的收敛速度。但是该算法在狭窄空间或障碍物密集等复杂环境下，算法收敛性能会有明显下降。Rodriguez等[12]提出了B样条平滑函数，考虑了路径的曲率连续性和机器人自身的微分约束，但是该算法在步长增加的权重上没有一个很好的导向性，使得算法收敛速度慢。Guo等[13]提出了一种鲁棒动态多目标车辆路径选择方法，采用多目标粒子群优化算法，将动态客户从鲁棒虚拟路径中移除，形成静态车辆路径，只有在找不到适合车辆路径优化的位置时才会触发动态路径规划，避免了耗时的车辆路径规划。上述改进算法都是以增加算法的计算成本来求解距离最优的搜索路径。

在RRT算法研究初期，RRT及相应变种均采用单一随机树生成的思想，由初始构型作为随机扩展树的初始节点，在环境空间内进行扩展。单随机扩展算法构造简单，但是无论是基础RRT算法还是其改进算法，均存在收敛速度过慢的缺点。

基于双向搜索的思想，双向随机扩展算法(bidirectional RRT，BI-RRT)构造两棵分别以起始构型和目标构型为初始点的随机扩展树，递归进行节点扩展以构建可表达构型空间的树形网络[14]。相较于RRT算法，BI-RRT算法的收敛速度更快。但是该算法采用RRT算法的随机节点扩展思想，这导致BI-RRT算法也存在构型无目标导向性的缺点。为提升BI-RRT算法的收敛速度，结合贪婪思想，Kuffner等[10]提出了RRT-Connect算法。在BI-RRT算法扩展过程中，从最近点到随机点仅步进一个固定步长，即使在无障碍物空间内也需多次扩展过程才可到达随机点。在RRT-Connect算法的扩展过程中，从最近点到随机点会持续步进，直至遇到障碍物或到达最近点。贪心思想的应用使得RRT-Connect算法具有更高的扩展效率，在自由构型空间内这一提升更为明显。但该算法扩展过程是以自由采样构型随机点为目标点进行扩展，没有改变其导向性差的缺点。Akgun等[15]基于启发式采样策略，提出了概率优化的RRT*算法，提升了RRT*算法的收敛速度及路径质量，但是该算法采样过程中使用的局部偏置思想在复杂环境中的适应性较差，存在导致算法收敛速度变缓慢的可能。张顺等[16]提出PRRT-Connected算法,将人工势场法与RRT-Connected算法结合，优化采样和扩展策略；同时考虑到无人机的性能约束，对规划出来的路径修剪并采用三阶贝塞尔曲线对最终路径平滑，很好地解决复杂环境下无人机路径规划问题。这些改进的RRT算法均存在收敛速度缓慢的缺点，没有考虑到圆盘移动机器人的避障思想。

针对BI-RRT算法研究中存在目标导向性差、收敛速度慢、路径拐点多的问题，提出了一种改进的BI-RRT算法。该算法是在BI-RRT算法的基础上，通过搜索两棵树的最近节点，利用目标导向思想产生随机点，加快了路径收敛速度；同时引入贪婪算法，解决了现在BI-RRT算法的“绕远路”问题。本文以圆盘移动机器人为模型，还提出了k点碰撞检测算法，可有效检测新增节点是否为自由构型。最后，将本文方法与基本BI-RRT算法和目标导向的BI-RRT算法做仿真实验对比，验证所提出的改进算法在路径规划中的优势。

1 BI-RRT算法

在BI-RRT中，定义了两棵随机树Ta和Tb，树Ta以qi为树的根节点(起始点)开始扩展，树Tb以qg为目标点开始扩展，p为每次延伸的步长，qr为任意扩展的随机节点，qn为每次扩展时任选两棵树中距离qr最近的节点，以qx为新节点。首先在整个搜索空间中采取随机的方式生成随机树的随机扩展节点qr，然后遍历当前已有的随机树，从树中的节点寻找距离qr最近的节点qn，在qn向qr延伸一定步长p之后可以得到新节点qx，之后需要对新节点qx进行碰撞检测，若qx碰到障碍物便将这个节点舍去，反之，即将qx添加到树中，可知此时qx的父节点是qn，按照上述方式继续扩展，直到两棵树的qx小于一定的步长阈值时，则可确定Ta和Tb连通，即路径规划成功。图1表示BI-RRT算法随机树的生长过程。

图1 BI-RRT算法随机树生长Fig.1 Growth of BI-RRT algorithm random tree

2 改进BI-RRT算法

改进BI-RRT算法倾向于解决目标导向性差和路径拐点冗余的问题，本文通过目标导向、路径平滑处理和圆盘k点碰撞检测来实现对BI-RRT算法的改进。

2.1 目标导向算法

原方案仅采用随机生成采样点，以树中最近点沿当前方向延伸一定步长p得到新的节点，该过程主要的计算任务在碰撞检测阶段。所提出的基于目标导向的方案，改进了BI-RRT算法只生成一个qr确定qx，增加目标导向思想是以随机点qr和树Ta的最近节点qn生成扩展方向，定义了树的最近节点qn和树Tb的最近节点qn′生成扩展方向，再分别以步长p和kp(k为导向系数)生成qr″和qn″,最后通过平行四边形法则求新的节点qx。导向系数k的取值不同，会对目标导向算法的收敛性有一定的影响。尽管在目标导向阶段增加了计算量，但节点的选择更具有导向性，使树的生成更偏向目标点。图2表示基于目标导向思想生成新节点的过程。

图2 目标导向生成qxFig.2 qx generated from target orientation

y=[(yr-yn)(xr-xn)]x+b，

(1)

假设qr′的坐标为(xr′,yr′)，步长p表示为：

(2)

(yr-yn)(xr-xn)=(yr-yn)(xr-xn)。

(3)

由式(2)和(3)得到qr′的坐标。

假设qr″的坐标为(xn″,yn″)，步长kp表示为：

(4)

(yn′-yn)(xn′-xn)=(yn″-yn)(xn″-xn) 。

(5)

由式(4)和(5)得到qn″的坐标。

依据qr′和qn″的坐标，qn的坐标，假设qx的坐标为(xa,ya)，依据平行四边形法则可得：

(6)

针对一次节点扩展，利用目标导向得到新节点qx的计算过程如下：

Step1：给定树Ta的最近点qn和随机点qr的坐标，给定树Tb的最近点qn′的坐标。

Step2：根据给定的步长p和kp求解式(2)、(3)、(4)和(5)得到qr′qn″的坐标。

2.2 路径平滑算法

加入目标导向算法的BI-RRT树，能快速向目标节点“生长”，但是由于随机点的生成有很强的随机性，路径会有很多拐点，特别是在障碍物较多的复杂环境中，BI-RRT生成的路径有很多拐点，如图3所示。

图3 多拐点的路径规划Fig.3 Path planning of multiple inflection points

为了消除冗余节点，减少圆盘移动机器人在不必要转向的机械损耗，需要对规划出来的路径进行平滑处理，如图4所示，红色线表示平滑之后的路径。通过规划出来的路径，对目标点qg尝试依次连接前面的路径点，若两点之间没有障碍物则将该路径点删除，连接上一个节点，直到碰撞发生。如果发生碰撞就将发生碰撞节点的上一个未发生碰撞的节点保存，并且以该点作为父节点再次执行上述操作，直到连接到起始点qi。

图4 路径平滑后的路径规划Fig.4 Path planning after path smoothing

2.3 圆盘k点碰撞检测算法

假设圆盘的圆心坐标为(x0,y0)，半径为r。将圆盘以圆心将周长k等份，设圆上任意一点坐标为(xi,yi)，i=1,2,3,…,k,设该点与圆心的连线和平行于x轴的直线y=y0的夹角为α。图5表示圆盘k点碰撞检测算法。

坐标(xi,yi)表示为：

(7)

图5 圆盘k点碰撞检测算法Fig.5 Collision detection algorithm for k point of disk

2.4 复杂度分析

单节点扩展总的计算复杂度由目标导向的复杂度和圆盘k点碰撞检测复杂度两部分组成。

2.4.1 目标导向的复杂度

由式(3)可以得到

yr′=[(yr-yn)(xr-xn)](xr′-xn)，

(8)

再将yr′带入到式(2)中得到

其次，要对种子进行处理，在处理的过程中要分为几个阶段。其一，选择高质量的种子放于55-60℃的温水中进行搅拌，使温度降到30℃左右，之后将种子浸泡2 h；其二，种子浸泡后取出风干，风干后将其置于200 mg/kg赤霉素溶液中浸泡24 h后催芽，并用1%的高锰酸钾溶液浸种30 min，捞出淘干净，再放入55℃温水中浸种，用水量为种子的5倍；其三，在用药水浸泡种子之后，用25℃左右的温水将种子浸泡8-12 h，用细砂搓去种皮上的黏液，洗净后摊开晾一晾，准备播种。

p=(xr′-xn)2+[(xr′-xn)(yr-yn)(xr-xn)-yn]2，

(9)

图6 正、反向生长区Fig.6 Positive and negative growth zones

2.4.2 圆盘k点碰撞检测复杂度

由式(7)，可见求解(xi,yi)包含二次乘法和二次加法，圆盘k点碰撞检测算法的复杂度为2k次乘法和2k次加法。圆盘多点碰撞检测的复杂度为O(n2log2k)。

图7为BI-RRT和改进的BI-RRT复杂度对比图与随机采样比较，目标导向可以减少无效随机采样点生成，随着扩展节点数量的增长，通过目标导向思想的算法改进得更明显。

图7 BI-RRT和改进的BI-RRT复杂度对比图Fig.7 Comparison of complexity between BI-RRT and improved BI-RRT

3 伪代码实现

3.1 BI-RRT算法

BI-RRT算法的原理为首先初始状态被添加到搜索树，主循环是line3～line24，在n次迭代后终止。显然，BI-RRT算法通过采样随机点，扩展完树Ta的新节点qx后，以qx作为Tb的扩展方向。按照上述方式继续扩展，直到两棵树的qx小于一定的步长阈值时，则可确定Ta和Tb连通，即整个算法结束。每次迭代中必须考虑两棵树的平衡性，即两棵树的节点数的多少(也可以考虑两棵树总共花费的路径长度)，交换次序选择“小”的那棵树进行扩展。BI-RRT算法的伪代码见OSID。

3.2 改进BI-RRT算法

3.2.1 目标导向算法

图8 目标导向生成qxFig.8 qx generated from target orientation

3.2.2 路径平滑算法

路径平滑处理的步骤如下：

(1)上一程序周期中，从目标节点qg，追溯到随机树的根节点qi，所生成的路径，形成路径节点path(q0,q1,…,qn)。其中q0为起始节点qi，qn为目标节点qg。

(2)搜索树的平滑。在程序下一周期中对已生成的路径进行贪婪算法的平滑处理，令qt=q0，依次尝试用q0连接q1,…,qn，直到碰到障碍物，即qt无法连接到第一个节点qi，但qt到qi-1之间是可以直接连接的，则将qi-1存入到路径缓存区域T0中。

(3)令qt=qi-1，依次尝试用qt连接qi,qi+1,…,qn,若qt无法连接到节点qm，则将qm-1存入到路径缓存区域T1中，重复以上的步骤，直到qt=qn。

(4)最后，根据路径缓存数组中的节点，生成平滑后的路径。

路径平滑算法的伪代码见OSID。

3.2.3 圆盘k点碰撞检测算法

圆盘k点碰撞检测算法:改进了单点碰撞检测函数，提出了一种圆盘k点碰撞检测算法，具体操作：假设圆盘机器人的圆心为xd，半径为r，首先将圆盘机器人分割成k等份，本文中取k=50，再对分割出来点的坐标合成一个集合{qi}，i=1,2,3，…，50，对{qi}碰撞检测，来检测圆盘上的点是否在障碍物上，当这50个点都不在障碍物里和地图外时，将qx加入到路径中。圆盘k点碰撞检测算法的伪代码见OSID。

4 实验与分析

仿真分析平台由Matlab开发，并在主频为2.3 GHz的PC上运行。通过分析障碍物数量、迭代次数及导向系数k等参数对方案收敛性的影响，同时本方案还通过分析拐点个数对三种算法进行了比较，验证了所提的改进BI-RRT算法有较好的优势。

4.1 障碍物数量

按照障碍物的数量和密度，仿真实验环境设定在不同的环境(宽阔环境、通道环境、栅格环境、迷宫环境)中的路径规划，设置实验空间尺寸为500 m×500 m，起始点坐标设置成(10,10), 终点坐标设置成(490,490)。考虑到是现实生活，本文取圆盘移动机器人直径为1 m。分别对BI-RRT算法、目标导向BI-RRT算法和改进的BI-RRT算法在4种不同地图上进行仿真对比，每种对比实验进行50次仿真，取均值进行比较。图9～12分别表示宽阔环境、通道环境、栅格环境、迷宫环境下的路径规划图，绿色线表示BI-RRT算法的规划路径，蓝色线表示目标导向BI-RRT算法的规划路径，红色线表示对目标导向BI-RRT算法路径规划加入平滑后的改进BI-RRT算法。

图9 宽阔环境下的路径规划Fig.9 Path planning in broad environment

图10 通道环境下的路径规划Fig.10 Path planning in channel environment

图11 栅格环境下的路径规划Fig.11 Path planning in raster environment

图12 迷宫环境下的路径规划Fig.12 Path planning in maze environment

4.2 参数分析

4.2.1 迭代次数对三种算法收敛速度的影响

表1分别记录了4种不同环境下的统计结果。由表1可知，在路径规划过程中，改进BI-RRT算法相比BI-RRT算法和目标导向BI-RRT算法平均路径长度上能够在很短的时间里寻得路径。在同样的环境和参数下，在平均迭代次数上，改进BI-RRT算法与目标导向BI-RRT算法次数差不多，相比BI-RRT算法，在4种环境下，迭代次数平均增加了44.24%。对于平均规划时间，改进BI-RRT算法相比目标导向BI-RRT算法增加了路径平滑算法，路径规划过程中增加了规划时间，4种环境时间平均增加了3.38%，相比BI-RRT算法，在平均规划时间上有着很明显的优势，4种环境时间平均缩短了27.82%，加快了收敛速度。

表1 4种环境下的仿真数据比较

4.2.2 拐点个数分析对比

由表2可知，在宽阔环境的路径规划过程中，改进BI-RRT算法相比BI-RRT算法和目标导向BI-RRT算法拐点个数分别减少了95.12%和89.47%；在通道环境的路径规划过程中，改进BI-RRT算法相比BI-RRT算法和目标导向BI-RRT算法拐点个数分别减少了87.5%和75%；在栅格环境的路径规划过程中，改进BI-RRT算法相比BI-RRT算法和目标导向BI-RRT算法拐点个数分别减少了95.12%和90%；在迷宫环境的路径规划过程中，改进BI-RRT算法相比BI-RRT算法和目标导向BI-RRT算法拐点个数分别减少了90.90%和83.33%。

表2 4种环境下的拐点个数比较

4.2.3 导向系数k分析对比

不同的导向系数k会对算法的收敛性有一定的影响。为了验证导向系数对实验数据的影响，在上述提到的4种环境对不同导向系数k进行50次的仿真实验，取均值对规划时间进行比较。由表3可知，在4种环境下，导向系数k=1.0，相比k=0.5，1.5，2.0时，平均规划时间相对较短。

表3 不同导向系数k对时间的影响

5 结论

本文针对BI-RRT算法路径规划中存在目标导向性差、收敛速度缓慢、路径拐点多的问题提出了改进BI-RRT算法。该算法通过目标导向启发式思想对随机树中随机点的产生进行改进，并加入了贪婪算法对路径进行平滑处理，同时和圆盘k点碰撞算法相结合，通过数学模型分析，相比于BI-RRT算法，降低了算法复杂度。在4种不同地图环境的仿真实验中，验证了改进BI-RRT算法在圆盘移动机器人上的优势。虽然，相比目标导向BI-RRT算法增加了平均规划时间，但使路径更加的平滑，提高了整体的路径规划。因此本文提出的改进BI-RRT算法适用于多种不同环境，能够以更少的搜索节点、更快的收敛速度得到路径，有较大的实用价值。在后续的研究中，将考虑在平均路径长度上的优化和考虑应用到移动智能车的路径转角上进行改进。