方 斌 馮曉鋒 李 杰

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

0 引 言

在復雜環(huán)境下執(zhí)行任務的過程中，無人機(UAV)必須具備在線航跡規(guī)劃的能力以規(guī)避任務區(qū)域中的橋梁、樓宇以及突發(fā)威脅等空間障礙，確保飛行安全。目前，國內(nèi)外的研究學者對UAV在線航跡規(guī)劃問題已經(jīng)開展了大量研究。文獻[1-2]基于人工勢場法構(gòu)造由目標點引力和障礙物的斥力共同作用的人工勢場，沿著勢函數(shù)梯度下降的方向來尋找無碰撞軌跡。其中，目標點對UAV產(chǎn)生“引力”，障礙物對UAV產(chǎn)生“斥力”。文獻[3-4]基于采樣方法，通過對構(gòu)型空間或狀態(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)

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

(2)

式中：x=[x,y,z,φ,θ,ψ,V,α,β,p,q,r]T為UAV狀態(tài)矢量，x、y、z分別為UAV位置在地理坐標系下的北向分量、東向分量和垂直分量，φ、θ、ψ分別表示滾轉(zhuǎn)角、俯仰角和偏航角，V為UAV空速，α、β分別為攻角和側(cè)滑角，p、q、r為角速度；xf表示UAV固定終止狀態(tài)矢量；tf表示任意終止時間；Sf為12×12維終端代價加權(quán)對角矩陣。等式右側(cè)第二項中的被積函數(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)方程一起構(gòu)成受約束最優(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。具體求解過程如下:首先，采用改進格萊姆-施密特正交化方法得到正交向量組；然后，采用吉文斯旋轉(zhuǎn)優(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；加權(quán)矩陣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的三維飛行軌跡?？梢钥闯鲈撍惴軌驗閁AV有效規(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]，并進行對比分析，結(jié)果如表4所示。

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

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

4 結(jié) 語

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