復雜動態環境下無人飛行器動態避障近似最優軌跡規劃

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

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

0 引 言

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

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

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

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

1 無人飛行器動態避障問題描述

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

(1)

|a|≤amax

(2)

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

對于動態障礙，其典型運動模式如下：

1)勻速直線運動

該運動形式下，動態障礙的運動學為：

(3)

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

2)斜向勻速簡諧運動

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

(4)

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

3)勻加速轉彎運動

該運動形式下，動態障礙的運動學為：

(5)

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

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

(6)

式中，Ds為安全距離。

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

(7)

無人飛行器動態避障問題的數學描述為：

(8)

(9)

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

(10)

無人飛行器狀態、控制和終端約束分別為：

(11)

|ak|≤amax

(12)

(13)

式中,N為總步數，滿足：

N=(xf-x1)/h+1

(14)

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

(15)

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

2 無人飛行器動態避障近似最優軌跡規劃

2.1 初始軌跡生成

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

表1 無人飛行器和動態障礙基本運動參數Table 1 Basic kinetics parameters for the UAV and dynamic obstacles

假設系統數學模型和期望終端狀態為

(16)

(17)

最優問題的性能指標為能量最少，即：

(18)

式中,Rk為正定權重矩陣。則最優控制為：

(19)

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

2.2 動態障礙規避軌跡設計

1)規避部分設計

E(x;t)≤0

(20)

(21)

連續對式(21)進行求導直至出現控制u：

(22)

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

(23)

由此系統狀態方程轉化為

(24)

(25)

(26)

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

-cm-1χ(m-1)

(27)

i=1,2,3

(28)

式中,ps≥1為安全系數。對上式連續求導：

(29)

式中,

(30)

(31)

并且松弛變量子系統可以描述為：

(32)

式中,

(33)

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

(34)

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

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

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

(35)

(36)

式中,

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

(37)

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

(38)

(39)

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

(40)

2)恢復部分設計

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

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

(41)

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

(42)

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

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

(43)

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

(44)

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

2.3 動態障礙規避軌跡優化

2.2節不僅設計規避軌跡，同時也得到避障的危險區域，因此軌跡優化性能指標為：

(xN-xf)TQN(xN-xf)

(45)

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

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

(46)

則最優剩余性能指標為：

(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)

根據變分法可得：

(50)

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

(51)

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

(52)

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

(53)

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

(54)

式中，0<ρ≤1為算法可調節系數。

3 動態避障軌跡規劃仿真校驗

仿真條件如表2所示。

3.1 初始軌跡生成仿真

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

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

3.2 規避軌跡設計仿真

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

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

表2 相關仿真參數值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 無人飛行器動態規避飛行軌跡Fig.4 UAV’s avoidance flight trajectory

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

3.3 軌跡優化仿真

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

圖8 無人飛行器優化飛行軌跡Fig.8 UAV’s optimized flight trajectory

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

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

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

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

3.4 算法計算效率說明

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

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

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

4 結 論

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