基于流體擾動算法與深度神經(jīng)網(wǎng)絡(luò)的無人機(jī)自適應(yīng)路徑規(guī)劃

王延祥，王宏倫，吳健發(fā)，倫岳斌

（1.北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京100191；2.北京航空航天大學(xué)飛行器控制一體化技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京100191）

1 引言

無人機(jī)因其具有性價(jià)比高、生存性強(qiáng)和可執(zhí)行高風(fēng)險(xiǎn)任務(wù)等優(yōu)點(diǎn)，在軍事和民用領(lǐng)域得到了廣泛的發(fā)展［1］。在軍事應(yīng)用領(lǐng)域中，無人機(jī)已廣泛應(yīng)用于對地攻擊、邊境巡邏、戰(zhàn)術(shù)偵察、目標(biāo)識別等各種軍事任務(wù)中；在民用領(lǐng)域中，無人機(jī)可有效地執(zhí)行森林火災(zāi)檢測、資源探測、搜救、航空拍攝、天氣預(yù)報(bào)、測繪等任務(wù)。最近30 多年來，隨著人工智能技術(shù)、計(jì)算機(jī)技術(shù)、控制技術(shù)、電子信息等技術(shù)的發(fā)展，世界各國對無人機(jī)領(lǐng)域持續(xù)密切關(guān)注并加大投入，無人機(jī)技術(shù)取得了長足的發(fā)展和進(jìn)步，代表了當(dāng)今高新技術(shù)發(fā)展的方向。無人機(jī)作為一類典型的自主無人控制系統(tǒng)，智能化與自主化是其本質(zhì)特征與未來趨勢。路徑實(shí)時(shí)規(guī)劃對提高自主飛行能力具有重要作用，而安全避障技術(shù)是提高無人機(jī)自主飛行能力的關(guān)鍵技術(shù)之一。因此，本文對三維復(fù)雜動態(tài)環(huán)境下的無人機(jī)實(shí)時(shí)路徑規(guī)劃技術(shù)進(jìn)行了研究。

本文研究的無人機(jī)自主避障問題是指由已知環(huán)境信息或傳感器（如前視聲納、高頻雷達(dá)等）實(shí)時(shí)檢測到的環(huán)境信息（如障礙物信息等），自主決定無人機(jī)的三維避障行為。此外，避障行為應(yīng)引導(dǎo)無人機(jī)由出發(fā)點(diǎn)按照一條較優(yōu)的航路飛向目標(biāo)點(diǎn)，針對復(fù)雜環(huán)境下的避障問題，需要考慮環(huán)境約束（如各種障礙物、禁飛區(qū)、突發(fā)威脅等）和無人機(jī)運(yùn)動學(xué)約束（如最大轉(zhuǎn)彎速度和最大爬升速率等），使避障問題求解復(fù)雜化。在過去的幾十年里，經(jīng)過眾多學(xué)者們的努力，雖然無人機(jī)的自主避障技術(shù)取得了豐碩的研究成果，但大多方法都難以在三維復(fù)雜動態(tài)環(huán)境下實(shí)時(shí)規(guī)劃出性能指標(biāo)較優(yōu)的路徑。

現(xiàn)有的這些無人機(jī)避障方法主要包括模型預(yù)測控制（Model Predictive Control，MPC）方法［2］，快速探索隨機(jī)樹（Rapidly-exploring Random Trees，RRT）方法［3］，人工勢場（Artificial Potential Field，APF）法［4］，智能優(yōu)化算法（如遺傳算法［5］、粒子群優(yōu)化算法［6］）。然而，上述方法主要適用于二維平面，而在三維復(fù)雜環(huán)境中，計(jì)算量將顯著增加。此外，生成路徑的平滑度也不夠理想。

