基于改进RRT路径规划算法*

李金良，舒翰儒，刘德建，徐 磊

(山东科技大学机械电子工程学院，山东 青岛 266560)

0 引言

随着人工智能的不断推进，机器人技术得到了飞速的发展，智能化成为未来机器人领域的新航标。其中，机器人技术重要的研究领域—路径规划，也开始迅速发展。

路径规划主要研究在障碍物空间环境下，机器人自主搜寻一条从起点到目标点的可行路径或最优路径[1]。机器人的路径规划算法有图搜索法[2]、可视图法[3]、人工势场法[4]、概率路图法[5]、快速扩展随机树法等。因快速扩展随机树法更适合移动机器人和多自由度工业机器人路径规划而被广泛应用及改进。

王中玉等[6]改进传统A*算法的评价函数，减少了一定的计算量；通过改变评价函数的权重比例，减少了不必要的点的搜索，但在三维空间中依然存在计算量大的问题。宋宇等[7]通过人工势场法和RRT算法相结合，以合力方向引导搜索树的扩张，可以减少采样点的个数和搜索路径长度，但搜索时间明显增加。代彦辉等[8]通过引入起始点到目标点的方向向量，使得搜索树扩展具有方向性，虽然大大减少了算法的迭代次数和搜索时间，但极容易陷入障碍物包围圈内，从而导致算法的失败。

本文提出一种目标偏向的RRT算法，结合上述一些算法作出以下改进：基本RRT算法搜索具有盲目性，因此引入目标偏向策略[9]，以减少采样点的数目；通过对采样点距离和速度约束，减少搜索范围，提高计算效率；对规划后的路径采用贪心算法[10]简化路径并对其用三次B样条曲线[11]进行优化。

1 RRT算法

RRT算法是以空间中起点为根节点，通过特殊的增量方式进行扩展，生成一棵随机扩展树，判定随机扩展树上的采样点是否包含目标点或在目标范围内，直到该条件满足，随机扩展树就很快覆盖起点和终点的所有位姿空间，当树长好后，路径也就找到了，而且起点到终点有且仅有一条路径。基本RRT算法扩展示意图如图1所示。

图1 RRT算法扩展示意图

RRT算法步骤：在空间环境中，定义一个起点xinit；在空间环境中，随机确定一个点xrand；若点xrand不在障碍区范围内，则连接点xinit和点xrand，得到一条连接直线L；若直线L没有和障碍物发生碰撞，则沿着直线L，从xinit向xrand的方向扩展一定的距离λ，得到新的点xnew，该点被添加到已经存在的扩展树上；重复以上步骤，直到目标点被添加在扩展树上或在一定误差范围内，于是可以寻找到起点到终点唯一一条路径。

在上述算法需要引起注意的是：在随机扩展树上，初始的随机数只包含一个根节点；随机选择一个采样点，在目标点和这个采样点中创建一个新的节点(如果这个新节点和障碍物之间发生碰撞则取消该节点，如果没有碰撞则把这个节点加入到随机扩展树中)；在扩展过程中为了控制算法，加入了运行时间上限和搜索节点个数上限条件，如果没有在规定条件内达到目标点，判定算法失败。

2 改进RRT算法

在基本RRT算法的基础上，采用目标偏向策略减少树扩展的随机性；添加采样点时角度约束，减少计算量；并对规划后的节点进行贪心策略，简化搜索路径长度。

改进RRT算法步骤：

第1步：在空间地图中，定义起点xinit，终点xgoal，偏向概率p，扩展步长λ，最大迭代次数kmax。

刚恋爱那会儿，林蓝与大赵也有说不完的话。那时他俩都是初入职场的小白领，林蓝和父母住，大赵在离林家两站路的地方租了一间出租屋。每次约会大赵送林蓝回家最有意思：往往都走到门口了，两个人觉得刚刚聊的话题还没说完，于是又手牵手往回走，由林蓝送大赵回家去，大赵怎么会允许女朋友送自己呢？于是到地儿又把林蓝送回来。

第2步：检测xinit与xgoal之间的距离是否小于λ，若条件成立，则对扩展树采用贪心策略剪枝，生成新的路径节点；若条件不成立，进行下一步。

第3步：以一定的偏向概率p选择采样点xrand，并根据改进度量函数确定就近点xnear。

第4步：在xnew和xrand之间的连线L上找到新节点，xnew沿着直线L，从xinit向xrand的方向扩展一定的距离λ，得到新的节点xnew。

第5步：判断扩展树上的所有节点是否达到终点，若不能，返回第3步；若能，进行下一步。

第6步：对已经生成的扩展树采用贪心策略，生成新的路径点。

图2为改进RRT算法流程图。

图2 改进RRT算法流程图

2.1 扩展步长

在RRT算法中，xnew是由xnear向xrand扩展一定步长得到的，下式为采样点求得公式：

