基于C/FD-GMRES方法的UAV在線航跡規劃算法

方 斌 馮曉鋒 李 杰

1(湖南警察學院交通管理系 湖南 長沙 410138)2(國防科技大學智能科學學院 湖南 長沙 410073)

0 引 言

在復雜環境下執行任務的過程中，無人機(UAV)必須具備在線航跡規劃的能力以規避任務區域中的橋梁、樓宇以及突發威脅等空間障礙，確保飛行安全。目前，國內外的研究學者對UAV在線航跡規劃問題已經開展了大量研究。文獻[1-2]基于人工勢場法構造由目標點引力和障礙物的斥力共同作用的人工勢場，沿著勢函數梯度下降的方向來尋找無碰撞軌跡。其中，目標點對UAV產生“引力”，障礙物對UAV產生“斥力”。文獻[3-4]基于采樣方法，通過對構型空間或狀態空間中的采樣點進行碰撞檢測，能夠有效地解決高維空間和復雜約束的軌跡規劃問題，特別適合在線實時應用。此外，遺傳算法(Genetic Algorithm, GA)、人工神經網絡、粒子群算法(PSO)以及蟻群算法等智能算法都在UAV航線規劃問題中也得到了較為廣泛的應用[5-9]。采用智能算法求解航線規劃問題時，首先根據問題特性進行建模，將問題模型中的決策變量、約束條件等映射為算法模型中的要素(如粒子、人工螞蟻等)，并對各要素進行編碼。例如：在PSO中，將一條飛行軌跡建模為一個粒子，利用粒子的多維坐標、速度等信息來表征航線中的關鍵要素。在蟻群算法中，將一個UAV平臺映射為一個螞蟻，UAV在任務空間的運動過程被建模為在螞蟻信息素和啟發信息引導下的運動過程。文獻[10]采用滾動時域控制(Receding Horizon Control, RHC)方法，基于模型預測控制(Model Predictive Control，MPC)的思想對UAV軌跡進行在線的有限時段規劃，其關鍵步驟是利用滾動的有限時段規劃取代一成不變的全局規劃。

上述規劃算法有效地提升了UAV的空中避障能力，但大多只是規劃引導UAV安全到達目標位置，卻忽略了其承擔的任務使命，即必須確保UAV在達到目標位置后能夠滿足執行任務的終端約束狀態，包括UAV的位形和速度等狀態約束。例如：某偵察UAV在執行避障算法并安全到達目標位置后，還必須滿足偵察高度和偵察角度的約束，以保證滿足成像條件，順利完成后續的情報偵察任務。因此，本文重點研究滿足終端狀態約束的UAV在線航跡規劃算法設計問題。

1 問題建模

UAV航跡規劃一般為非線性、帶有狀態約束和控制約束的最優控制問題(Optimal Control Problem, OCP)[11]，這類問題難點之一在于如何對UAV的運動學/動力學約束以及各種路徑約束、最優指標等進行形式化建模。采用最優控制理論可以很好地處理該問題，具體求解過程是尋找一組最優控制變量序列，滿足波爾扎(Bolza)型性能指標的最小化，即：

(1)

等式右側第一項表示終端代價：

(2)

式中：x=[x,y,z,φ,θ,ψ,V,α,β,p,q,r]T為UAV狀態矢量，x、y、z分別為UAV位置在地理坐標系下的北向分量、東向分量和垂直分量，φ、θ、ψ分別表示滾轉角、俯仰角和偏航角，V為UAV空速，α、β分別為攻角和側滑角，p、q、r為角速度；xf表示UAV固定終止狀態矢量；tf表示任意終止時間；Sf為12×12維終端代價加權對角矩陣。等式右側第二項中的被積函數為拉格朗日函數表示為：

(3)

對于UAV在線規劃，需要滿足以下約束條件。

(2) 邊界條件x(t0)=x0，x(tf)=xf。其中，x0和xf分別為UAV初始狀態和終止狀態。

(3) 路徑約束。

一般情況下，控制量需要滿足輸入飽和約束：

(4)

式中：u1min、u1max為給定常量，分別表示發動機推力幅值的下界和上界；u2max、u3max、u4max分別表示升降舵、方向舵以及副翼偏角的幅值約束。

根據最優控制的相關理論可知[12-13]，UAV運動規劃問題等價于優化求解最優控制問題的一階最優必要條件，定義哈密頓函數為：

H(x,λ,u,μ)=L(x,u)+λTf(x,u)+μTC(x,u)

(5)

(6)

在代價函數中增加小的懲罰因子，得到修改后的拉格朗日函數為：

(7)

H(x,λ,u,μ)=L(x,u)+λTf(x,u)+μTC(x,u)=

(8)

根據變分法推導得到UAV在線航跡規劃問題的一階最優必要條件，即最優控制量、拉格朗日乘子以及共態量滿足：

(9)

(10)

(11)

將式(8)分別代入式(9)和式(10)，整理可得：

(12)

(13)

將式(2)代入式(11)，整理可得：

(14)

(15)

