唐大全，鄧偉棟，唐管政，鹿珂珂，陳 正

（1.海軍航空大學(xué)，煙臺264001；2.中國人民解放軍91967 部隊(duì)，褡褳054100）

隨著科技的不斷進(jìn)步，無人機(jī)技術(shù)發(fā)展十分迅速。在軍事領(lǐng)域，無人機(jī)最早出現(xiàn)于20世紀(jì)20年代，在第一次世界大戰(zhàn)期間，英國的卡德爾和皮切爾兩位將軍就提出了無人作戰(zhàn)的設(shè)想，由此逐漸設(shè)計成型，并在伊拉克戰(zhàn)爭、阿富汗戰(zhàn)爭以及在阿伯塔巴德突襲等戰(zhàn)爭活動中發(fā)揮了重要作用[1-2]。在民用領(lǐng)域，隨著無人機(jī)技術(shù)的不斷成熟，從最早的航拍無人機(jī)開始，逐漸應(yīng)用于電力巡檢、交通巡查、快遞運(yùn)輸?shù)阮I(lǐng)域，減輕了人類的負(fù)擔(dān)，提高了工作效率。

當(dāng)前，無人機(jī)作業(yè)往往以單架次的形式執(zhí)行相關(guān)活動，隨著任務(wù)需求和復(fù)雜性的不斷提高，單架無人機(jī)在執(zhí)行任務(wù)過程中具有一定的局限性，這就促進(jìn)了無人機(jī)編隊(duì)形式的誕生。為確保無人機(jī)編隊(duì)整體效能大于單架無人機(jī)效能的累加，要求無人機(jī)編隊(duì)內(nèi)部之間具有較強(qiáng)的協(xié)調(diào)一致性、 整體協(xié)同性。因此，精確的導(dǎo)航定位性能是進(jìn)行無人機(jī)編隊(duì)隊(duì)型設(shè)計、航跡規(guī)劃的基礎(chǔ)，也是圓滿完成各項(xiàng)任務(wù)的重要保證。

1 無人機(jī)編隊(duì)協(xié)同導(dǎo)航的算法研究

目前用于編隊(duì)導(dǎo)航的方式有很多，研究較為深入的有主從式、并行式、平行式協(xié)同導(dǎo)航[3]，具體各個編隊(duì)方式的優(yōu)缺點(diǎn)也有了較為全面的概括，這里不再贅述；由于協(xié)同導(dǎo)航過程是非線性，所以應(yīng)用較為廣泛的算法是在卡爾曼濾波基礎(chǔ)上衍生的擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波以及粒子濾波等濾波算法。

卡爾曼濾波由學(xué)者卡爾曼提出，只能用于解決線性系統(tǒng)濾波和預(yù)測問題[4]，因?yàn)闊o人機(jī)在協(xié)同導(dǎo)航過程中的觀測方程是非線性的，所以傳統(tǒng)的卡爾曼濾波不能解決此類問題。對此，文獻(xiàn)[5-8]將卡爾曼濾波算法推廣到了非線性領(lǐng)域，并針對性地提出了擴(kuò)展卡爾曼濾波EKF（extended Kalman filter），其原理是將非線性系統(tǒng)的模型函數(shù)進(jìn)行泰勒展開，并且忽略二階及以上高階項(xiàng)，此方法能夠較好地解決弱非線性系統(tǒng)方程。但當(dāng)系統(tǒng)的非線性較強(qiáng)時，如果采用局部線性化處理非線性系統(tǒng)的估計問題，會帶來較大的誤差，同時在濾波過程中，需要計算系統(tǒng)方程的Jacobian 矩陣，因而增加了濾波計算量。

無跡卡爾曼濾波[9-10]UKF（unscented Kalman filter）以UT（unscented transformation）無損變換為基礎(chǔ)，使用卡爾曼濾波框架對非線性系統(tǒng)進(jìn)行濾波估計，選取一定數(shù)量的采樣點(diǎn)，利用這些采樣點(diǎn)的均值和協(xié)方差逼近原非線性系統(tǒng)。與EKF 相比，該方法避免了求解Jacobian 矩陣和海塞矩陣，同時可以使非線性系統(tǒng)精確到二階泰勒級數(shù)，提高濾波精度。

