復(fù)雜動(dòng)態(tài)環(huán)境下無(wú)人飛行器動(dòng)態(tài)避障近似最優(yōu)軌跡規(guī)劃

郭 行，符文星，付 斌，陳 康，閆 杰

(西北工業(yè)大學(xué)航天學(xué)院，西安 710072)

0 引 言

如何在復(fù)雜動(dòng)態(tài)環(huán)境中利用探測(cè)信息規(guī)避各種障礙并高效完成飛行任務(wù)是無(wú)人飛行器路徑規(guī)劃質(zhì)量的重要衡量標(biāo)準(zhǔn)。

目前，無(wú)人飛行器避障軌跡規(guī)劃主要分3類(lèi)：第一類(lèi)為通過(guò)幾何方法進(jìn)行路線規(guī)劃，保證飛行路線與障礙沒(méi)有交集，但曲率限制不直接對(duì)應(yīng)控制約束，代表方法有PH[1]、dubins等[2]；第二類(lèi)為將避障問(wèn)題融入性能指標(biāo)，依據(jù)Hamilton原理求解，代表方法有比例導(dǎo)引法[3]、模型預(yù)測(cè)控制等[4-5]；第三類(lèi)為依據(jù)概率論進(jìn)行避障，通過(guò)計(jì)算概率分布密度函數(shù)確保避障概率超過(guò)門(mén)限值，但其依賴(lài)大量數(shù)據(jù)樣本或者近似概率分布函數(shù)[6]。

針對(duì)動(dòng)態(tài)避障近似最優(yōu)路徑規(guī)劃問(wèn)題，本文采取第二類(lèi)方法。Han等[3]基于傳統(tǒng)比例導(dǎo)引法推導(dǎo)避障飛行制導(dǎo)律，然而沒(méi)有解決過(guò)載發(fā)散缺點(diǎn)；Singh[4]依據(jù)模型預(yù)測(cè)控制設(shè)計(jì)自主規(guī)避軌跡，但忽略控制約束且算法效率低；Gagnon等[5]依據(jù)模型預(yù)測(cè)控制研究無(wú)人飛行器避障軌跡，但情形較為簡(jiǎn)單；Watanabe等[7]通過(guò)給定中間點(diǎn)研究能量最少避障制導(dǎo)律，但中間點(diǎn)難以確定；Keith等[8]設(shè)計(jì)混合整數(shù)線性規(guī)劃(MILP)迭代算法，完成固定區(qū)域避障；Smith等[9]利用直接正交配置法求解最優(yōu)避障走廊，但結(jié)果有概率性；Snyder等[10]設(shè)計(jì)軌跡生成、軌跡跟蹤和時(shí)間協(xié)調(diào)的協(xié)作任務(wù)系統(tǒng)完成避障，但不具最優(yōu)性；馮麗程等[11]基于有限時(shí)間滑模變結(jié)構(gòu)控制與人工勢(shì)函數(shù)實(shí)現(xiàn)航天器安全交會(huì)，但障礙運(yùn)動(dòng)形式較為簡(jiǎn)單；羊帆等[12]基于障礙物偽距離技術(shù)實(shí)現(xiàn)避障規(guī)劃-跟蹤一體化控制，但僅描述障礙形狀，障礙運(yùn)動(dòng)描述比較簡(jiǎn)單。

本文在以往無(wú)人飛行器規(guī)避簡(jiǎn)單障礙基礎(chǔ)上進(jìn)一步研究其規(guī)避動(dòng)態(tài)障礙近似最優(yōu)路徑規(guī)劃。首先依據(jù)OMPSP算法解決終端約束與控制約束，然后提出松弛變量與滑模變結(jié)構(gòu)的組合方法應(yīng)對(duì)安全避障的不等式約束，最后采取RHDDP算法進(jìn)行軌跡優(yōu)化，得到動(dòng)態(tài)避障近似最優(yōu)軌跡。

1 無(wú)人飛行器動(dòng)態(tài)避障問(wèn)題描述

本文研究二維平面動(dòng)態(tài)避障軌跡規(guī)劃，然而其可擴(kuò)展至三維情形。假設(shè)無(wú)人飛行器勻速率運(yùn)動(dòng)，其運(yùn)動(dòng)學(xué)模型和控制約束為：

(1)

|a|≤amax

(2)

式中,(x,y)為無(wú)人飛行器前向和橫向位置,V和γ分別為其速度和軌跡傾角,a和amax分別為其轉(zhuǎn)彎加速度和最大轉(zhuǎn)彎加速度。