因此，式(13)、式(14)和式(15)以及微分狀態方程一起構成受約束最優控制問題的一階最優必要條件。其中式(15)中涉及大量非線性方程和微分運算等復雜計算，計算量大。因此，為滿足實時性要求，本文提出一種基于同倫連續-前向差分近似廣義最小殘差法(Continuation/Forward Difference-Approximation Generalized Minimum Residual Method,C/FD-GMRES)實時求解控制量U。

2 方法設計

針對里卡蒂微分方程的求解過程十分復雜，為提高計算效率，采用非線性滾動時域控制實時方法C/FD-GMRES進行簡化替代求解[14]。具體過程如下：在每個采樣時刻計算一次殘差向量線性方程，然后運用Krylov 子空間方法廣義最小殘差法(GMRES)來求解大規模線性方程。GMRES 具備兩個顯著優點，一是每次迭代計算過程中殘差都會單調減小；二是計算收斂的迭代次數等同于方程維度數，且能收斂至指定的誤差容限。

(16)

(17)

將式(16)代入式(17)，即得：

(18)

(19)

(20)

(21)

按照上述思路，基于 C/FD-GMRES 的實時在線規劃求解算法主要包含兩個步驟：① 初始化起始控制量u0；② 利用u0作為初始值，滾動優化求解控制量uk,k=1,2，…，N。

2.1 初始化求解控制量啟動值

‖F(u(0),x(0),0)‖≈

(22)

式中：δ為任意小的正實數。輸入為初始狀態x，起始時刻t0，步長Δt，殘差容限rtol，迭代優化次數，GMRES迭代次數kmax。具體求解過程如下:首先，采用改進格萊姆-施密特正交化方法得到正交向量組；然后，采用吉文斯旋轉優化控制量變化率，并在迭代次數內收斂到殘差容限。

2.2 滾動優化求解控制量

圖1 滾動優化求解控制量uk

3 仿真實驗

在MATLAB 2012a下開展仿真實驗，實驗的硬件環境為Inter Core Duo CPU i3-7100，內存4 GB。表1給出了UAV狀態與控制飽和約束條件。

表1 UAV狀態與控制飽和約束

假設任務空間中的威脅中心位置為(3 300 m，0 m，18 000 m)，為滿足目標偵察條件，UAV的終端狀態期望值必須滿足一定的條件。其中，偵察UAV的飛行高度不低于17 500 m，偏航角ψ為70 °，詳細的仿真初始狀態量以及終端狀態量的參數設置如表2所示。

表2 UAV初始狀態量和終止狀態量

仿真參數選擇為：采樣時間Δt為0.01 s，殘差容限rtol為1.0e-6，優化迭代次數iternum為100，穩定因子ζ為100(ζΔt=1)，則穩定因子矩陣為As=-ζI；在初始化控制量啟動值u(0)和滾動優化求解控制量u(t)算法中，GMRES迭代次數kmax分別為2和15；加權矩陣Q、Sf和R均選擇為單位矩陣；時域參數T(t)=Tf(1-e-αt)，Tf=1，α=0.5，N=10，Δτ=T(t)/N；懲罰因子ri=0.1(i=5,6,7,8)。

圖2中的橢圓形球體為空間障礙，曲線為UAV的三維飛行軌跡。可以看出該算法能夠為UAV有效規劃出從起始點到目標點，并且合理規避障礙的運動軌跡。圖3和圖4給出了UAV為完成任務必須滿足的兩個終端約束條件，高度以及航向角的變化曲線，顯然，高度和偏航角都逐漸收斂至表2給出的目標狀態期望值，并且高度的誤差不超過50 m，航向角的誤差不超過5 °。

圖2 UAV的三維飛行軌跡

圖3 UAV飛行高度/速度變化曲線

圖4 UAV偏航角變化曲線

表3給出了存在障礙的情況下，通過多次實驗統計得到的C/FD-GMRES算法總規劃時長和單步滾動優化時長的統計平均值。可知，算法總規劃時長約為85 s，單步滾動時域優化時長統計平均值約為279 ms。能夠滿足無人機在線運動規劃對實時性的要求。

表3 規劃總時長和單步滾動優化時長均值

為測試基于C/FD-GMRES規劃算法的性能，采用傳統的牛頓迭代法對式(15)進行求解[15]，并進行對比分析，結果如表4所示。

表4 傳統算法規劃總時長和單步滾動優化時長均值

由表4可見，傳統算法在求解過程中收斂速度慢甚至無法收斂，基于C/FD-GMRES規劃算法的性能明顯高于傳統牛頓迭代法。

4 結 語

本文設計了一種基于C/FD-GMRES的UAV在線運動規劃問題滾動時域實時優化求解方法。該方法實時性好且不敏感于初始估計值的選取，能夠有效規避空間障礙，同時滿足UAV的終端狀態約束條件，例如：UAV的高度、航向角等，以滿足執行任務的姿態需求，能夠處理輸入飽和約束。仿真實驗結果表明，該算法能夠有效規劃出穩定收斂至固定終端狀態的運動軌跡，在存在障礙的情形下，算法能夠保證UAV有效地規避障礙，且滾動優化時間約為279 ms，能夠滿足在線規劃實時性要求。