宋 超, 李 波, 馬云紅, 黃晶益

(西北工業(yè)大學(xué)電子信息學(xué)院, 陜西 西安 710072)

0 引 言

無人機(jī)是一種能夠執(zhí)行各種復(fù)雜、困難任務(wù)的智能飛行器[1-3],具有操作靈活、功能多樣、無人員傷亡等特點(diǎn)[4-5]。隨著無人機(jī)應(yīng)用環(huán)境的日益復(fù)雜,如何根據(jù)現(xiàn)有任務(wù)需求、應(yīng)用條件與飛行環(huán)境,解決無人機(jī)快速高效航跡規(guī)劃問題[6-7],具有重要研究意義。

MPC[23]算法作為典型的滾動(dòng)時(shí)間窗方法之一,可以實(shí)現(xiàn)無人機(jī)實(shí)時(shí)預(yù)測(cè)、實(shí)時(shí)航跡規(guī)劃[24]。MPC算法基于當(dāng)前環(huán)境信息進(jìn)行實(shí)時(shí)更新,進(jìn)而預(yù)測(cè)下一步參考航跡,同時(shí)基于無人機(jī)的相關(guān)約束及其自身控制誤差等因素,通過反饋-校正機(jī)制進(jìn)一步修正參考航跡[25-26]。Sahu[27]等基于MPC算法開展二維平面多無人機(jī)跟蹤多移動(dòng)目標(biāo)研究,建立了基于數(shù)據(jù)驅(qū)動(dòng)高斯過程(Gaussian processes, GP)的模型。Ille等[28]基于MPC算法開展二維環(huán)境多無人機(jī)編隊(duì)避碰研究,利用懲罰項(xiàng)方法優(yōu)化了MPC成本函數(shù),基于避碰約束控制無人機(jī)跟蹤移動(dòng)目標(biāo)航跡規(guī)劃等。

1 無人機(jī)與環(huán)境模型

1.1 無人機(jī)運(yùn)動(dòng)模型

不同于以往文獻(xiàn)對(duì)無人機(jī)航跡規(guī)劃問題建模多停留在二維平面,本文將無人機(jī)視為質(zhì)點(diǎn),基于慣性參考系建立三維空間無人機(jī)離散化運(yùn)動(dòng)模型。假設(shè)采樣時(shí)間為Δt,無人機(jī)運(yùn)動(dòng)模型為

(1)

式中:(x(k),y(k),z(k))為無人機(jī)位置坐標(biāo);v(k)為無人機(jī)速度;φ(k)為無人機(jī)航向角;θ(k)為無人機(jī)俯仰角。

1.2 環(huán)境模型

無人機(jī)在執(zhí)行任務(wù)時(shí),為規(guī)避雷達(dá)探測(cè),其需要盡可能超低空飛行,不僅要避開山峰以及未知障礙物的威脅,還需要規(guī)劃出一條從起點(diǎn)到終點(diǎn)的最短平滑可飛安全路徑。基于此,建立復(fù)雜環(huán)境模型對(duì)于無人機(jī)航跡規(guī)劃而言至關(guān)重要。

1.2.1 地圖模型

本文地圖建模基于起伏不平的地勢(shì)地貌,具體數(shù)學(xué)模型可描述為

(2)

式中:I表示地形山峰數(shù)量;κB表示每個(gè)山峰的區(qū)域,是關(guān)于山峰高度z和山峰水平地面投影中心位置坐標(biāo)(x,y)的函數(shù),具體計(jì)算公式為

(3)

式中:Q,W,E,Rr,P,F,G為地形系數(shù)。

1.2.2 障礙物模型

(1) 靜態(tài)障礙物模型

在復(fù)雜三維環(huán)境下,靜態(tài)障礙物模型用圓柱體近似,具體示意圖如圖1所示。設(shè)靜態(tài)障礙物平面中心為Po,其坐標(biāo)為[Pox,Poy],半徑與高度分別用Por與Poz表示。

圖1 靜態(tài)障礙物示意圖Fig.1 Schematic diagram of static obstacle

本文將靜態(tài)障礙物周圍環(huán)境劃分為碰撞區(qū)(用LOD與ΔHOD表示)與威脅區(qū)(用LOD與ΔHOD表示)。LOD表示為最小接近安全距離,ΔHOD表示為最小高度接近距離,即當(dāng)無人機(jī)與靜態(tài)障礙物接近及高度距離分別小于LOD與ΔHOD時(shí),無人機(jī)即發(fā)生碰撞。LOD與ΔHOD表示靜態(tài)障礙物最大威脅距離,即無人機(jī)與靜態(tài)障礙物接近及高度距離分別小于LOD與ΔHOD時(shí),無人機(jī)存在碰撞的風(fēng)險(xiǎn)。

