在现代战争中,无人机所处的战场环境瞬息万变,同时由于其所执行任务的不确定性,无人机要能够根据任务过程中的实时环境信息进行在线的自主航迹规划,这对规划算法的实时性提出了很高要求[1, 2]。传统的航迹规划方法是基于预先确定的代价函数生成一条具有最小代价的路径。由于无人机航迹规划的规划区域广阔,虽然近年来研究人员从规划环境建模和求解算法两方面均提出了许多改进策略,但由于规划算法强调航迹的最优性,加上要考虑无人机的性能约束等要求,传统的规划方法如遗传算法[3, 4]、蚁群算法[5, 6]、稀疏A*算法[7, 8]、粒子群算法[9, 10]等要获得一条最优路径需要很长的收敛时间和极大的内存需求,失去了在线航迹规划的现实可行性。
快速扩展随机树(RRT)方法是一种基于采样的单查询随机搜索算法,由S.M.LaValle在1998年首次提出[11]。RRT的节点扩展不需要在规划前执行预处理,能够根据当前的环境信息快速有效地搜索规划空间,在可行路径的搜索概率意义上是完全的。RRT方法已被成功应用于许多非完整系统的规划问题中,包括地面移动机器人运动规划[12]和无人机航迹规划[13]等问题。虽然基本RRT方法具有很多良好特性,但是其随机性太强,只能够保证高效快速地获取无人机的可行轨迹,无法获得较优轨迹。针对基本RRT方法的不足,研究者们从RRT方法的节点采样方式、节点扩展方式和节点选择方式3个方面对其进行了改进,取得了一定的成果[14, 15, 16, 17, 18, 19]。
基本RRT及其改进方法虽然能够满足无人机在线自主航迹规划的应用要求,但是它们都是在二维平面上进行航迹搜索,因而未能利用地形的高度信息,不能有效地利用地形进行地形规避和威胁规避。本文在基本RRT方法的基础上,提出了一种三维快速航迹规划方法。并在生成的虚拟数字地形图上进行了仿真验证,实验结果表明,该方法能够充分利用地形的高程信息,有效地进行地形回避和威胁回避,是一种快速有效的三维航迹规划算法。
1 问题提出无人机三维在线航迹规划要求无人机根据预先装载的任务区的三维地形数据,规划出一条从规划起始点到目标点的三维航迹,该航迹避开了所有三维障碍物和威胁,并且满足无人机自身的飞行性能约束。无人机在惯性坐标系下的三自由度质点运动方程为:
![](PIC/20160403-M1.jpg)
具体来说,三维航迹规划生成的目标航迹要求满足以下几个约束条件[20]:
1) 最小航迹段长度:无人机在开始改变飞行姿态前必须保持直飞的最短距离。这一约束取决于无人机的机动能力和导航要求。
2) 最大拐弯角:无人机只能在小于或等于由自身水平平面机动性能预先确定的最大拐弯角范围内转弯。
3) 最大爬升、俯冲角:无人机只能在小于或等于由自身垂直平面机动性预先确定的上升和下滑的最大角度内爬升或俯冲。
4) 最大航迹长度:无人机的规划航迹的长度必须小于或等于一个预先设置的最大距离,它由无人机所携带的燃料以及指定任务中到达目标的允许飞行时间决定。
5) 最低飞行高度:无人机利用地形进行地形回避和威胁回避时,需要尽可能在离地面低的高度上飞行,但是飞得过低往往会增加与地面的相撞概率。一般在保持离地高度不小于某一指定高度的前提下,使飞行高度尽量降低。
只有满足了以上基本约束条件的三维航迹才是无人机的三维可行航迹。本文将这些约束条件结合到基本RRT算法中,从而得到一种快速有效的无人机三维航迹规划方法。
2 基于RRT方法的三维航迹规划基于RRT方法的航迹规划以状态空间中的规划起始点为根节点,通过随机采样逐渐增加叶节点的方式生成随机扩展树。当随机树的叶节点中包含了目标点或者目标区域的点时,随机树的扩展停止,便可在随机树中找到一条以根节点组成的从起始点到目标点的路径。RRT的扩展方式如图 1所示。
![]() |
图 1 RRT算法的扩展过程 |
图 1中T表示当前存在的扩展树,qrand表示状态空间中的随机采样点,qnear表示离随机采样点qrand最近的一个树节点,然后在qrand和qnear的连线上以扩展步长step为单位截取一个新节点qnew,如果在向新节点qnew行进的过程中没有碰到障碍物,则将应qnew加入到扩展树中,否则需要重新选择为qrand。继续迭代计算直到qnew到达目标区域则算法结束,此时可以在扩展树T中找到一条从起点qstart到目标点qgoal的路径。
RRT算法主要包括3个部分:采样点选择;搜索扩展树上距采样点最近的节点;扩展节点。下面将从这3个部分,结合无人机的性能约束和地形高程数据,将RRT方法引入到无人机三维航迹规划。
2.1 采样点选择采样点qrand的选择不仅影响航迹质量,而且影响规划速度。三维航迹规划中采样点的随机选取原则如下:以概率p选择目标点作为采样点;以概率1-p在整个规划窗口内随机选择采样点。对于随机采样点,根据采样点的在水平面的位置信息可以查询到采样点的高程信息。令随机采样点在水平面的投影坐标为(xrand,yrand),则随机采样点的高程信息为z(xrand,yrand)。令无人机的最低飞行高度约束为Hmin,则随机采样点的三维坐标为qrand=(xrand,yrand,z(xrand,yrand)+Hmin)。
2.2 节点扩展通过随机采样点qrand,可以找出已存在扩展树T的树节点q中离随机采样点距离最近的一个树节点qnear,使得
![](PIC/20160403-M2.jpg)
在qnear和qrand的连线上,计算从qnear以最小航迹段长度L到达的新点qnew
![](PIC/20160403-M3.jpg)
令qnew的坐标为(xnew,ynew,znew),qnear的坐标为(xnear,ynear,znear),qnear的根节点为qnear-root(xnear-root,ynear-root,znear-root)。qnew必须满足避障和无人机自身性能约束的条件下才能加入到扩展树T中。
1) 最大拐弯角约束
如图 2所示,令航迹段qnear-rootqnear和qnearqnew的水平投影分别为
![](PIC/20160403-G1.jpg)
![](PIC/20160403-M4.jpg)
![]() |
图 2 拐弯角约束示意图 |
2) 最大爬升/俯冲角约束
如图 3所示,航迹段qnearqnew的坐标为
![](PIC/20160403-G2.jpg)
![](PIC/20160403-M5.jpg)
![]() |
图 3 爬升角约束示意图 |
3) 最大航迹长度
最大航迹长度是无人机的航迹距离约束,它取决于无人机的燃料载荷和任务执行时间的限制,记为dmax。节点扩展过程中长度大于这个距离的航迹认为是无效航迹,新节点为无效点。该约束在二维空间的投影表示如图 4所示。新的扩展节点qnew,通过追溯根节点得到从起始点到qnew的航迹距离为D(qnew),图中黑色粗实线航迹。SL(qnew)是从扩展节点qnew到目标点qgoal的欧氏距离。只有当满足D(qnew)+SL(qnew)≤dmax时,才能把qnew加入到已有的扩展树T中。
![]() |
图 4 最大航迹长度约束 |
由于RRT方法是一种随机方法。在RRT方法产生的可行航迹中可能会包含冗余的节点。去除冗余的航迹节点,可以缩短无人机的航迹长度,减少无人机的机动次数。
设经过RRT方法求得的可行路径的节点序列为{wp1,…,wpn},其中wpn为目标点位置。记经过冗余节点剪裁后的节点序列为φ,φ初始为空。令j=n,首先将目标点wpj添加到φ中。对于i∈[1,…j-1],循环检查(wpj,wpi)之间的连线是否存在障碍或者威胁,是否存在不满足无人机自身性能约束的情况。如果存在,则令i=i+1;否则,只要检测出第一个不违背条件的节点wpi就停止循环,令j=i,并将wpi添加到φ中。重复上述循环,直到j=1时结束。
2.4 算法描述基于RRT方法的三维航迹规划算法的具体步骤如下:
1) 以无人机当前位置作为规划起始点qstart,初始化搜索树T。
2) 以概率p选择目标点作为采样点;以概率1-p在整个规划窗口内随机选择采样点qrand。
3) 通过随机采样点qrand,找出已存在扩展树T的树节点q中离随机采样点距离最近的一个树节点qnear。在qnear和qrand的连线上,计算从qnear以最小航迹段长度L到达的新点qnew。
4) 判断qnew是否满足避障和文中所述无人机自身性能约束,若满足,则将qnew加入到扩展树T中。否则转到步骤 2)。
5) 判断|qnew-qgoal|≤L,若满足,转到步骤6),否则转到步骤 2)。
6) 通过形成的扩展树T,获得从起始点qstart到终点qnear的可行路径。
7) 对冗余航迹节点进行剪裁,得到最终航迹。
3 数字地图数据的模拟通过对实际地形的研究,一定范围内的地形特性,可以通过地形高度均值、地形起伏的均方差及地形点之间的抗相关系数来描述[21]。
地形的高程可以表示为
![](PIC/20160403-M6.jpg)
实际地形的起伏可以表征为高斯分布的随机序列。文中采用通用的两维一阶离散自递推过程进行求解,即
![](PIC/20160403-G3.jpg)
![](PIC/20160403-M7.jpg)
![](PIC/20160403-M8.jpg)
![](PIC/20160403-M9.jpg)
![](PIC/20160403-M10.jpg)
两维离散自递推过程需要有2个一维随机过程产生该地区随机地形在x方向和y方向上的地形边界的初始条件,即
![](PIC/20160403-M11.jpg)
![](PIC/20160403-M12.jpg)
![](PIC/20160403-M13.jpg)
![](PIC/20160403-M14.jpg)
![](PIC/20160403-M15.jpg)
![](PIC/20160403-M16.jpg)
文中使用多项式产生地形数据,模拟山峰地形:
![](PIC/20160403-G4.jpg)
基于上述算法设计,在Intel 2.5 GHz 主频,8GB内存的普通PC机上使用MATLAB编程进行算法仿真实验。无人机执行任务区域的地形为1 001×1 001(无量纲)大小的由上述数字地图的模拟算法生成的虚拟三维地形图和模拟威胁地图。地形高度均值为0;地形起伏方差为50;该地形在x方向和y方向上抗相关距离均为300。5座模拟山峰在坐标(400,500)、(600,800)、(600,200)、(350,200)和(300,300)处,山峰高度分别为450、300、400、400和300;1个模拟湖泊位于坐标(500,100)处,深度为150;2个防空导弹威胁区位于坐标(330,750)和(750,500)处,用半球体表示,球体半径均为200。无人机的最大转弯角为60°;最大爬升/俯冲角为30°;最低飞行高度为50;最小航迹段长度为100。
4.2 仿真结果图 5~图 6和表 1显示了规划起始点坐标为(10,10,49),目标点坐标为(900,900,0)进行的试验1的结果。图 5中最大航迹约束dmax取值为起始点到目标点的直线距离S的1.3倍;而图 6中采用一个较大的dmax,取值为起始点到目标点的直线距离S的2倍。图中a)为规划航迹的三维显示结果。b)为规划航迹航路点的高度与该点地形高度对比。图 7、图 8和表 2显示了规划起始点坐标为(10,900,49),目标点坐标为(900,10,50)进行的试验2的结果。
![]() |
图 5 试验1中采用小dmax值规划的结果 |
![]() |
图 6 试验1中采用较大dmax值规划的结果 |
![]() |
图 7 试验2中采用小dmax值规划的结果 |
![]() |
图 8 试验2中采用较大dmax值规划的结果 |
实验结果表明,基于RRT方法的三维航迹规划算法对于不同的规划起始点和目标点均能够快速地规划出满足地形、威胁回避和无人机自身性能约束条件的可行航迹,通过对冗余节点的剪裁,可以去除不必要的航迹点,缩短了规划航迹的长度。对于相同的规划起始点和目标点,通过引入航迹距离约束,限制了生成路径中可能的转弯次数,减少了可扩展节点,提高了搜索效率,同时以一定概率选择目标点作为采样点来加强搜索过程中的启发信息,搜索树将沿着路径距离最短的近似最优航迹的方向进行扩展。该算法克服了基本RRT方法随机性强,只能快速获得可行航迹,无法获得较优航迹的缺点。
5 结 论本文针对无人机自主航迹规划的实时性问题,提出了基于RRT方法的三维快速航迹规划算法。该算法继承了RRT方法用于航迹规划的快速性,同时将地形信息和无人机的自身性能约束结合到扩展树的节点扩展中,使得规划出来的航迹能够满足实战要求。通过引入航迹距离约束,增加启发信息克服搜索的随机性,使得规划出的可行航迹接近最优航迹,有效地提高了无人机自主航迹规划的时间性能,兼顾了航迹规划的最优性与实时性。
[1] |
沈林成,牛轶峰,朱华勇. 多无人机自主协同控制理论与方法[M]. 北京: 国防工业出版社, 2013 Shen L C, Niu Z F, Zhu H Y. Theories and Methods of Autonomous Cooperative Control for Multiple UAVs[M]. Beijing, National Defense Industry Press, 2013: 109-110 (in Chinese) |
[2] |
沈林成,陈璟,王楠. 飞行器任务规划技术综述[J]. 航空学报, 2014, 35(3): 593-606 Shen L C, Chen J, Wang N. Overview of Air Vehicle Mission Planning Techniques[J]. Acta Aeronoutica et Astronautica Sinica, 2014, 35(3): 593-606 (in Chinese) |
Cited By in Cnki (16) | |
[3] | Zheng C W, Ding M Y, Zhou C P. Real-Time Route Planning for Unmanned Air Vehicle with An Evolutionary Algorithm[J]. International Journal of Pattern Recognition and Artificial Intelligence, 2003, 17(1): 63-81 |
Click to display the text | |
[4] |
何珮,屈香菊,武哲. 应用自适应遗传算法进行参考航迹规划[J]. 航空学报, 2003, 24(6): 499-502 He P, Qu X J, Wu Z. Aircraft Referenced Flight Path Planning by Using Adaptive Genetic Algorithms[J]. Acta Aeronoutica et Astronautica Sinica, 2003, 24(6): 499-502 (in Chinese) |
Cited By in Cnki (37) | |
[5] | Chen M, Wu Q X, Jiang C S. A Modified Ant Optimization Algorithm for Path Planning of UCAV[J]. Applied Soft Computation, 2008, 8(4): 1712-1718 |
Click to display the text | |
[6] | Duan H B, Zhang X Y, Wu J. Max-Min Adaptive Ant Colony Optimization Approach to Multi-UAVs Coordinated Trajectory Replanning in Dynamic and Uncertain Environments[J]. Journal of Bionic Engineering, 2009, 6(2): 161-173 |
Click to display the text | |
[7] |
李春华,郑昌文,周成平. 一种三维航迹快速搜索方法[J]. 宇航学报, 2002, 23(3):13-16 Li C H, Zheng C W, Zhou C P. Fast Search Algorithm for 3D-Route Planning[J]. Journal of Astronautics, 2002, 23(3): 13-16 (in Chinese) |
Cited By in Cnki (146) | |
[8] | Wang Z, Liu L, Long T. Enhanced Sparse A* Search for UAV Path Planning Using Dubins Path Estimation[C]//Proceedings of the 33rd Chinese Control Conference, Nanjing, 2014: 738-742 |
Click to display the text | |
[9] | Li S B, Sun X X, Xu Y J. Particle Swarm Optimization for Route Planning of Unmanned Air Vehicles[C]//Proceedings of the Congress on Information Acquisition. Weihai, 2006: 1213-1218 |
[10] | Fu Y G, Ding M Y, Zhou C P. Routing Planning for Unmanned Aerial Vehicle(UAV) on the Sea Using Hybrid Differential Evolution and Quantum-Behaved Particle Swarm Optimization[J]. IEEE Trans on Systems, Man, and Cybernetics: Systems, 2013,43(6): 1451-1465 |
Click to display the text | |
[11] | LaValle S M. Rapidly-Exploring Random Trees: A New Tool for Path Planning[R]. Computer Science Department, Iowa State University, 1998 |
[12] |
周金良,黄彦文,曹其新. 对抗环境下足球机器人路径规划[J]. 上海交通大学学报, 2006,40(11):1827-1831 Zhou J L, Huang Y W, Cao Q X. The Path Planning for Robot Soccer Under Antagonistic Environment[J]. Journal of Shanghai Jiaotong University, 2006,40(11): 1827-1831 (in Chinese) |
Cited By in Cnki (10) | |
[13] | Amin J N, Boskovic J D, Mehra R K. A Fast and Efficient Approach to Path Planning for Unmanned Vehicles[C]//Proceedings of AIAA Guidance, Navigation and Control Conference and Exhibit. Keystone, 2006 |
Click to display the text | |
[14] | Kuffner J J, LaValle S M. RRT-Connect: An Efficient Approach to Single-Query Path Planning[C]//Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, 2000:995-1001 |
Click to display the text | |
[15] | Kalisiak M, Panne M. RRT-Blossom: RRT with a Local Flood-Fill Behavior[C]//Proceedings of the 2006 IEEE International Conference on Robotics and Automation, Florida, 2006: 1237-1242 |
Click to display the text | |
[16] | Melchior N A, Simmons R. Particle RRT for Path Planning with Uncertainty[C]//Proceedings of the IEEE International Conference on Robotics and Automation, Roma, 2007: 1617-1624 |
Click to display the text | |
[17] |
彭辉,王林,沈林成. 区域搜索中基于改进RRT的UAV实时航迹规划[J]. 国防科学技术大学学报, 2009, 31(5): 86-91 Peng H, Wang L, Shen L C. The Modified RRT-Based Real-Time Route Planning for UAV Area Target Searching[J]. Journal of National University of Defense Technology, 2009, 31(5): 86-91 (in Chinese) |
Cited By in Cnki (23) | |
[18] | Bruce J, Veloso M. Real-Time Randomized Path Planning for Robot Navigation[C]//Proceedings of the IEEE International Conference on Intelligent Robots and Systems, Lausanne, 2002: 2383-2388 |
Click to display the text | |
[19] | Yershova A, Jaillet L, Simeon T. Dynamic-Domain RRTs: Efficient Exploration by Controlling the Sampling Domain[C]//Proceedings of the IEEE International Conference on Robotics and Automation, Barcelona, 2005: 3851-3861 |
Click to display the text | |
[20] |
丁明跃,郑昌文,周成平,严平. 无人飞行器航迹规划[M]. 北京: 电子工业出版社, 2009 Ding M Y, Zheng C W, Zhou C P, Yan P. Route Planning for Unmanned Aerial Vehicles[M]. Beijing, Publishing House of Electronics Industry, 2009: 41-43 (in Chinese) |
[21] |
叶文,范洪达,朱爱红. 无人飞行器任务规划[M]. 北京: 国防工业出版社, 2011 Ye W, Fan H D, Zhu A H. Mission Planning for Unmanned Aerial Vehicles[M]. Beijing, National Defense Industry Press, 2011: 42-47 (in Chinese) |