基于修正似然濾波的無(wú)人機(jī)編隊(duì)相對(duì)導(dǎo)航方法

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

(1.中國(guó)直升機(jī)設(shè)計(jì)研究所，天津 300300；2.陸軍裝備部駐天津地區(qū)航空軍代室，天津 300384)

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

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

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

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

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

1.1 問(wèn)題闡述及預(yù)處理

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

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

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

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

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

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

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

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

1.2.2 傳播預(yù)測(cè)分布

假設(shè)先驗(yàn)分布 p(Xk-1|y1:k-1)和狀態(tài)傳遞分布p(Xk|Xk-1)均為高斯分布，根據(jù)Chapman-Kolmogorov方程，可得預(yù)測(cè)分布：

1.2.3 更新后驗(yàn)分布

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

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

采用三階球面-徑向容積準(zhǔn)則計(jì)算修正似然非線性高斯濾波中的高斯加權(quán)積分，詳細(xì)給出修正似然容積卡爾曼濾波算法的遞推計(jì)算更新過(guò)程。

1.3.1 狀態(tài)預(yù)測(cè)

根據(jù)式(5)，可得k -1時(shí) 刻的擴(kuò)增狀態(tài)估計(jì)X?k-1/k-1及其協(xié)方差 Pk-1/k-1：

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

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

2 相對(duì)導(dǎo)航系統(tǒng)建模

以僚機(jī)本體系為基準(zhǔn)建立僚機(jī)和長(zhǎng)機(jī)之間的相對(duì)姿態(tài)、位置和速度方程，以長(zhǎng)機(jī)上的導(dǎo)航相機(jī)與僚機(jī)上特征光點(diǎn)之間的視線矢量作為量測(cè)量，將長(zhǎng)機(jī)上的慣性和視覺(jué)量測(cè)信息傳輸給僚機(jī)進(jìn)行融合估計(jì)得到僚機(jī)和長(zhǎng)機(jī)之間的相對(duì)運(yùn)動(dòng)信息。

2.1 相對(duì)導(dǎo)航方程

2.1.1 相對(duì)姿態(tài)運(yùn)動(dòng)方程

采用工程實(shí)用性強(qiáng)的四元數(shù)表示姿態(tài)，即

2.2 相對(duì)導(dǎo)航傳感器測(cè)量模型

2.2.1 慣性導(dǎo)航系統(tǒng)測(cè)量模型

加速度計(jì)和陀螺儀是慣性導(dǎo)航系統(tǒng)中重要的傳感器，其測(cè)量模型分別為

3 相對(duì)導(dǎo)航濾波器設(shè)計(jì)

3.1 狀態(tài)向量

若直接把受歸一化約束的四元數(shù)作為狀態(tài)分量，存在四元數(shù)姿態(tài)協(xié)方差陣奇異性問(wèn)題，因此，采用一種無(wú)約束的三參數(shù)羅德里格斯參數(shù)表示姿態(tài)誤差來(lái)傳播和更新姿態(tài)四元數(shù)，進(jìn)行濾波器設(shè)計(jì)。

定義狀態(tài)矢量：

2.2.2 視覺(jué)導(dǎo)航系統(tǒng)測(cè)量模型

視覺(jué)導(dǎo)航系統(tǒng)的量測(cè)量是長(zhǎng)機(jī)上導(dǎo)航相機(jī)與僚機(jī)上特征光點(diǎn)之間的視線矢量，其測(cè)量原理如圖2

3.2 狀態(tài)預(yù)測(cè)

k-1時(shí)刻相對(duì)導(dǎo)航濾波器的擴(kuò)增狀態(tài)為

將式(64)～式(66)和式(23)～式(26)代入式(20)中可得到擴(kuò)增狀態(tài)預(yù)測(cè)及其協(xié)方差。

3.3 量測(cè)更新

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

1) 狀態(tài)預(yù)測(cè)。

① 根據(jù)式(21)計(jì)算狀態(tài)預(yù)測(cè)容積點(diǎn)；

② 根據(jù)式(54)～式(57)計(jì)算姿態(tài)四元數(shù)誤差；

③ 根據(jù)式(58)和式(59)計(jì)算狀態(tài)預(yù)測(cè)所需的姿態(tài)四元數(shù)容積點(diǎn)；

④ 根據(jù)式(60)、式(65)和式(66)進(jìn)行相對(duì)姿態(tài)、相對(duì)位置、相對(duì)速度、加速度計(jì)漂移和陀螺儀漂移遞推；

⑤ 根據(jù)式(64)計(jì)算經(jīng)過(guò)狀態(tài)遞推后的羅德里格參數(shù)估計(jì)值；

⑥ 根據(jù)式(23)～式(26)和式(20)計(jì)算擴(kuò)增狀態(tài)及其協(xié)方差。

2) 量測(cè)更新。

① 根據(jù)式(27)計(jì)算量測(cè)更新所需容積點(diǎn)；