(1)

其中，λ为扩展步长，d(xrand,xnear)为点xrand与点xnear之间的欧几里得距离。

2.2 目标偏向策略

基本RRT算法的采样点的范围很大，且随机性强，随机树在扩展方向上具有盲目性，缺乏一定的引导性，导致算法搜索时间较长。所以，采用目标偏向策略，以一定的概率P把目标点作为采样点进行随机树扩展，提高随机树向目标点的扩展的概率，以达到减少采样点的个数，加速随机扩展树的形成。

2.3 度量函数

度量函数的目的是在随机扩展树上找到一点xnear离采样点xrand最近的哪一个。其中，基本RRT算法的度量函数是欧几里得距离。在改进的RRT算法中除了考虑两点之间的距离，还引入了角度约束，使得节点的确立对整条路径平滑有着重要作用。

由于距离和角度的量纲不同，所以将其进行归一化处理。下式中，D(d)为规划后的距离函数，G(θ)为规划后的偏角函数。在已形成的搜索扩展树上的临近的节点，使得M函数取得最小值的节点，即可作为xnear。如图2所示节点选择图。

图3 改进RRT算法节点扩展图

D(d)=(dmax-d)/dmax

(2)

G(θ)=(θmax-θ)/θmax

(3)

(4)

(5)

M(xi,xj)=D(d)d(i,j)+G(θ)Δθ(i,j)

(6)

其中，xnear坐标为(xi,yi)，xrand坐标为(xj,yj)，dmax和θmax为搜索最大值。

2.4 路径的剪枝及平滑

基本RRT算法采样点的选择具有随机性，生成的路径节点众多，存在一些不必要的冗余节点。在障碍物较多的复杂空间下，随着采样节点的增多，会导致机器人运动无法得到有效的跟踪。为此，改进RRT算法对已生成的路径节点进行了剪枝，删除了冗余节点，最后通过贪心算法对剪枝后的路径进行简化。

具体修剪步骤：对初次规划出的节点集合，从起点xinit开始，依次遍历后面的节点xk，如果两个节点的连线没有与障碍物发生碰撞，则删除点xinit与点xk之间的所有节点；如果两个节点的连线与障碍物发生碰撞，则要从点xi的父节点开始重复上述过程，直到终点xgoal添加到新路径中，最后将新的节点连接成路径。

上述形成的路径并不符合机器人运动学规律，因此在此基础上对曲线进行平滑。B样条曲线的曲率具有一定的连续性，能够更好地控制曲线的平滑度，并在实际工程中得到广泛应用。

K次B样条曲线表示：

(7)

其中，pi为曲线控制的节点，Bi,k(u)为K次B样条基函数，节点向量U=[u0,u1,…,un+k+1]确定K次分段曲线。

三次B样条曲线的基函数：

(8)

3 仿真验证

为了验证改进RRT算法的可行性和优越性，通过MATLAB语言进行了仿真验证。通过基本RRT算法与改进RRT算法在步长λ=0.4的条件下，得到在地图1、地图2、地图3路径规划的长度、节点个数以及搜索时间。起点坐标(0,0)，终点坐标(10,10)，红色代表最终规划出的可行路径，图4为各个算法在不同地图的路径规划图。

(a) 地图1基本RRT算法 (b) 地图2基本RRT算法 (c) 地图3基本RRT算法

(d) 地图1改进RRT算法 (e) 地图2改进RRT算法 (f) 地图3改进RRT算法图4 不同算法路径规划图

在仿真结果中，如表1、表2、表3所示仿真数据，基本RRT算法和改进RRT算法在搜索时间、搜索路径长度、迭代次数上有着明显的差异。改进RRT算法在这三个维度衡量标准下有着更显著的优势。随着地图障碍物的增加，改进后的RRT算法在路径规划中更加具有优势，在地图3中，与基本RRT算法相比：搜索时间缩短53.8%，路径长度缩短了14%，迭代次数减少了74.9%。以此证明了改进RRT算法的可行性和优越性。

表1 不同算法搜索时间表

表2 不同算法搜索路径长度表

表3 不同算法迭代次数表

4 轨迹平滑仿真

为了使得路径更符合机器人运动学规律，将改进RRT算法规划出的路径进行三次B样条曲线拟合，如图5所示。

(a) 地图1路径优化 (b) 地图2路径优化 (c) 地图3路径优化图5 三次B样条曲线优化图

5 结论

针对静态全局空间下机器人的路径规划问题，提出了一种改进RRT算法。该算法通过搜索范围的约束、剪枝运算以及贪心算法的应用达到了对路径的修正，最后通过三次B样条曲线对路径进行优化，使其更符合机器人运动学规律。

通过仿真验证，与基本RRT算法对比，证明了改进RRT算法的正确性和可行性，对机器人运动规划具有一定的参考价值。