基于修正似然濾波的無人機編隊相對導航方法

蘇炳志，王磊，張紅偉，汪海涵，石璐璐

(1.中國直升機設計研究所，天津 300300；2.陸軍裝備部駐天津地區航空軍代室，天津 300384)

無人機采用編隊形式開展協同偵查、聯合攻擊和戰場毀傷評估等任務，可以有效提高作戰效率，是未來體系化作戰的主要模式[1]。高精度、高可靠的相對導航系統是無人機編隊協同完成既定任務的關鍵技術。基于INS/GPS 的相對導航方式是初期最常用的一種導航機制，然而從外界接收到的GPS 信號存在極易受到故意干擾和電子欺騙的缺陷[2-3]。因此，學者們研究了一種不依賴于外部傳感器的相對導航系統-視覺輔助慣性的相對導航[4]。基于INS/Vision 的相對導航精度很大程度上取決于視覺傳感器和慣性傳感器數據融合方法。

由于形式簡單和易于實現，擴展卡爾曼濾波(extended Kalman filtering, EKF)是相對導航濾波器中最常用的一種濾波算法[5]。由于只利用一階泰勒級數對非線性系統進行線性化，EKF 在強非線性系統中表現出不令人滿意的性能甚至使濾波器發散[6]。學者們使用采樣方法近似非線性分布解決非線性問題的思維，提出一系列確定性采樣非線性高斯濾波。文獻[7-8]從直觀的統計信息轉換視角開發了基于無跡變換的卡爾曼濾波(unscented Kalman filtering, UKF)。然而，在處理高維系統時，UKF 中存在負權重的Sigma 點，致使狀態估計協方差出現非正定，這將影響濾波器的穩定性。Arasaratnam等將笛卡兒坐標系下的積分轉換成球面和徑向積分推導了基于三階球面-徑向容積準則的容積卡爾曼濾波(cubature Kalman filtering, CKF)[9-10]。與UKF不同，CKF 的權重全為正值，因此，其具有更好的數值穩定性。

文獻[5-10]中各種濾波算法都是基于量測是實時可用的假設開發得到的。但實際上，由于通訊或網絡堵塞等因素的存在，到達濾波數據處理中心的量測數據往往存在隨機時延[11]。為了處理量測隨機時延問題，學者們基于向后逆推和向前正推導提出了無序Sigma 點卡爾曼濾波[12]和亂序粒子濾波[13]。盡管這些濾波算法能夠很好地處理隨機延遲量測，但需要時間戳來計算延遲時間，并且只能處理延遲時間不變的量測數據。為了擺脫這種限制和克服不足，學者們從延遲概率的角度出發，利用伯努利隨機變量建立實際接收量測與理想量測之間的關系，對隨機時延系統重新建模，提出了多種濾波體制以解決量測數據具有一步或多步延遲問題。Hermoso-Carazo 和Linares-Perez[14-15]針對一步隨機延遲量測提出改進型的EKF 和UKF，通過對狀態和量測噪聲的擴增，將其推廣到多步隨機時延系統中。Wang 等[16]概括性地推導了具有一般普遍性的單步隨機時延非線性高斯濾波算法，并給出了單步隨機時延容積卡爾曼濾波(one-step randomly delayed CKF, ORD-CKF)算法的詳細過程。張勇剛等[17]提出一種帶多步隨機延遲量測高斯濾波器的一般框架，并給出多步隨機時延容積卡爾曼濾波(multiplestep randomly delayed CKF, MRD-CKF)算法的具體計算流程。Esmzad 和Esfanjani[18-19]通過修正常規卡爾曼濾波的似然函數，提出一種能夠處理多步隨機時延的修正似然卡爾曼濾波，該算法利用量測殘差調節權重因子，使得濾波算法具有更高的估計精度；之后將其推廣到非線性高斯系統中。

在文獻[18-19]的研究基礎上，采用估計精度和穩定性較好的三階球面-徑向容積準則計算非線性系統中的高斯加權積分，提出一種修正似然容積卡爾曼濾波(modified likelihood CKF, ML-CKF)算法，解決無人機編隊相對導航系統中視覺量測存在多步隨機時延問題。所提算法的遞歸過程包括狀態預測和量測更新2 個步驟。狀態預測是基于三階球面-徑向容積準則和多步隨機時延系統得到的。由于存在時延，接收到的量測與系統過去狀態相關，在量測更新中通過邊緣化延遲變量來計算濾波的似然函數以從延遲量測中準確地提取信息。建立無人機編隊相對導航系統模型；采用一種無約束的三參數羅德里格斯參數表示姿態誤差來傳播和更新姿態四元數，設計基于ML-CKF 的相對導航濾波器。最后，開展仿真驗證，對比了本文所提算法ML-CKF 與CKF、ORD-CKF 和MRD-CKF 的性能。

