基于改进RRT算法的差动机器人路径规划

陈 敏 李 笑 武交峰

(广东工业大学机电工程学院 广东 广州 510006)

0 引 言

差动机器人作为一种移动机器人，在物流、交通、军事等领域应用广泛。路径规划是实现机器人智能化的关键技术，其旨在为机器人规划出一条从初始状态到目标状态的符合约束条件的路径[1]。传统规划算法如A*算法[2]、人工势场法[3]、遗传算法[4]、粒子群算法[5]等对于复杂环境下受各种约束的机器人效率低下，求解困难[6]。

LaValle提出快速扩展随机树(RRT)算法[7]，它是一种基于随机采样的规划算法，无需对状态空间显示建模，规划速度快且能考虑机器人的各种约束，对解决复杂环境下的路径规划问题表现出极大的优越性[8]。但RRT算法也存在一些固有缺陷，如最近邻函数不合理、收敛速度慢、生成的路径曲折等[9]。

针对上述不足，国内外学者对RRT算法进行了各种改进，以适用于不同场景。在RRT算法中，理想的最近邻函数需解决两点边界值问题，难以准确定义[8]，Perez等[10]提出使用LQR控制器作为最近邻函数，但求解最优控制比较耗时，不利于实时计算。为了加快收敛速度，Kuffner和LaValle先后提出bi-RRT算法[11]和

RRT-connect算法[12]。bi-RRT算法同时以初始状态和目标状态为根节点构建两棵树，交替向对方扩展，提高了算法的搜索效率；RRT-connect算法在bi-RRT算法基础上改进了扩展步长以提高节点扩展效率，但同时增加了每次节点扩展的碰撞检测次数。文献[13]中引入目标偏向RRT算法，节点以固定概率向目标点方向扩展，减小了算法的随机性，提高了算法的搜索效率，但在复杂场景中由于探索不足易陷入局部最小点。对于随机采样导致的路径曲折问题，文献[14]使用回旋曲线对初始路径进行平滑，但回旋曲线不存在闭合解，无法实时准确获取。文献[15]采用贝塞尔曲线平滑，但贝塞尔曲线阶数取决于路径点个数，无法灵活设置。

针对RRT算法在差动机器人路径规划中存在的上述问题，本文提出一种改进RRT算法。首先，考虑差动机器人运动学约束，在最近邻函数中加入角度变化，使规划路径趋于平缓；其次，在节点扩展阶段引入启发步长因子，使扩展步长根据节点位置和扩展方向动态调整，加快收敛速度的同时兼顾规划成功率；最后，对规划路径进行修剪和平滑处理，以生成差动机器人易于跟踪控制的路径。通过仿真实验，证实了该算法的优越性和实用性。

1 差动机器人运动学模型

差动机器人的简化运动学模型如图1所示。差动机器人的状态可表示为X=(x,y,θ)，其中：(x,y)为车轮轴中点在系统坐标系下的位置；θ为机器人行进方向与X轴夹角。给定两个车轮的距离L、车轮半径r、角速度u=(ul,ur)，其中ul和ur分别为左右两车轮角速度，如果ul=ur>0，机器人向前直行；如果ul=-ur≠0，机器人沿顺时针方向旋转。差动机器人车轮只能前后滚动，不能侧滑，在运动过程中满足：

(1)

图1 差动机器人运动学模型

非完整约束是指含有系统广义坐标导数且不可积分的约束[16]。差动机器人是一个典型的非完整约束系统[17]。由式(1)可知其存在非完整性约束：

dx·sinθ-dy·cosθ=0

(2)

差动机器人状态空间为3维，该约束将其可控维度变为2维。

对差动机器人的运动学模型分析可知，路径规划算法需考虑机器人自身约束，否则所得路径可能无法应用于实际环境中。

2 改进RRT算法

2.1 基本RRT算法

机器人路径规划问题可作如下定义[18]：将机器人状态空间表示为C=[0,1]d，其中维度d∈N,d≥2。令Cobs为障碍物空间，则自由空间可表示为Cfree=C/Cobs。机器人初始状态qstart和目标状态qgoal位于自由空间Cfree中，路径规划问题可抽象为一个三元组(Cfree,qstart,qgoal)，即在自由空间Cfree中找到一条从初始状态qstart到目标状态qgoal的路径。路径可表示为函数σ:[0,1]→Rd，如果对于路径中所有的τ∈[0,1]，均有σ(τ)∈Cfree,则称其为无碰撞路径；对于无碰撞路径，如果其中σ(0)=qstart、σ(1)=qgoal，则称其为可行路径。

