朱代武，劉 豪，路東林，魯 力

（1.中國(guó)民用航空飛行學(xué)院圖書(shū)館，四川 廣漢 618307；2.中國(guó)民用航空飛行學(xué)院空中交通管理學(xué)院，四川 廣漢 618307）

0 引言

對(duì)于分散、動(dòng)態(tài)、多平臺(tái)多元觀(guān)測(cè)信息進(jìn)行有效的態(tài)勢(shì)融合，完成復(fù)雜環(huán)境感知和理解，是實(shí)現(xiàn)UAV 自主協(xié)同控制的必有舉措；同時(shí)UAV 具有一定自治組織與協(xié)調(diào)能力，可通過(guò)控制器對(duì)其姿態(tài)、運(yùn)動(dòng)模式進(jìn)行調(diào)整。UAV 控制系統(tǒng)主要依據(jù)任務(wù)進(jìn)行飛行姿態(tài)調(diào)整，即需要實(shí)際飛行姿態(tài)與參考飛行姿態(tài)保持一致使得誤差最?。?-2］；ROTONDO D 等提出將非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性處理的變參數(shù)方法，雖然結(jié)構(gòu)控制器比較簡(jiǎn)單但難以用于實(shí)際工程［3］；KORKMAZ M 等提出比例積分微分控制辦法，但處理數(shù)據(jù)過(guò)大響應(yīng)時(shí)間過(guò)長(zhǎng)，使得UAV 組織控制存在時(shí)滯［4］；MELKOU L 等提出高階滑?？刂破鳎捎行p小偏振影響但犧牲該組織控制系統(tǒng)的魯棒性［5］；KARIMODDINI I、HAFEZ A T 及DENTLER J 等提出基于神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)控制的UAV 姿態(tài)控制模型，但由于數(shù)據(jù)中心點(diǎn)的不確定性使得控制器最優(yōu)化處理結(jié)果存在偏差［6-8］；此外常用的控制算法如反步法［9-10］、模糊控制［11］、強(qiáng)化學(xué)習(xí)控制［12］等，但上述模型所需飛行數(shù)量量大，對(duì)未建模動(dòng)態(tài)特性和未知干擾的適應(yīng)性較差。因此，本文在考慮到UAV 時(shí)變系統(tǒng)的高度非線(xiàn)性、欠驅(qū)動(dòng)、高耦合的特性［13-14］、外界環(huán)境的多變性與干擾性，提出一種UIF-MPC 仿真的UAV 環(huán)航姿態(tài)控制模型，使得UAV 在運(yùn)動(dòng)過(guò)程中具有較高的魯棒性、抗干擾性與適應(yīng)能力。

1 UAV 運(yùn)動(dòng)觀(guān)測(cè)模型

假設(shè)目標(biāo)運(yùn)動(dòng)場(chǎng)面環(huán)境為X-Y，UAV 定高飛行且速度大于目標(biāo)移動(dòng)速度，UAV 與運(yùn)動(dòng)目標(biāo)體的相對(duì)運(yùn)動(dòng)如圖1 所示。

圖1 無(wú)人機(jī)環(huán)航示意圖Fig.1 Schematic diagram of UAV circumnavigation

其中，Po為UAV 位置；PT為目標(biāo)位置；為目標(biāo)估計(jì)位置；v0為UAV 的飛行速率；β（t）為測(cè)量角度；η（t）為UAV 與目標(biāo)的測(cè)向矢量為η（t）逆時(shí)針旋轉(zhuǎn)90°的向量；D（t）為兩者之間的實(shí)際距離。

在對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行純角度目標(biāo)定位時(shí)，UAV 以目標(biāo)作為圓心質(zhì)點(diǎn)，以設(shè)定距離作為UAV 環(huán)巡半徑飛行。因?qū)嶋H航跡與期望航跡存在誤差，在實(shí)際操作中希望誤差極限值趨于0，即極差差值收斂于坐標(biāo)軸。UAV 的運(yùn)動(dòng)模型為：

令vo為該無(wú)人機(jī)的運(yùn)動(dòng)速率，Po=［xo，yo］T∈R2為UAV 位 置，PT=［xt，yt］T∈R2指 目 標(biāo) 的 位 置，vo=［vocosαo，vosinαo］T∈R2為UAV 在X-Y 軸上的分速度，αo為UAV 航向，ωo為控制角速度。目標(biāo)的運(yùn)動(dòng)模型為：

