基于改进RRT-connect的四旋翼无人机避障轨迹规划算法*

李成雷， 贺继林， 邓 宇,2， 敖小乐

(1.中南大学 高性能复杂制造国家重点实验室,湖南 长沙 410083；2.山河智能装备股份有限公司,湖南 长沙 410100)

0 引 言

如何规划出一条无碰撞、满足连续性约束且能保证输出稳定性的航迹，实现无人机的自主巡航[1]，已成为无人机智能化的研究热点。早期针对地面移动机器人开发的人工势场、谐函数等矢量场方法近年来被不少研究人员应用于无人机规划[2,3]，但难以避免陷入局部极小点以及曲线振荡等问题。A*，D*等图搜索算法在三维空间中计算复杂度会剧增。快速搜索随机树[4](rapidly-exploring random tree,RRT)算法通过随机抽取一些采样点来进行搜索，可以大大降低算法的时间和空间复杂度，在高维度空间中有着更明显的优势。为进一步提高RRT算法的效率，国内外学者在此基础上有不同程度的改进，目标偏向RRT，RRT-connect等算法都具有不错的效果[5～7]。

而RRT算法得到的路径是由直线段依次连接而成的路径，在转折节点处不光滑，不利于后续的跟踪控制。因而需要对折线路径进行拟合平滑处理。常用的Dubin曲线不能保证加速度的连续性，螺旋曲线(clothoid curves)由于没有解析解不适用于实时处理[8]，贝塞尔曲线构造简单但受控制点的限制，灵活性差。B样条曲线与控制点的数量独立，可对各分段的曲线作局部修正，并保证各分段区间之间的连续性。

本文对于四旋翼无人机在分布大量障碍物的低空复杂环境下的航迹规划问题，提出了一种对RRT-connect算法的改进方法，并采用B样条函数进行后处理。最后，通过实验验证了该算法的有效性和稳定性。

1 路径规划

1.1 问题描述

定义C⊂Rd为规划问题的d维位形空间(configuration space)，Cobs⊂C表示障碍物空间，Cfree=CCobs为自由空间。对于给定的初始位形qinit∈Cfree及目标位形qgoal∈Cfree，规划问题可以描述为：找到一条连续无碰撞路径σ:[0,1]→Cfree满足σ(0)=qinit和σ(1)=qgoal。

四旋翼无人机是欠驱动系统，如果在其12维状态空间中进行路径规划会造成问题求解的代价非常高。利用四旋翼飞行器[9]的微分平坦特性降低规划空间的维数，可以大大降低规划问题的复杂度。微分平坦系统是指系统的状态和输入都可以表示成平坦输出及其有限阶导数的函数[10]，本文选取平坦输出为

ξ=[xyzψ]

式中x,y,z为飞行器的质心在惯性坐标系的坐标；ψ为偏航角。通过飞行器的平坦输出可以唯一确定全部状态和输入[11]。为进一步简化问题，本文选取[x,y,z]作为规划空间，平坦输出ψ则保持为arctan(y/x)。

1.2 RRT-connect算法步骤

1)分别以初始位形qinit和目标位形qgoal为根节点，初始化两棵搜索树Tinit和Tgoal。

2)在自由空间Cfree中抽取采样点qrand，并在树Tinit中搜索与qrand欧氏距离最小的节点qnear；在qnear与qrand的连线上，从qnear处以步长r截取节点qnew,1。若qnear～qnew,1之间均在空间Cfree中，则将节点qnew,1添加到树Tinit中，否则重复本步骤。

3)在树Tgoal中搜索与步骤(2)中qnew,1欧氏距离最小节点qnear，在qnear与qnew,1的连线上，从qnear处以步长r截取节点qnew,2，若qnear到qnew,2之间均在空间Cfree中，则将节点qnew,2添加到树Tgoal中。重复本步骤，当两棵树相遇时终止程序并输出路径，遇到障碍物时进入下一步。

4)交换两棵树Tinit,Tgoal，返回执行步骤(2)。

2 RRT-connect算法的改进

2.1 规划策略

从RRT-connect算法步骤可知：由于采样点qrand的随机分布，导致输出路径节点过于发散，路径中包含有大量锯齿状的折线，路径长度偏离最优值较远。为提高路径质量，有必要对算法加以改进:

1)转角约束

在RRT-connect算法步骤(2)和步骤(3)中，通过查找与搜索树中欧氏距离最小的节点qnear就可以进行后续的扩展。为了避免生成路径中存在“打结”的现象，本文提出在扩展前施加一个转角约束，即

(1)

2)路径修剪

设RRT-connect算法输出的路径节点序列为P=[P1,P2,…,Pn]。如果对于节点对(Pi,Pi+2)是无碰撞的，则节点Pi+1就是冗余路径节点，将其从节点序列P中删除。在遍历了整个路径节点序列后，就可以去除冗余的节点使输出路径的质量得到大大改善。如图1所示。

图1 转角约束和路径修剪示意

2.2 平滑策略

