基于综合改进RRT算法的无人机三维航路规划

王 康，郭剑东，桑 标

(1.南京航空航天大学自动化学院，江苏 南京 210016;2.南京航空航天大学中小型无人机先进技术工信部重点实验室，江苏 南京 210016)

1 引言

无人机因具有灵活轻便、机动性强、隐蔽性好等优点，已经广泛应用于民用和军用领域[1]。但随着无人机执行任务和工作环境错综复杂复杂，自主避障技术成为无人机发展的关键技术之一。作为常用的无人机自主避障方法，无人机航路规划方法受到广泛关注。

无人机的航路规划问题是指在一定区域范围内、一定约束条件下，无人机具有自主搜索一条无障碍可行航路连接给定起始点和终点[2]。无人机航路规划算法主要可以分为启发式算法和非启发式算法，启发式算法有人工势场法[3]、遗传算法[4]、A*算法[5]等，非启发式算法有维诺图法[6]、快速探索随机树法[7-8](Rapidly-exploring Random Tree， RRT)等。文献[9]综合比较了以上算法，对比结果表明，人工势场法收敛速度较快，但容易陷入局部最优；遗传算法虽然从群体出发同时进行多次迭代，但随着细胞数量的增加对时间的敏感度降低；A*算法主要通过牺牲收敛速度来提高最优性；维诺图法一般需结合智能算法搜索最优路径，收敛速度较慢。航路规划算法通常要在收敛速度和最优性之间存在矛盾，因此，要根据实际情况选择合适的算法。

RRT算法[7]由于采用增量方式增长、随机采样等特点，能够有效解决复杂环境中障碍物带来的航路搜索困难问题，其优势在于无需对飞行任务环境进行几何划分，对任务空间的搜索范围广、覆盖率高。但基本RRT算法节点搜索随机性较强，导致航路搜索效率低，且搜索到的航路往往整体上较差。针对基本RRT算法的缺点，文献[10]在基本RRT的基础上提出RRT-Connect算法，将单向随机搜索树改进为双向随机搜索树，提高了全局搜索能力和效率，但规划航路平滑度仍然较差；文献[11]提出RRT*算法，通过最优准则动态调整扩展随机树的父节点，提高了航路的渐进最优性，但是降低了收敛速度；文献[12]将RRT-Connect与RRT*相结合有效地弥补了RRT*算法收敛速度较慢的问题。为提高RRT算法的收敛速度，改善规划航路质量，也提出一些优化策略，如启发式策略[13]和目标引力策略[14]等。

本文针对复杂飞行环境下基本RRT算法节点扩展随机性强、航路搜索效率低、规划航路曲折等问题提出了综合改进方法。首先在启发式引导策略的基础上，加入贪婪策略，提高了节点向终点方向扩展的收敛速度；然后在目标引力策略的基础上，提出动态引力步长策略，保证了节点搜索概率完备性的同时，降低了节点扩展方向的随机性；最后设计了一种航路平滑度优化策略，提高了规划航路的平滑度。仿真结果表明，本文提出的综合改进RRT算法在复杂飞行环境下有较好的避障效果。

2 航路规划问题

2.1 任务空间建模

无人机飞行任务环境中常常存在各种类型障碍物，如建筑物、山峰、树林等，这些障碍物大多情况下都是不规则的，直接处理起来比较困难，过分关注障碍物的细节问题，也将大大增加了任务空间建模的困难程度，增加了航路规划算法计算量。因此，本文根据障碍物信息特征简化障碍物模型，选择半球、圆柱、圆锥、圆台等标准的凸多面体替代各类障碍物，以确保能覆盖障碍物的整体或关键部分。它们的近似表达式可以统一为

(1)

其中，(x，y，z)为障碍物模型上任意一点，(x0，y0，z0)为障碍物模型的中心坐标，参数a，b，c和p，q，r决定障碍物模型的形状和大小。Γ<1、Γ=1、Γ>1分别表示在障碍物模型内、表面、外。

通过对障碍物建模，可以将复杂环境中的障碍物用合适的一个或多个凸面体等效代替。复杂环境下任务空间模型如图1所示，任务空间大小为10km×10km×3km，起点为(0，0，0)km，终点为(10，10，0.5)km。

图1 复杂环境下的任务空间模型

2.2 问题描述

本文将飞行任务空间中所有障碍物的表面与内部均视为禁飞区或威胁区。假设任务空间为S，Sobs表示障碍物威胁区域，即Γ≤1，Sfree表示可飞安全区域，即Γ>1，三者的关系为：