1 修正似然容積卡爾曼濾波

1.1 問題闡述及預處理

考慮式(1)所示的離散系統：

式中：yk為實際接收到的量測量；τ0=1，τs(1 ≤s ≤d)為相互獨立的伯努利隨機變量，τ(s,j)為

式中：τ(s,j)(s=0,1,···,d)中只有一個系數取1，其他均為0；這意味著每個時刻接收到的實際量測 yk是當前時刻或者過去時刻量測量 zk-s(s=0,1,···,d)中的一個。

量測延遲 s(0 ≤s ≤d)步的概率為

式中：pj為伯努利隨機變量 τj(0 ≤j ≤d)的期望，即τj取1 的概率。

1.2 修正似然非線性高斯濾波框架

由式(2)可知濾波器實際接收到的量測 yk是{zk-s}ds=0中 的 一個，即 yk的 值與 xk,xk-1,···,xk-d相關。因此，需要將 xk-1,xk-2,···,xk-d擴增到當前狀態當中，得到擴增狀態：

不同于常規貝葉斯濾波中的單峰概率密度函數，式(9)似然函數是一多峰概率密度函數。由于式(9)包含更多關于量測與狀態之間的信息，因此，與單峰概率密度函數相比，式(9)所表示的似然函數更加精確。在量測噪聲服從高斯分布時，似然函數式(9)可以寫成混合高斯形式：

1.2.2 傳播預測分布

假設先驗分布 p(Xk-1|y1:k-1)和狀態傳遞分布p(Xk|Xk-1)均為高斯分布，根據Chapman-Kolmogorov方程，可得預測分布：

1.2.3 更新后驗分布

將式(11)和式(12)代入式(8)，可得到后驗分布：

1.3 修正似然容積卡爾曼濾波算法

采用三階球面-徑向容積準則計算修正似然非線性高斯濾波中的高斯加權積分，詳細給出修正似然容積卡爾曼濾波算法的遞推計算更新過程。

1.3.1 狀態預測

根據式(5)，可得k -1時 刻的擴增狀態估計X?k-1/k-1及其協方差 Pk-1/k-1：

修正似然容積卡爾曼濾波算法的具體流程如圖1 所示。

圖1 修正似然容積卡爾曼濾波流程Fig.1 Flowchart of modified likelihood cubature Kalman filtering

2 相對導航系統建模

以僚機本體系為基準建立僚機和長機之間的相對姿態、位置和速度方程，以長機上的導航相機與僚機上特征光點之間的視線矢量作為量測量，將長機上的慣性和視覺量測信息傳輸給僚機進行融合估計得到僚機和長機之間的相對運動信息。

2.1 相對導航方程

2.1.1 相對姿態運動方程

采用工程實用性強的四元數表示姿態，即

2.2 相對導航傳感器測量模型

2.2.1 慣性導航系統測量模型

加速度計和陀螺儀是慣性導航系統中重要的傳感器，其測量模型分別為

3 相對導航濾波器設計

3.1 狀態向量

若直接把受歸一化約束的四元數作為狀態分量，存在四元數姿態協方差陣奇異性問題，因此，采用一種無約束的三參數羅德里格斯參數表示姿態誤差來傳播和更新姿態四元數，進行濾波器設計。

定義狀態矢量：

2.2.2 視覺導航系統測量模型

視覺導航系統的量測量是長機上導航相機與僚機上特征光點之間的視線矢量，其測量原理如圖2

3.2 狀態預測

k-1時刻相對導航濾波器的擴增狀態為

將式(64)～式(66)和式(23)～式(26)代入式(20)中可得到擴增狀態預測及其協方差。

3.3 量測更新

融合式(19)～式(36)修正似然容積卡爾曼濾波算法和式(52)～式(71)相對導航濾波器設計過程，基于ML-CKF 的相對導航濾波器流程如下。

1) 狀態預測。

① 根據式(21)計算狀態預測容積點；

② 根據式(54)～式(57)計算姿態四元數誤差；

③ 根據式(58)和式(59)計算狀態預測所需的姿態四元數容積點；

④ 根據式(60)、式(65)和式(66)進行相對姿態、相對位置、相對速度、加速度計漂移和陀螺儀漂移遞推；

⑤ 根據式(64)計算經過狀態遞推后的羅德里格參數估計值；

⑥ 根據式(23)～式(26)和式(20)計算擴增狀態及其協方差。

2) 量測更新。

① 根據式(27)計算量測更新所需容積點；