其中，vt為目標(biāo)的運(yùn)動(dòng)，vt=［vtcosαt，vtsinαt］T∈R2為目標(biāo)在X-Y 軸上的分速度；αt為目標(biāo)航向；ωt為控制角速度。設(shè)UAV 與目標(biāo)之間的相對(duì)速度為vr，則vr=vo-vt，根據(jù)式（2）～式（3）得相對(duì)運(yùn)動(dòng)方程為：

該運(yùn)動(dòng)方程下給出UAV 對(duì)目標(biāo)進(jìn)行環(huán)巡的控制策略。

在該策略中ed=d-Dd表示半徑偏差；Dd為期望環(huán)航半徑；β 為UAV 與目標(biāo)之間的方向角；σ 為相對(duì)速度與傳感器視線(xiàn)之間的夾角；為控制器參數(shù)。

基于成像傳感器觀(guān)測(cè)信息，目標(biāo)被識(shí)別后可得到目標(biāo)到UAV 的相對(duì)方位信息。對(duì)于非線(xiàn)性系統(tǒng)可根據(jù)噪聲狀態(tài)、狀態(tài)轉(zhuǎn)移參量以及量測(cè)函數(shù)對(duì)當(dāng)前或者下一刻姿態(tài)進(jìn)行估計(jì)［15］，其模型如下：

式中，x［k］∈Rn表示k 時(shí)刻系統(tǒng)狀態(tài)；z［k］∈Rm表示k 時(shí)刻測(cè)量數(shù)據(jù)；q［k-1］～N（0，Q［k-1］）是k-1時(shí)刻過(guò)程噪聲；r［k］～N（0，R［k］）是k 時(shí)刻過(guò)程噪聲；f 與h 為動(dòng)態(tài)模型函數(shù)與測(cè)量函數(shù)模型，通過(guò)對(duì)當(dāng)前狀態(tài)估計(jì)，即

因在實(shí)際控制系統(tǒng)中假設(shè)系統(tǒng)模型隨時(shí)間改變，變化的系統(tǒng)特性很難通過(guò)固定模型進(jìn)行描述，因此，在估計(jì)中需考慮系統(tǒng)模型改變的可能性［16］。假設(shè)當(dāng)前模型由M 個(gè)離散子模型集合f={f1，f2，…，fM}，則式（6）可改寫(xiě)為：

假設(shè)對(duì)每個(gè)狀態(tài)轉(zhuǎn)移子模型都有一個(gè)預(yù)先的概率μj［0］=P{fj［0］}，且在下一時(shí)刻從模型i 到模型j的切換概率已知，記為pij=P{fj［K］|fj［K-1］}?？紤]到目標(biāo)函數(shù)的多模態(tài)特性，可對(duì)式（8）進(jìn)行改進(jìn)。

2 MPC 下運(yùn)動(dòng)觀(guān)測(cè)模型構(gòu)建

模型預(yù)測(cè)控制（model predictive control，MPC）適應(yīng)于優(yōu)化和控制兩個(gè)領(lǐng)域的交叉工程。該模型善于處理多輸入多輸出系統(tǒng)，且對(duì)于UAV 控制系統(tǒng)存在耦合關(guān)系或者系統(tǒng)過(guò)于冗雜的問(wèn)題可通過(guò)MPC進(jìn)行多控制變量的交互；且MPC 可將先驗(yàn)信息及未來(lái)參考信息納入控制優(yōu)化以改善控制器的性能，其多信道傳輸和交互性如圖2 所示。

圖2 MPC 模型預(yù)測(cè)控制特性Fig.2 MPC model predicts control characteristics

MPC 可以提前考慮UAV 控制器及飛行性能，從而可以提供更好的環(huán)航體驗(yàn)，對(duì)UAV 環(huán)航運(yùn)動(dòng)進(jìn)行改進(jìn)，改進(jìn)后如圖3 所示。

圖3 基于MPC 的UAV 轉(zhuǎn)向示意圖Fig.3 Schematic diagram of an MPC-based UAV steering

其中，φ 為航向角；δf為UAV 前旋轉(zhuǎn)軸偏角；vr和vf為旋翼前軸與后軸的實(shí)時(shí)測(cè)定的速度；R 為相對(duì)于目標(biāo)的轉(zhuǎn)向半徑；P 為UAV 的瞬時(shí)轉(zhuǎn)動(dòng)速度；M 為UAV 后旋翼質(zhì)點(diǎn)；N 為UAV 后旋翼質(zhì)點(diǎn)。在后旋翼軸心（xr，yr）處，速度可分解為橫縱坐標(biāo)變化率分量的疊加，且UAV 垂直于自身的瞬時(shí)速度為0。

