用于無人機三維航跡規(guī)劃改進連接型快速擴展隨機樹算法

田疆，李二超

(蘭州理工大學(xué) 電氣工程與信息工程學(xué)院，蘭州 730050)

0 引 言

無人機航跡規(guī)劃[1]是指在人工不干預(yù)情形下，系統(tǒng)根據(jù)飛機性能、環(huán)境影響要素等約束條件自動計算無人機可行航跡，確保無人機安全、高效完成任務(wù)的方法。

近年來，在航跡規(guī)劃方法中，基于全空間貪婪搜索思想的啟發(fā)式航跡規(guī)劃算法由于其不會限于局部極值、不依賴航跡規(guī)劃建模也能生成可行航跡的特點備受關(guān)注。其中，將快速擴展隨機樹(Rapidly-exploring Random Tree，簡稱RRT)算法[2]及其各種改進型算法或融合型算法應(yīng)用于求解無人機航跡規(guī)劃成果較多。崔挺等[3]將經(jīng)典的最短路徑搜索算法Dijkstra算法嵌入RRT算法中，成功應(yīng)用于無人機二維航跡規(guī)劃問題；之后對經(jīng)典的雙向快速隨機搜索樹(Dual-RRT)算法進行改進，分別在前向樹和反向樹隨機節(jié)點搜索過程中加入了前向積分拓展算子和反向積分拓展算子，在無人機遭遇突發(fā)威脅的二維航跡規(guī)劃情形時，能迅速提供更多可行航跡點來提高生存概率。Lin Yucong等[4]將三維Dubins曲線結(jié)合經(jīng)典RRT算法應(yīng)用于四旋翼無人機航跡規(guī)劃，可以較平滑地避開三維環(huán)境中的柱體障礙物和橢球體障礙物。Wu Xinggang等[5]將航跡規(guī)劃空間柵格化并引入覆蓋率的概念，添加最大轉(zhuǎn)彎角約束條件于隨機節(jié)點的產(chǎn)生過程中，提出了一種可變概率的雙向隨機搜索樹(VPB-RRT)算法，成功應(yīng)用于無人機二維航跡規(guī)劃問題。劉洋[6]針對無人機遭遇三維動態(tài)環(huán)境中的突發(fā)威脅情況時，如何迅速找到一條安全可替換航跡，在經(jīng)典的RRT*算法中引入排序機制并提出一種DRRT*算法，充分利用初次航跡規(guī)劃得到的信息，簡化了碰撞檢測環(huán)節(jié)的復(fù)雜度，成功避開了可移動圓臺障礙物，高效實現(xiàn)了航跡規(guī)劃任務(wù)。尹高揚等[7]針對無人戰(zhàn)斗機三維航跡規(guī)劃的實時性要求，將無人機基本約束條件諸如最大轉(zhuǎn)彎角約束、最大下滑或爬升角約束、最大航跡長度約束等加入經(jīng)典的RRT算法中，在由零均值高斯白噪聲序列產(chǎn)生的三維起伏地形中，增加啟發(fā)信息以克服搜索的隨機性，兼顧了航跡規(guī)劃的最優(yōu)性與實時性。

然而，現(xiàn)有研究成果中多數(shù)是固定無人機飛行高度，將高維航跡規(guī)劃問題降維處理為二維平面內(nèi)的航跡規(guī)劃問題[8-10]，障礙物的設(shè)置只能以圓形、橢圓形、矩形等規(guī)則幾何形式呈現(xiàn)[11-13]，難以適應(yīng)無人機在真實三維空間中自由飛行的復(fù)雜情況。

連接型快速擴展隨機樹(RRT with Connection，簡稱RRT-Connect)算法由J.J.Kuffner等[14]于2000年提出，用于求解7自由度運動鏈在棋盤上抓取和放置棋子的路徑規(guī)劃問題。該算法在RRT算法的基礎(chǔ)上，將單隨機樹轉(zhuǎn)化為雙向隨機樹，并引入了連接型啟發(fā)式貪婪搜索機制。與RRT算法相比，RRT-Connect算法具有更好的全局搜索能力和更高的搜索效率；之后，將RRT-Connect算法應(yīng)用于二維和三維剛體的路徑規(guī)劃中，還應(yīng)用于6自由度模擬美洲獅的前腿路徑規(guī)劃中，均取得了很好的效果。RRT-Connect算法及其改進已成功應(yīng)用于多種機器人路徑規(guī)劃問題，例如：雙足[15-18]、四足[19-21]和六足[22-24]機器人，以及機械手等[25]。但是將該算法用于無人機航跡規(guī)劃問題特別是三維航跡規(guī)劃問題，至今未見成熟的研究成果，該算法的潛在應(yīng)用價值和意義尚未被廣泛了解和認識。

針對上述問題，本文建立數(shù)學(xué)模型，提出求解該模型的改進連接型快速擴展隨機樹算法并進行實驗，以期對無人機三維航跡規(guī)劃提供一種新的思路。

