基于空間變形的無人機路徑規(guī)劃算法研究

肖志慧，劉久富，王志勝，賴幸君，林 磊

(1 南京航空航天大學自動化學院，南京 210016；2 國家無線電監(jiān)測中心檢測中心，北京 100041)

0 引言

隨著智能技術的發(fā)展，無人機(unmanned aerial vehicle，UAV)表現出的功能更加豐富，在農業(yè)植保、城鎮(zhèn)規(guī)劃、商業(yè)宣傳、防火救災等各個方面的應用均表現出色[1]。但應用場景復雜化后也帶來了工程倫理問題和頻發(fā)的安全問題。

對于無人機實現任務的過程來說，其核心是飛行路徑的規(guī)劃。無人機的路徑規(guī)劃，簡單來說，旨在設計一條安全的運動軌跡，完成從起始點飛向目標點的任務，盡量避免事故的發(fā)生[2]。目前，國內外對路徑規(guī)劃算法的研究，可以按照環(huán)境是否已知進行分類。對環(huán)境完全已知的情況，基于建立的全局地圖模型，路徑規(guī)劃算法有A*算法、隨機采樣法、蟻群算法(ACO)、粒子群算法(PSO)、人工魚群算法(AFSA)、螢火蟲算法(FA)等群智能算法；對環(huán)境不完全已知的情況，有源于物理學思想的人工勢場法、基于行為法、滾動窗口法以及與傳統(tǒng)算法相結合的聯(lián)合算法[3-6]。

吳振等[7]在環(huán)境已知情況下，為了獲取最短最優(yōu)航路，提出了一種改進的遺傳算法，通過標定適應度值，引入精英保留策略，避免陷入局部最優(yōu)，從而實現航路最優(yōu)；焦慶宇等[8]考慮人群密度風險，提出了改進的A*算法，并利用卷積神經網絡預測人群密度，從而實現無人機實時的路徑規(guī)劃；田廣鍵等[9]將傳統(tǒng)人工勢場法分層，并加入分段函數和復合引力場，顯著提高了規(guī)劃效率，彌補了曲率過大的問題；張瀚等[10]提出了聯(lián)合深度確定性策略梯度算法(DDPG)和人工勢場法的混合算法，通過干預DDPG選擇動作時的策略來規(guī)化環(huán)境不完全已知時的可靠路徑，但損失了收斂速度，動態(tài)環(huán)境下效果一般。近年來計算機技術快速發(fā)展，深度學習和強化學習也越來越多地應用于無人機路徑規(guī)劃[11]。于盛[12]基于馬爾科夫決策過程，用強化學習算法進行優(yōu)化，提出了基于MADDPG的多機路徑規(guī)劃算法，效果顯著。Liu等[13]針對無人機的移動邊緣，提出了一種基于QoS的動作選擇策略，優(yōu)化運動軌跡，收斂速度明顯提高。

當前路徑規(guī)劃方法已經比較成熟，可以同時考慮最短長度和最小威脅。然而，城市環(huán)境下低空飛行的無人機還需要綜合考慮安全風險。在環(huán)境越來越復雜的當下，工程倫理下的人員安全問題越來越重要。若忽略飛行中的安全風險，會給無人機的實際飛行過程帶來隱患。文中綜合考慮了路徑的平滑性、可實現性和安全性，首先，在地圖中同時添加安全裕度和風險信息；然后，在基于空間變形算法的基礎上，對灰狼算法生成的全局路徑進行二次變形，使其更為安全。根據目標點在原灰狼算法產生的全局路徑中的分段比例，在起點終點之間的連線上確定控制點，控制點與目標點一一對應，將薄板樣條所在的空間進行變形，產生一條更優(yōu)的飛行路徑，同時使得變形所需能量最小，以實現無人機平滑飛行。

1 環(huán)境模型

1.1 安全裕度

柵格法可以表示無人機飛行的工作環(huán)境，適當大小的柵格粒子能夠有效將地圖簡化，降低難度[14]。障礙物在柵格圖中表現為不可穿越的黑色柵格，見圖1(a)。在規(guī)劃過程中，為了得到步數最短的路徑，極易出現距離障礙物過近、貼障礙物飛行的情況?？紤]到飛行環(huán)境的復雜，為了保障飛行安全，應留有避險空間[15]。將障礙物在柵格圖中的表示區(qū)域進行擴展，擴展的柵格可以避免和障礙物之間距離過近，進而提升無人機在不同環(huán)境下飛行的安全性。

具體來說，環(huán)境地圖中以(a,b)坐標為障礙物中心，實際長為p，實際寬為q，單位柵格邊長代表的實際距離為d，擴大其坐標為：

