基于改进RRT算法的无人机三维避障规划

2021-11-17李克玉陆永耕鲍世通徐培真

计算机仿真 2021年8期

李克玉，陆永耕，鲍世通，徐培真

(上海电机学院电气学院上海 200240)

1 引言

无人机的避障规划是指在未知障碍物的环境中，无人机能够借助信息的反馈，在一些约束条件(如时间最短、距离最短和能耗最低等)下，自主分析环境信息并规划出一条从起始状态到目标状态的无碰撞路径[1，2]。国内外很多学者对无人机的路径规划避障算法进行了研究，而且提出了很多优化的算法。其中启发式算法有遗传模拟退火算法[3]、蚁群算法[4]、遗传算法[5]、A*算法[6，7]等。数学优化算法主要有人工势场法[8]、快速扩展随机树(RRT)法[9]等。

RRT算法由于其无需对系统进行建模，无需对搜索区域进行几何划分，搜索的范围广等众多优势[10]，其在避障规划上的运用受到了很多学者的青睐，改进方法也层出不穷。RRT*算法以新增节点半径r的邻域内随机树上所有的节点为备选节点，很好的解决RRT算法生成避障路径非最优问题[11]；RRT-Connect算法通过在起始点和目标点并行生成两棵随机树的方式来提高寻找避障路径的速度[12]，将其与桥梁检测(Bride Test)算法融合，很好的解决了RRT-connect算法在窄道内采样难的问题[13]；在RRT算法中引入人工势场法，很好的利用了势场函数法能够携带障碍物和目标点等优点，使扩展树搜索更具有方向性的同时还提高了障碍物搜索效率[14]。

由此可以看出，在二维空间内，目前国内外对RRT算法的避障路径快速生成[15]和优化窄道局部采样[16]等方面做了很好的改进，使得改进后的RRT算法在避障能力和搜索效率等方面都有很大的提高。但是对于三维空间，由于维度的增加，相关建模和数据处理难度也相应的增加[17]，如果依旧使用二维空间的RRT算法以及相关的改进方法进行无人机路径规划，其效率将会降低甚至无法完成完成路径规划[18]。因此本文提出了一种基于预规划路径优化RRT算法的无人机三维避障规划算法，该算法首先在障碍物膨胀规则和起点到终点连线与障碍物相交规则下生成预规划路径，此预规划路径可强化RRT扩展树搜索的连贯性，减少障碍物碰撞检测的时间；将预规划路径看做由连续的质点构成，连续质点可作为搜索树在扩展中的随机状态点，可使搜索树的扩展具有方向性，进而减少避障搜索的时间，提高无人机避障规划的效率，使得生成避障路径的时间更优。

2 改进算法的基本原理

2.1 RRT算法原理

在度量空间X(包括障碍区Xobs和自由区Xfree)中，首先将起始状态点Xstart作为随机树的根节点，向每个方向扩展一定距离，在自由区Xfree取一个状态点Xrand作为根节点的目标点，然后选择距离此状态点最近的树扩展节点Xnearest作为新的生长基点，向此状态点方向扩展一个定步长L，得到该方向的新节点Xnew，若在步长L扩展过程中与障碍物相撞，则重新选择状态点按上述规则重新扩展，重复上述过程，直到有树扩展节点到达目标状态点Xgoal附近为止，最后路径回溯，生成的回溯路径即为RRT算法生成的路径[19，20]。图1为RRT搜索树节点扩展过程。

图1 RRT搜索树节点扩展过程

2.2 障碍物的膨胀规则

为了保证预规划路径的生成和无人机遇到障碍物时的安全，便于计算机识别和处理，本文基于文献[21，22]，对障碍物膨胀过程的方式提出改进。具体改进方法是利用最小的长方体完全包围障碍物，以长方体的对角线长度的1.2倍为直径做障碍物的外接球。

图2 障碍物的膨胀

2.3 相交规则

2.3.1 具体步骤

设膨胀化的障碍物的球心Xoi(xoi，yoi，zoi)及球的半径为Roi，无人机的起点为X0(x0，y0，z0)，终点为Xg(xg，yg，zg)。

Step1：连接起点和终点。

Step2：计算球心到连线的垂足坐标。

(xni-xoi)(xg-x0)+(yni-yoi)(yg-y0)

+(zni-zoi)(zg-z0)=0

(1)

垂足Xni在连线上，根据向量共线可知：

(2)

将(2)式代入(1)式，式中只有一个未知数k，可化解k为

(3)

