基于迭代無跡卡爾曼濾波的無人機編隊協同導航

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

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

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

當前，無人機作業往往以單架次的形式執行相關活動，隨著任務需求和復雜性的不斷提高，單架無人機在執行任務過程中具有一定的局限性，這就促進了無人機編隊形式的誕生。為確保無人機編隊整體效能大于單架無人機效能的累加，要求無人機編隊內部之間具有較強的協調一致性、 整體協同性。因此，精確的導航定位性能是進行無人機編隊隊型設計、航跡規劃的基礎，也是圓滿完成各項任務的重要保證。

1 無人機編隊協同導航的算法研究

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

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

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

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

2 無人機編隊協同導航原理

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

一般情況下，無人機編隊協同導航方式分為主從式和平行式。在無人機編隊系統中，如果每架無人機均配備高精度導航設備，一方面會增加成本，且在戰場運用時，容易遭受打擊，增加損失。另一方面，無法突出無人機編隊的優勢，實用性和效能性不高。在此重點研究主從式協同導航，即一臺配備高精度定位設備的主無人機，其余無人機配備精度較差的導航定位設備。

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

協同導航的優勢在于將少部分具有高精度導航定位能力的無人機的位置信息，廣播給其它無人機，使配備低精度導航設備的無人機仍然具有高精度定位能力。

3 無人機編隊協同導航模型

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

導航坐標系下，x，y，z 分別為主從無人機在東西方向、南北方向、垂直方向的飛行距離；vx，vy，vz分別為無人機的前向速度、側向速度、爬升速度；w 為無人機的旋轉角速度。

選取導航坐標系下無人機各個方向上的位置，各個方向上的速度作為狀態向量。

假設，從無人機在x，y，z 方向上做勻速運動，則其飛行過程中的狀態方程為

其中

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

式中：vk為系統量測噪聲，且屬于高斯白噪聲。

wk，vk滿足：

式中：Qk為系統的噪聲方差矩陣；Rk為量測噪聲方差矩陣。

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

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

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

步驟1濾波器初始化。當k=0 時，系統的初始狀態值為

系統的初始狀態協方差矩陣為

步驟2構造2n+1 個sigma 樣本點通過狀態方程傳遞，即

步驟3時間更新。其權重系數為

式中：n 為系統狀態的維數；β 為系數，在高斯分布下其最優值為2。

步驟4量測更新。對一步預測值進行UT 變換，產生新的sigma 點集，再次初始化，設定迭代次數為j：

則新的sigma 采樣點為

為使算法具有更好的穩定性，采用Levenberg-Marquardt 方法調整預測協方差矩陣，即

變換后狀態的一步預測值為

誤差協方差矩陣為

從無人機輸出的理論方差矩陣為

協方差矩陣為

計算卡爾曼增益、更新后的系統狀態和更新后的協方差為

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

在此定義代價函數為

在主從式無人機編隊協同導航系統中，由于狀態方程為線性方程，而量測方程為非線性方程，此類型求解代價函數最優值轉化為求解代價函數的非零極值點。

當J（Xk，j+1）＜J（Xk，j）時，代價函數呈收斂趨勢，則第j+1 次迭代結果更接近最優解。

故

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

5 仿真結果與分析

為直觀檢測算法的有效性，運用均方誤差RMSE（root mean square error）來表示主從式無人機協同導航的精確度。有

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

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

由圖可見，主從無人機導航系統在各種算法下的導航效果。主從無人機的飛行軌跡以及濾波后的從無人機飛行軌跡表明，采用Levenberg-Marquardt方法調整預測協方差陣后的IUKF 效果最好，誤差最小；UKF 濾波后的軌跡與從無人機實際軌跡逐漸接近；EKF 濾波后的軌跡與從無人機實際軌跡呈發散趨勢，精度最差。

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

經過直觀驗證后，從在各個濾波估計下的均方誤差對比效果中，比較各濾波處理后的導航精度。

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

圖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 左右趨于穩定；UKF 最大誤差可達到120 m 左右，然后逐漸下降；EKF 誤差突變非常明顯，在26 s 左右誤差精度最好，而后逐漸擴大。

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

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

根據上述仿真結果可知，采用Levenberg-Marquardt方法，調整預測協方差陣后的IUKF，相較于UKF 及EKF 的導航效果更好。

6 結語

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