為解決上述避障方法中存在的問題，近年來，受河流中的流水能夠順利避開巖石并最終到達(dá)目的地這一自然現(xiàn)象啟發(fā)，王宏倫等提出了一系列基于流體計(jì)算的方法［7-11］，該方法可滿足三維復(fù)雜環(huán)境中的實(shí)時(shí)路徑規(guī)劃和避障任務(wù)需求。其中擾動流體動態(tài)系統(tǒng)（Interfered Fluid Dynamical System，IFDS）是最具代表性的方法，該方法具有計(jì)算效率高、處理不同形狀障礙物、規(guī)劃路徑平滑、適用范圍廣等諸多優(yōu)點(diǎn)。需要注意的是，IFDS 有很多參數(shù)，這些參數(shù)會影響路徑的質(zhì)量。因此尋找合適的參數(shù)以生成高質(zhì)量的路徑顯得尤為重要，Yao 等［12］提出了基于改進(jìn)的灰狼優(yōu)化算法（Grey Wolf Optimizer，GWO）的IFDS 的參數(shù)優(yōu)化方法。但是這種方法只適用于靜態(tài)已知的環(huán)境，而無人機(jī)的實(shí)際飛行環(huán)境通常是動態(tài)的、未知的，需要處理各種動態(tài)情況。為了提高動態(tài)復(fù)雜環(huán)境中的路徑質(zhì)量，Wu 等［13］根據(jù)無人機(jī)實(shí)時(shí)的周圍環(huán)境信息，采用滾動時(shí)域優(yōu)化控制來不斷優(yōu)化IFDS 參數(shù)。然而，這種方法計(jì)算量較大，很難保證路徑規(guī)劃的實(shí)時(shí)性。到目前為止，缺乏對動態(tài)環(huán)境下參數(shù)實(shí)時(shí)優(yōu)化的相關(guān)研究。

近年來，隨著人工智能技術(shù)的發(fā)展，人工神經(jīng)網(wǎng)絡(luò)在路徑規(guī)劃中得到了廣泛的應(yīng)用［14］?；谏疃壬窠?jīng)網(wǎng)絡(luò)（Deep Neural Network，DNN）的路徑規(guī)劃存在的難點(diǎn)和重點(diǎn)是如何獲取高質(zhì)量、足數(shù)量的訓(xùn)練樣本，樣本的數(shù)量及質(zhì)量直接影響了路徑規(guī)劃的效率。目前，基于DNN 的路徑規(guī)劃研究還處于探索階段，研究成果較少。

基于以上分析，在復(fù)雜環(huán)境下如何根據(jù)環(huán)境信息實(shí)時(shí)自適應(yīng)調(diào)整IFDS 參數(shù)是一個(gè)難題。本文提出了一種基于DNN 和IFDS 的無人機(jī)路徑規(guī)劃方法。本文主要由以下三部分組成：（1）基于IFDS 的路徑規(guī)劃模塊是基礎(chǔ)，（2）基于灰狼優(yōu)化算法和滾動時(shí)域控制（Receding Horizon Control，RHC）的仿真樣本生成模塊主要用來優(yōu)化樣本質(zhì)量，（3）基于DNN 的IFDS 系數(shù)自適應(yīng)優(yōu)化模塊是提高路徑質(zhì)量、減少計(jì)算開銷的關(guān)鍵。

2 無人機(jī)避障問題描述

2.1 無人機(jī)運(yùn)動學(xué)模型

本文假設(shè)無人機(jī)裝備了穩(wěn)定的底層控制系統(tǒng)，可以實(shí)現(xiàn)對俯仰角、偏航角、滾轉(zhuǎn)角等姿態(tài)角以及速度的穩(wěn)定跟蹤或保持。因此可將無人機(jī)模型簡化為三自由度質(zhì)點(diǎn)模型，在三維坐標(biāo)系下的運(yùn)動學(xué)模型如下所示：

其中，p=（x，y，z）表示在慣性坐標(biāo)系下無人機(jī)的位置，表示無人機(jī)速度，v，γ，ψ分別表示無人機(jī)的速度大小、爬升角和航向角，γ應(yīng)滿足無人機(jī)動力學(xué)約束條件γ≤γmax。爬升速率和轉(zhuǎn)彎速率作為無人機(jī)的控制輸入，其應(yīng)滿足控制輸入約束條件。

本文采用比例反饋和前饋控制來確定控制輸入：

