基于優(yōu)化和MPC融合算法的三維無人機航跡規(guī)劃

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

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

0 引 言

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

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

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

1.1 無人機運動模型

不同于以往文獻對無人機航跡規(guī)劃問題建模多停留在二維平面,本文將無人機視為質點,基于慣性參考系建立三維空間無人機離散化運動模型。假設采樣時間為Δt,無人機運動模型為

(1)

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

1.2 環(huán)境模型

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

1.2.1 地圖模型

本文地圖建?；谄鸱黄降牡貏莸孛?具體數(shù)學模型可描述為

(2)

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

(3)

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

1.2.2 障礙物模型

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

在復雜三維環(huán)境下,靜態(tài)障礙物模型用圓柱體近似,具體示意圖如圖1所示。設靜態(tài)障礙物平面中心為Po,其坐標為[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表示為最小高度接近距離,即當無人機與靜態(tài)障礙物接近及高度距離分別小于LOD與ΔHOD時,無人機即發(fā)生碰撞。LOD與ΔHOD表示靜態(tài)障礙物最大威脅距離,即無人機與靜態(tài)障礙物接近及高度距離分別小于LOD與ΔHOD時,無人機存在碰撞的風險。

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

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

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

1.3 避碰約束模型

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

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

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

(4)

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

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

(5)

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

(6)

式中:l表示柵格邊長。

H(x)是從當前位置到目標位置預計代價總和的啟發(fā)函數(shù),具體計算公式為

(7)

式中:(xn,yn,zn)表示當前無人機位置坐標;(xd,yd,zd)表示既定目標點位置坐標。

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

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

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

步驟 3計算從當前節(jié)點到擴展節(jié)點的代價值,選取出每一個扇面上代價值最小的節(jié)點。

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

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

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

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

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

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

假定轉彎節(jié)點Ai的坐標為(xi,yi,zi),設定無人機的航向角為φ,俯仰角為θ,無人機轉彎半徑為R,則無人機轉彎球心位置的計算如下所示:

(8)

(2) 判斷航跡轉彎方向

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

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

(9)

1)γ(δ,ε)<0:表示當前無人機沿航跡Li飛行到航跡Li+1需要逆時針轉彎;

2)γ(δ,ε)>0:表示當前無人機沿航跡Li飛行到航跡Li+1需要順時針轉彎;

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

(3) 求取切點M1和M2

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

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

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

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

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

(10)

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

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

3.1 MPC算法

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

(11)

(12)

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

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

4 仿真結果與分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結 論