1 無人機航跡規(guī)劃問題的數(shù)學(xué)模型

建立三維航跡規(guī)劃問題的數(shù)學(xué)模型時，不但考慮無人機基本約束，還考慮復(fù)雜的飛行環(huán)境，包括山體地形和雷暴威脅區(qū)。

1.1 無人機基本約束

規(guī)劃的無人機三維航跡，通常需要滿足一些基本約束，包括最大轉(zhuǎn)彎角、最大爬升角或下滑角、最小航跡段長度、最低和最高飛行高度，以及最大航跡長度等約束。其中，最大轉(zhuǎn)彎角約束，是指無人機只能在水平面內(nèi)小于或等于指定的最大轉(zhuǎn)彎角內(nèi)轉(zhuǎn)彎；最大爬升角或下滑角約束，是指無人機只能在垂直平面內(nèi)小于或等于指定的最大爬升角或下滑角內(nèi)爬升或下滑；最小航跡段長度約束，要求無人機改變飛行姿態(tài)之前，按目前的航跡方向飛行的最短航程；最低和最高飛行高度約束，要求無人機在指定的飛行高度區(qū)間飛行；最大航跡長度約束，是指無人機的航跡長度小于或等于指定的閾值。

記q(x,y,z,θ,ψ)為無人機的飛行位置與姿態(tài)，其中，(x,y,z)為無人機的位置，θ為無人機的水平轉(zhuǎn)彎角，ψ為無人機的豎直爬升角或下滑角，進而建立上述基本約束的數(shù)學(xué)表達式。

(1)

(2)

(3)

(4) 最低和最高飛行高度約束。記最低飛行高度為zmin，最高飛行高度為zmax，那么，zmin≤zi≤zmax。

1.2 飛行環(huán)境障礙物和威脅區(qū)建模

在飛行環(huán)境中，高聳的山體近似采用圓錐體等效表示，用以e為底的自然指數(shù)圖形生成[26]，那么，山體地形可以通過多個位置不同的圓錐體疊加而成。若將參考海拔基準(zhǔn)高度設(shè)置為xOy平面，記(x,y,z)為山體地形中的點，那么

(4)

1.3 目標(biāo)函數(shù)及航跡表示

在本文中，執(zhí)行任務(wù)的某型無人機，其航跡規(guī)劃的目標(biāo)函數(shù)是生成一條由起始點到目標(biāo)點的無碰撞可行航跡。采用q(x,y,z,θ,ψ)表示無人機在飛行空域中某特定位置的特定姿態(tài)，那么(x,y,z)則表示無人機所在航跡點，θ表示無人機的水平轉(zhuǎn)彎角，ψ表示無人機的豎直爬升角或下滑角。采用r(q)表示由起始點qinitial到目標(biāo)點qgoal的無碰撞可行航跡，那么航跡規(guī)劃的過程可以寫成如下形式：

1.4 數(shù)學(xué)模型表述

基于1.1～1.3節(jié)，得到無人機航跡規(guī)劃問題的數(shù)學(xué)模型。

目標(biāo)函數(shù)：

約束：

經(jīng)典RRT-Connect算法僅依賴本身隱含的連接啟發(fā)式貪婪搜索函數(shù)，已能獲得可行航跡，雖然不是最優(yōu)航跡，且可行航跡點搜索時間成本較高，但在不引入外部數(shù)學(xué)模型就能求解得出無人機的可行航跡，這是該算法的一個顯著特點；當(dāng)引入數(shù)學(xué)模型求解無人機航跡規(guī)劃問題后，隨機樹對于航跡點的搜索將由之前的全空間貪婪搜索轉(zhuǎn)為在滿足數(shù)學(xué)模型約束函數(shù)的區(qū)域內(nèi)貪婪搜索，極大地減小了搜索空間范圍，同時節(jié)約了可行航跡點搜索時間，并在一定程度上縮短了生成的可行航跡總長度。

2 RRT-Connect算法及其改進算法

2.1 RRT-Connect算法

RRT-Connect算法的基本思想是：自起始點和目標(biāo)點，通過多步長啟發(fā)式擴展策略，等概率的交替向?qū)Ψ綌U展新節(jié)點，形成隨機樹，直至兩棵隨機樹相遇，形成的軌跡即為規(guī)劃的機器人可行路徑。已有研究表明，RRT-Connect算法能夠生成可行的機器人路徑。本文探索該算法在無人機三維可行航跡規(guī)劃中的應(yīng)用，具體的執(zhí)行過程如下所示：

2.2 改進型RRT-Connect算法

為了采樣隨機節(jié)點qrand，RRT-Connect算法通常引入一個閾值，記為Pg，并產(chǎn)生(0,1)的隨機數(shù)p。如果p