其中，ψd，γd分別表示期望的航向角和爬升角，這可以通過期望飛行速度vd求得，角速率指令和可以通過差分計(jì)算得到。比例因子kψ，kγ分別表示航向角和爬升角時(shí)間常數(shù)的倒數(shù)。令無人機(jī)的航向角誤差和爬升角誤差分別為eψ=ψd-ψ、eγ=γd-γ，通過公式（2）可以推出-kγ·eγ。這說明無人機(jī)的航向角誤差和爬升角誤差均以指數(shù)形式衰減到0，無人機(jī)的飛行速度v能迅速收斂到期望的飛行速度vd。因此，本文重點(diǎn)研究如何獲得期望的飛行速度vd。

2.2 障礙物模型

在復(fù)雜的飛行環(huán)境中，通常存在許多類型的障礙物，如山峰、建筑物等。這些障礙物可以用圓錐體、圓柱體、長方體、半球體等標(biāo)準(zhǔn)的凸多面體來等效包絡(luò)，它們可以由統(tǒng)一的公式建模得到：

其中，（xb，yb，zb）表示障礙物的中心位置；m，n，l和a，b，c均為大于0 的數(shù)，他們決定了障礙物的尺寸和外形。Γ(p) ＜1，Γ(p) = 1，Γ(p) ＞1 分別表示障礙物的內(nèi)部、表面、外部。在本文中，所有障礙物的內(nèi)部和表面被定義為禁飛區(qū)或危險(xiǎn)區(qū)：

其中K表示障礙物的個(gè)數(shù)，為了確保飛行安全，無人機(jī)在飛行過程中應(yīng)始終飛行在禁飛區(qū)外，即p?DF。

3 IFDS

受自然界流水避石現(xiàn)象的啟發(fā)，把河流中的巖石等效替代為無人機(jī)飛行環(huán)境中的障礙物，當(dāng)河流中不存在巖石時(shí)，流水沿筆直的流線到達(dá)目標(biāo)點(diǎn)，這可以看作為初始流場，初始流線可視為無障礙環(huán)境下無人機(jī)飛行航路。當(dāng)河流中存在巖石時(shí)，流水可以平滑的繞過巖石到達(dá)目標(biāo)點(diǎn)，這等效為擾動流場，擾動流線可視為障礙物環(huán)境下的無人機(jī)避障航路。因此，當(dāng)飛行環(huán)境中不存在障礙物時(shí)，無人機(jī)應(yīng)從當(dāng)前位置沿直線飛行至目的地。假設(shè)目的地為(xd，yd，zd)，初始期望的流體速度可以定義為：

當(dāng)飛行環(huán)境中存在障礙物時(shí)，假設(shè)障礙物的數(shù)量為K。將障礙物對初始流場的影響用總的擾動矩陣M來量化：

其中wk表示第k個(gè)障礙物的權(quán)重系數(shù)，其值取決于無人機(jī)與障礙物表面之間的距離，以及障礙物的尺寸、形狀等：

其中θk∈[-π，π]為切向方向系數(shù)，表示切向量沿nk（即z′軸）旋轉(zhuǎn)的角度。在慣性坐標(biāo)系o-xyz中表示為tk，可通過如下公式轉(zhuǎn)換得到：

Rk表示坐標(biāo)系o′-x′y′z′到o-xyz的坐標(biāo)轉(zhuǎn)換矩陣。

對于靜態(tài)障礙物，通過利用擾動矩陣修正初始流場流速即可得到擾動流場流速：

接下來考慮無人機(jī)對動態(tài)障礙物的規(guī)避，首先根據(jù)第k個(gè)障礙物實(shí)際運(yùn)動速度的預(yù)測值uk，obs，定義參考速度：

然后，構(gòu)建相對流場，其中相對初始流場的流速為vd-vobs，相對擾動流場的流速為，因此可以將動態(tài)障礙物轉(zhuǎn)換為靜態(tài)障礙物來處理：

即可得到無人機(jī)躲避動態(tài)障礙物時(shí)的期望飛行速度。

4 基于GWO 和滾動時(shí)域優(yōu)化仿真樣本的生成

4.1 GWO