無論是EKF 還是UKF，其本質(zhì)都是一種近似的估計方法，在給定初始狀態(tài)的前提下，利用狀態(tài)方程和觀測方程進(jìn)行估計，得到逼近真實(shí)值的最優(yōu)估計，如果使用更加靠近真實(shí)狀態(tài)的估計值進(jìn)行濾波更新，會進(jìn)一步提高近似精度，故在此提出一種迭代卡爾曼濾波算法。該算法以UKF 為基礎(chǔ)，通過極大似然函數(shù)確定迭代條件，利用Levenberg-Marquardt方法[11]調(diào)整預(yù)測協(xié)方差矩陣，對無人機(jī)編隊(duì)量測環(huán)節(jié)進(jìn)行迭代更新，多次用觀測更新后的狀態(tài)估計作為輸入，對觀測方程再次進(jìn)行濾波處理，所得的狀態(tài)估計值更加準(zhǔn)確。

2 無人機(jī)編隊(duì)協(xié)同導(dǎo)航原理

在已知基準(zhǔn)坐標(biāo)系的初始位置和速度的前提下，慣性導(dǎo)航系統(tǒng)不依賴任何外界信息，同時不向外輻射任何能量，在準(zhǔn)確測量載體航行位置的基礎(chǔ)上，具有較強(qiáng)的隱蔽性能[12]。但在長時間使用時，會由于誤差的積累導(dǎo)致導(dǎo)航定位精度下降。為此，現(xiàn)有的無人機(jī)一般采用慣性導(dǎo)航系統(tǒng)INS（inertial navigation system）和全球定位系統(tǒng)GPS（global positioning system）相搭配的導(dǎo)航模型，采用高精度的GPS 對無人機(jī)慣導(dǎo)系統(tǒng)進(jìn)行實(shí)時校正，保證定位精度。

一般情況下，無人機(jī)編隊(duì)協(xié)同導(dǎo)航方式分為主從式和平行式。在無人機(jī)編隊(duì)系統(tǒng)中，如果每架無人機(jī)均配備高精度導(dǎo)航設(shè)備，一方面會增加成本，且在戰(zhàn)場運(yùn)用時，容易遭受打擊，增加損失。另一方面，無法突出無人機(jī)編隊(duì)的優(yōu)勢，實(shí)用性和效能性不高。在此重點(diǎn)研究主從式協(xié)同導(dǎo)航，即一臺配備高精度定位設(shè)備的主無人機(jī)，其余無人機(jī)配備精度較差的導(dǎo)航定位設(shè)備。

在協(xié)同定位的過程中，主從無人機(jī)首先進(jìn)行時間校對，確保無人機(jī)編隊(duì)在相同的時間維度下，主無人機(jī)按照規(guī)定的時間間隔向外發(fā)射自身導(dǎo)航信息，從無人機(jī)接收到主機(jī)精確的導(dǎo)航信息后，與自身導(dǎo)航設(shè)備測得的位置信息相融合，通過迭代卡爾曼濾波算法減小估計誤差，不斷逼近真實(shí)軌跡，實(shí)現(xiàn)從無人機(jī)的導(dǎo)航信息較正。

協(xié)同導(dǎo)航的優(yōu)勢在于將少部分具有高精度導(dǎo)航定位能力的無人機(jī)的位置信息，廣播給其它無人機(jī)，使配備低精度導(dǎo)航設(shè)備的無人機(jī)仍然具有高精度定位能力。

3 無人機(jī)編隊(duì)協(xié)同導(dǎo)航模型

在無人機(jī)協(xié)同導(dǎo)航領(lǐng)域，研究較多的是二維協(xié)同導(dǎo)航模型，即將主從無人機(jī)投影到二維平面上或者將其調(diào)整到同意高度，從而將立體空間問題簡化為平面問題，同時減少計算量，但是這樣的計算與實(shí)際情況會存在較大的誤差，且無法知悉無人機(jī)在飛行高度上的變化。因此，為使研究結(jié)果更具普適性，本文在已有研究的基礎(chǔ)上增加了編隊(duì)在高度方向上的研究。

導(dǎo)航坐標(biāo)系下，x，y，z 分別為主從無人機(jī)在東西方向、南北方向、垂直方向的飛行距離；vx，vy，vz分別為無人機(jī)的前向速度、側(cè)向速度、爬升速度；w 為無人機(jī)的旋轉(zhuǎn)角速度。

選取導(dǎo)航坐標(biāo)系下無人機(jī)各個方向上的位置，各個方向上的速度作為狀態(tài)向量。

