动态步长BI-RRT的无人机航迹规划算法

武晓晶　许磊　甄然　吴学礼

摘 要：为解决传统RRT算法收敛速度慢、生成的航径距离过长等问题，提出动态步长BI-RRT算法。首先，采用引向目标的采样策略对空间进行探索以得到采样点，利用动态步长策略确定该采样点的增长步长以确定新节点;之后，通过树枝裁剪策略对新节点进行调整，当探索到目标节点时，算法返回初始航迹，对于初始航迹，应用贪心算法对航迹点进行筛选，以减少无人机（UAV）的无效节点与总航迹长度;最后，利用B样条进行平滑处理，得到一条可行航迹。搭建了二维和三维环境下的仿真地图模型，验证了该算法在保证无人机避障的基础上获得一条有效航迹。动态步长BI-RRT算法在无人机航迹规划方面不仅有实时性强、航迹光滑的优点，而且与分段优化RRT算法相比，在优化航迹节点个数的前提下，提高了收敛速度且降低了航迹距离。

关键词：航空、航天科学技术基础学科其他学科;双向快速扩展随机树（BI-RRT）;概率搜索;引向目标动态步长;树枝裁剪策略

中图分类号：TP273;V279+.2 文献标志码：A

doi：10.7535/hbkd.2019yx05005

Dynamic step BI-RRT UAV path planning algorithm

WU Xiaojing1，2， XU Lei1， ZHEN Ran1，2， WU Xueli1，2

（1.School of Electrical Engineering， Hebei University of Science and Technology， Shijiazhuang， Hebei 050018， China; 2.Hebei Engineering Technology Research Center of Production Process Automation， Shijiazhuang， Hebei 050018， China）

Abstract：To solve the problems of traditional RRT algorithm， such as slow convergence and long path length， the dynamic step BI-RRT algorithm is proposed. The sampling efficiency of the RRT algorithm is improved by using a target-oriented sampling strategy. The dynamic step algorithm improves the local smoothness of the path in the obstacle environment. A new tree cutting strategy is proposed to make the growth trend of random trees more obvious and improve the convergence rate of the algorithm. For the initial path， the path nodes are filtered by applying greedy algorithm to reduce the invalid nodes and path length of the unmanned aerial vehicle （UAV）. Finally， a feasible smooth path is obtained by using B-Spline. The simulation maps in 2D and 3D environment are built to verify the effectiveness of the algorithm. Compared to the segmentated optimization RRT algorithm， the dynamic step BI-RRT algorithm proposed in this paper improves the convergence rate and reduces the path distance， while ensuring that the number of path nodes is optimal. This path planning algorithm has some advantages such as good real-time performance and smoothness of path.

Keywords：basic science and technology of aeronautics and astronautics other disciplines; bidirectional rapidly-exploring random tree （BI-RRT）; probability search; target oriented dynamic step size; branch cutting strategy

近年來，随着无人机在现场救援、地形探测等方面的应用，航迹规划已成为无人机技术的一个重要领域[1]。无人机航迹规划是在考虑无人机自身运动品质与地域环境等约束条件下，计算出品质最佳的航迹。航迹的安全性、平滑度和最短航距等多种动态约束对智能算法提出了更大的挑战。

传统的航迹规划算法包括A*算法[2]、蚁群算法[3]、人工势场法[4]、神经网络算法[5]，但这些离散化算法均存在收敛速度较慢、难以应用在高维的复杂环境中、需要已知空间模型、针对未知环境会出现失效等问题。RRT算法具有快速性与强大的空间探索能力，可通过随机采样拓展解空间，区别于依赖已知障碍物的传统算法，RRT算法不需要依赖于环境建模就能够有效解决未知复杂障碍物空间和高维动态环境的航迹规划问题[6]。

基于RRT算法，研究人员在路径规划方面做了大量工作。其中，文献[7]在随机树节点增长过程中融入无人机动力学约束条件，优化了节点配置。文献[8]通过引入基于障碍物的动态步长算法，解决了固定步长引起的局部极小问题。文献[9]改进了随机配置节点的机制，避免了路径上的冗余节点，减少路径上生长的节点，降低规划成本，避免了对空间的过度搜索。文献[10]和文献[11]针对大规模以及复杂环境下收敛速度过慢的问题提出PB-RRT*和Quad-RRT算法，在提高算法收敛速度的同时更有效地利用了内存。针对随机采样机制导致的路径不平滑问题，文献[12]利用四次样条策略生成平滑路径。