(1)

式中：g(a,b)表示此區(qū)域是否可穿越，數值取1為不可穿越，數值取0為可穿越；x和y表示柵格地圖坐標信息。

對于不規(guī)則的障礙物，可以將其分割為規(guī)則的障礙物，然后運用式(1)進行坐標擴大。為了避免規(guī)劃時的空間資源浪費，將不與障礙物緊鄰的安全區(qū)域柵格剔除，可得到最終留有安全距離的障礙物柵格表示，見圖1(b)。

圖1 障礙物的柵格表示

1.2 風險建模

基于工程倫理方面的考慮，在復雜情況下，需要考慮無人機帶來的風險，因此對于可飛行的區(qū)域，增加風險因素值，建立風險評估矩陣：

(2)

式中Rij為第i行第j列柵格所對應的區(qū)域量化以后的風險值。

對風險進行估計：

R=fAP

(3)

式中：f為無人機的安全指標，表示其墜機概率；A為墜機后受影響的人數，取決于人口密度ρ和地面撞擊面積S[17]；P為受影響人群的傷亡，可表示為：

(4)

式中：d為環(huán)境遮蔽系數；pd為傷亡程度；e為死亡率至50%所需撞擊能量(d=0.5時)；β為死亡率至100%所需撞擊能量(無遮蔽時)；E為無人機產生碰撞時的動能。

E的表達式為：

(5)

式中：vmax為運行最大速度；vt為無人機墜落速度；ρa為當地實際空氣密度。

因此，無人機飛行產生的風險代價為：

(6)

式中：RN為實際飛行路徑點的風險值；Rs為標準風險值。

2 空間變形算法

2.1 問題基礎

薄板樣條(thin-plate spline，TPS)是一種插值方法，其本質是進行數據插值，也可以進行平滑[18-19]。TPS算法經過所有控制點，最后尋找出一個光滑曲面，使得生成此曲面的彎曲能最小[20]。

在工作環(huán)境C中，有若干原點xi(i=1,2,…,n)，如圖2(a)所示。在基于TPS的原理上，將C看作獨立的可變形的金屬薄板，施加一定的形變，利用形變函數，使得原本的位置移動到x′i(i=1,2,…,n)。原點xi為控制點，移動終點x′i為目標點。在構造金屬板空間時，如式(7)所示，移動控制點到目標點，過程中產生的變形影響如圖2(b)所示。

圖2 薄板變形對平面點產生的影響

f(xi)=x′i

(7)

根據TPS算法，形變過程中施加的彎曲能大小為[20]：

(8)

彎曲能最小時，相對應點的形變函數為：

(9)

2.2 空間變形算法

對工作環(huán)境中的兩個目標，二者之間最短路徑為連接的直線。然而由于障礙物、任務點或者其它條件的約束，最短路徑無法實現。在空間變形算法中，工作環(huán)境視為一塊可變性的金屬板，獨立于此工作環(huán)境而單獨存在，在薄板樣條的基礎上，可以將兩個目標之間連線看作具有無限的長度，以便于變形扭曲。

在工作環(huán)境C中，有兩個點xs與xg，分別為起點與終點，xs經過路徑L到達xg，并經過工作環(huán)境中的任務點xt，如圖3(a)所示。L是不平滑的路徑，無人機實際中沿此路徑飛行不合理。將起點與終點之間連接的直線d視為金屬板上長度無限的直線，金屬板獨立于此工作環(huán)境單獨存在，通過對此金屬板施加彎曲能進行變形，L產生扭曲，不改變原先經過的任務點xt，在存在障礙物時避開障礙物，使得路徑平滑，產生起點與終點間新的路徑L′。根據TPS原理，當對金屬板施加彎曲能最小時，產生起點與終點之間的最優(yōu)路徑。在直線d上取控制點，對應L上的拐點為目標點。由此，金屬板空間變形過程中產生的影響如圖3(b)所示。

圖3 薄板變形對平面路徑產生的影響

對于金屬板空間的變形，計算其變形系數M：

M=E-1Y

(10)

(11)

式中：Y是由目標點坐標和0填充構成的矩陣；E的組成有兩部分，一部分是控制點，一部分是由控制點間歐氏距離構成的徑向基K。由于路徑的特殊性，起點與終點不產生形變，可以對矩陣進行升維并重構。當彎曲能最小時，對應變形見式(12)?？臻g變形算法中，Y矩陣與E矩陣具體組分如式(13)所示。

(12)

(13)

由此可得經過空間算法變形后的路徑L′為：

