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

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

0 引 言

如何在復雜動態(tài)環(huán)境中利用探測信息規(guī)避各種障礙并高效完成飛行任務是無人飛行器路徑規(guī)劃質量的重要衡量標準。

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

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

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

1 無人飛行器動態(tài)避障問題描述

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

(1)

|a|≤amax

(2)

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

對于動態(tài)障礙，其典型運動模式如下：

1)勻速直線運動

該運動形式下，動態(tài)障礙的運動學為：

(3)

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

2)斜向勻速簡諧運動

該運動形式即為橫向簡諧運動與斜向勻速直線運動的疊加，可以描述為：

(4)

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

3)勻加速轉彎運動

該運動形式下，動態(tài)障礙的運動學為：

(5)

式中，ao為動態(tài)障礙轉彎加速度，其它變量含義與式(3)中對應相同。

為保證對動態(tài)障礙的安全規(guī)避，無人飛行器需要滿足以下不等式約束：

(6)

式中，Ds為安全距離。

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

(7)

無人飛行器動態(tài)避障問題的數(shù)學描述為：

(8)

(9)

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

(10)

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

(11)

|ak|≤amax

(12)

(13)

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

N=(xf-x1)/h+1

(14)

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

(15)

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

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

2.1 初始軌跡生成

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

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

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

(16)

(17)

最優(yōu)問題的性能指標為能量最少，即：

(18)

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

(19)

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

2.2 動態(tài)障礙規(guī)避軌跡設計

1)規(guī)避部分設計

E(x;t)≤0

(20)

(21)

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

(22)

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

(23)

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

(24)

(25)

(26)

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

-cm-1χ(m-1)

(27)

i=1,2,3

(28)

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

(29)

式中,

(30)

(31)

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

(32)

式中,

(33)

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

(34)

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

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

(1)無人飛行器進行預警規(guī)避的條件為：

(35)

(36)

式中,

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

(37)

(4)無人飛行器進行絕對規(guī)避的條件為：

(38)

(39)

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

(40)

2)恢復部分設計

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

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

(41)

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

(42)

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

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

(43)

在軌跡恢復過程中，控制量uk仍需確保安全規(guī)避，因而其選擇標準設置為：

(44)

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

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

2.2節(jié)不僅設計規(guī)避軌跡，同時也得到避障的危險區(qū)域，因此軌跡優(yōu)化性能指標為：

(xN-xf)TQN(xN-xf)

(45)

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

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

(46)

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

(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)可得以下近似關系：

(53)

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

(54)

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

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

仿真條件如表2所示。

3.1 初始軌跡生成仿真

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

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

3.2 規(guī)避軌跡設計仿真

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

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

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

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

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

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

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

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

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

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

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

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

由以上仿真結果可得如下結論。

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

3.4 算法計算效率說明

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

表3 各個軌跡計算耗時情況表Table 3 Time consumption for each trajectory

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

4 結 論

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