在无人机航迹规划方面，文献[13]采用冗余节点裁剪策略减小航线长度，但未充分利用RRT算法在高维空间的探索能力。文献[14]通过无人机间共享威胁信息，测试航迹规划的实时性，但未考虑规划航迹的可飞性与平滑性。文献[15]提出了分段优化RRT的无人机动态规划算法，加快了无人机航迹规划速度，缩短了航迹距离，但在复杂环境下规划效果难以保证。以上文献虽都基于RRT算法做出无人机航迹规划的优化，但均在二维环境下进行验证，未涉及三维环境，实用方面具有局限性。目前，RRT算法在三维环境进行航迹规划的应用还非常有限[16-17]。

本文提出了动态步长BI-RRT算法与一种新的树枝裁剪策略，并通过无人机航迹规划仿真实验对该算法进行验证。

1 无人机航迹规划问题

1.1 航迹规划问题描述

无人机航迹规划问题旨在复杂环境中于起始点与终止点之间寻找一条满足约束条件的航迹[18]。其中障碍物区域设定为静态，即认为是建筑物或山体。对于非完整约束条件下实现近似最优解的问题，大多算法都停留于二维环境。RRT算法对于高维与动态环境具有很高的探索效率与适应性。

1.2 二维环境模型

二维隔断式地图如图1所示。用黑色像素代表障碍物区域，白色像素代表自由区域。通过建立隔断式地图对算法进行测试。

1.3 三维环境模型

无人机三维环境下航迹规划空间环境可以表示为点集{（x，y，z）|xmin≤x≤xmax，ymin≤y≤ymax，zmin≤z≤zmax}，其中x，y表示无人机在水平面投影的坐标，z表示海拔高度。通过连接航迹点{Pinit，P2，…，Pgoal}，生成无人机规划的航迹。对于无人机的飞行环境，用圆锥体近似表示山体，实现对现实环境空间的建模，通过指数函数生成多个不同的圆锥体并进行叠加形成山体[19]。考虑z=0平面作为参考海拔，山体建模如式（1）所示：

式中：h0为空间基准高度;hk为第k座山峰的高度;n为空间中山峰的个数;x0k和y0k分别为第k座山峰中心点所对应的坐标;xik和yik分别为第k座山峰对应的横向坡度与纵向坡度。

2 动态步长BI-RRT航迹规划算法

2.1 BI-RRT算法

BI-RRT通过实现2棵随机树的初始化和生长，完成对空间的高效探索。第1棵树T1以起始点为根节点，朝着第2颗树生长。第2棵树T2以目标点为根节点，朝着第1棵树生长。当两棵树的叶子节点相同或距离小于一个步长时，认为完成航迹规划。BI-RRT算法的流程如表1所示。

2.2 动态步长BI-RRT算法

2.2.1 基于引向目标的偏向性算法优化原理

针对随机树探索过程中扩展效率低、收敛速度慢的問题，在BI-RRT算法的结构上进行改进。通过配置引力因子，削弱随机采样机制。在随机采样过程中，根据引力因子的值来决定随机树的生长方向，降低采样过程的随机性。引力因子以一定的概率配置随机节点Xrand与目标点Xgoal的关系，使得Xrand=Xgoal，随机树以一定趋势朝目标生长，减少随机采样的规划过程。引向目标的采样策略保证随机树在充分探索空间的同时提高收敛速度。

2.2.2 动态步长优化策略

BI-RRT算法通过对整个解空间进行规划，构建从起始点Xinit朝向目标点Xgoal生长的可行路径树T1。通过迭代生长，生成随机节点Xrand，同时找到在T1上与之最近的节点Xnear，再以固定步长S向XnearXrand方向进行生长，生成新节点Xnew。检测Xnew是否在地图内且在障碍物区域外，直到搜索到第2棵树T2。更新新节点Xnew的计算公式如式（2）所示。

固定步长增长机制导致随机树遇到障碍物时，生成的航迹不平滑，使无人机产生过多无效动作。针对这一问题，本文算法提出一种动态步长策略。基本思想：设定一个阈值q，表示无人机与障碍物之间的最小安全距离。当无人机与障碍物之间距离d大于或等于阈值，认为无人机处于空旷空间，此时以固定步长进行生长探索。当无人机与障碍物之间距离d小于阈值时，认为无人机处于障碍物环境，此时动态缩短步长S，以保证航迹的平滑性。动态步长的数学描述如式（3）所示：

