基于數(shù)字地形平滑和人工勢(shì)場(chǎng)的無(wú)人機(jī)近地三維路徑規(guī)劃

朱 熠,田 輝,郝向宇,史涵意,馬文峰,王 聰

(陸軍工程大學(xué) 野戰(zhàn)工程學(xué)院, 南京 210007)

0 引言

通常來(lái)說(shuō),路徑規(guī)劃的目的是生成一條最快、最簡(jiǎn)單的路徑,擺脫復(fù)雜地形、障礙物和威脅源等所有不利因素[1-3]。然而,當(dāng)無(wú)人機(jī)在近地區(qū)域飛行時(shí),由于飛行任務(wù)可能對(duì)地形有所限制和要求,如地形跟隨(terrain following,TF)和地形回避(terrain avoidance,TA)。因此,威脅因素仍在考慮之中,此時(shí)三維路徑規(guī)劃的任務(wù)就變得艱巨而富有挑戰(zhàn)性。

在處理近地區(qū)域三維路徑規(guī)劃問題時(shí),首先需要一個(gè)數(shù)字地形模型。以前,地形模型通常由路徑設(shè)計(jì)者自己創(chuàng)建,現(xiàn)在有許多真實(shí)地圖的數(shù)字模型可用。例如,航天飛機(jī)地貌雷達(dá)任務(wù)(shuttle radar topography mission,SRTM)模型是最常用的模型之一[4-6],其所有數(shù)據(jù)覆蓋了地球80%以上的陸地表面[7-8]。通過對(duì)SRTM地形數(shù)據(jù)進(jìn)行一些輕微的轉(zhuǎn)換,可以使用Matlab軟件進(jìn)行編輯和處理[9-10]。通過地形平滑方法,可以生成安全飛行曲面。不少文獻(xiàn)通過對(duì)飛行軌跡的優(yōu)化,以滿足飛機(jī)的機(jī)動(dòng)性和地形跟隨的要求[11-12]。

在通過地形平滑生成安全飛行曲面后,可以通過搜索算法獲得最優(yōu)飛行路徑。關(guān)于無(wú)人機(jī)路徑規(guī)劃算法大多是基于搜索算法的改進(jìn)和評(píng)估指標(biāo)的優(yōu)化[13-14]。本文中提出一種基于數(shù)字地形平滑和人工勢(shì)場(chǎng)法的三維路徑規(guī)劃方法,該方法將地形平滑技術(shù)和向量場(chǎng)搜索相結(jié)合。仿真結(jié)果表明,所設(shè)計(jì)的方法可以在安全飛行曲面上獲得最優(yōu)的三維路徑。

1 數(shù)字地形平滑

1.1 數(shù)字地形模型

為獲得數(shù)字地形模型,采用SRTM數(shù)字高程模型。下載SRTM的59_06地形數(shù)據(jù),如圖1(a)所示。由于原始59_06地形非常大,因此只選擇了其中的一小部分,如圖1(b)所示。

圖1 59_06地形數(shù)據(jù)和所選定區(qū)域

使用Global Mapper或Surfer軟件可以方便地編輯SRTM地形數(shù)據(jù)。通過Surfer,SRTM地形數(shù)據(jù)可以從.grd格式轉(zhuǎn)換為.dat格式,.dat格式的文件可以在Matlab軟件中進(jìn)行編輯。

1.2 地形平滑處理

無(wú)人機(jī)的飛行狀態(tài)會(huì)受到其機(jī)動(dòng)性能的影響,這些性能主要包括最大爬升角、最小轉(zhuǎn)彎半徑、最大機(jī)動(dòng)過載、飛行速度和飛行高度的限制。通過平滑地形模型并生成安全飛行面可以滿足無(wú)人機(jī)機(jī)動(dòng)性能的這些要求。在本節(jié)中,提出了設(shè)置地面以上安全高度、減小坡度和減小曲率的設(shè)計(jì)思路。