定義擺角速度ω=φ，并根據(jù)正弦余弦展開(kāi)公式進(jìn)行聯(lián)合的MPC 得運(yùn)動(dòng)模型，令UAV 的狀態(tài)變量為（x，y，φ），（v，δ）為控制輸入。

此時(shí)環(huán)航的UAV 可以看作一個(gè)輸入χ（x，y，φ）與u（v，δ）的控制系統(tǒng)，即S=f（χ，u）。UAV 的環(huán)巡參考運(yùn)動(dòng)軌跡形式采用泰勒級(jí)別展開(kāi)（忽略高階項(xiàng)）的線(xiàn)性化得UAV 誤差模型。

上述線(xiàn)性化的UAV 模型是連續(xù)的，需要對(duì)其進(jìn)行離散化處理，才可由當(dāng)前時(shí)刻的狀態(tài)量誤差和控制量誤差推導(dǎo)到下一刻的狀態(tài)量誤差，離散化為

由于狀態(tài)量偏差不要求全輸出，所以定義輸出方程。

因UAV 環(huán)航運(yùn)動(dòng)模型由M 個(gè)離散子模型集合f={f1，f2，…，fM}，令M=Nu，則輸出方程為，

其中，ξ（k）與Δu 為當(dāng)前時(shí)刻的狀態(tài)量和控制時(shí)域內(nèi)的增量。

3 基于UIF 的MPC 融合估計(jì)結(jié)構(gòu)模型

觀(guān)測(cè)對(duì)UAV 狀態(tài)信息和時(shí)域增量信息產(chǎn)生的貢獻(xiàn)與狀態(tài)似然函數(shù)相關(guān)，假設(shè)傳感器滿(mǎn)足獨(dú)立分布特征，即可得到運(yùn)動(dòng)姿態(tài)信息融合濾波［17］。在該融合結(jié)構(gòu)中將信息狀態(tài)y 與Y 可以寫(xiě)成：

式中，ii［k］與Ii［k］分別為傳感器在k 時(shí)刻的觀(guān)測(cè)對(duì)信息狀態(tài)與信息矩陣的貢獻(xiàn)，其集中式融合結(jié)構(gòu)如圖4 所示。

圖4 基于UIF 的集中融合預(yù)測(cè)結(jié)構(gòu)Fig.4 Centralized fusion prediction structure based on UIF

簡(jiǎn)單的特性相加使得信息濾波在多傳感器融合估計(jì)中較為簡(jiǎn)單實(shí)用。各傳感器節(jié)點(diǎn)只需要產(chǎn)生信息項(xiàng)Ii［k］與ii［k］在全局融合濾波器中進(jìn)行相加便可以得到全局融合姿態(tài)估計(jì)［18-19］。當(dāng)UAV 感知對(duì)象為多子模型的移動(dòng)目標(biāo)時(shí)可以將MPC 方法用于融合濾波中。在圖4 中可以同時(shí)在局部濾波器與中心濾波器中利用MPC-UIF。通過(guò)計(jì)算信息收益對(duì)環(huán)航姿態(tài)進(jìn)行評(píng)估，對(duì)應(yīng)信息收益公式為，

綜上所述基于MPC-UIF 的融合估計(jì)算法整體流程如圖5 所示。

圖5 基于MPC-UIF 融合估計(jì)算法流程Fig.5 Flow of estimation algorithm based on MPC-UIF fusion

4 仿真分析

針對(duì)UAV 跟蹤單運(yùn)動(dòng)目標(biāo)的任務(wù)進(jìn)行仿真實(shí)驗(yàn)，假定目標(biāo)在2D 平面內(nèi)運(yùn)動(dòng)記為x=［x，y，x1，y1，φ，φ1］，UAV 的初始位置為［-25，0］；其飛行速率為20 m/s，期望環(huán)航半徑為Dd=1km，k1=0.1，k2=0.01，α=0.6，γ=5，ξ=2，σ=0.001。其中，φ 表示目標(biāo)運(yùn)動(dòng)方向，運(yùn)動(dòng)目標(biāo)初始狀態(tài)設(shè)定如下：