(14)

3 基于空間變形算法的路徑規(guī)劃系統(tǒng)

將無人機的路徑規(guī)劃總體上分為兩個部分：1)利用全局路徑規(guī)劃算法產生最短路徑；2)利用空間變形算法對全局路徑進行修改，形成最終路徑，使無人機按照變形后的路徑飛行。

對于所提出的基于空間變形的混合路徑規(guī)劃算法，可以將智能算法與空間變形算法進行有效的融合，在保留智能算法優(yōu)點的同時，也有效改善了這兩種算法在單獨使用時的缺陷。所使用的混合路徑規(guī)劃算法步驟流程圖如圖4所示。

圖4 混合路徑規(guī)劃算法流程圖

3.1 全局規(guī)劃階段

采用灰狼優(yōu)化算法進行全局路徑規(guī)劃以產生無人機的初始路徑?；依莾?yōu)化算法模擬了自然界灰狼的領導層級和狩獵機制，具有強收斂性，參數少等特點[21]。

狼群從高到低一般分為α,β,δ,ω四個模擬領導階層等級。按照上述等級的劃分，灰狼α對灰狼β,δ和ω有絕對的支配權；灰狼ω在灰狼群中的比例最大，同時灰狼ω又必須完全服從灰狼α,β和δ。

為了模擬灰狼群的等級制度，同時又能簡化算法，因此，假設有一只灰狼α、一只灰狼β和一只灰狼δ作為灰狼領導者。將最優(yōu)方案作為灰狼α，第二和第三個最佳解決方案分別作為灰狼β和灰狼δ。剩下的候選解被假定為灰狼ω。在灰狼優(yōu)化算法中狩獵行為主要由灰狼α，β和δ進行引導，灰狼ω跟隨這3只狼。

將灰狼包圍獵物的行為定義為[21]：

Di=|Ci·Xi(t)-X(t)|

(15)

Xi(t+1)=Xi(t)-Ai·Di,i=1,2,3

(16)

(17)

式(15)表示灰狼α，β和δ與灰狼ω間的距離；式(16)是灰狼ω各自跟隨灰狼α，β和δ的位置更新；式(17)是灰狼ω最終位置公式。t是當前的迭代次數；X(t)和X(t+1)分別為灰狼ω當前位置和生成的最終位置，X1(t),X2(t),X3(t)分別為灰狼α，β和δ當前位置，X1(t+1)，X2(t+1)，X3(t+1)分別為灰狼ω各自跟隨灰狼α，β和δ生成的位置；D1,D2,D3為灰狼α，β和δ與灰狼ω間的距離；A1,A2,A3和C1,C2,C3是生成的隨機向量，Ai=2arand(m)-a，Ci=2rand(m)，其中a為收斂因子，隨著迭代次數線性減小，rand(m)為0到1之間的隨機數?；依莾?yōu)化算法的實現流程如圖5所示。

3.2 路徑修改階段

由全局路徑規(guī)劃器生成的全局路徑是不碰撞的路徑，但是在轉折點處過于鋒利，不符合無人機實際飛行情況。在飛行時應用空間變形算法，更符合無人機飛行動力學，并保證無人機的穩(wěn)定移動。將無人機的工作環(huán)境視為一塊可變形的薄板，獨立于工作環(huán)境而單獨存在，將全局路徑、起點和終點之間的直線看作可以任意變形。將全局路徑中的不平滑的拐點視為路徑必須經過的任務點，即為變形后的目標點(xt,yt)，根據目標點在原全局路徑中的分段比例，在起點終點之間連線d上確定控制點(xc,yc)，控制點與目標點一一對應。以改進TPS算法為基礎，將薄板所在的空間進行變形。

f(L)=L′

(18)

由于在變形中仍需考慮障礙物避讓，在進行實際的路徑變形修改時，將目標點矩陣與控制點矩陣升維，加入障礙物頂點的約束。此外，還應考慮工作環(huán)境的約束。多重約束下的最小彎曲能實現變形，此時，經過改進后的空間變形算法，P與Y具體組分為：

(19)

式中：(xo,yo)為障礙物頂點坐標；(xp,yp)為構成工作環(huán)境的邊界點。

路徑修改可分為5個步驟：

1)獲取全局路徑的起點、終點、轉折點坐標，構建目標點矩陣。

2)在連接起點終點的直線段上，根據全局路徑轉折點，等比例提取控制點，構建控制點矩陣。

3)獲取障礙物頂點坐標和工作環(huán)境的頂點坐標。對基礎控制點目標點矩陣進行升維。

4)計算使彎曲能最小時的變形系數M。