(2)

起点Pstart∈Sfree，终点Pgoal∈Sfree，则航路规划问题可以表示在任务空间S中搜索出从起点Pstart到终点Pgoal的可行航路。

2.3 无人机性能约束

无人机在飞行过程中除了受到环境约束外，还受到自身飞行性能约束，在进行航路规划时，需充分考虑各类约束条件，主要有：

1)最大航程

假设一条完整航路由n条长度为li的航路段组成，最大飞行航程用Lmax表示，则最大航程约束可以表示为

(3)

2)最短航路段

为确保无人机在飞行姿态调整时的安全性，则需要一定的直飞距离，最短直飞航路段用lmin表示，则最短航路段约束表示为

li

(4)

3)最大转弯角

ai和ai+1分别表示无人机第i段和i+1段航路的水平投影向量，最大转弯角用φmax表示，则最大转弯角约束表示为

(5)

4)最大爬升/俯冲角

(xi，yi，zi)和(xi+1，yi+1，zi+1)分别表示无人机航路上的第i和i+1个航点坐标，最大爬升/俯冲角用ϑmax表示，则最大爬升/俯冲角约束表示为

(6)

5)最大飞行高度

假设最大飞行高度为Hmax，则无人机飞行过程中，实时高度Hi不能超过其最大飞行高度约束，即

Hi≤Hmax

(7)

3 基本RRT算法原理

图2 基本RRT算法示意图

4 综合改进RRT算法

基本RRT算法在随机树构造过程中，节点选择随机性太强，会产生很多冗余节点，增加了航路的搜索时间，且航路整体上平滑度较差。因此RRT算法在随机树构造中既要满足节点有一定的随机性，保证了复杂任务空间中的搜索能力；也要尽可能满足随机树朝终点方向生长，提高收敛速度和航路平滑度。故本文提出以下综合改进措施。

4.1 贪婪启发式策略

为降低基本RRT算法节点搜索的随机性，文献[13]提出了一种基于概率的启发式引导策略，即通过选择一个合适的概率p1∈(0，1)，令随机目标点Prand等于终点Pgoal的概率为p1，即

p(Prand=Pgoal)=p1

(8)

当随机目标点Prand以概率p1等于终点Pgoal时，相当于目标最优搜索，此时为了尽可能让节点朝着终点方向扩展，本文在终点方向的节点扩展上加入贪婪策略，即一直沿着随机目标点Prand与终点Pgoal连线方向上扩展，直至遇到障碍物，再进行下一随机选取目标点。

通过在启发式引导策略上引入贪婪策略，减少了目标最优搜索时节点的选择，不仅提高了局部收敛速度，也让搜索航路尽可能的为直线，提高了航路平滑度。

4.2 动态引力步长策略

贪婪启发式策略提高了节点朝终点方向扩展速度，但并没有考虑随机节点的选择。文献[14]在随机节点的选择上提出目标引力策略，该策略的思想是通过将随机目标点Prand与终点Pgoal经过一定的组合得到新的目标点Pnode，则新的扩展节点Pnew的计算公式为

=Pnear+Pnode

(9)

其中，ρ1为随机目标点方向扩展步长，ρ2为终点方向扩展步长。ρ1、ρ2的大小选择直接决定了新节点的扩展方向和步长，故ρ1、ρ2的选择尤为重要。但文献[14]并没有给出不同任务空间下步长ρ1、ρ2的选择方法。本文根据节点Pnear到障碍物的距离来确定ρ1、ρ2的大小，得到ρ1、ρ2的表达式为

(10)

其中，ρ为固定步长，kp为引力系数，其表达式为

(11)

其中，(xi，yi)表示第i个障碍物圆心坐标，N表示障碍物个数，Ri，z表示节点Pnear高度下障碍物的半径，W为调节因子，其大小由无人机飞行环境复杂程度决定。当节点Pnear与障碍物距离较远时，kp>1，ρ2>ρ1，新节点Pnew会尽可能地沿着终点Pgoal方向扩展；反之，当节点Pnear与障碍物距离较近时，kp<1，ρ1>ρ2，新节点Pnew会尽可能地沿着随机点Prand方向扩展。通过根据障碍物模型设计动态引力步长策略，不仅保证了复杂环境下航路搜索概率的完备性，而且尽可能降低了节点的随机性，提高了航路的整体品质。