對(duì)于動(dòng)態(tài)障礙，其典型運(yùn)動(dòng)模式如下：

1)勻速直線運(yùn)動(dòng)

該運(yùn)動(dòng)形式下，動(dòng)態(tài)障礙的運(yùn)動(dòng)學(xué)為：

(3)

式中,(xo,yo)為動(dòng)態(tài)障礙前向和橫向坐標(biāo),Vo為其速度,γo為其軌跡傾角。

2)斜向勻速簡(jiǎn)諧運(yùn)動(dòng)

該運(yùn)動(dòng)形式即為橫向簡(jiǎn)諧運(yùn)動(dòng)與斜向勻速直線運(yùn)動(dòng)的疊加，可以描述為：

(4)

式中,Vo為動(dòng)態(tài)障礙斜向勻速直線運(yùn)動(dòng)速度,γo為其斜向勻速直線運(yùn)動(dòng)軌跡傾角,Ao為其簡(jiǎn)諧運(yùn)動(dòng)幅值,fo為其簡(jiǎn)諧運(yùn)動(dòng)頻率,其它變量含義與式(3)中對(duì)應(yīng)相同。

3)勻加速轉(zhuǎn)彎運(yùn)動(dòng)

該運(yùn)動(dòng)形式下，動(dòng)態(tài)障礙的運(yùn)動(dòng)學(xué)為：

(5)

式中，ao為動(dòng)態(tài)障礙轉(zhuǎn)彎加速度，其它變量含義與式(3)中對(duì)應(yīng)相同。

為保證對(duì)動(dòng)態(tài)障礙的安全規(guī)避，無(wú)人飛行器需要滿(mǎn)足以下不等式約束：

(6)

式中，Ds為安全距離。

假設(shè)動(dòng)態(tài)環(huán)境中無(wú)人飛行器終端約束為(x,y,γ)f=(xf,yf,γf)，由于終端時(shí)刻難以確定，因而對(duì)于此“有限時(shí)間”，本文以坐標(biāo)x代替時(shí)間t作為自變量，同時(shí)引入約束：

(7)

無(wú)人飛行器動(dòng)態(tài)避障問(wèn)題的數(shù)學(xué)描述為：

(8)

(9)

式中,h為步長(zhǎng),下標(biāo)k表示k時(shí)刻狀態(tài)值。安全避障的不等式約束具體為：

(10)

無(wú)人飛行器狀態(tài)、控制和終端約束分別為：

(11)

|ak|≤amax

(12)

(13)

式中,N為總步數(shù)，滿(mǎn)足：

N=(xf-x1)/h+1

(14)

式中,x1為初始時(shí)刻無(wú)人飛行器x坐標(biāo)。軌跡規(guī)劃的性能指標(biāo)為能量最少，即：

(15)

由此，無(wú)人飛行器軌跡規(guī)劃狀態(tài)向量xk=(tk,yk,γk,xo1k,yo1k,xo2k,yo2k,γo2k,xo3k,yo3k)T，控制向量uk=ak，輸出向量yk=(yk,γk)T。相關(guān)基本運(yùn)動(dòng)參數(shù)如表1。

2 無(wú)人飛行器動(dòng)態(tài)避障近似最優(yōu)軌跡規(guī)劃

2.1 初始軌跡生成

MPSP[13-14]收斂速度快、精度高、算法簡(jiǎn)便，其優(yōu)化算法同時(shí)解決控制和終端約束[15-16]，因此本文用其進(jìn)行初始軌跡生成。

表1 無(wú)人飛行器和動(dòng)態(tài)障礙基本運(yùn)動(dòng)參數(shù)Table 1 Basic kinetics parameters for the UAV and dynamic obstacles

假設(shè)系統(tǒng)數(shù)學(xué)模型和期望終端狀態(tài)為

(16)

(17)

最優(yōu)問(wèn)題的性能指標(biāo)為能量最少，即：

(18)

式中,Rk為正定權(quán)重矩陣。則最優(yōu)控制為：

(19)

為保持算法簡(jiǎn)便并保證收斂速度，依據(jù)文獻(xiàn)[16]，在收斂后根據(jù)控制約束更新控制量，并同時(shí)自適應(yīng)調(diào)整Rk，促進(jìn)下一次收斂時(shí)滿(mǎn)足控制約束。初始軌跡記為(x0,u0)。

2.2 動(dòng)態(tài)障礙規(guī)避軌跡設(shè)計(jì)

1)規(guī)避部分設(shè)計(jì)