(2) 突發(fā)障礙物模型

在復(fù)雜環(huán)境下,突發(fā)障礙物模型用圓球體近似,具體示意圖如圖2所示。設(shè)突發(fā)障礙物中心為Pt,其坐標(biāo)為[Ptx,Pty,Ptz],半徑為Rt,避碰區(qū)半徑為Rp,威脅區(qū)半徑為Rw。

圖2 突發(fā)障礙物示意圖Fig.2 Schematic diagram of sudden obstacle

1.3 避碰約束模型

無人機(jī)避碰約束是無人機(jī)有效完成航跡規(guī)劃的前提。為減少計(jì)算量,本文簡(jiǎn)化了無人機(jī)避碰約束模型,以無人機(jī)質(zhì)心為中心設(shè)置球形安全區(qū),具體示意圖如圖3所示。

圖3 無人機(jī)防碰撞球形安全區(qū)示意圖Fig.3 Schematic diagram of unmanned aerial vehicle anti-collision spherical safety zone

設(shè)空間障礙oi為以Roi為半徑的球體,中心坐標(biāo)為(oix,oiy,oiz),障礙物集合為o={1,2,…,Nall},則無人機(jī)避碰約束模型可表示為

(4)

式中:Ld為無人機(jī)與障礙物的距離;Ra為無人機(jī)安全區(qū)半徑。

2.1 啟發(fā)函數(shù)優(yōu)化

(5)

式中:a和b為相關(guān)代價(jià)的權(quán)值,a的取值范圍為[0.5,1],當(dāng)a的值取1時(shí),即為全局規(guī)劃,此時(shí)不是啟發(fā)式搜索,相當(dāng)于Dijkstra算法;c為權(quán)重比,取值大于1。

(6)

式中:l表示柵格邊長(zhǎng)。

H(x)是從當(dāng)前位置到目標(biāo)位置預(yù)計(jì)代價(jià)總和的啟發(fā)函數(shù),具體計(jì)算公式為

(7)

式中:(xn,yn,zn)表示當(dāng)前無人機(jī)位置坐標(biāo);(xd,yd,zd)表示既定目標(biāo)點(diǎn)位置坐標(biāo)。

2.2 稀疏優(yōu)化算法

步驟 1從當(dāng)前節(jié)點(diǎn)確定可擴(kuò)展的區(qū)域,搜索半徑為當(dāng)前無人機(jī)搜索步長(zhǎng),設(shè)定沿航路的垂直方向的張角為最大俯仰角的1.5倍,水平方向的張角同樣設(shè)定為最大航向角的1.5倍。

步驟 2對(duì)可擴(kuò)展的區(qū)域的爬升/俯沖方向的扇區(qū)進(jìn)行M等分,對(duì)轉(zhuǎn)彎方向的扇面進(jìn)行N等分,得到M×N個(gè)點(diǎn),M與N值的取值要恰當(dāng),M與N越大,尋找到理想航路的概率越大,但同時(shí)也增加了存儲(chǔ)內(nèi)存需求和搜索時(shí)間。一般來說,M與N在[3,5]選取能夠滿足需求,這些節(jié)點(diǎn)作為待擴(kuò)展節(jié)點(diǎn)。

步驟 3計(jì)算從當(dāng)前節(jié)點(diǎn)到擴(kuò)展節(jié)點(diǎn)的代價(jià)值,選取出每一個(gè)扇面上代價(jià)值最小的節(jié)點(diǎn)。

步驟 4對(duì)于步驟3選取的節(jié)點(diǎn),判斷其是否滿足最低飛行高度以及最大飛行距離的約束,若不滿足則直接舍棄;若滿足,比較可行扇面最小代價(jià)函數(shù),取代價(jià)值最小的節(jié)點(diǎn)。

本文設(shè)定無人機(jī)的最大航向角為45°,最大俯仰角為45°,規(guī)劃空間的柵格邊長(zhǎng)設(shè)為最小航跡段長(zhǎng)度。在整個(gè)三維空間中,對(duì)當(dāng)前節(jié)點(diǎn)進(jìn)行擴(kuò)展,將三維擴(kuò)展節(jié)點(diǎn)縮小至9個(gè)節(jié)點(diǎn)(V4～V12),具體示意圖如圖4所示。

圖4 三維空間可擴(kuò)展節(jié)點(diǎn)示意圖Fig.4 Schematic diagram of scalable nodes in 3D space

2.3 連續(xù)優(yōu)化算法