将(3)式代入(2)式即可求出垂足的坐标Xni，设各球心到连线的距离为Li。

Step3：检查连线是否与障碍物相交，只要判断球心到连线的距离与球的半径的关系即可。

1)当Li>Roi时，直线与球不相交，则可判断此连线为预规划路线；

2)当Li=Roi时，直线与球相切，则可判断此连线为预规划路线，且切点为预规划路线与球面的交点；

3)当Li

Step4：确定预规划路径新起点

延长垂线交球面于点Xi，可求得交点坐标为

(4)

Step5：以交点为新起点，跳转至step 1。

2.3.2 预规划路径生成

在障碍物膨胀规则和相交规则下，预规划路径的生成有以下俩种情况：

1)当Li>=Roi时，起点与终点的连线与障碍物的膨胀球体不相交或与球面相切，可判断此连线为求得的预规划路径。此预规划路径可看作是由连续的质点组成，而RRT算法的随机状态点Xrand可从连续的质点中通过每单位长度提取来得到，具体实现方法如下：

①根据已知的起点X0和终点Xg，由三维空间俩点式求出预规划路径的直线方程如下式

(5)

②通过控制X或Y或Z轴按一定的扩展树步长的比例取值来确定随机状态点Xrand(i)(xrand(i)，yrand(i)，zrand(i))的选取及坐标。设xrand(i)=x0+i×AL，根据上式(5)可得

这样在三维空间X中，RRT算法在预规划路径上等量划分的随机状态点的指引下，使搜索树更具有方向性，减免了很多不必要的扩展节点，进而提高避障规划的效率。

2)当Li

图3 球体切割面

以交点为新起点连接终点，通过相交规则判断新连线是否可以作为预规划路径，循环往复，直至最后连接终点时的连线与障碍物没有相交(Li

此种情况生成的预规划路径可分成一段一段来处理，可根据每条线段的俩个端点坐标来确定分段函数，每个分段函数可根据(1)中的处理方式来确定RRT搜索树的随机状态点，RRT搜索树可沿着这些随机状态点进行扩展。其示意图如图4。

图4 预规划路径分段处理示意图

3 仿真分析

3.1 仿真环境

为了验证上述改进算法的有效性，本文基于MATLAB2018a在ThinkPad Intel(R)Core(TM)i3-5005主频2.00GHz的计算机上进行仿真。仿真的环境设置为100km*100 km*100km的区域，在空间中随机分布6个障碍物，障碍物经过膨胀化处理后为球体。这6个球体的相关信息见表1所示。

表1 障碍物相关信息表

在上述环境下，无人机从起点X0(0，0，0)匀速飞行到终点Xg(100，100，100)。

3.2 实验图例

分别采用原RRT算法、文献[22]中基于切点优化人工势场法和本文改进的RRT算法在相同环境下对无人机进行三维避障规划，通过生成避障路径的相关数据对比，来证明本文改进算法的优势。

3.2.1 预规划路径及随机状态点的生成

在上述障碍物建模中运用本文提出的预规划路径生成策略，设置步长L=5，比例系数A=2，在分段的预规划路径上分别采样的RRT的随机状态点，在环境约束下，本文总共采样11个随机状态点，如图5。

图5 随机状态点采样

4 各路径规划算法仿真对比图

4.1 原RRT算法仿真图

图6 正面图

图7 Y轴视图

4.2 文献[22]改进的算法仿真图

图8 正视图

4.3 本文改进的RRT算法仿真图

图9 正视图

图10 Y轴视图

4.4 三种算法的相关数据对比

本文基于原RRT算法和本文改进RRT算法分别采样了算法的运行时间、搜索树的扩展节点总数、生成路径节点数和生成路径的长度四种数据，基于文献[22]介绍的算法值采样了算法的运行时间和生成避障路径的长度俩种数据，并在相同环境下各取10次仿真结果的平均值来比较三种算法的特性。表2中法一代表原RRT算法，法二代表文献[22]介绍的算法，法三本文改进的RRT算法。从表2可以很清楚的看出，在设定的相同静态障碍物的三维环境下，本文改进的算法较原RRT算法的平均节点个数生成总量减少将近一半，生成的避障路径节点占比明显增大，无效节点明显减少，避障路径趋于平缓，算法复杂度降低，效率提高；较原RRT算法和文献[22]介绍的算法的避障路径搜索时间要短，生成的避障路径平均长度也要短；由此可见，基于预规划路径优化RRT算法有效的减少了无人机避障规划时间，并提高了避障路径搜索的效率。