1) 設(shè)置高于地面的安全高度。地面的隱蔽要求和防撞要求之間存在矛盾。為了實(shí)現(xiàn)平衡,設(shè)置了地面以上的最低安全高度。當(dāng)無(wú)人機(jī)飛得高于安全高度時(shí),飛機(jī)可以避免撞到地面。在飛行高度高于安全高度的情況下,無(wú)人機(jī)可以最大限度地降低航跡的高度。在不同的地形中,最低安全海拔是不同的:在海洋和平原地區(qū),安全海拔較低;而在丘陵和山區(qū),安全海拔較高。一般情況下,適宜的安全海拔為:15～30 m的海域、60～80 m的平原區(qū)和約120 m的丘陵區(qū)。

2) 降低坡度。在低空突防任務(wù)中,無(wú)人機(jī)有一個(gè)最大爬升角的限制。如果前方有坡度大于最大爬升角的山峰或障礙物,無(wú)人機(jī)需要提前爬升。否則,將導(dǎo)致與山體或障礙物發(fā)生碰撞。因此,需要對(duì)坡度較大的地形進(jìn)行修改,以滿足無(wú)人機(jī)縱向飛行的約束。坡度約束由無(wú)人機(jī)的最大爬升角決定。最常用的方法是用無(wú)人機(jī)的最大爬升角代替原始地形的最大坡度。坡度降低方法如圖2所示。

圖2 減小坡度

圖2中,i為描述高度信息的離散網(wǎng)格點(diǎn),h(i)為對(duì)應(yīng)的高程值,d為網(wǎng)格間距。記坡度為k=(h(i+1)-h(i))/d,最大可飛坡度為kmax,坡度處理邏輯為:

ifk>kmax,h(i)=h(i+1)-d·kmax,

i=1,…,n

else ifk<-kmax,h(i+1)=h(i)-l·kmax

elseh(i)=h(i),h(i+1)=h(i+1)

上述邏輯只考慮了相鄰2個(gè)點(diǎn)之間的坡度問題,沒有考慮坡度的整體變化趨勢(shì),需要多次迭代才能得到理想的效果,甚至可能會(huì)出現(xiàn)峰巔相互拉高而不收斂的現(xiàn)象。

綜合來(lái)說(shuō)主要的改進(jìn)思路包括2個(gè)方面:

① 以最高山峰點(diǎn)為起點(diǎn),依次對(duì)其兩側(cè)地形坡度進(jìn)行處理。首先尋找每行每列最高的山峰,標(biāo)記它們的位置;然后從最高山峰開始分別從正向和負(fù)向2個(gè)方向進(jìn)行坡度約束處理。

② 對(duì)山峰進(jìn)行保護(hù),對(duì)小于最大可飛坡度的山峰盡量少做處理。

采用上述改進(jìn)邏輯,可以提高程序處理的速度,山峰不會(huì)互相削平或者相互拔高。

3) 降低曲率。無(wú)人機(jī)在鉛垂平面內(nèi)運(yùn)動(dòng)軌跡的曲率ρa(bǔ)和法向加速度nz的關(guān)系為

(1)

根據(jù)無(wú)人機(jī)的速度和最大法向加速度計(jì)算出地形曲率的最大值ρa(bǔ)max,然后對(duì)地形的曲率ρx、ρy進(jìn)行約束處理,使它們的絕對(duì)值小于ρa(bǔ)max。由于相互正交的x和y方向的處理方法相同,下面只介紹x方向的處理方法。

設(shè)地形高程函數(shù)為h=h(x,y),則曲率ρx為

(2)

圖3 地形曲率約束處理