圖5 無人機(jī)航跡連續(xù)曲線優(yōu)化示意圖Fig.5 Schematic diagram of unmanned aerial vehicle trajectory continuous curve optimization

(1) 確定航跡轉(zhuǎn)彎球心C(k)

假定轉(zhuǎn)彎節(jié)點(diǎn)Ai的坐標(biāo)為(xi,yi,zi),設(shè)定無人機(jī)的航向角為φ,俯仰角為θ,無人機(jī)轉(zhuǎn)彎半徑為R,則無人機(jī)轉(zhuǎn)彎球心位置的計(jì)算如下所示:

(8)

(2) 判斷航跡轉(zhuǎn)彎方向

本文采用混合向量積算法判斷航跡轉(zhuǎn)彎點(diǎn)Ai前后相鄰兩段航跡的轉(zhuǎn)彎方向。假定當(dāng)前航跡點(diǎn)Ai前一段航跡為L(zhǎng)i,后一段航跡為L(zhǎng)i+1,δ=[δx,δy,δz]T與ε=[εx,εy,εz]T分別為前后兩個(gè)航跡段Li,Li+1方向上的單位向量,則可以通過下式來確定無人機(jī)的轉(zhuǎn)彎方向:

γ(δ,ε)=sign(δ·ε)

(9)

1)γ(δ,ε)<0:表示當(dāng)前無人機(jī)沿航跡Li飛行到航跡Li+1需要逆時(shí)針轉(zhuǎn)彎;

2)γ(δ,ε)>0:表示當(dāng)前無人機(jī)沿航跡Li飛行到航跡Li+1需要順時(shí)針轉(zhuǎn)彎;

3)γ(δ,ε)=0:表示當(dāng)前航跡Li與航跡Li+1在同一條直線上,但根據(jù)無人機(jī)飛行約束等條件,不會(huì)出現(xiàn)無人機(jī)180°轉(zhuǎn)彎的情況,故表示無人機(jī)直行,無需轉(zhuǎn)彎,即當(dāng)前航跡節(jié)點(diǎn)為非航跡轉(zhuǎn)彎點(diǎn)。

(3) 求取切點(diǎn)M1和M2

針對(duì)無人機(jī)航跡轉(zhuǎn)彎開展平滑處理,要求轉(zhuǎn)彎弧要與前一航跡Li和后一航跡Li+1均相切,設(shè)定切點(diǎn)為M1和M2。當(dāng)無人機(jī)航跡經(jīng)過平滑處理后,無人機(jī)不再途經(jīng)節(jié)點(diǎn)Ai,航跡由原來的兩段(Li與Li+1)變?yōu)橄铝腥?

1)Ai-1M1航跡段繼續(xù)維持直線飛行狀態(tài);

2) 由M1到M2航跡段執(zhí)行半徑為R的圓周飛行;

3)M2Ai+1航跡段執(zhí)行直線飛行。

依據(jù)式(8)確定轉(zhuǎn)彎球心坐標(biāo)C(k),轉(zhuǎn)彎半徑為R,δ,ε分別為航跡段Li與Li+1上的單位向量,設(shè)定向量q′與向量δ垂直,向量q″與向量ε垂直,且均指向球心C(k),|q′|=1,|q″|=1。假定向量δ到ε是順時(shí)針旋轉(zhuǎn),則切點(diǎn)M1,M2可表示為

(10)

圖6 優(yōu)化算法流程圖Fig.6 Flowchart of optimized algorithm

3 融合優(yōu)化算法

3.1 MPC算法

基于MPC算法可以求解無人機(jī)局部實(shí)時(shí)航跡規(guī)劃。在每個(gè)采樣周期,根據(jù)無人機(jī)狀態(tài)信息及預(yù)測(cè)航跡求解最優(yōu)控制序列,將最優(yōu)控制序列作用于當(dāng)前時(shí)刻,進(jìn)而得到實(shí)時(shí)局部航跡規(guī)劃,具體航跡規(guī)劃模型為

(11)

(12)

3.2 優(yōu)化算法與MPC算法融合

圖7 基于融合算法的無人機(jī)航跡規(guī)劃流程圖Fig.7 Flowchart of unmanned aerial vehicle trajectory planning based on fusion algorithm

4 仿真結(jié)果與分析

表1 系統(tǒng)參數(shù)初始化