式中：d≥q时，表示空旷环境下随机树的固定增长步长，通过选取较大步长，加快航迹规划收敛速度;d

2.3 半角树枝裁剪策略

BI-RRT算法对整个规划空间进行随机采样，虽有利于航迹点在解空间中的均匀分布，但部分扩展区域对规划并没有实质性帮助，反而降低了收敛速度。因此，本文提出一种半角树枝裁剪策略，半角树枝裁剪策略原理图如图2所示。

BI-RRT算法树枝增长原理：基于随机点Xrand，找到树上T1与之最近顶点Xnear，再通过预设步长S生成Xnew。由于随机采样机制会出现采样域偏离，使得随机树出现“逆向生长”，导致收敛速度降低。半角树枝裁剪策略通过及时更新向量XnearXnew与向量XnearXgoal之间的角度α，保证航迹点可以探索空间的同时，加快规划速度。裁剪公式如式（4）所示：

式中：当角度α满足0≤α<90°时，认为Xnew满足规划约束条件，进行保留;当角度α满足90°≤α≤180°时，通过半角树枝裁剪策略对其进行修剪。其原理是以Xnear为轴心，S为轴长，将向量XnearXnew朝向量XnearXgoal旋转[SX（]α[]2[SX）]角度，获得修剪后的新节点X′new。

2.4 基于贪心算法优化成本函数

贪心算法是作出基于当前状态的最优选择。其原理为不从整体进行最优考虑，而是选择某种意义上的局部最优解。贪心算法通过每一步最优选择，最终使得整体解不断接近最优解。由于BI-RRT算法随机采样的特性，尽管通过引向目标的优化算法进行偏向性增长，但依然会产生很多曲折的路径与无效动作。考虑到无人机自身性能约束，大量无效动作会增加无人机的不稳定性与能耗。针对这一问题提出基于贪心算法的最短路径优化思想。贪心启发式算法与概率完备的BI-RRT随机采样特性相结合，很好地避免了局部极小的缺陷。

成本函数定义如式（5）所示：

式中Li表示航迹中相邻2个节点Pi，Pi+1之间的距离。

贪心算法原理：首先导入已规划得到的航迹节点，以第1个航迹点Pinit为优化路径的起点，每一次迭代挑选出距离起始点Pinit最近的后续节点Pnearest，并判断其与Pinit的连线PnearestPinit[TX-]是否会与障碍物相交，若不相交，则认为该节点Pnearest为无效节点，删除该无效节点，并继续检查后续节点Pnearest。若经过障碍物则保留该航迹点Pnearest，并以该航迹点为新起点继续向后续节点发起贪心选择，挑选离新起点最近的后续节点并检查其连线是否会经过障碍物，重复以上动作直到所有航迹点处理完毕。基于贪心选择的优化策略减少了规划器得到的航迹点，降低了无人机规划的复杂度，大大减少了无人机的无效动作与飞行距离。

2.5 基于B样条的平滑处理

由于BI-RRT随机采样的机制，使得算法规划的路径呈现出不规则的折线，航迹不够平滑、可飞性差。故需要作进一步的平滑处理。

B样条曲线具有设计自由型曲线曲面的强大功能。B样条兼备了Bezier方法的优点，如仿射不变性，同时克服了Bezier方法由于整体表示所带来的局限性。本文采用B样条曲线拟合方法，针对航迹拐点部分进行平滑处理，B样条曲线方程如式（6）所示：

式中：di（i=0，1，…，n）为控制顶点坐标;Ni，k（i=0，1，…，n）为k次规范B样条基函数，k为最高次数;基函数是由1个节点矢量的非递减参数u的序列U：u0≤u1≤…≤un+k+1所决定的k次分段多项式[20]。B样条的基函数采用Cox-deBoor递推公式：

Ni，0（u）=1，ui≤u≤ui+1，0，其他，

定义（7）

式中：i为节点序号;k是基函数次数，共有n+1个控制顶点。

2.6 算法流程图

动态步长BI-RRT算法的总体流程图如图3所示。

步骤1：随机点选取。通过引向目标的采样策略，在整个解空间选取采样点。

步骤2：随机点的调整。对步骤1选取的节点判别是否需要进行步长调整以满足障碍物约束条件。若满足约束条件，则以固定步长扩展新节点;若不满足，则动态调整步长并扩展新节点。计算新节点与目标点之间的偏离角度，根据角度判定条件对新节点进行裁剪调整。