② 根據(jù)式(67)計(jì)算延遲 s(0 ≤s ≤d)步量測(cè)預(yù)測(cè)容積點(diǎn)；

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

④ 根據(jù)式(32)～式(33)計(jì)算第 s個(gè)子更新結(jié)果；

⑤ 根據(jù)式(34)～式(35)進(jìn)行擴(kuò)增狀態(tài)及其協(xié)方差更新；

⑥ 根據(jù)式(71)計(jì)算量測(cè)更新后的羅德里格斯參數(shù)，根據(jù)式(70)可見(jiàn)相對(duì)導(dǎo)航位置和速度可從擴(kuò)增狀態(tài)中獲取。

4 仿真分析

為了驗(yàn)證所提算法在處理隨機(jī)時(shí)延量測(cè)上的優(yōu)越性，將開(kāi)展數(shù)值仿真驗(yàn)證，將ML-CKF 與現(xiàn)有CKF[9]、ORD-CKF[16]和MRD-CKF[17]進(jìn)行比較。

4.1 仿真初始條件

慣性器件、視覺(jué)導(dǎo)航系統(tǒng)的采樣時(shí)間及濾波步長(zhǎng)均為0.1 s，傳感器的偏差相關(guān)參數(shù)如表1 所示。

表1 慣性和視覺(jué)傳感器偏差參數(shù)Table 1 Deviation parameter of inertial and visual sensors

3 個(gè)特征光點(diǎn)即可保證慣性/視覺(jué)相對(duì)導(dǎo)航系統(tǒng)完全可觀測(cè)，為了提高相對(duì)導(dǎo)航精度，將特征光點(diǎn)數(shù)提高一倍，6 個(gè)特征光點(diǎn)位置分布如表2 所示。

表2 特征光點(diǎn)位置列表Table 2 List of beacon locations

假設(shè)長(zhǎng)機(jī)運(yùn)動(dòng)在北-天-東坐標(biāo)系內(nèi)，初始時(shí)刻從經(jīng)度 120°、緯度 40°和高度1 000 m 位置開(kāi)始運(yùn)動(dòng)，僚機(jī)與長(zhǎng)機(jī)形成編隊(duì)飛行，參考文獻(xiàn)[20]規(guī)劃典型軌跡，長(zhǎng)機(jī)和僚機(jī)的位置變化方程如式(72)和式(73)所示，相應(yīng)的運(yùn)動(dòng)軌跡如圖3 所示。

圖3 長(zhǎng)機(jī)和僚機(jī)飛行軌跡Fig.3 Flight path of leader and follower

假設(shè)視覺(jué)量測(cè)最大延遲步數(shù)為3 步，無(wú)延遲概率 p(0,j)、延遲1 步概率 p(1,j)、延遲2 步概率 p(2,j)和延遲3 步概率 p(3,j)分別為

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

取延遲概率 pd=0.3進(jìn)行仿真，得到仿真結(jié)果如圖4～圖9 所示。

圖4 相對(duì)位置估計(jì)誤差Fig.4 Estimation error of relative position

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

圖5 相對(duì)速度估計(jì)誤差Fig.5 Estimation error of relative velocity

圖6 相對(duì)姿態(tài)估計(jì)誤差Fig.6 Estimation error of relative attitude

圖7 相對(duì)位置估計(jì)精度對(duì)比Fig.7 Comparison of estimation accuracies of relative positions

圖8 相對(duì)速度估計(jì)精度對(duì)比Fig.8 Comparison of estimation accuracies of relative velocities

圖9 相對(duì)姿態(tài)估計(jì)精度對(duì)比Fig.9 Comparison of estimation accuracies of relative attitudes

表3 不同濾波算法的計(jì)算耗時(shí)Table 3 Single computation time of different filtering algorithms

5 結(jié) 論

1) 仿真結(jié)果表明，采用無(wú)約束三參數(shù)羅德里格斯參數(shù)表示姿態(tài)誤差來(lái)傳播和更新姿態(tài)，設(shè)計(jì)基于修正似然容積卡爾曼濾波的無(wú)人機(jī)編隊(duì)，相對(duì)導(dǎo)航濾波器在視覺(jué)延遲量測(cè)存在多步隨機(jī)延遲的情況下，也能精確估計(jì)出相對(duì)位置、速度和姿態(tài)。

2) 通過(guò)蒙特卡羅打靶仿真統(tǒng)計(jì)多次仿真試驗(yàn)的均方根誤差，基于ML-CKF 的相對(duì)導(dǎo)航精度高于CKF、ORD-CKF 和MRD-CKF，驗(yàn)證了所提算法的優(yōu)越性。

3) 提出的基于ML-CKF 的視覺(jué)輔助慣性相對(duì)導(dǎo)航算法還可應(yīng)用于無(wú)人機(jī)空中加油、衛(wèi)星編隊(duì)相對(duì)導(dǎo)航和航天器交會(huì)對(duì)接等航空航天領(lǐng)域。