為求柵格點(diǎn)的曲率必須已知其一階偏導(dǎo)數(shù)和二階偏導(dǎo)數(shù)。利用離散數(shù)據(jù)求近似偏導(dǎo)數(shù)的方法較多?？紤]到在實(shí)時(shí)軌跡優(yōu)化時(shí),采用3次卷積插值算法對(duì)數(shù)字地形進(jìn)行內(nèi)插。用上述方法內(nèi)插出的地形來(lái)求柵格點(diǎn)的一階偏導(dǎo)數(shù)和二階偏導(dǎo)數(shù),然后求出柵格點(diǎn)的曲率。曲率約束處理采用與坡度約束處理同樣的邏輯,具體步驟如下:

步驟1在坡度約束處理完成的基礎(chǔ)上,從最高山峰點(diǎn)兩側(cè)分別開始進(jìn)行曲率約束處理;

步驟2網(wǎng)格點(diǎn)(xi,yj)的一階偏導(dǎo)數(shù)為

h′x=(hi+1, j-hi-1, j)/2l

(3)

由于3次卷積插值在柵格點(diǎn)處的二階偏導(dǎo)數(shù)不連續(xù),因此網(wǎng)格點(diǎn)(xi,yj)處的二階偏導(dǎo)數(shù)需要分左右2種情況來(lái)求。網(wǎng)格點(diǎn)(xi,yj)處的二階右偏導(dǎo)數(shù)為

h″xr=(2hi-1, j-5hi, j+4hi+1, j-hi+2, j)/l2

(4)

網(wǎng)格點(diǎn)(xi,yj)處的二階左偏導(dǎo)數(shù)須利用網(wǎng)格點(diǎn)(xi-1,yj)處展開的插值公式求得,表達(dá)式為

h″xl=(-hi-2, j+4hi-1, j-5hi, j+2hi+1, j)/l2

(5)

把式(3)代入式(5)中可得網(wǎng)格點(diǎn)處的左右曲率。

步驟3網(wǎng)格曲率約束的處理會(huì)改變?cè)瓉?lái)已進(jìn)行過坡度修正的地形,因而每次計(jì)算網(wǎng)格曲率之前都要進(jìn)行網(wǎng)格坡度的重新判斷和調(diào)整;

步驟4措施3已確保了地形坡度約束是滿足的,因而山峰和山谷之間的網(wǎng)格點(diǎn)是不需要進(jìn)行曲率修正的,曲率的約束處理只存在于山峰和山谷。為了對(duì)山峰進(jìn)行保護(hù),山峰和山谷須分別設(shè)計(jì)處理方法。

1.3 安全飛行曲面生成

在本節(jié)中,在Matlab中分析了SRTM高程地形模型。Matlab中的“l(fā)oad”函數(shù)可用于讀取SRTM的.dat文件,該文件已轉(zhuǎn)換為包含經(jīng)度、緯度和高度分量的三維矢量?！熬W(wǎng)格”函數(shù)用于展開平面中的點(diǎn)。插值函數(shù)“gridata”用于完成高度方向上的點(diǎn)。然后可以在Matlab中使用“網(wǎng)格”函數(shù)生成數(shù)字地形?！癱ontour3”函數(shù)可用于生成等高線,如圖4(a)和圖4(b)所示。

圖4 地形數(shù)據(jù)的等高線

然后,上述方法可以用于平滑數(shù)字地形,主要考慮的因素是海拔、坡度和曲率的安全系數(shù)。平滑的結(jié)果如圖5所示。

通過比較圖5和圖4(a),可以看到地形平滑的結(jié)果,即生成的安全飛行曲面。可見,總體趨勢(shì)是完全保留的,最低高度提高了約150 m,最高山峰的高度保持不變,相鄰的陡峭山峰平滑地連接在一起,因此為了安全飛行,坡度和曲率都減小了。

圖5 平滑處理后的安全飛行曲面

2 三維最優(yōu)路徑規(guī)劃

2.1 機(jī)動(dòng)性約束指標(biāo)

基于戰(zhàn)術(shù)運(yùn)用和飛行器性能的要求,無(wú)人機(jī)的機(jī)動(dòng)性約束一般情況下包括以下5點(diǎn):