GWO 是一種非常高效的智能優(yōu)化算法，通過模仿灰狼群的社會等級制度和捕食策略，不斷迭代尋找最優(yōu)值［15］。它具有穩(wěn)定性好、搜索能力強(qiáng)、收斂性快等優(yōu)點(diǎn)，已被證明適用于眾多工程問題?；依鞘侨壕觿游?，有嚴(yán)格的社會等級制度和合作工作，如圖1所示，一個(gè)種群中的灰狼可以從高到低分為四個(gè)等級：首領(lǐng)狼α、副首領(lǐng)狼β、普通狼δ和底層狼ω，且等級越高，個(gè)體數(shù)量越少。通常等級較低的狼會服從等級較高的狼的命令，有時(shí)也會向高等級狼提出建議。為了便于對狼群等級制度進(jìn)行數(shù)學(xué)建模，定義群體中歷史最優(yōu)解為首領(lǐng)狼α，次最優(yōu)解和第三最優(yōu)解分別為副首領(lǐng)狼β和普通狼δ，所有其他個(gè)體為ω。

圖1 灰狼群體等級制度Fig.1 Grey wolf group hierarchy

灰狼在捕食過程中主要有三個(gè)步驟：第一，狼群追蹤獵物；第二，狼群包圍獵物；第三，狼群逐漸縮小包圍范圍，接近獵物并將其捕獲。假設(shè)在D維的搜索空間內(nèi)，狼群X的個(gè)體數(shù)量為N，即X=(X1，X2，X3，…，XN)，第i只灰狼的位置定義為Xi=，其中表示第i只灰狼在第d維上的位置。

首先描述灰狼逐漸靠近并包圍獵物的行為，第i只灰狼在第d維的位置滿足以下公式：

其中t為當(dāng)前迭代次數(shù)，表示獵物在第d維的位置，表示包圍步長，定義如下：

其中，rand1，rand2表示［0，1］之間的隨機(jī)數(shù)，變量a隨迭代次數(shù)的增加從最大值amax遞減到最小值amin：

其中tmax表示最大迭代次數(shù)。

灰狼在圍捕獵物時(shí)往往知道獵物的位置Xp（即實(shí)際參數(shù)優(yōu)化問題中的最優(yōu)解），但在實(shí)際參數(shù)優(yōu)化過程中，獵物位置Xp是未知的。通常情況下，灰狼α、β、δ能更好地反應(yīng)獵物的潛在位置，因此灰狼群體可以根據(jù)α、β、δ的位置Xα、Xβ、Xδ進(jìn)行搜索：

其中，ωj(j=α，β，δ)表示α、β、δ的權(quán)重系數(shù)：

其中f(Xj(t))表示第j只狼在t時(shí)刻的適應(yīng)度值，在本文中f為規(guī)劃的路徑長度。

4.2 滾動時(shí)域優(yōu)化

基于DNN 的自適應(yīng)優(yōu)化效果在很大程度上取決于樣本的質(zhì)量和數(shù)量。對于無人機(jī)在軍事航空方面的應(yīng)用：在樣本數(shù)量方面，獲取足夠數(shù)量的真實(shí)飛行樣本需要付出較高的代價(jià)；在樣本質(zhì)量方面，難以保證無人機(jī)飛行的路徑每一步都是最優(yōu)的。本文通過仿真生成訓(xùn)練樣本的方式解決以上難題，算法流程如圖2所示。首先，在區(qū)域S={(x，y，z)|0＜x＜5000 m，0＜y＜5000 m，0＜z＜1500 m}內(nèi)隨機(jī)產(chǎn)生20 個(gè)大小、形狀各不相同的障礙物，隨機(jī)選擇不同的位置作為無人機(jī)路徑規(guī)劃的初始位置和目標(biāo)點(diǎn)。然后，使用IFDS 來規(guī)劃避障航路，在規(guī)劃的每一步中，均采用GWO 尋找基于當(dāng)前相對位置、速度及航向的IFDS 的最佳參數(shù)，通過尋找合適的參數(shù)使剩余路徑規(guī)劃成功且長度最短。之后，更新IFDS 的參數(shù)并前進(jìn)一步，再繼續(xù)基于當(dāng)前狀態(tài)調(diào)用GWO 優(yōu)化和更新IFDS 參數(shù)。該策略也可以看作是一種特殊的滾動時(shí)域控制，時(shí)域長度是從當(dāng)前位置到路徑規(guī)劃結(jié)束。