假設(shè)，從無人機(jī)在x，y，z 方向上做勻速運(yùn)動，則其飛行過程中的狀態(tài)方程為

其中

式中：Φk+1，k為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；Δt 為采樣時間；wk為系統(tǒng)噪聲矩陣，且屬于高斯白噪聲。系統(tǒng)的量測方程為

式中：vk為系統(tǒng)量測噪聲，且屬于高斯白噪聲。

wk，vk滿足：

式中：Qk為系統(tǒng)的噪聲方差矩陣；Rk為量測噪聲方差矩陣。

4 迭代無跡卡爾曼濾波算法

當(dāng)通過構(gòu)造與被估計量相同統(tǒng)計特性的sigma采樣點(diǎn)進(jìn)行逼近時，傳統(tǒng)的UKF 算法受系統(tǒng)模型噪聲影響較大，容易出現(xiàn)精度降低甚至發(fā)散的情況[13]。迭代卡爾曼濾波是將觀測更新后的狀態(tài)估計作為新的參考點(diǎn)，對觀測方程再次進(jìn)行算法處理，提高定位精確度同時為保證算法的穩(wěn)定行，利用Levenberg-Marquardt 方法調(diào)整協(xié)方差矩陣，保證算法的收斂性[14]。

迭代無跡卡爾曼濾波IUKF 算法步驟如下：

步驟1濾波器初始化。當(dāng)k=0 時，系統(tǒng)的初始狀態(tài)值為

系統(tǒng)的初始狀態(tài)協(xié)方差矩陣為

步驟2構(gòu)造2n+1 個sigma 樣本點(diǎn)通過狀態(tài)方程傳遞，即

步驟3時間更新。其權(quán)重系數(shù)為

式中：n 為系統(tǒng)狀態(tài)的維數(shù)；β 為系數(shù)，在高斯分布下其最優(yōu)值為2。

步驟4量測更新。對一步預(yù)測值進(jìn)行UT 變換，產(chǎn)生新的sigma 點(diǎn)集，再次初始化，設(shè)定迭代次數(shù)為j：

則新的sigma 采樣點(diǎn)為

為使算法具有更好的穩(wěn)定性，采用Levenberg-Marquardt 方法調(diào)整預(yù)測協(xié)方差矩陣，即

變換后狀態(tài)的一步預(yù)測值為

誤差協(xié)方差矩陣為

從無人機(jī)輸出的理論方差矩陣為

協(xié)方差矩陣為

計算卡爾曼增益、更新后的系統(tǒng)狀態(tài)和更新后的協(xié)方差為

步驟5利用Gauss-Newton 方法確定迭代次數(shù)。高斯-牛頓方法是用于解決非線性最小二乘問題的一種方法，廣泛應(yīng)用于迭代求解最優(yōu)化問題中。該方法的主要原理是通過設(shè)置代價函數(shù)J（X），求解其最小值或者最優(yōu)值。

在此定義代價函數(shù)為

在主從式無人機(jī)編隊(duì)協(xié)同導(dǎo)航系統(tǒng)中，由于狀態(tài)方程為線性方程，而量測方程為非線性方程，此類型求解代價函數(shù)最優(yōu)值轉(zhuǎn)化為求解代價函數(shù)的非零極值點(diǎn)。

當(dāng)J（Xk，j+1）＜J（Xk，j）時，代價函數(shù)呈收斂趨勢，則第j+1 次迭代結(jié)果更接近最優(yōu)解。

故

步驟6當(dāng)滿足迭代條件時，重復(fù)進(jìn)行量測更新；當(dāng)不滿足迭代條件時，跳出迭代過程，此時可以得到從無人機(jī)的位置估計和估計協(xié)方差矩陣Pk，j。

5 仿真結(jié)果與分析

為直觀檢測算法的有效性，運(yùn)用均方誤差RMSE（root mean square error）來表示主從式無人機(jī)協(xié)同導(dǎo)航的精確度。有

