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

方 斌 馮曉鋒 李 杰

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

0 引 言

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

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

1 問題建模

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

(1)

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

(2)

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

(3)

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

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

(3) 路徑約束。

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

(4)

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

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

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

(5)

(6)

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

(7)

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

(8)

根據(jù)變分法推導得到UAV在線航跡規(guī)劃問題的一階最優(yōu)必要條件，即最優(yōu)控制量、拉格朗日乘子以及共態(tài)量滿足：

(9)

(10)

(11)

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

(12)

(13)

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

(14)

(15)

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

2 方法設計

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

(16)

(17)

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

(18)

(19)

(20)

(21)

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

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

‖F(xiàn)(u(0),x(0),0)‖≈

(22)

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

2.2 滾動優(yōu)化求解控制量

圖1 滾動優(yōu)化求解控制量uk

3 仿真實驗

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

表1 UAV狀態(tài)與控制飽和約束

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

表2 UAV初始狀態(tài)量和終止狀態(tài)量

仿真參數(shù)選擇為：采樣時間Δt為0.01 s，殘差容限r(nóng)tol為1.0e-6，優(yōu)化迭代次數(shù)iternum為100，穩(wěn)定因子ζ為100(ζΔt=1)，則穩(wěn)定因子矩陣為As=-ζI；在初始化控制量啟動值u(0)和滾動優(yōu)化求解控制量u(t)算法中，GMRES迭代次數(shù)kmax分別為2和15；加權矩陣Q、Sf和R均選擇為單位矩陣；時域參數(shù)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有效規(guī)劃出從起始點到目標點，并且合理規(guī)避障礙的運動軌跡。圖3和圖4給出了UAV為完成任務必須滿足的兩個終端約束條件，高度以及航向角的變化曲線，顯然，高度和偏航角都逐漸收斂至表2給出的目標狀態(tài)期望值，并且高度的誤差不超過50 m，航向角的誤差不超過5 °。

圖2 UAV的三維飛行軌跡

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

圖4 UAV偏航角變化曲線

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

表3 規(guī)劃總時長和單步滾動優(yōu)化時長均值

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

表4 傳統(tǒng)算法規(guī)劃總時長和單步滾動優(yōu)化時長均值

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

4 結 語

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