在win10 的運(yùn)行環(huán)境下進(jìn)行UAV 協(xié)同仿真，同時(shí)采用MATLAB2018.a 進(jìn)行模擬仿真實(shí)現(xiàn)。下頁(yè)圖6 為環(huán)航跟蹤的軌跡圖，紅色實(shí)線(xiàn)為目標(biāo)運(yùn)動(dòng)物體，圖7 為在X/Y 方向的位置估計(jì)誤差。通過(guò)圖7 可以看出，融合算法的x，y 方向估計(jì)誤差波峰區(qū)間為［-10，10］，遠(yuǎn)小于單純UIF 算法的峰值［-25，25］，說(shuō)明MPC-UIF 融合預(yù)測(cè)能夠在較小誤差下完成對(duì)目標(biāo)的位置估計(jì)并完成跟蹤，且跟蹤效果良好。

圖6 目標(biāo)環(huán)航軌跡圖Fig.6 Target circumnavigation trajectory diagram

圖7 位置估計(jì)誤差Fig.7 Position estimation error

本文中目標(biāo)初始位置為［-25，0］，終點(diǎn)目標(biāo)位置為［0，0］。UAV 在對(duì)目標(biāo)環(huán)航過(guò)程中因外部擾動(dòng)、模型偏差、狀態(tài)誤差及控制器響應(yīng)時(shí)間諸多元素，會(huì)使得期望路徑與MPC 預(yù)測(cè)路徑存在一定誤差，對(duì)期望路徑與預(yù)測(cè)路徑進(jìn)行擬合，結(jié)果如圖8 所示?？芍S著迭代次數(shù)的增加，對(duì)應(yīng)期望路徑誤差與MPC預(yù)測(cè)路徑誤差在迭代次數(shù)為15 之后，均逐漸縮減且UAV 預(yù)測(cè)路徑誤差≤UAV 期望路徑誤差，UAV兩路徑在迭代次數(shù)為7 時(shí)擬合效果趨于一致，證明預(yù)測(cè)環(huán)航半徑收斂于期望環(huán)航半徑，即在保證完成對(duì)運(yùn)動(dòng)目標(biāo)環(huán)航的過(guò)程中提高路徑的預(yù)測(cè)精度。

圖8 預(yù)測(cè)環(huán)航和期望環(huán)航擬合輸出Fig.8 Predicted circumnavigation and expected circumnavigation fitting output

在實(shí)際UAV 巡航過(guò)程中，不僅要保證實(shí)際環(huán)航半徑收斂于期望環(huán)航半徑，即關(guān)于ed 函數(shù)極限趨于0。同時(shí)考慮到UAV 性能、運(yùn)動(dòng)區(qū)間范圍及PID控制器處理能力，應(yīng)保證環(huán)航航跡的平滑性，即需滿(mǎn)足航跡過(guò)渡自然且光滑，航向不發(fā)生突變［20］。同時(shí)還保證了曲線(xiàn)的保凸性，平滑后的航跡經(jīng)過(guò)所有的航點(diǎn)且逼近原始的航跡曲線(xiàn)，通過(guò)對(duì)該環(huán)航數(shù)據(jù)進(jìn)行處理，MPC-UIF 融合后平滑性函數(shù)輸出如圖9所示。

圖9 基于MPC-UIF 融合的平滑性研究Fig.9 Smoothness study based on MPC-UIF fusion

通過(guò)圖9 可知，隨著迭代次數(shù)的增加UAV 航向趨于平緩，航跡平滑性有所增強(qiáng)，精度得到提高，側(cè)向證明基于融合MPC-UIF 的控制精度和收斂速度得到保證；同時(shí)隨著步長(zhǎng)設(shè)定系數(shù)的增加，預(yù)測(cè)航跡會(huì)自適應(yīng)調(diào)整以尋求到真實(shí)結(jié)束點(diǎn)的最短距離。

5 結(jié)論

本文針對(duì)復(fù)雜干擾項(xiàng)下的固定翼UAV 系統(tǒng)，結(jié)合自適應(yīng)MPC 預(yù)測(cè)和UIF 精度的學(xué)習(xí)能力，提出具有高性能環(huán)航的跟蹤控制器。根據(jù)所屬UAV 性能、全局融合響應(yīng)設(shè)定控制器參數(shù)，使得在保證UAV 能夠快速收斂于期望半徑的同時(shí)，提高控制器的精度和速度，較之傳統(tǒng)的PID 控制具有更高抗干擾能力和高魯棒性。未來(lái)進(jìn)一步將深度學(xué)習(xí)與自監(jiān)督機(jī)制進(jìn)行耦合控制，來(lái)解決反無(wú)人機(jī)和多UAV自主協(xié)同路徑規(guī)劃問(wèn)題。