為保證單主領(lǐng)航系統(tǒng)的可觀測性，假定：主機(jī)做固定運(yùn)動角速度的盤旋上升運(yùn)動，運(yùn)動角速度為w=0.1 rad/s；系統(tǒng)在各方向的速度誤差為Δv=1 m/s，距離誤差為Δ=1 m；從無人機(jī)在各方向上以vf=［10 m/s，10 m/s，5 m/s］的速度飛行；主無人機(jī)的初始位置為Xl=［315 m，425 m，35 9 m］；從無人機(jī)的初始位置為Xf=［349 m，452 m，200 m］。在各種算法下，主從無人機(jī)導(dǎo)航系統(tǒng)的導(dǎo)航效果如圖1所示。

圖1 幾種濾波算法的估計軌跡示意圖Fig.1 Schematic diagram of estimation trajectory of several filtering algorithms

由圖可見，主從無人機(jī)導(dǎo)航系統(tǒng)在各種算法下的導(dǎo)航效果。主從無人機(jī)的飛行軌跡以及濾波后的從無人機(jī)飛行軌跡表明，采用Levenberg-Marquardt方法調(diào)整預(yù)測協(xié)方差陣后的IUKF 效果最好，誤差最?。籙KF 濾波后的軌跡與從無人機(jī)實(shí)際軌跡逐漸接近；EKF 濾波后的軌跡與從無人機(jī)實(shí)際軌跡呈發(fā)散趨勢，精度最差。

由圖1中各濾波軌跡示意圖，根據(jù)主從無人機(jī)飛行軌跡以及濾波軌跡的契合程度，可以直觀地了解哪種濾波效果更加精確。在此，通過觀測各濾波在x，y，z 方向的均方誤差，對各濾波進(jìn)行定量分析。其對比效果如圖2所示。

經(jīng)過直觀驗(yàn)證后，從在各個濾波估計下的均方誤差對比效果中，比較各濾波處理后的導(dǎo)航精度。

從時間角度來看，各濾波在x 軸均方誤差中，IUKF最大誤差為20 m，且隨著時間的增長，精度逐漸增加，在30 s 逐漸趨于穩(wěn)定，誤差在7 m 左右；UKF 濾波誤差首先呈下降趨勢，然后迅速增大，在5 s 左右逐漸穩(wěn)定，最大誤差在35 m 左右，在40 s 后誤差逐漸下降；EKF 誤差起伏幅度較大，最高誤差達(dá)到120 m左右，而后迅速下降，接著快速上升，呈發(fā)散趨勢。

圖2 幾種濾波在x，y，z 軸均方誤差的對比Fig.2 Comparison of root mean square errors of several filters in x，y and z axes

各濾波在y 軸均方誤差中，IUKF 誤差逐漸下降，在40 s 左右趨于穩(wěn)定；UKF 最大誤差可達(dá)到120 m 左右，然后逐漸下降；EKF 誤差突變非常明顯，在26 s 左右誤差精度最好，而后逐漸擴(kuò)大。

各濾波在z 軸均方誤差中，IUKF 誤差非常平穩(wěn)；UKF 最大誤差在100 m 左右，而后逐漸下降，在35 s 左右達(dá)到最佳值，逐漸趨于穩(wěn)定；EKF 誤差最大值在110 m 左右，在5 s 以后誤差呈上升趨勢。

綜上所述，IUKF 的收斂效果最好，其次為UKF，再次為EKF。從均方誤差角度來看，IUKF 的濾波精度最高，其次為UKF，再次為EKF。

根據(jù)上述仿真結(jié)果可知，采用Levenberg-Marquardt方法，調(diào)整預(yù)測協(xié)方差陣后的IUKF，相較于UKF 及EKF 的導(dǎo)航效果更好。

6 結(jié)語

當(dāng)前對無人機(jī)編隊(duì)協(xié)同導(dǎo)航的研究，大都將位置信息投影到二維平面，本文在其基礎(chǔ)上增加了飛行高度方向上的研究，使研究結(jié)果更具現(xiàn)實(shí)意義；在無跡卡爾曼濾波的基礎(chǔ)上，為使濾波精度進(jìn)一步提高，在此提出了迭代無跡卡爾曼濾波算法，并通過高斯牛頓法設(shè)置代價函數(shù)，確定迭代次數(shù)；針對協(xié)方差發(fā)散問題利用Levenberg-Marquardt 方法調(diào)整預(yù)測協(xié)方差陣，使算法更加平穩(wěn)，保證算法的全局收斂性；針對非線性濾波在協(xié)同導(dǎo)航領(lǐng)域存在收斂速度和收斂精度問題，通過與EKF 和UKF 相對比，證明了本文所提算法的實(shí)用性。