1) 最短直飛距離:即飛行器在姿態(tài)改變前必須保持的最短直飛距離,設(shè)航跡由{Li|i=1,2,…,n}組成,最短直飛距離為L(zhǎng)min,該約束可表示為

Li>Lmin(i=1,…,n)

(6)

2) 最小轉(zhuǎn)彎半徑:描述的是無(wú)人機(jī)過載約束的能力,在轉(zhuǎn)彎過程中不考慮切向加速度因素,則有:

(7)

式(7)中:R為轉(zhuǎn)彎半徑,V為無(wú)人機(jī)飛行速度,φ為滾轉(zhuǎn)角。

由于無(wú)人機(jī)有最小飛行速度Vmin,而且有最大滾轉(zhuǎn)角φmax限制,所以R有最小限制,當(dāng)規(guī)劃的航跡的曲率大于該曲率時(shí),無(wú)人機(jī)受最大過載限制將不能準(zhǔn)確跟蹤航跡飛行。因此,路徑的曲率半徑必須大于無(wú)人機(jī)最小轉(zhuǎn)彎半徑。即約束表達(dá)式如下:

(8)

3) 最大爬升/俯沖角:最大爬升/俯沖角的限制是產(chǎn)生的航跡在飛行控制系統(tǒng)能夠允許的最大爬升/俯沖角內(nèi)。假設(shè)最大爬升/俯沖角為γmax,該約束可以這樣表示:

(9)

4) 最大飛行路徑:對(duì)于燃油和飛行時(shí)間的限制,飛行路徑的總長(zhǎng)度必須小于預(yù)設(shè)的最大距離。將飛行路徑的最大長(zhǎng)度設(shè)置為L(zhǎng)max,這樣該約束可以表示為

(10)

5) 目標(biāo)進(jìn)入方向:對(duì)于困難任務(wù)的要求,有時(shí)無(wú)人機(jī)需要從某個(gè)方向接近目標(biāo)。這就是進(jìn)入方向的約束。

2.2 威脅評(píng)估指標(biāo)

飛行路徑規(guī)劃的評(píng)估指標(biāo)設(shè)計(jì)為以下2種:

1) 代價(jià)指標(biāo)

(11)

2) 威脅指標(biāo)

(12)

(13)

式(13)中:Kj為第j個(gè)威脅源的強(qiáng)度,Rsj為飛行器距第j個(gè)威脅源的斜距,Nt為已知威脅的個(gè)數(shù)。

2.3 最優(yōu)路徑搜索算法

航跡生成的原理分成2步:首先在xy平面內(nèi)用路徑搜索算法規(guī)劃出能夠規(guī)避威脅的最優(yōu)二維路徑;然后根據(jù)第1節(jié)所得地形平滑后的安全曲面,擴(kuò)展出三維航路。故本節(jié)重點(diǎn)介紹xy平面內(nèi)的路徑搜索算法。

1) 算法描述。算法采用人工勢(shì)場(chǎng)法的基本思想,其基本原理就是對(duì)所處障礙環(huán)境中每一點(diǎn)都賦一個(gè)勢(shì)場(chǎng)值,其值是目標(biāo)點(diǎn)的引力和障礙點(diǎn)的斥力疊加之和。因此路徑規(guī)劃也就是從起始點(diǎn)沿著勢(shì)場(chǎng)下降最快的方向到達(dá)目標(biāo)點(diǎn)。

將威脅源看成“規(guī)避場(chǎng)”,即每一個(gè)威脅障礙的位置原點(diǎn)都看作在產(chǎn)生一個(gè)排斥性的斥力場(chǎng)作用在即將路過的無(wú)人機(jī)上面。產(chǎn)生的斥力大小與它們之間距離的平方成反比,平面內(nèi)任意一點(diǎn)受到第i個(gè)威脅障礙的斥力計(jì)算公式為

(14)

對(duì)于路徑的目標(biāo)點(diǎn)同樣規(guī)劃一個(gè)場(chǎng),這是一個(gè)吸引力場(chǎng),吸引力的大小正比于無(wú)人機(jī)和目標(biāo)的距離,其計(jì)算公式為