E(x;t)≤0

(20)

(21)

連續(xù)對(duì)式(21)進(jìn)行求導(dǎo)直至出現(xiàn)控制u：

(22)

式中,上標(biāo)表示求導(dǎo)階數(shù)。則控制u為：

(23)

由此系統(tǒng)狀態(tài)方程轉(zhuǎn)化為

(24)

(25)

(26)

式中,k1>0,ε1>0,δ1>0。由式(25)、式(26)有：

-cm-1χ(m-1)

(27)

i=1,2,3

(28)

式中,ps≥1為安全系數(shù)。對(duì)上式連續(xù)求導(dǎo)：

(29)

式中,

(30)

(31)

并且松弛變量子系統(tǒng)可以描述為：

(32)

式中,

(33)

由式(26)、(31)、(32)聯(lián)立可得：

(34)

將無(wú)人飛行器規(guī)避區(qū)域劃分為預(yù)警規(guī)避區(qū)(橙色區(qū)域)、執(zhí)行規(guī)避區(qū)(紫色區(qū)域)和絕對(duì)規(guī)避區(qū)(紅色區(qū)域)，如圖1所示。

圖1 無(wú)人飛行器規(guī)避區(qū)域劃分示意圖Fig.1 Partition of UAV’s avoidance region

(1)無(wú)人飛行器進(jìn)行預(yù)警規(guī)避的條件為：

(35)

(36)

式中,

(2)無(wú)人飛行器進(jìn)行執(zhí)行規(guī)避的條件為：

(37)

(4)無(wú)人飛行器進(jìn)行絕對(duì)規(guī)避的條件為：

(38)

(39)

當(dāng)無(wú)人飛行器需要同時(shí)規(guī)避多個(gè)障礙時(shí)，將其統(tǒng)一為單個(gè)障礙并擴(kuò)大安全距離。以同時(shí)規(guī)避三個(gè)障礙為例，則有：

(40)

2)恢復(fù)部分設(shè)計(jì)

依據(jù)上述方法可以實(shí)現(xiàn)避障，然而避障完成后仍需軌跡恢復(fù)。因此，本文設(shè)計(jì)滑模面s2將軌跡恢復(fù)至初始軌跡，即：

s2=d1(tk-tk 0)+d2(yk-yk 0)+(γk-γk 0)

(41)

由于避障消耗額外的時(shí)間，因此取d1=0,d2>0。對(duì)于s2仍采取指數(shù)趨近律，并以飽和函數(shù)sat(·)代替符號(hào)函數(shù)：

(42)

ε2sat(s2)]=ak 0-V2cosγk[d2(tanγk-

tanγk 0)+k2s2+ε2sat(s2)]

(43)

在軌跡恢復(fù)過(guò)程中，控制量uk仍需確保安全規(guī)避，因而其選擇標(biāo)準(zhǔn)設(shè)置為：

(44)

綜合軌跡設(shè)計(jì)部分(1)和(2)可得規(guī)避軌跡，記為(xc,uc)。

2.3 動(dòng)態(tài)障礙規(guī)避軌跡優(yōu)化

2.2節(jié)不僅設(shè)計(jì)規(guī)避軌跡，同時(shí)也得到避障的危險(xiǎn)區(qū)域，因此軌跡優(yōu)化性能指標(biāo)為：

(xN-xf)TQN(xN-xf)

(45)

式中，QN、Qk為非負(fù)定權(quán)重矩陣。在危險(xiǎn)區(qū)域內(nèi)Qk較大，保證動(dòng)態(tài)規(guī)避。由于RHDDP[19-20]搜索能力強(qiáng)、收斂速度快、收斂精度高，因此依據(jù)此算法進(jìn)行優(yōu)化。

對(duì)于系統(tǒng)描述式(16)，定義逆序控制量Uk?{uk,…,uN-1}，定義剩余性能指標(biāo)為：

(46)

則最優(yōu)剩余性能指標(biāo)為：

(47)

令G(xk,uk)=l(xk,uk)+Vk+1(F(xk,uk))，則其在(xk,uk)附近的變分為：

G(xk+δx,uk+δu)≈G(xk,uk)+(Gx)kδx+

(48)

式中：

(49)

根據(jù)變分法可得：

(50)

將式(50)代入式(48)可得：

(51)

Vk(xk)在xk附近的變分可表示為：

(52)

由式(51)、(52)可得以下近似關(guān)系：

(53)

式(49)和式(53)構(gòu)成RHDDP逆向優(yōu)化遞推方程。其前向優(yōu)化遞推方程為：