由于扩展步长过长，在复杂环境下很难通过障碍物威胁检测，影响收敛速度；扩展步长过小，则会降低随机树生长速度和航路平滑度。本文对新节点的扩展步长大小做出如下约束

(12)

其中，ρmin、ρmax为根据无人机性能特性和飞行环境复杂程度确定的最小、最大步长。

4.3 平滑度优化方法

图3 航路平滑平面示意图

当得到规划航路后，从起点Pstart开始，按上述方法逐个节点递推直到到达终点Pgoal。该平滑方法去除了航路中的冗余节点，不仅减小了航路长度，而且提高了航路平滑度。

综合改进RRT算法流程如图4所示。

图4 综合RRT改进算法流程图

5 数字仿真验证

为验证综合改进RRT算法在复杂环境下三维航路规划性能，在MATLAB R2017a中进行仿真验证，计算机处理器为Inter Core i5，主频2.5GHz。仿真中，节点固定搜索步长ρ=1km，最大搜索步长ρmax=1.5km，最小搜索步长ρmin=0.5km。通过仿真100次，比较平均规划时间、平均航路长度和平均节点个数三个参数验证算法性能。

试验一为改进贪婪启发式RRT算法与启发式RRT算法比较试验。两种算法的性能对比结果如表1所示，图5为其中一次规划结果，其中“·”表示节点。

表1 试验一仿真性能对比

图5 试验一对比仿真结果

从图5可以看出，相比于启发式RRT算法，贪婪启发式RRT算法规划出的航路直线段更多、更长，且当到终点的视线内没有障碍物时，不存在随机节点导致航路不平滑的情况，因此整体航路更平滑。从表1可以看出，贪婪启发式RRT算法在整体性能上都有提升，尤其在平均规划时间上提升显著。

试验二为动态引力步长RRT算法与目标引力RRT算法比较试验。目标引力RRT算法的终点方向扩展步长固定，引力系数kp=1。两种算法的性能对比结果如表2所示，图6为其中一次规划结果。

表2 试验二仿真性能对比

图6 试验二对比仿真结果

从图6可以看出，相比于目标引力RRT算法，动态引力步长RRT算法在节点附近障碍物较少时，节点倾向于朝着终点方向扩展，整体航路更优。从表2可以看出，动态引力步长RRT算法整体性能也都有提升，尤其在平均节点个数和平均规划时间上提升显著算法

试验三为综合改进RRT与贪婪启发式RRT算法和动态引力步长RRT算法比较试验。三种算法的性能对比结果如表3所示，图7为其中一次规划结果。

表3 试验三仿真性能对比

图7 试验三对比仿真结果

从图7可以看出，贪婪启发式RRT算法规划出的航路虽然存在较长的直线段，但可能由于随机点选择问题导致出现较大的偏差和不合理的折线，整体航路较差；动态引力步长RRT算法虽然约束了随机点的选择，航路朝着终点方向扩展，但多次随机点的选择导致航路出现不必要的转向，降低了航路平滑度。相比与以上两种算法，综合改进RRT算法结合了两者的优势，规划出的航路不仅朝着终点方向扩展，且航路直线段更多，没有较大的转弯角度，航路整体更平滑。从表3可以看出，综合改进RRT算法结合了贪婪启发式RRT算法规划时间快、节点个数少和动态引力步长RRT算法航路长度短的优点，收敛速度更快、规划航路更短，也更平滑。

图8 航路平滑度优化对比仿真结果

图8为航路平滑度优化仿真结果对比图，从图中可以看出，平滑度优化方法有效地去除原规划航路中的冗余节点。且平滑后的航路节点个数从原本的14个降低到4个，航路长度更短，缩短了17.98%，航路转弯角度更小，整体上更平滑，有利于无人机避障与跟踪飞行。

6 结论

本文提出了一种综合改进RRT航路规划算法，针对复杂飞行环境下无人机的三维航路规划问题开展研究。提出贪婪启发式策略，以一定概率引导节点朝终点方向扩展，并提高终点扩展方向的收敛速度；提出动态引力步长策略，保证节点搜索概率完备性的同时，降低节点搜索的随机性；设计平滑度优化方法，出去航路冗余节点，提高航路平滑度。仿真结果表明，本文提出的综合改进RRT算法在复杂环境下能有效提高航路规划的收敛速度、缩短航路长度和提高航路平滑度。本文还考虑了无人机自身的限制和运动学特性，在实际工程中有较好的实用性。