RRT算法在状态空间C中构造随机树T，以初始状态qstart为根节点，在C中迭代随机采样节点qrand，以树状结构扩展直至到达目标状态qgoal，然后由目标状态qgoal回溯到根节点qstart，得到规划路径。基本RRT算法伪代码如算法1所示，其中：random_state()在C中随机采样节点qrand；nearest_neighbor()选取当前树T中距离qrand最近的节点qnear；steer()驱使qnear向qrand方向扩展一定步长到达节点qnew；collision()对qnear和qnew之间的节点进行碰撞检测，如果均在Cfree中，则将节点qnew及对应的边加入到树T中。

算法1RRT算法

1.T.init(qstart);

2. fork=1 toKdo

3.qrand←random_state();

4.qnear←nearest_neighbor(qrand,T);

5.qnew←steer(qnew,qrand);

6. if !collision(qnear,qnew)

7.T.add_node(qnew);

8.T.add_edge(qnear,qnew);

9. end if

10. end for

11. ReturnT

2.2 最近邻函数

RRT算法采样节点qrand后，需通过最近邻函数从树T中找出距离采样节点qrand最近的节点qnear。最近邻函数是RRT算法的关键部分，决定着节点选取的合理性。差动机器人的路径可用轮轴中点所行进的曲线表示，如图2所示，状态间的最短路径为一条直线(图中虚线)，可用欧氏距离度量，但方向角也会影响两车轮的转动次数，合理的最近邻函数需考虑方向角变化。

图2 差动机器人直线行进示意图

基本RRT算法采用欧式距离作为最近邻函数，未考虑差动机器人运动学约束，选取的近邻节点qnear不合理。因此，本文考虑差动机器人方向角变化，使用一种能更合理度量其状态间代价的最近邻函数：

(3)

式中：d(qrand,qi)为采样点qrand与节点qi间的欧式距离；dmax为qrand与树中节点距离最大值；w1和w2为欧式距离与角度的加权因子，可根据实际需求设定。如图3所示，qip为节点qi的父节点，φ为qipqi与qiqrand的夹角。

图3 近邻节点计算示意图

2.3 启发步长因子

在RRT算法节点扩展步骤中，qnew由qnear向qrand扩展一定步长得到，具体扩展表达式为：

(4)

式中：step为扩展步长；d(qrand,qnear)为qrand和qnear状态点的欧式距离。

基本RRT算法在环境中均匀采样且扩展步长step固定，随机性大，收敛速度慢。调整采样偏向是加快收敛速度的常用方法，如目标偏向RRT算法[13]以固定概率向目标点方向扩展，能显著提高搜索效率，在无障碍物环境下得到的路径趋于理想路径。但在实际的规划任务中，障碍物或多或少且分布多样，调整采样偏向会降低算法的探索能力，容易导致规划失败。

为了提高算法搜索效率，同时保证规划成功率，本文保留基本RRT算法全局均匀采样策略，在其节点扩展步骤中引入启发步长因子γ，使步长根据节点位置和扩展方向动态调整，具体计算步骤为:

(5)

式中：α为qstart与qgoal关于qnear的夹角，如图4所示；Δd为qnear与起始点qstart和目标点qgoal之间的距离差：

Δd=|d(qnear,qstart)-d(qnear,qgoal)|

(6)

图4 节点扩展角度示意图

如图5所示，当qnear位于双曲线上时，Δd满足图中所示关系，qnear在不同位置对应不同步长。

图5 节点扩展位置示意图

由于角度α与距离Δd量纲不同，W(α)与D(Δd)分别对其作归一化处理：

(7)

λ1与λ2分别为计算启发步长因子γ时W(α)和H(Δd)对应的权重，为使后续实验中算法易于对比，取λ1+λ2=2，以使平均启发步长与固定步长step一致。