(54)

式中，0<ρ≤1為算法可調(diào)節(jié)系數(shù)。

3 動(dòng)態(tài)避障軌跡規(guī)劃仿真校驗(yàn)

仿真條件如表2所示。

3.1 初始軌跡生成仿真

通過(guò)OMPSP算法，初始軌跡進(jìn)行8次迭代后收斂，無(wú)人飛行器終端狀態(tài)為(tN,xN,yN,γN)=(30.50,300.00,50.05,9.33×10-7)。

由仿真結(jié)果可得：OMPSP算法能夠在滿(mǎn)足控制約束和終端約束情況下高效完成能量最少的近似最優(yōu)初始軌跡生成。

3.2 規(guī)避軌跡設(shè)計(jì)仿真

規(guī)避軌跡仿真所得到的最終狀態(tài)為(tN,xN,yN,γN)=(33.15,300.00,50.60,3.90×10-3)。

由仿真結(jié)果可得：動(dòng)態(tài)避障方法能夠同時(shí)規(guī)避多個(gè)動(dòng)態(tài)障礙；控制量存在較大波動(dòng)；規(guī)避軌跡顯著消耗能量并增加飛行時(shí)間。

表2 相關(guān)仿真參數(shù)值Table 2 Values of relevant simulation parameters

圖2 無(wú)人飛行器初始飛行軌跡Fig.2 UAV’s initial flight trajectory

圖3 初始軌跡控制量隨時(shí)間變化曲線Fig.3 Control input’s profile for initial trajectory

圖4 無(wú)人飛行器動(dòng)態(tài)規(guī)避飛行軌跡Fig.4 UAV’s avoidance flight trajectory

圖5 規(guī)避軌跡控制量隨時(shí)間變化曲線Fig.5 Control inputs’ profile for avoidance trajectory

3.3 軌跡優(yōu)化仿真

無(wú)人飛行器終端狀態(tài)為：(tN,xN,yN,γN)=(31.92,300.00,50.01,-1.80×10-3)。

圖8 無(wú)人飛行器優(yōu)化飛行軌跡Fig.8 UAV’s optimized flight trajectory

圖9 優(yōu)化軌跡控制量隨時(shí)間變化曲線Fig.9 Control profiles for optimized trajectory

圖10 無(wú)人飛行器與各障礙距離變化曲線Fig.10 Profiles for the distances between UAV and obstacles in optimized flight trajectory

由以上仿真結(jié)果可得如下結(jié)論。

優(yōu)化軌跡仍能規(guī)避動(dòng)態(tài)障礙且能量消耗明顯減少；優(yōu)化軌跡綜合考慮動(dòng)態(tài)避障約束、能量最少性能指標(biāo)和終端狀態(tài)約束，是促使式(45)性能指標(biāo)最小的近似最優(yōu)軌跡。

3.4 算法計(jì)算效率說(shuō)明

本文仿真平臺(tái)為CPU：Inter(R) Core(TM) i5-3470，主頻：3.2 GHz，內(nèi)存：4 GB。程序運(yùn)行耗時(shí)如表3。

表3 各個(gè)軌跡計(jì)算耗時(shí)情況表Table 3 Time consumption for each trajectory

根據(jù)表3，算法整體具備很高的計(jì)算效率，具備一定的在線實(shí)時(shí)規(guī)劃潛力。

4 結(jié) 論

本文針對(duì)無(wú)人飛行器復(fù)雜動(dòng)態(tài)環(huán)境下動(dòng)態(tài)避障問(wèn)題，創(chuàng)新地提出動(dòng)態(tài)避障近似最優(yōu)軌跡規(guī)劃方法。它將動(dòng)態(tài)障礙威脅、控制約束、終端約束等分解處理，依次設(shè)計(jì)初始軌跡、規(guī)避軌跡、優(yōu)化軌跡以使最終飛行軌跡具備近似最優(yōu)特性，并保證動(dòng)態(tài)避障和滿(mǎn)足各種約束。同時(shí)，本文算法簡(jiǎn)潔高效，充分利用各子算法優(yōu)勢(shì)。此外，本文動(dòng)態(tài)障礙模型極具代表性，包含同時(shí)規(guī)避多個(gè)動(dòng)態(tài)障礙的特殊情況，因此本文軌跡規(guī)劃方法具有很強(qiáng)的應(yīng)用性，為將來(lái)無(wú)人飛行器避障軌跡規(guī)劃提供了新的思路與借鑒。