步骤3：航迹优化。当2棵随机树的叶子节点重合或相距1个步长，认为找到航迹。对航迹节点进行贪心选择，得到最短航迹，并对其进行平滑处理，获得最终航迹。

3 实验验证

3.1 二维环境仿真

设置实验空间尺寸为500 km×500 km，起始点坐标设置为（0， 500），终点坐标设置为（500， 0）。分别用RRT算法、分段优化RRT算法、动态步长BI-RRT算法进行50次仿真实验，得到的结果如图4所示。图4中虚线部分表示算法生长的随机树，实线部分表示算法所得到的无人机航迹。其中，图4 a）—c）分别代表传统RRT算法、分段优化RRT算法[15]和动态步长BI-RRT算法得到的结果。从路径长度、航迹点数、规划时间3方面对比得到的仿真实验数据如表2所示。

数据分析如下。

从图4可以看出，与传统RRT算法和分段优化RRT算法相比，本文所提出的动态步长BI-RRT算法在保证航迹平滑性的同时节约了扩展空间。从表2可以看出，本文所提出的动态步长BI-RRT算法大大降低了规划时间，缩短了路径长度。

注1：传统RRT算法基于全空间进行航迹规划，航迹距离过长、收敛速度过慢。分段优化RRT算法[15]由于采用了面向目标的采样策略，使得规划器在扩展过程中节约了扩展空间，但收敛速度较慢，依然有过多无效树枝。而采用本文所提出的动态步长BI-RRT算法，通过2棵树同时生长大大缩短了规划时间，提高了航迹规划的实时性，同时采用树枝裁剪策略，使随机树的扩展趋势更加明显。

注2：与文献[15]相比，动态步長BI-RRT算法大大提高了航迹规划的收敛速度与算法的偏向性，降低了成本函数。同时本文所提出的算法可推广应用到三维环境，解决三维环境下的航迹规划问题，算法更具实用性。

3.2 三维环境仿真

規划空间采用山峰威胁模型实现对现实环境的模拟。设置实验空间尺寸为80 km×80 km×40 km，规划空间中设置6座山峰，中心坐标分别为（10，10），（40，25），（45，50），（60，30），（20，45），（20，10）。高度分别为20，35，25，38，20，25 km。威胁源主要为山体，起始点设置为[5，70，5]，终点设置为[80，30，10]，并对空间进行建模，如图5所示。

采用RRT算法与动态步长BI-RRT算法进行50次仿真实验，得到的三维航迹规划结果如图6所示。其中，传统RRT算法在三维环境的规划结果如图6 a）所示;图6 b）为本文所提出的算法得到的三维仿真结果。从路径长度、航迹点数、规划时间3方面得到对比数据，如表3所示。

从图6和表3可以看出，本文所提出的算法在三维环境下仍然具有很好的收敛性，与传统RRT算法对比，大大缩短了路径长度和规划时间，提高了飞行效率。

4 结 语

针对RRT算法随机性过大、收敛速度较慢、搜索节点过多等问题，提出了动态步长BI-RRT的无人机航迹规划算法，通过仿真实验验证了该算法的优势。与分段优化RRT算法面向目标的采样策略相比，本文所提出的目标引向策略和半角树枝裁剪策略，提高了算法的采样效率，缩短了航迹规划的时间，加快了算法的收敛速度。同时，提出的动态步长策略提高了随机树对障碍物的敏感性。仿真数据表明，该算法得到的航迹具有航径短、收敛速度快、可飞性强、内存需求小等优点。今后的研究中，将考虑动态障碍物综合避险问题，并对此算法进行优化，以提高其动态性能。

参考文献/References：

[1]MONTIEL O， OROZCO-ROSAS U， SEPULVEDA R. Path planning for mobile robots using Bacterial Potential Field for avoiding static and dynamic obstacles[J]. Expert Systems with Applications， 2015， 42（12）： 5177-5191.

[2]SONG Rui， LIU Yuanchang， BUCKNALL R. Smoothed A* algorithm for practical unmanned surface vehicle path planning[J]. Applied Ocean Research， 2019， 83： 9-20.

[3]PEREZ-CARABAZA S， BESADA-PORTAS E， LOPEZ-OROZCO J A， et al. Ant colony optimization for multi-UAV minimum time search in uncertain domains[J]. Applied Soft Computing， 2018， 62： 789-806.

[4]HU Hongyu， ZHANG Chi， SHENG Yuhuan， et al. An improved artificial potential field model considering vehicle velocity for autonomous driving[J]. IFAC-PapersOnLine， 2018， 51： 863-867.