本文仿真環(huán)境基于軟件自主搭建,地圖建模基于山區(qū)起伏不平地勢(shì)地貌,地形障礙物構(gòu)成主要源于原始地形與山峰威脅,人為擬定地形數(shù)學(xué)模型。為進(jìn)一步貼近真實(shí)飛行場(chǎng)景,在無人機(jī)周圍設(shè)立安全緩沖區(qū),并將障礙物分為靜態(tài)障礙物與突發(fā)障礙物,靜態(tài)障礙物模型用圓柱體近似,突發(fā)障礙物模型用球體近似。此外,為進(jìn)一步增強(qiáng)仿真準(zhǔn)確度,柵格化地圖環(huán)境,即綜合考慮地形障礙、威脅區(qū)等,對(duì)地圖進(jìn)行柵格化處理。將每一個(gè)網(wǎng)格稱為一個(gè)單元,將地圖三維數(shù)學(xué)模型轉(zhuǎn)換為矢量結(jié)構(gòu)數(shù)據(jù),再轉(zhuǎn)換為柵格結(jié)構(gòu),賦予每個(gè)柵格單元獨(dú)特屬性以表示實(shí)體。本文確定柵格化地圖單位長(zhǎng)度為5 m,精度為0.1 m,其三維高度信息由人為擬定,具體仿真結(jié)果如圖8～圖10所示。

圖8 基于傳統(tǒng)算法的無人機(jī)航跡規(guī)劃Fig.8 Unmanned aerial vehicle trajectory planning based on traditional algorithm

圖9 基于稀疏算法無人機(jī)航跡規(guī)劃Fig.9 Unmanned aerial vehicle trajectory planning based on sparse algorithm

圖10 基于優(yōu)化算法無人機(jī)航跡規(guī)劃Fig.10 Unmanned aerial vehicle trajectory planning based on optimized algorithm

表2 基于算法無人機(jī)航跡規(guī)劃數(shù)據(jù)對(duì)比

表3 突發(fā)障礙物1信息

圖11 優(yōu)化算法與融合算法無人機(jī)航跡規(guī)劃對(duì)比實(shí)驗(yàn)Fig.11 Contrast experiment of optimized algorithm and fusion algorithm for unmanned aerial vehicle trajectory planning

為進(jìn)一步檢驗(yàn)融合算法的避障能力,在滿足無人機(jī)性能約束等條件下,將突發(fā)障礙物1移動(dòng)到突發(fā)障礙物2位置,且調(diào)整部分地圖信息及靜態(tài)障礙物位置信息,突發(fā)障礙物2的具體信息如表4所示,具體仿真結(jié)果如圖12所示。

表4 突發(fā)障礙物2信息

圖12 復(fù)雜環(huán)境下基于優(yōu)化算法與融合算法的 無人機(jī)避障航跡規(guī)劃對(duì)比實(shí)驗(yàn)Fig.12 Comparative experiments on unmanned aerial vehicle obstacle avoidance trajectory planning based on optimized algorithm and fusion algorithm in complex environment

圖13 基于融合算法的無人機(jī)與突發(fā)障礙物實(shí)時(shí)距離Fig.13 Real-time distance between unmanned aerial vehicle and sudden obstacles based on fusion algorithm

為進(jìn)一步驗(yàn)證融合算法應(yīng)用于多障礙物場(chǎng)景下的無人機(jī)航跡規(guī)劃的有效性,測(cè)試融合算法的實(shí)時(shí)避障能力,本文增加靜態(tài)障礙物數(shù)量至6個(gè),同時(shí)調(diào)整靜態(tài)障礙物的具體位置。此外,更改無人機(jī)始發(fā)點(diǎn)坐標(biāo)位置(200 m,0 m,0 m)與目標(biāo)點(diǎn)坐標(biāo)位置(208 m,450 m,210 m),具體仿真結(jié)果如圖14所示。

圖14 基于變目標(biāo)點(diǎn)復(fù)雜環(huán)境下的無人機(jī)避障航跡規(guī)劃Fig.14 Unmanned aerial vehicle obstacle avoidance trajectory planning in complex environment based on variable target points

為進(jìn)一步驗(yàn)證融合算法在不同場(chǎng)景下無人機(jī)航跡規(guī)劃的有效性,檢驗(yàn)融合算法在應(yīng)用于無人機(jī)航跡規(guī)劃時(shí)的實(shí)時(shí)避碰避撞能力,本文在更改6個(gè)靜態(tài)障礙物位置基礎(chǔ)上,更改復(fù)雜任務(wù)環(huán)境。同時(shí),更改無人機(jī)始發(fā)點(diǎn)(500 m,0 m,0 m)與目標(biāo)點(diǎn)(60 m,400 m,65 m),具體仿真結(jié)果如圖15所示。

圖15 基于復(fù)雜變化環(huán)境下的無人機(jī)避障航跡規(guī)劃Fig.15 Unmanned aerial vehicle obstacle avoidance trajectory planning based on complex changing environment

5 結(jié) 論