(15)

式(15)中:xaim、yaim為目標(biāo)點(diǎn)的位置坐標(biāo)。這個(gè)力同樣對(duì)平面中的點(diǎn)生成表格,是一張二維引力表。設(shè)當(dāng)無(wú)人機(jī)處于初始位置時(shí),引力達(dá)到最大值;當(dāng)無(wú)人機(jī)運(yùn)行到目標(biāo)點(diǎn)時(shí),引力達(dá)到最小值。這樣的一個(gè)10×10網(wǎng)格點(diǎn)的引力表格如表2所示,表中數(shù)值為900的是無(wú)人機(jī)初始位置即引力最大的點(diǎn),數(shù)值為300的是目標(biāo)位置即引力最小的點(diǎn)。

2) 搜索過程。對(duì)斥力表和引力表進(jìn)行合成,可以生成一張合力表,通過合力引導(dǎo)無(wú)人機(jī)運(yùn)行出最優(yōu)軌跡。具體步驟如下:

步驟1將整個(gè)平面區(qū)域離散化成100×100的網(wǎng)格地圖,然后從第一個(gè)網(wǎng)格點(diǎn)P1,1開始,選取一個(gè)3×3的子圖,共包含9個(gè)網(wǎng)格元素;

步驟2計(jì)算這個(gè)3×3的子圖中的9個(gè)元素相互之間的受力值,選出所受合力最小的網(wǎng)格點(diǎn);

步驟3進(jìn)行解的攝動(dòng)操作,即以步驟2中所得的合力最小網(wǎng)格點(diǎn)為中心,重新選取一個(gè)3×3的子圖;

步驟4比較當(dāng)前最小網(wǎng)格點(diǎn)的坐標(biāo)與目標(biāo)點(diǎn)是否相等,如果相等則終止程序,否則繼續(xù)程序;

步驟5重復(fù)步驟2—步驟4直到完成所有100×100網(wǎng)格點(diǎn)。

3) 局部最優(yōu)的處理。如果在第2)步搜索過程中無(wú)人機(jī)進(jìn)入了一個(gè)區(qū)域,算出的當(dāng)前最小合力網(wǎng)格點(diǎn)與上一步得出的網(wǎng)格點(diǎn)相同,而且這個(gè)點(diǎn)不是目標(biāo)點(diǎn),則搜索算法進(jìn)入了一個(gè)局部最優(yōu)循環(huán)中。

處理這個(gè)局部最優(yōu)循環(huán)的方法是在這個(gè)點(diǎn)的位置增加一個(gè)高能量的威脅場(chǎng),然后重新開始第2)步,這樣無(wú)人機(jī)就會(huì)避開這個(gè)區(qū)域。

表1 10×10網(wǎng)格點(diǎn)的單個(gè)威脅場(chǎng)斥力表

表2 10×10網(wǎng)格點(diǎn)的目標(biāo)點(diǎn)引力表

3 三維路徑規(guī)劃仿真

根據(jù)上節(jié)描述的路徑搜索算法,并參考文獻(xiàn)[15]中的仿真框架設(shè)計(jì)思路,采用Matlab軟件設(shè)計(jì)了GUI(graphical user interface,圖形用戶界面)程序,優(yōu)化了程序界面布局和回調(diào)函數(shù)中的搜索算法,改進(jìn)后的程序能夠直接導(dǎo)入1.3節(jié)生成的平滑后的安全飛行曲面。其中相關(guān)控件的名稱和功能如表3所示。

表3 仿真界面中各控件功能說(shuō)明

平滑后的數(shù)字地形及其等高線就顯示在坐標(biāo)軸系1和坐標(biāo)軸系2中。在表3中所有控件的回調(diào)函數(shù)都已編碼之后,GUI程序就設(shè)計(jì)完成了。