[5]BHARADWAJ H， KUMAR E V. Comparative study of neural networks in path planning for catering robots[J]. Procedia Computer Science， 2018， 133： 417-423.

[6]王全， 王维， 李焱，等.基于混合策略的轮式机器人路径规划方法[J].计算机工程与应用， 2014， 50（4）：45-49.

WANG Quan， WANG Wei， LI Yan， et al. Wheeled robot path planning method based on hybrid strategy[J]. Computer Engineering and Applications， 2014， 50（4）： 45-49.

[7]尹高扬， 周绍磊， 吴青坡.基于改进RRT算法的无人机航迹规划[J]. 电子学报， 2017， 45（7）：1764-1769.

YIN Gaoyang， ZHOU Shaolei， WU Qingpo. An improved RRT algorithm for UAV path planning[J]. Acta Electronica Sinica， 2017， 45（7）： 1764-1769.

[8]林娜， 张亚伦. 自适应RRT无人机航路规划算法研究与仿真[J]. 计算机仿真， 2015， 32（1）：73-77.

LIN Na， ZHANG Yalun. Research and simulation on adaptive RRT algorithm for UAVs path planning[J]. Computer Simulation， 2015， 32（1）： 73-77.

[9]ZHANG Haojian， WANG Yunkuan， ZHENG Jun， et al. Path planning of industrial robot based on improved RRT algorithm in complex environments[J]. IEEE Access， 2018：2871222.

[10]TAHIR Z， QURESHI A H， AYAZ Y， et al. Potentially guided bidirectionalized RRT* for fast optimal path planning in cluttered environments[J]. Robotics     and Autonomous Systems， 2018， 108： 13-27.

[11]HIDALGO-PANIAGUA A， BANDERA J P， RUIZ-de-QUINTANILLA M， et al. Quad-RRT： A real-time GPU-based global path planner in large-scale real environments[J]. Expert Systems with Applications， 2018， 99： 141-154.

[12]JANJO  F， REICHART R， NIERMEYER P. Smooth path-generation aroud obstacles using quartic splines and RRTs[J]. IFAC-Papers-OnLine， 2017， 50： 9108-9113.

[13]李俊涛， 毛红保， 张鹏，等.基于多优化策略RRT的无人机实时航线规划[J]. 火力与指挥控制， 2017， 42（12）：115-119.

LI Juntao， MAO Hongbao， ZHANG Peng， et al. Multi-optimization RRT based UAV real-time trajectory planning algorithm[J]. Fire Control & Command Control， 2017， 42（12）： 115-119.

[14]ZHENG Zheng， LIU Yang， ZHANG Xiaoyi. The more obstacle information sharing， the more effective real-time path planning？[J]. Knowledge-Based Systems， 2016， 114： 36-46.

[15]李文广， 孙世宇， 李建增，等. 分段优化RRT的无人机动态航迹规划算法[J]. 系统工程与电子技术， 2018， 40（8）：1786-1793.

LI Wenguang， SUN Shiyu， LI Jianzeng， et al. UAV dynamic path planning algorithm based on segmentated optimization RRT[J]. Systems Engineering and Electronics， 2018， 40（8）： 1786-1793.

[16]WEN Naifeng， ZHAO Lingling， SU Xiaohong， et al. UAV online path planning algorithm in a low altitude dangerous environment[J]. IEEE/CAA Journal of Automatica Sinica， 2015， 2（2）： 173-185.

[17]RAMANA M V， VARMA S A， KOTHARI M. Motion planning for a fixed-wing UAV in urban environments[J]. IFAC-PapersOnLine，2016， 49： 419-424.

[18]刘华伟， 张帅， 赵搏欣，等. 改进RRT算法陷阱空间下的无人机航迹规划[J]. 计算机工程与应用， 2018， 54（8）：119-122.

LIU Huawei， ZHANG Shuai， ZHAO Boxin， et al. UAV path planning based on improved RRT algorithm in trap space[J]. Computer Engineering and Applications， 2018， 54（8）： 119-122.

[19]XUE Qian， CHENG Peng， CHENG Nong. Offline path planning and online replanning of UAVs in complex terrain[C]// Guidance， Navigation & Control Conference.[S.l.]：IEEE， 2015：7007525.

[20]施法中. 計算机辅助几何设计与非均匀有理B样条（修订版）[M]. 北京：高等教育出版社， 2013：217-248.