5)對全局路徑進行變形修改以獲取最優(yōu)路徑。

4 算法實現與仿真

采用MATLAB/Simulink軟件對上述基于空間變形的路徑規(guī)劃算法進行仿真分析。根據實際情況設置地圖場景。地面人員密度已知，依照先驗知識估算相關計算參數，參數設置見表1。設置的地圖不同，遮蔽系數也不同，可以計算出不同區(qū)域的不同風險成本見表2。

表1 風險計算參數設置

表2 風險成本

將截取的地圖柵格化，考慮到緊貼障礙物飛行有安全隱患，故引入安全裕度。橫縱坐標分別為20個柵格單位，每個柵格單位為1 m，將黃色坐標點(1，1)設為起始點，綠色坐標點(20，20)設為終點，黑色柵格為障礙物，灰色柵格為預留出的安全裕度，彩色柵格為可通行柵格，由淺及深表示風險等級逐漸增加。獲取地圖邊界點坐標(xp,yp)和障礙物頂點坐標(xo,yo)。含有風險成本的環(huán)境模型如圖6所示。

圖6 留有安全距離考慮風險的柵格地圖模型

在地圖中運用灰狼優(yōu)化算法進行全局路徑的規(guī)劃。設置種群里有12只狼，最大迭代次數為100，產生的路徑L如圖7所示，產生的路徑拐點為：

圖7 初始全局路徑

(20)

可以看出，產生的路徑有一些過大的轉折，路徑有不合理的曲率，可進行二次路徑修改。

根據路徑的轉折點，獲取目標點坐標，其中對一些過近的目標點進行優(yōu)化，適當取中。再根據轉折點在全局路徑中的比例，確定控制點坐標，計算出變形系數。目標點矩陣T，控制點矩陣H可表示為：

(21)

(22)

(23)

對產生的全部路徑運用空間變形算法，變形后產生的最優(yōu)路徑L′如圖8藍色路徑所示。

對結果進行對比分析見表3。

表3 算法對比(含安全裕度)

如表3所示，在考慮安全裕度的情況下，基于空間變形的混合算法與傳統(tǒng)的灰狼算法相比，規(guī)劃出了更短的路徑。如圖8和圖9所示，混合算法規(guī)劃出的路徑連續(xù)，更平滑，沒有鋒利拐點，且曲率值均小于1，符合無人機性能約束。因此，文中提出的基于空間變形的混合路徑規(guī)劃算法，可以獲得更短的可靠路徑。

圖8 二次修改路徑

圖9 曲率變化曲線

將文中方法與未考慮安全裕度的灰狼算法進行對比，以證明所提出算法的安全性。傳統(tǒng)灰狼算法在未考慮安全裕度的情況下，規(guī)劃出的路徑如圖10所示。表4對比兩種情況下的路徑長度和風險成本，可以發(fā)現，文中提出的混合算法，損失了5.7%的路徑長度的同時，降低了19%的風險代價，規(guī)劃出了一條更安全更符合無人機飛行動力學的最佳路徑。有利于復雜情況下的安全飛行。

圖10 灰狼算法規(guī)劃路徑(無安全裕度)

表4 算法對比

5 結論

針對UAV應用，提出的路徑規(guī)劃算法在賦予地圖風險信息的基礎上分兩部分進行：首先進行全局路徑規(guī)劃，然后為路徑修改階段。將灰狼算法和TPS算法進行改進，提出一種基于空間變形的混合路徑規(guī)劃算法，利用薄板樣條的特性，對工作環(huán)境進行變形，從而獲取無碰撞的最佳路徑。

仿真結果表明，所采用的基于空間變形的混合路徑規(guī)劃算法，與傳統(tǒng)灰狼算法相比，在路徑長度方面，基于空間變形的混合路徑規(guī)劃算法規(guī)劃出的路徑長度為30.97 m，比傳統(tǒng)灰狼算法多了5.7% ；而在安全風險方面，基于空間變形的路徑規(guī)劃算法與建筑物間留有裕度，安全系數更高，相比于傳統(tǒng)灰狼算法，降低了19%的風險代價值，以較低的路徑長度成本換取了更安全的路徑；在光滑度方面，基于空間變形的路徑規(guī)劃算法更加平滑，鋒利拐點個數降低，無人機飛行路徑更加平穩(wěn)，貼合實際?？偟膩碚f，所提出的算法使得無人機路徑規(guī)劃性能和安全性得到大幅度提升，但還存在應用場景的局限性，在更為復雜的場景中還有優(yōu)化空間。未來，可以發(fā)展到多機飛行和三維空間中。