5 IFDS系數(shù)自適應(yīng)優(yōu)化

雖然采用智能優(yōu)化算法可使規(guī)劃路徑全局最優(yōu)，但該方法優(yōu)化產(chǎn)生的系數(shù)是始終不變的，無法根據(jù)環(huán)境的變化做出相應(yīng)的調(diào)整，且計(jì)算量較大，因此該方法僅適用于靜態(tài)已知的環(huán)境。滾動時(shí)域優(yōu)化可用于動態(tài)環(huán)境下的局部優(yōu)化，但通常情況下該算法耗時(shí)較長，難以保證路徑規(guī)劃的實(shí)時(shí)性。針對以上問題，本文引入DNN 基于當(dāng)前環(huán)境實(shí)時(shí)自適應(yīng)調(diào)整IFDS 參數(shù)。以ψ、d_min 和φ作為DNN 的輸入，其中ψ表示無人機(jī)當(dāng)前航向，d_min表示無人機(jī)與障礙物的最近距離，φ表示無人機(jī)指向目標(biāo)點(diǎn)的矢量和無人機(jī)到最近障礙物的矢量之間的夾角；IFDS 的排斥系數(shù)ρ、切向系數(shù)σ、方向系數(shù)θ作為網(wǎng)絡(luò)輸出端的特征提取。通過經(jīng)驗(yàn)數(shù)據(jù)和不斷嘗試將隱藏層神經(jīng)元數(shù)目設(shè)為6，隱藏層設(shè)為3 層，并選擇Sigmoid函數(shù)作為激活函數(shù)，最大迭代次數(shù)設(shè)為5000。采用DNN和帶沖量的梯度下降算法，以均方差為損失函數(shù)，通過離線訓(xùn)練的方式以消除訓(xùn)練時(shí)間對實(shí)時(shí)路徑規(guī)劃的影響。為測試訓(xùn)練所得網(wǎng)絡(luò)的泛化性能，隨機(jī)選取20%的樣本作為測試集，經(jīng)測試均方根誤差為0.00241，說明訓(xùn)練所得網(wǎng)絡(luò)具有較強(qiáng)的泛化能力。該方法的總體框架如圖3所示。

6 仿真驗(yàn)證

為驗(yàn)證本文提出的基于DNN 和IFDS 的實(shí)時(shí)路徑規(guī)劃方法的高效性，在Matlab 2018b 中進(jìn)行了仿真驗(yàn)證。仿真參數(shù)如下：采樣周期ΔT設(shè)為0.5 s；無人機(jī)速度v=20 m/s；最大轉(zhuǎn)彎速率最大爬升速率；最大爬升角γmax=π/6；控制器增益設(shè)為kψ= 0.1，kγ= 0.1。

圖2 仿真樣本生成框架Fig.2 The framework of simulation sample generation

圖3 整體系統(tǒng)架構(gòu)Fig.3 The structure of the overall system

6.1 躲避靜態(tài)障礙物

為證明該方法的魯棒性，隨機(jī)選取六個(gè)不同位置作為無人機(jī)航路規(guī)劃的起始點(diǎn)和目標(biāo)點(diǎn)。如圖4所示，路徑1 中，無人機(jī)初始位置為（0，0，400）m，目的地為（5000，5000，500）m，初始航向角為π/2，初始爬升角為0；路徑2 中，無人機(jī)初始位置為（0，1000，200）m，目標(biāo)點(diǎn)為（5000，25000，300）m，初始航向角和初始爬升角都設(shè)為0；路徑3 中，無人機(jī)初始位置為（2000，0，300）m，目標(biāo)點(diǎn)為（3500，5000，400）m，初始航向角為π，初始爬升角為0。仿真結(jié)果表明，針對不同的飛行環(huán)境，本文提出的方法所規(guī)劃的航路均可安全到達(dá)目標(biāo)點(diǎn)。該方法的優(yōu)點(diǎn)是可以根據(jù)周圍環(huán)境實(shí)時(shí)自適應(yīng)的調(diào)整參數(shù)，具有較強(qiáng)的魯棒性，而經(jīng)典方法根據(jù)經(jīng)驗(yàn)手動調(diào)整參數(shù)可能會因參數(shù)不合適而導(dǎo)致路徑規(guī)劃失敗。

