無人機快速三維航跡規劃算法

尹高揚, 周紹磊, 吳青坡

(海軍航空工程學院 控制科學與工程系, 山東 煙臺　264001)

?

無人機快速三維航跡規劃算法

尹高揚, 周紹磊, 吳青坡

(海軍航空工程學院 控制科學與工程系, 山東 煙臺264001)

摘要：針對無人機三維航跡規劃的實時性問題，提出了基于快速擴展隨機樹的三維航跡規劃方法。該算法能夠根據當前環境快速有效搜索規劃空間，通過隨機采樣點將搜索導向空白區域，使三維航跡規劃能夠用于實時航跡規劃。通過引入航跡距離約束，搜索樹將沿著路徑距離最短的近似最優航跡的方向進行擴展，克服了基本快速擴展隨機樹方法隨機性強，只能快速獲得可行航跡，無法獲得較優航跡的缺點。在搜索過程中無人機的航跡約束條件和地形信息得到了充分利用，使算法生成的航跡能夠自動回避地形和威脅，同時滿足無人機的動力學約束。通過生成的虛擬數字地圖對算法進行了仿真驗證，仿真結果表明該方法能夠快速有效地規劃出滿意的無人機三維航跡。

關鍵詞：無人機；快速擴展隨機樹；實時性；地形回避；三維航跡

在現代戰爭中，無人機所處的戰場環境瞬息萬變，同時由于其所執行任務的不確定性，無人機要能夠根據任務過程中的實時環境信息進行在線的自主航跡規劃，這對規劃算法的實時性提出了很高要求[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-19]。

基本RRT及其改進方法雖然能夠滿足無人機在線自主航跡規劃的應用要求，但是它們都是在二維平面上進行航跡搜索，因而未能利用地形的高度信息，不能有效地利用地形進行地形規避和威脅規避。本文在基本RRT方法的基礎上，提出了一種三維快速航跡規劃方法。并在生成的虛擬數字地形圖上進行了仿真驗證，實驗結果表明，該方法能夠充分利用地形的高程信息，有效地進行地形回避和威脅回避，是一種快速有效的三維航跡規劃算法。

1問題提出

無人機三維在線航跡規劃要求無人機根據預先裝載的任務區的三維地形數據，規劃出一條從規劃起始點到目標點的三維航跡，該航跡避開了所有三維障礙物和威脅，并且滿足無人機自身的飛行性能約束。無人機在慣性坐標系下的三自由度質點運動方程為：

(1)

式中,(x,y,z)表示無人機在慣性坐標系中的位置;v表示無人機的速度;ψ為無人機的航向角;γ為無人機的航跡傾角。

具體來說,三維航跡規劃生成的目標航跡要求滿足以下幾個約束條件[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,使得

(2)

在qnear和qrand的連線上,計算從qnear以最小航跡段長度L到達的新點qnew

(3)

令qnew的坐標為(xnew,ynew,znew),qnear的坐標為(xnear,ynear,znear),qnear的根節點為qnear-root(xnear-root,ynear-root,znear-root)。qnew必須滿足避障和無人機自身性能約束的條件下才能加入到擴展樹T中。

1) 最大拐彎角約束

如圖2所示,令航跡段qnear-rootqnear和qnearqnew的水平投影分別為

設最大允許拐彎角為θ,則qnew必須滿足

(4)

圖2　拐彎角約束示意圖

2) 最大爬升/俯沖角約束

如圖3所示,航跡段qnearqnew的坐標為

設最大爬升、俯沖角為φ,則qnew必須滿足

(5)

圖3　爬升角約束示意圖

3) 最大航跡長度