本文通過提出多種隨機節(jié)點產(chǎn)生方式，改進RRT-Connect算法。為此，首先引入2個參數(shù)，記為pd和pr，其中，pd為待生長樹的末節(jié)點、q0或qn對非生長樹所有節(jié)點的貢獻率；pr為待生長樹的所有節(jié)點對q0或qn的貢獻率。

對于根為qn的非生長樹，采用以下方法產(chǎn)生qrand：

(1) 選擇非生長樹中，距末節(jié)點最近的節(jié)點；

(2) 選擇非生長樹中，距q1最近的節(jié)點；

(5) 引入隨機數(shù)pl∈(0,1)，如果pl

對于根為q1的非生長樹，將上述方法中的q1改為qn，即可得到與之對應(yīng)的qrand產(chǎn)生方法，不再贅述。

2.3 基于改進型RRT-Connect算法的航跡規(guī)劃數(shù)學(xué)模型約束轉(zhuǎn)化

采用本文所提的改進型RRT-Connect算法求解航跡規(guī)劃問題數(shù)學(xué)模型時，需要對約束進行轉(zhuǎn)化。

(5)

(6)

(7)

(4) 轉(zhuǎn)化最低和最高飛行高度約束：當(dāng)前新生成備選航跡點qnew的飛行高度znew，需要滿足zmin≤znew≤zmax。

2.4 基于改進型RRT-Connect算法的航跡規(guī)劃數(shù)學(xué)模型

基于2.1～2.3節(jié)，得到基于改進型RRT-Connect算法的無人機航跡規(guī)劃問題數(shù)學(xué)模型。

目標(biāo)函數(shù)：

約束：

3 實 驗

將提出的六種改進型RRT-Connect算法，應(yīng)用于我國西南橫斷山區(qū)存在森林火險隱患的山體地形和雷暴威脅區(qū)的某區(qū)域，以無人機定期執(zhí)行常規(guī)森林防火巡查檢測任務(wù)的在線可行航跡規(guī)劃問題為應(yīng)用背景的無人機三維可行航跡規(guī)劃問題中，并與RRT算法和RRT-Connect算法進行對比。

3.1 實驗環(huán)境和參數(shù)設(shè)置

所有算法均采用MATLAB2010B編程實現(xiàn)，運行環(huán)境為：Windows 7專業(yè)版、Intel(R) Core(TM) i7-5500U CPU@2.40 GHz處理器、4.00 GB內(nèi)存、64位操作系統(tǒng)。

3.2 對比算法和性能指標(biāo)

為了便于說明，記融入第i種隨機節(jié)點生成策略的RRT-Connect的算法為RRT-Connecti。對于RRT-Connect算法，指定非生長樹的末節(jié)點qe為qrand；對于RRT算法，指定qn為qrand。采用八種算法分別求解無人機三維航跡規(guī)劃問題，性能指標(biāo)為平均航跡長度和航跡規(guī)劃平均時間。每種算法獨立執(zhí)行10次。運行結(jié)果的平均值，作為評價算法性能的依據(jù)。

3.3 實驗結(jié)果及數(shù)據(jù)分析

不同算法的平均航跡長度和航跡規(guī)劃平均時間如表1所示，表中加粗的數(shù)據(jù)為最優(yōu)值。

表1 平均航跡長度和航跡規(guī)劃平均時間

從表1可以看出：對于平均航跡長度而言，RRT-Connect2和RRT-Connect6均優(yōu)于RRT算法和RRT-Connect算法，RRT-Connect1、RRT-Connect4和RRT-Connect5均優(yōu)于RRT算法，但劣于RRT-Connect算法，性能最差的是RRT-Connect3，甚至劣于RRT算法；關(guān)于航跡規(guī)劃平均時間，RRT-Connect3、RRT-Connect4、RRT-Connect5和RRT-Connect6均優(yōu)于RRT算法和RRT-Connect算法，RRT-Connect1和RRT-Connect2優(yōu)于RRT算法，但劣于RRT-Connect算法；在所有算法中，RRT-Connect6具有最優(yōu)的平均航跡長度和航跡規(guī)劃平均時間，因此，是最優(yōu)的算法。

為了直觀說明，不同算法規(guī)劃的最短航跡如圖1～圖8所示，圖中，粉色細線為無人機的三維飛行航跡，各坐標(biāo)軸單位均為百米(102m)。

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

(a) 水平45°顯示

(b) 俯仰75°顯示

4 結(jié) 論

所提六種改進型RRT-Connect算法，均能夠不同程度的改進平均航跡長度或航跡規(guī)劃平均時間等性能指標(biāo)，有的策略在上述兩個性能指標(biāo)上均有改進。尤其是提出的改進RRT-Connect6算法，在求解無人機三維航跡規(guī)劃問題上，相較其他五種改進策略具有更大優(yōu)勢。今后將在此研究的基礎(chǔ)上探索更合理的航跡規(guī)劃方法以及更高效的航跡規(guī)劃求解算法。