為了進(jìn)一步分析本文方法所規(guī)劃的路徑性能指標(biāo)，以路徑1 為案例，將本方法規(guī)劃的路徑與GWO 優(yōu)化的全局最優(yōu)路徑進(jìn)行比較，如圖5所示。全局最優(yōu)路徑長度為7590 m，本文方法規(guī)劃的路徑長度為7660 m，可見其規(guī)劃效果接近于全局最優(yōu)解。

圖4 本文方法路徑規(guī)劃結(jié)果Fig.4 The paths planned by the proposed method

圖5 本文方法與全局最優(yōu)路徑對比Fig.5 The proposed method is compared with the global optimal path

6.2 躲避動態(tài)障礙物

假設(shè)無人機(jī)從（0，0，400）m 處出發(fā)，向終點(diǎn)（5000，5000，500）m 處飛行，初始航向角為π/2，爬升角為0。在任務(wù)空間中，存在兩個(gè)運(yùn)動威脅：圓柱形威脅半徑200 m，初始位置為（200，1800，0）m，運(yùn)動方程為vx=9+0.01t，vy=20cos（0.03π·t）-0.03t，vz=0；橢球形威脅半徑為200 m，初始位置為（800，3500，500）m，運(yùn)動方程為vx=8+0.02t，vy=-8+0.03t，vz=0。關(guān)于移動威脅的運(yùn)動信息可用擴(kuò)展卡爾曼濾波（EKF）在線估計(jì)得到［16］。動態(tài)威脅軌跡及無人機(jī)航路規(guī)劃效果如圖6所示，無人機(jī)與動態(tài)威脅表面之間的距離如圖7所示，無人機(jī)與動態(tài)威脅表面的最短距離均大于0，這表明無人機(jī)能夠成功地避開動態(tài)威脅到達(dá)目標(biāo)點(diǎn)。經(jīng)統(tǒng)計(jì)，該方法的單步規(guī)劃平均耗時(shí)為0.0131 s，遠(yuǎn)遠(yuǎn)小于采樣周期，而傳統(tǒng)的滾動時(shí)域優(yōu)化方法平均耗時(shí)為0.4532 s，說明本文所提出的方法計(jì)算開銷小，實(shí)時(shí)性強(qiáng)，能夠完成復(fù)雜動態(tài)環(huán)境下的路徑規(guī)劃任務(wù)。

圖6 無人機(jī)躲避動態(tài)障礙物Fig.6 UAV avoids dynamic obstacles

圖7 無人機(jī)與動態(tài)威脅表面的距離Fig.7 The distance between the UAV and the dynamic threat surface

7 結(jié)論

本文針對三維復(fù)雜動態(tài)環(huán)境下無人機(jī)的路徑規(guī)劃問題，首次將DNN 強(qiáng)大的自適應(yīng)學(xué)習(xí)能力和IFDS高效的避障效率有效地結(jié)合起來，以滿足各種任務(wù)的要求。經(jīng)仿真驗(yàn)證結(jié)果表明，該方法具有較高的魯棒性，較強(qiáng)的實(shí)時(shí)性，同時(shí)規(guī)劃的航路具有較好的性能指標(biāo)。主要結(jié)論如下：

（1）基于GWO 和滾動時(shí)域優(yōu)化來生成仿真樣本，有效地解決了DNN 訓(xùn)練樣本質(zhì)量不高和數(shù)量不足的難題。

（2）引入離線訓(xùn)練好的DNN來根據(jù)當(dāng)前時(shí)刻無人機(jī)所面對的環(huán)境信息自適應(yīng)調(diào)整IFDS 系數(shù)，解決了難以選取合適系數(shù)的問題，提高了避障效率。

（3）本文提出的方法在保證路徑質(zhì)量的前提下，同時(shí)適用于靜態(tài)環(huán)境和動態(tài)環(huán)境。有效解決了智能優(yōu)化算法只適用于靜態(tài)環(huán)境，而動態(tài)環(huán)境下的滾動時(shí)域優(yōu)化計(jì)算開銷較大的問題。