改进后RRT-connect算法生成的路径仍是折线，四旋翼飞行器在跟踪所生成的路径时需要在转折点附近减速并完成转向来减小轨迹跟踪误差[13]，这导致飞行效率大幅下降。因此，需要对路径作平滑处理，以获得几何连续的轨迹。

2.2.1 B样条曲线

在规划空间[x,y,z]中，飞行器轨迹的各分量可以用一个含有n+1个控制点的k阶B样条曲线来表示，即

(2)

式中Bi,k(u)为基函数，Pi为第i个控制点向量。基函数表达式由de Boor-Cox递推公式[14]给出

(3)

(4)

定义节点向量为U=[u0u1…um]，其中各元素保持单调不减(ui≤ui+1)，且m=k+n+1。本文取u∈[0,1]作为参数区间。

由上述递推公式可知，基函数与节点向量各元素的分布以及曲线的阶数有关，而与控制点独立，这使得曲线具有良好的局部可修改性。

2.2.2 控制点调整策略

与文献[11,15]中通过施加硬约束来保证分段多项式连续的平滑策略不同，本文将利用全部路径节点作为控制点生成单条连续的参数化轨迹，可以预先求得解析解。为了使曲线经过指定的起点和终点，令节点向量U中两端元素满足k+1次重合度，并使得内部节点元素均匀分布，即

(5)

虽然B样条曲线在控制点的作用下对路径有较好的跟随性，但仍然存在轨迹偏离路径节点较远的情况，特别是在控制边较长及相邻控制边转角较大时尤其明显[16]，这大大增加了平滑后轨迹发生碰撞的概率。为此，本文在每条控制边等分点处插入若干中间控制点，降低轨迹偏离原路径的程度。在每次生成平滑曲线后都进行碰撞检测，直到插入的控制点使曲线无碰撞为止。

如图2所示，通过在每条控制边中点处各插入一个控制点，使平滑后的轨迹更加贴合原始路径，避免了生成的轨迹与障碍物间的干涉。

图2 插入中间控制点前、后轨迹

3 算法验证与分析

3.1 算法验证

实验中，将规划空间限定为100 m×100 m×30 m的三维区域中，障碍物区域由若干个半径为8 m的随机分布的球形区域组成。设定规划任务的起点坐标为(5,5,10)m，目标点坐标为(95,95,15)m，规划步长r为3，B样条曲线阶数k为3。

图3所示为某次规划的结果。图中两个圆形标记分别表示起点和目标点，虚线路径表示修剪前的路径，点划线为修剪后的路径，实线为平滑后的最终轨迹。从图3可以看出，规划算法能找到一条连续路径避开所有障碍区域，连接起点和目标点。

图3 规划结果

图4(a)为插入中间控制节点后生成的平滑轨迹，与图3(b)中实线路径相对应，“+”标记的为新插入控制节点的位置。从图中可以看出生成的B样条轨迹贴近原始路径，能避免与障碍物发生碰撞。图4(b)为图4(a)中轨迹对应的曲率随参数u变化的曲线，可以看出最终轨迹的曲率是连续有界变化的，且在起始、目标位置曲率为0，这说明三阶B样条曲线能保证加速度的连续性，且是有界的。

图4 B样条曲线轨迹与对应参数曲率变化

3.2 性能分析

由于本文算法包含随机性，为了分析其综合性能及稳定性，以图3(a)所示环境模型为基础在MATLAB 2017上进行1 000次重复规划实验，并与标准RRT算法,RRT-connect算法及目标偏向RRT(RRT-biasing)算法规划结果进行对比。4种规划算法的运行时间、路径长度分布如图5所示，详细统计数据见表1。

图5 各算法性能对比

算法时间/s平均值标准差路径长度/m平均值标准差本文算法0.02960.0115140.466.94RRT-connect0.02400.0116173.6813.36标准RRT0.50110.6367204.7227.67Biasing-RRT0.02950.0095167.6111.81

从图5(a)中可以看出，本文算法的运行时间相较于标准RRT算法，分布更集中且均在0.1 s以内，可以满足实时性要求。结合表1的数据，本文算法平均运行时间与RRT-connect算法相比慢0.005 6 s，与RRT-biasing算法相比大致持平，这是由于本文算法在RRT-connect算法基础上增加了路径修剪及平滑处理过程；从运行时间的标准差来看，本文算法与RRT-connect算法几乎一致，这说明本文算法的附加处理过程对算法的稳定性影响十分微小。

如表1所示，本文算法路径长度的平均值为140.46 m(十分接近直线距离127.37 m)，与RRT-connect算法、标准RRT算法及RRT-biasing算法相比分别缩短了19 %,31 %和16 %；在图5(b)中可以看出，本文算法输出路径长度主要分布在150 m以内，少数离群异常点也都能维持在160 m左右。这说明本文的路径修剪策略是有效的，与其他三种算法的结果相比有较大优势。同时表1的标准差数据表明本文算法输出路径长度的稳定性也要优于其他三种算法。

4 结 论

在进行1 000次重复规划实验后，结果表明：本文算法运行时间较短具有实时性，路径长度接近理论最优值，规划质量得到提高。