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

方 斌 馮曉鋒 李 杰

1(湖南警察學(xué)院交通管理系 湖南 長(zhǎng)沙 410138)2(國(guó)防科技大學(xué)智能科學(xué)學(xué)院 湖南 長(zhǎng)沙 410073)

0 引 言

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

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

1 問(wèn)題建模

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

(1)

等式右側(cè)第一項(xiàng)表示終端代價(jià)：

(2)

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

(3)

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

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

(3) 路徑約束。

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

(4)

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

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

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

(5)

(6)

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

(7)

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

(8)

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

(9)

(10)

(11)

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

(12)

(13)

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

(14)

(15)

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

2 方法設(shè)計(jì)

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

(16)

(17)

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

(18)

(19)

(20)

(21)

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

2.1 初始化求解控制量啟動(dòng)值

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

(22)

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

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

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

3 仿真實(shí)驗(yàn)

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

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

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

表2 UAV初始狀態(tài)量和終止?fàn)顟B(tài)量

仿真參數(shù)選擇為：采樣時(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；在初始化控制量啟動(dòng)值u(0)和滾動(dòng)優(yōu)化求解控制量u(t)算法中，GMRES迭代次數(shù)kmax分別為2和15；加權(quán)矩陣Q、Sf和R均選擇為單位矩陣；時(shí)域參數(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的三維飛行軌跡?？梢钥闯鲈撍惴軌?yàn)閁AV有效規(guī)劃出從起始點(diǎn)到目標(biāo)點(diǎn)，并且合理規(guī)避障礙的運(yùn)動(dòng)軌跡。圖3和圖4給出了UAV為完成任務(wù)必須滿足的兩個(gè)終端約束條件，高度以及航向角的變化曲線，顯然，高度和偏航角都逐漸收斂至表2給出的目標(biāo)狀態(tài)期望值，并且高度的誤差不超過(guò)50 m，航向角的誤差不超過(guò)5 °。

圖2 UAV的三維飛行軌跡

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

圖4 UAV偏航角變化曲線

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

表3 規(guī)劃總時(shí)長(zhǎng)和單步滾動(dòng)優(yōu)化時(shí)長(zhǎng)均值

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

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

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

4 結(jié) 語(yǔ)

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