路徑規(guī)劃結(jié)果分4個(gè)步驟如下:

步驟1加載地圖?！拜d入地形”按鈕是指加載平滑處理后的三維地形的地圖,如圖6所示,即包括平滑處理后的安全飛行曲面及其相應(yīng)的等高線。在圖6中,3D地形的軸的單位由之前的緯度(°)、經(jīng)度(°)和米(m)轉(zhuǎn)換而來(lái)。

圖6 在GUI界面中載入三維平滑地形

步驟2添加新威脅。點(diǎn)擊界面中的“新增威脅障礙”按鈕,即可添加威脅源。在等高線的任意位置左鍵單擊1次,再單擊鼠標(biāo)右鍵即可完成一處威脅源的添加,柱狀威脅源將出現(xiàn)在三維地形和等高線中。在任意位置先后隨機(jī)設(shè)置5個(gè)威脅源后,得到如圖7中所示的5排柱形威脅源。

圖7 設(shè)置威脅障礙

步驟3確定目標(biāo)點(diǎn)。點(diǎn)擊界面中的“輸入目標(biāo)位置”按鈕。然后左鍵單擊等高線圖中威脅屏障外的任何位置,就會(huì)顯示一個(gè)紅色的小圓圈作為標(biāo)記。

步驟4開始搜索。點(diǎn)擊界面中的“開始生成航路”按鈕。程序開始運(yùn)行,依電腦計(jì)算性能不同,等待約1～2 s后,最佳飛行路徑就出現(xiàn)在3D地形圖和等高線圖中,如圖8(a)所示。

在圖8(a)中,在右側(cè)的等高線圖中,繪制了一條藍(lán)色星形線作為最佳路徑。同時(shí),在左側(cè)的3D地形圖中,繪制了一條紅線作為最佳路徑?？梢钥闯?所設(shè)計(jì)的路徑規(guī)劃算法可以有效地避免威脅,快速找到到達(dá)目標(biāo)的最短路徑,并緊貼3D安全飛行曲面之上飛行。

在圖8(b)中單獨(dú)繪制出了規(guī)劃好最優(yōu)軌跡(圖8中紅色曲線)和搜索過程中形成的最優(yōu)航點(diǎn)的連線(圖8中藍(lán)色曲線),顯然,最終的最優(yōu)軌跡是在最優(yōu)航點(diǎn)連線的基礎(chǔ)上平滑處理之后得到的。

圖8 三維最優(yōu)路徑規(guī)劃結(jié)果

4 結(jié)論

本文中設(shè)計(jì)了一種無(wú)人機(jī)近地三維最優(yōu)路徑規(guī)劃方法,通過仿真實(shí)驗(yàn),可以得到以下結(jié)論:

1) 通過真實(shí)地圖的數(shù)字高程模型作為無(wú)人機(jī)近地路徑規(guī)劃的地形數(shù)據(jù)來(lái)源是一種簡(jiǎn)潔、可行的地形建模方法。

2) 通過考慮安全高度、減小坡度和曲率等因素對(duì)原始數(shù)字地形進(jìn)行預(yù)處理,可以有效的生成無(wú)人機(jī)的安全飛行曲面,形成對(duì)復(fù)雜三維地形的路徑預(yù)規(guī)劃。

3) 所設(shè)計(jì)的GUI程序可以加載優(yōu)化后的安全飛行曲面,并通過最優(yōu)算法避開威脅區(qū)域到達(dá)目標(biāo)位置(其中威脅數(shù)量、威脅位置和目標(biāo)位置可自由指定),獲得最佳的飛行路徑。

本文中設(shè)計(jì)的規(guī)劃方法,在安全飛行曲面的基礎(chǔ)上,可以避開特定的威脅,獲得最佳的飛行路徑,且滿足無(wú)人機(jī)操縱性能的約束要求,為無(wú)人機(jī)近地三維路徑規(guī)劃的設(shè)計(jì)與仿真提供了一種新的思路。