② 根據式(67)計算延遲 s(0 ≤s ≤d)步量測預測容積點；

③ 根據式(28)～式(31)計算延遲 s(0 ≤s ≤d)步量測預測均值、協方差、互協方差和濾波增益；

④ 根據式(32)～式(33)計算第 s個子更新結果；

⑤ 根據式(34)～式(35)進行擴增狀態及其協方差更新；

⑥ 根據式(71)計算量測更新后的羅德里格斯參數，根據式(70)可見相對導航位置和速度可從擴增狀態中獲取。

4 仿真分析

為了驗證所提算法在處理隨機時延量測上的優越性，將開展數值仿真驗證，將ML-CKF 與現有CKF[9]、ORD-CKF[16]和MRD-CKF[17]進行比較。

4.1 仿真初始條件

慣性器件、視覺導航系統的采樣時間及濾波步長均為0.1 s，傳感器的偏差相關參數如表1 所示。

表1 慣性和視覺傳感器偏差參數Table 1 Deviation parameter of inertial and visual sensors

3 個特征光點即可保證慣性/視覺相對導航系統完全可觀測，為了提高相對導航精度，將特征光點數提高一倍，6 個特征光點位置分布如表2 所示。

表2 特征光點位置列表Table 2 List of beacon locations

假設長機運動在北-天-東坐標系內，初始時刻從經度 120°、緯度 40°和高度1 000 m 位置開始運動，僚機與長機形成編隊飛行，參考文獻[20]規劃典型軌跡，長機和僚機的位置變化方程如式(72)和式(73)所示，相應的運動軌跡如圖3 所示。

圖3 長機和僚機飛行軌跡Fig.3 Flight path of leader and follower

假設視覺量測最大延遲步數為3 步，無延遲概率 p(0,j)、延遲1 步概率 p(1,j)、延遲2 步概率 p(2,j)和延遲3 步概率 p(3,j)分別為

4.2 仿真結果與分析

取延遲概率 pd=0.3進行仿真，得到仿真結果如圖4～圖9 所示。

圖4 相對位置估計誤差Fig.4 Estimation error of relative position

圖4～圖6 分別為ML-CKF 估計得到的相對位置、相對速度和相對姿態誤差，從圖4～圖6 中曲線可知基于ML-CKF 的相對導航誤差曲線能夠快速收斂。圖7～圖9 是CKF、ORD-CKF、MRD-CKF和ML-CKF 算法100 次蒙特卡羅打靶仿真三維相對位置、速度、姿態誤差模值的比較結果。仿真結果表明，CKF 的估計精度最差，這是由于其未對延遲量測進行處理；MRD-CKF 和ML-CKF 的估計精度高于ORD-CKF，這是因為MRD-CKF 和ML-CKF能夠處理量測存在多步隨機時延問題，而ORDCKF 只能處理單步隨機延遲量測。此外，與MRDCKF 相比，ML-CKF 具有更高的估計精度，這歸功于ML-CKF 中的加權因子是在延遲概率的基礎上根據殘差實時調節的。如表3 所示4 種濾波算法單次計算耗時，從表3 中可見本文所提算法MLCKF 計算耗時低于MRD-CKF，高于CKF 和ORDCKF；但ML-CKF 的估計精度遠高于CKF 和ORDCKF；從估計精度和計算耗時綜合考慮角度出發，在工程應用中ML-CKF 是一種折中的選擇。

圖5 相對速度估計誤差Fig.5 Estimation error of relative velocity

圖6 相對姿態估計誤差Fig.6 Estimation error of relative attitude

圖7 相對位置估計精度對比Fig.7 Comparison of estimation accuracies of relative positions

圖8 相對速度估計精度對比Fig.8 Comparison of estimation accuracies of relative velocities

圖9 相對姿態估計精度對比Fig.9 Comparison of estimation accuracies of relative attitudes

表3 不同濾波算法的計算耗時Table 3 Single computation time of different filtering algorithms

5 結 論

1) 仿真結果表明，采用無約束三參數羅德里格斯參數表示姿態誤差來傳播和更新姿態，設計基于修正似然容積卡爾曼濾波的無人機編隊，相對導航濾波器在視覺延遲量測存在多步隨機延遲的情況下，也能精確估計出相對位置、速度和姿態。

2) 通過蒙特卡羅打靶仿真統計多次仿真試驗的均方根誤差，基于ML-CKF 的相對導航精度高于CKF、ORD-CKF 和MRD-CKF，驗證了所提算法的優越性。

3) 提出的基于ML-CKF 的視覺輔助慣性相對導航算法還可應用于無人機空中加油、衛星編隊相對導航和航天器交會對接等航空航天領域。