最大航跡長度是無人機的航跡距離約束,它取決于無人機的燃料載荷和任務執行時間的限制,記為dmax。節點擴展過程中長度大于這個距離的航跡認為是無效航跡,新節點為無效點。該約束在二維空間的投影表示如圖4所示。新的擴展節點qnew,通過追溯根節點得到從起始點到qnew的航跡距離為D(qnew),圖中黑色粗實線航跡。SL(qnew)是從擴展節點qnew到目標點qgoal的歐氏距離。只有當滿足D(qnew)+SL(qnew)≤dmax時,才能把qnew加入到已有的擴展樹T中。

圖4　最大航跡長度約束

2.3冗余擴展點剪裁

由于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]。

地形的高程可以表示為

(6)

式中,H(x,y)為(x,y)點的實際高度;H0為該地區的平均高度;Z(x,y)為地形平均高度基礎上的地形起伏高度。

3.1隨機地形的模擬

實際地形的起伏可以表征為高斯分布的隨機序列。文中采用通用的兩維一階離散自遞推過程進行求解,即

(7)

(8)

(9)

(10)

式中，Std為地形起伏高度的均方差;τxc和τyc分別為x方向和y方向上的抗相關距離。

兩維離散自遞推過程需要有2個一維隨機過程產生該地區隨機地形在x方向和y方向上的地形邊界的初始條件,即

(11)

(12)

(13)

(14)

(15)

(16)

3.2山峰的模擬

文中使用多項式產生地形數據,模擬山峰地形:

式中，Zi為基準地形高度Z0上的第i個山峰的高度;x0i、y0i是第i個山峰的坐標;xsi、ysi分別是與第i座山沿x方向和y方向上的與坡度有關的量。

4仿真實驗

4.1仿真參數設置

基于上述算法設計,在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的結果。

表1　試驗1數據

圖5　試驗1中采用小dmax值規劃的結果　　　　圖6　試驗1中采用較大dmax值規劃的結果

圖7　試驗2中采用小dmax值規劃的結果　　　　　圖8　試驗2中采用較大dmax值規劃的結果

最大航跡長度擴展節點數規劃航跡長度節點未處理冗余剪裁1.3S1636.3571597.91385.92S2517.4471799.31578.6

實驗結果表明，基于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)

[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

[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)

[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

[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

[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)

[8]Wang Z, Liu L, Long T. Enhanced Sparse A*Search for UAV Path Planning Using Dubins Path Estimation[C]∥Proceedings of the 33rdChinese Control Conference, Nanjing, 2014: 738-742

[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

[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)

[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

[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

[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

[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

[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)

[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

[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

[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)

Efficient Path Planning Algorithm in Three Dimensions for UAV

Yin Gaoyang, Zhou Shaolei, Wu Qingpo

(Department of Control Engineering, Naval Aeronautical and Astronautical University, Yantai 264001, China)

Abstract:To satisfy the real-time requirement of path planning in three dimensions for unmanned aerial vehicle, a path planning algorithm based on rapidly-exploring random tree is proposed. By random sampling point in configuration space, the search will be guided to empty area, thus the algorithm can search the high-dimension space quickly and efficiently according to the current environment, which can be used in real-time path planner. By introducing the path length constraint, the search tree will explore along the direction of the near optimal path. The proposed algorithm overcomes the disadvantage of basic RRT algorithm that only to quickly get feasible path, unable to obtain near optimal path. During the search process, the path constraints of UAV and the terrain information are fully utilized, so that the path generated by the algorithm can avoid terrain and threat automatically, and meet the dynamic constraints of UAV. Simulations for the algorithm are made on a generated virtual digital map. Simulation results demonstrated that this proposed method can complete path planning mission in three dimensions quickly and effectively.

Keywords:unmanned aerial vehicle (UAV); rapidly-exploring random tree; real-time; terrain avoidance; 3D-path

收稿日期：2016-02-12

基金項目：航空科學基金(20135184007)資助

作者簡介：尹高揚(1987—)，海軍航空工程學院博士研究生,主要從事導航、制導與控制的研究。

中圖分類號：V249.1

文獻標志碼：A

文章編號:1000-2758(2016)04-0564-07