由式(5)-式(7)可知，启发步长因子γ对应取值范围为[0,2]，假定λ1=λ2=1，当节点qnear往目标点qgoal锐角方向扩展(即α<π/2)且同时距离初始点qstart和目标点qgoal较远(即位于图5中阴影部分以外)时，启发步长因子γ>1，启发步长大于固定步长step，节点加速向目标点区域扩展。此外，当节点qnear向目标点qgoal钝角方向(即α>π/2)扩展且距离初始点qstart或目标点qgoal较近(即位于图5中阴影部分)时步长因子γ<1，可提高节点扩展的成功率，同时缓解目标点qgoal附近路径的抖动。

2.4 路径修剪与平滑

由于RRT算法采样随机，生成的路径曲折，存在许多不必要的节点。在障碍物密集的复杂环境下，大量的冗余节点让差动机器人难以跟踪。因此，为了得到差动机器人的可执行路径，本文对生成的初始路径进行修剪，去除不必要的节点，然后利用B样条曲线对修剪后的路径平滑处理。

具体修剪步骤如图6所示，对初始规划路径点集Q，从初始点qstart开始，依次遍历后序路径点qi，如果两个路径点连线与障碍物无碰撞，则删去qstart与qi之间路径点；如果发生碰撞，则从碰撞点的父节点开始重复上述过程，直至遍历到目标点qgoal，得到剩余路径点集P′。

图6 路径修剪示意图

B样条曲线曲率连续，可根据控制点局部调整，在路径规划中应用广泛[9]。因此，本文利用它对修剪后的路径点集P′进行平滑，以生成差动机器人易执行的平滑路径。k次B样条曲线可表示为：

(8)

式中：Pi为第i个控制点的坐标值；Bi,k(u)为k次B样条基函数，是由节点向量U=[u0,u1,u2,…,un+k+1]决定的k次分段多项式，可由cox-deBoor递推公式得到：

(9)

为了使B样条曲线经过初始点和目标点，节点向量需满足：

(10)

3 仿真实验

为了验证改进RRT算法的优越性和实用性，进行了仿真实验。鉴于目标偏向RRT算法近年被广泛应用，本文将改进RRT算法与基本RRT算法、目标偏向RRT算法进行了性能对比。仿真实验通过C++和QT编程实现，在个人PC上完成，操作系统为Ubuntu，处理器为i7，运行内存为8 GB。

图7为基本RRT算法和改进RRT算法在同一任务下的规划结果。仿真环境地图大小为500×500，图中左上角和右下角的差动机器人分别对应其初始状态和目标状态。(a)为基本RRT算法规划路径；(b)为改进RRT算法的初始规划路径；(c)为改进RRT算法修剪后的路径；(d)为改进RRT算法平滑后的路径。

(a) 基本RRT路径 (b) 改进RRT路径

(c) 改进RRT路径修剪 (d) 改进RRT路径平滑图7 基本RRT与改进RRT路径对比

从图7的规划路径可以看出，改进RRT算法最近邻函数由于考虑了角度变化，节点扩展更为平缓，修剪后的路径去除了多余的节点，经B样条曲线平滑后曲率连续，满足差动机器人的运动学约束，易于跟踪。

为了分析改进RRT算法在复杂环境下的规划性能，在0～60%(包含两端)间每隔10%设置了7个不同障碍物占比的地图环境，比较基本RRT、目标偏向RRT、改进RRT三种算法在这些地图环境中的规划结果。由于算法具有随机性，为保证规划算法评价客观，每个地图环境分别对三种算法各进行了20次测试，最大迭代次数为10 000次，步长step为3。图8(a)-(c)分别为三种算法在20次测试下的规划时间、成功率、迭代次数平均值。

(a) 平均规划时间

(b) 平均成功率

(c) 平均迭代次数图8 三种算法规划结果对比

从图8结果可以看出，在高障碍物占比的复杂环境下，与基本RRT算法相比，改进RRT算法缩短了规划时间和迭代次数，且成功率高于目标偏向RRT算法，提高差动机器人路径搜索效率的同时兼顾了搜索成功率。

4 结 语

RRT算法由于存在最近邻函数不合理、收敛速度慢、路径曲折等问题，其难以满足差动机器人的实际应用需求。对此，本文提出了一种改进RRT算法。仿真实验表明，该算法提高了路径搜索效率，规划的路径更为平滑，可进一步满足复杂环境下差动机器人路径规划的实时性和稳定性要求。