一種魯棒自適應(yīng)容積卡爾曼濾波方法及其在相對(duì)導(dǎo)航中的應(yīng)用

張旭， 崔乃剛， 王小剛， 崔祜濤， 秦武韜

(哈爾濱工業(yè)大學(xué) 航天學(xué)院， 黑龍江 哈爾濱 150001)

0 引言

無(wú)人機(jī)協(xié)同編隊(duì)作戰(zhàn)技術(shù)無(wú)疑是近年來(lái)引人矚目的高新技術(shù)之一。為使無(wú)人機(jī)之間保持特定的隊(duì)形實(shí)現(xiàn)通信與協(xié)調(diào)控制，需要獲得準(zhǔn)確的相對(duì)速度及相對(duì)位置信息。目前擴(kuò)展卡爾曼濾波(EKF)算法和無(wú)跡卡爾曼濾波(UKF)算法是解決相對(duì)導(dǎo)航問(wèn)題的主要途徑。EKF算法雖然廣泛應(yīng)用在非線性濾波中，但其存在一定的局限性，而UKF算法采用無(wú)跡變換對(duì)非線性概率密度分布進(jìn)行近似，相比EKF求雅克比矩陣計(jì)算量大的明顯不足，UKF算法收斂性好并能有效地改善濾波精度。為了更好地滿足相對(duì)導(dǎo)航精度的要求，有必要研究工程實(shí)現(xiàn)更簡(jiǎn)單、精度更高的濾波算法。容積卡爾曼濾波(CKF)算法[1-2]是近年來(lái)提出的一種新型非線性濾波算法，CKF算法與UKF算法相比，都屬于確定采樣型濾波算法，但CKF算法估計(jì)精度更高，實(shí)時(shí)性更好。傳統(tǒng)的CKF算法對(duì)于偏離真實(shí)概率分布的噪聲是敏感的，特別是一些具有“厚尾”的分布更加會(huì)引起濾波器性能不穩(wěn)定或影響估計(jì)精度。因此，一些學(xué)者將基于Huber的廣義最大似然估計(jì)理論[3-7]應(yīng)用到系統(tǒng)狀態(tài)估計(jì)問(wèn)題中。Huber技術(shù)是一種以混合l1/l2范數(shù)為代價(jià)函數(shù)的最優(yōu)估計(jì)方法，對(duì)偏離假定高斯分布的噪聲是魯棒的，但如果這些假設(shè)的參數(shù)與真實(shí)分布偏離很大時(shí)，同樣會(huì)導(dǎo)致濾波器有較大的誤差，甚至?xí)馂V波發(fā)散。

為了能夠在未知噪聲分布特性的情況下提高濾波性能，并減小濾波發(fā)散的可能性，引入自適應(yīng)技術(shù)[8-9]，與系統(tǒng)狀態(tài)一起實(shí)時(shí)地在線估計(jì)噪聲統(tǒng)計(jì)特性。目前，最常用的自適應(yīng)狀態(tài)估計(jì)方法是協(xié)方差匹配技術(shù)[10-11]，協(xié)方差匹配技術(shù)的核心思想是真實(shí)殘差協(xié)方差矩陣和理論預(yù)測(cè)協(xié)方差矩陣是相匹配的，真實(shí)協(xié)方差是對(duì)有限長(zhǎng)度滑動(dòng)窗口內(nèi)存儲(chǔ)殘差樣本點(diǎn)實(shí)時(shí)估計(jì)而得到的。

很少有學(xué)者將魯棒性估計(jì)與自適應(yīng)算法結(jié)合起來(lái)進(jìn)行狀態(tài)估計(jì)，本文將基于Huber技術(shù)的CKF估計(jì)與利用殘差序列實(shí)時(shí)估計(jì)、調(diào)整的自適應(yīng)噪聲統(tǒng)計(jì)特性估計(jì)兩種思想結(jié)合起來(lái)，提出了一種帶噪聲估計(jì)器的魯棒自適應(yīng)CKF算法，并應(yīng)用到無(wú)人機(jī)編隊(duì)相對(duì)導(dǎo)航問(wèn)題中。給出了編隊(duì)無(wú)人機(jī)相對(duì)導(dǎo)航模型，針對(duì)測(cè)量噪聲特性服從非高斯分布的情況，利用存儲(chǔ)殘差的樣本均值和樣本協(xié)方差估計(jì)測(cè)量噪聲和過(guò)程噪聲協(xié)方差矩陣的方法正好解決了魯棒性的問(wèn)題，最后進(jìn)行了仿真研究，對(duì)比了該算法與標(biāo)準(zhǔn)CKF算法及魯棒CKF算法的性能。

1 魯棒CKF算法

考慮如下離散非線性系統(tǒng)：

xk+1=f(xk)+vk，

(1)

zk=h(xk)+wk，

(2)

式中：xk為k時(shí)刻的系統(tǒng)狀態(tài)向量；zk為觀測(cè)向量；系統(tǒng)過(guò)程噪聲vk和量測(cè)噪聲wk為互不相關(guān)的高斯白噪聲序列，

(3)

Q為系統(tǒng)噪聲方差陣，R為量測(cè)噪聲方差陣，δkj為Kronecker-δ函數(shù)。

魯棒 CKF濾波算法具體步驟如下：

1)初始化。

0=E[x0],
P0=E[(x0-0)(x0-0)T],

(4)

2)計(jì)算容積點(diǎn)。

Xi,k-1|k-1=Sk-1|k-1ξi+k-1|k-1,i=1,2,…,2n，

(5)

3)時(shí)間更新。

傳播容積點(diǎn)：

(6)

狀態(tài)一步預(yù)測(cè)值：

(7)

(8)

4)量測(cè)更新。

Cholesky因式分解：

(9)

計(jì)算用于量測(cè)更新的容積點(diǎn)：

Xi,k|k-1=Sk|k-1ξi+k|k-1.

(10)

傳播容積點(diǎn)：

Zi,k|k-1=h(Xi,k|k-1).

(11)

量測(cè)的估計(jì)值為

(12)

協(xié)方差陣Pzz,k|k-1和互協(xié)方差陣Pxz,k|k-1分別為

(13)

(14)

預(yù)測(cè)誤差δk為

δk=xk-k|k-1，

(15)

式中：xk為k時(shí)刻狀態(tài)真實(shí)值；k|k-1為狀態(tài)預(yù)測(cè)值，

k|k-1=xk-δk.

(16)

量測(cè)方程近似為

zk≈k|k-1+Hkδk，

(17)

式中：Hk=((Pk|k-1)-1Pxz,k|k-1)T.

量測(cè)更新問(wèn)題轉(zhuǎn)換為線性回歸問(wèn)題的求解：

(18)

定義下列變量：

(19)

(20)

(21)

(22)

則線性回歸問(wèn)題可轉(zhuǎn)化為

(23)

魯棒CKF濾波量測(cè)更新轉(zhuǎn)化為求解代價(jià)函數(shù)的最小值：

(24)

式中：ζk,i為ζk的第i個(gè)分量，且ζk=Mkk|k-1-yk；m為ζk的維數(shù)；

(25)

γ為調(diào)節(jié)參數(shù)。ρ函數(shù)為最小l1、l2范數(shù)混合的形式。當(dāng)γ→0時(shí)，ρ函數(shù)趨近于l1范數(shù)最小；當(dāng)γ→∞時(shí)，ρ函數(shù)趨近于l2范數(shù)最小。利用ρ函數(shù)計(jì)算得到的估計(jì)值具有一定的魯棒性。

代價(jià)函數(shù)J最小的解滿足：

(26)

式中：φ(ζk,i)=ρ′(ζk,i). 定義函數(shù)ψ(ζk,i)和矩陣Ψ(ζk,i)分別為ψ(ζk,i)=φ(ζk,i)/ζk,i、Ψ(ζk,i)=diag[ψ(ζk,i)]，然后將ζk,i=(Mkk|k-1-yk)i代入(26)式得

(27)

(27)式的迭代解為

(28)

式中：c為迭代次數(shù)。

(29)

2 一種魯棒自適應(yīng)CKF方法

魯棒 CKF濾波對(duì)非線性問(wèn)題進(jìn)行濾波估計(jì)具有獨(dú)特的優(yōu)點(diǎn)，而且能克服CKF存在的一系列問(wèn)題。但針對(duì)魯棒CKF濾波在實(shí)際姿態(tài)測(cè)量傳感器測(cè)量噪聲統(tǒng)計(jì)特性不確定的情況下導(dǎo)致濾波精度下降及非魯棒性問(wèn)題，本文基于協(xié)方差匹配技術(shù)[10-11]，利用魯棒CKF濾波的殘差序列，設(shè)計(jì)了一種在線估計(jì)噪聲統(tǒng)計(jì)特性的魯棒自適應(yīng)CKF方法。

量測(cè)殘差序列主要是生成量測(cè)噪聲統(tǒng)計(jì)的估計(jì)值，狀態(tài)預(yù)測(cè)殘差主要是計(jì)算過(guò)程噪聲統(tǒng)計(jì)的估計(jì)值，利用長(zhǎng)度為N的滑動(dòng)窗口緩存器對(duì)量測(cè)噪聲和過(guò)程噪聲殘差進(jìn)行存儲(chǔ)，并對(duì)量測(cè)噪聲和過(guò)程噪聲的統(tǒng)計(jì)特性進(jìn)行計(jì)算。量測(cè)噪聲樣本協(xié)方差矩陣為

(30)

(31)

Cτ的期望值為

(32)

把(30)式代入到(32)式中，得到量測(cè)噪聲協(xié)方差矩陣的估計(jì)值為

(33)

(34)

式中：

(35)

(36)

最后通過(guò)量測(cè)噪聲和過(guò)程噪聲的初值及存儲(chǔ)殘差數(shù)據(jù)利用(33)式和(36)式更新協(xié)方差估計(jì)。

在計(jì)算量測(cè)噪聲和過(guò)程噪聲協(xié)方差矩陣的過(guò)程中，采用遺忘加權(quán)參數(shù)對(duì)接收到的量測(cè)數(shù)據(jù)進(jìn)行加權(quán)，其中最近接收到的量測(cè)數(shù)據(jù)獲得較大的權(quán)值。濾波器通過(guò)(37)式和(38)式實(shí)現(xiàn)：

(37)

(38)

3 相對(duì)導(dǎo)航模型

本文考慮采用領(lǐng)航- 跟隨模式的無(wú)人機(jī)雙機(jī)編隊(duì)控制策略。相對(duì)導(dǎo)航模型由相對(duì)姿態(tài)方程和相對(duì)質(zhì)心運(yùn)動(dòng)方程兩部分組成[12-14]。

3.1 相對(duì)姿態(tài)方程

采用歐拉軸/角(e1,e2,e3,θ)組成4元數(shù)表示姿態(tài)矩陣中的各元素：

(39)

式中：對(duì)于4元數(shù)q的分量，前三者稱為4元數(shù)的矢量部分，后者稱為標(biāo)量部分。

僚機(jī)與長(zhǎng)機(jī)之間的相對(duì)4元數(shù)qf|l表示為

(40)

式中：qf定義為僚機(jī)的姿態(tài)4元數(shù)；ql定義為長(zhǎng)機(jī)的姿態(tài)4元數(shù)；符號(hào)?表示4元數(shù)乘法。

因此，僚機(jī)與長(zhǎng)機(jī)之間的相對(duì)4元數(shù)滿足：

(41)

(42)

(43)

式中：p=[q1q2q3]T.

陀螺的測(cè)量模型為

(44)

3.2 相對(duì)質(zhì)心運(yùn)動(dòng)方程

(45)

對(duì)(45)式求1階導(dǎo)數(shù)、2階導(dǎo)數(shù)分別為

(46)

(47)

由于

(48)

可得

(49)

加速度計(jì)的測(cè)量模型為

(50)

3.3 量測(cè)方程

視覺(jué)導(dǎo)航系統(tǒng)主要包括視覺(jué)導(dǎo)航相機(jī)和特征光點(diǎn)兩部分，本文將視覺(jué)導(dǎo)航相機(jī)安裝在長(zhǎng)機(jī)上并將3個(gè)特征光點(diǎn)安置在僚機(jī)上，量測(cè)值即為長(zhǎng)機(jī)與僚機(jī)之間的相對(duì)視線矢量。(Xi,Yi,Zi)T是特征光點(diǎn)在僚機(jī)上的位置，i=1,2,3. (x,y,z)T是無(wú)人機(jī)之間相對(duì)位置矢量的3個(gè)分量。其中，單位視線矢量定義為

(51)

式中：

(52)

4 仿真分析

4.1 仿真條件

1)慣導(dǎo)系統(tǒng)及濾波初始條件。仿真實(shí)驗(yàn)的初始條件如表1所示。

表1 仿真初始條件

2)特征光點(diǎn)位置。由相對(duì)導(dǎo)航系統(tǒng)的可觀測(cè)性可知，至少需要3個(gè)特征光點(diǎn)，特征光點(diǎn)位置分布由表2給出。

表2 特征光點(diǎn)分布

3)視覺(jué)導(dǎo)航系統(tǒng)參數(shù)。視覺(jué)傳感器測(cè)量誤差來(lái)自混合的零均值高斯概率分布，受污染的噪聲概率密度函數(shù)表達(dá)式為

(53)

式中：ω為概率密度函數(shù)的自變量；ε為污染率；σv為視覺(jué)傳感器的測(cè)量噪聲方差，σi為污染噪聲方差，仿真中σv=3 500 μrad，σi=5σv.

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

圖1 相對(duì)姿態(tài)誤差Fig.1 Relative attitude errors

本文提出的魯棒自適應(yīng)濾波方法估計(jì)得到的相對(duì)姿態(tài)誤差、相對(duì)位置誤差和相對(duì)速度誤差如圖1～圖3所示。通過(guò)仿真結(jié)果可以看出該算法估計(jì)出的位置精度可以達(dá)到厘米級(jí)別，能夠?qū)ο到y(tǒng)過(guò)程噪聲和量測(cè)噪聲統(tǒng)計(jì)特性進(jìn)行實(shí)時(shí)估計(jì)，具有估計(jì)精度高、收斂速度快的特點(diǎn)。圖4給出了EKF算法、CKF算法、魯棒CKF算法和魯棒自適應(yīng)CKF算法4種濾波算法估計(jì)得到的位置誤差。采用這4種濾波算法求解得到的相對(duì)位置誤差分別為4.861 7 cm、3.496 6 cm、1.613 7 cm和0.681 6 cm. 從圖4可以看出，在視覺(jué)導(dǎo)航傳感器量測(cè)噪聲服從非高斯分布的情況下，魯棒自適應(yīng)CKF算法的估計(jì)精度比EKF算法、CKF算法和魯棒CKF算法有顯著的提升。

圖2 相對(duì)位置誤差Fig.2 Relative position errors

圖3 相對(duì)速度誤差Fig.3 Relative velocity errors

圖4 4種濾波方法相對(duì)位置誤差對(duì)比Fig.4 Comparation of relative position errors of four methods

5 結(jié)論

本文針對(duì)無(wú)人機(jī)編隊(duì)相對(duì)導(dǎo)航系統(tǒng)中視覺(jué)傳感器量測(cè)噪聲服從非高斯分布的問(wèn)題，提出一種混合魯棒/自適應(yīng)濾波方法，分析和仿真結(jié)果表明：

1) 利用Huber技術(shù)將標(biāo)準(zhǔn)CKF的量測(cè)更新步驟改為線性回歸問(wèn)題，對(duì)受污染的噪聲有較強(qiáng)的魯棒性。

2) 將基于Huber技術(shù)的魯棒性估計(jì)與自適應(yīng)噪聲統(tǒng)計(jì)特性估計(jì)結(jié)合起來(lái)，提出了一種魯棒自適應(yīng)CKF算法，解決了無(wú)人機(jī)編隊(duì)相對(duì)導(dǎo)航系統(tǒng)中視覺(jué)傳感器量測(cè)噪聲服從非高斯分布的問(wèn)題。

3) 對(duì)比標(biāo)準(zhǔn)CKF算法和魯棒CKF算法，提出的魯棒自適應(yīng)CKF算法對(duì)非高斯量測(cè)噪聲具有較強(qiáng)的自適應(yīng)性，能夠獲取更優(yōu)的相對(duì)導(dǎo)航參數(shù)，且具有更強(qiáng)的魯棒性。

)

[1] Wu H, Chen B F, Yang B F, et, al. Robust derivative-free cubature Kalman filter for bearings-only tracking[J]. Journal of Guidance, Control, and Dynamics, 2016, 39(8):1865-1870.

[2] 孫楓, 唐李軍. 基于 cubature Kalman filter的INS/GPS組合導(dǎo)航濾波算法[J]. 控制與決策, 2012, 27(7):1032-1036.

SUN Feng, TANG Li-jun. INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J]. Control and Decision, 2012, 27(7):1032-1036. (in Chinese)

[3] 黃玉, 武立華, 孫楓. 基于Huber M估計(jì)的魯棒Cubature卡爾曼濾波算法[J]. 控制與決策, 2014, 29(3):572-576.

HUANG Yu, WU Li-hua, SUN Feng. Robust cubature Kalman filter based on Huber M estimator[J]. Control and Decision, 2014, 29(3):572-576. (in Chinese)

[4] 秦康, 董新民, 陳勇. 基于Huber的魯棒高階容積卡爾曼濾波算法[J]. 計(jì)算機(jī)工程與應(yīng)用, 2017, 53(7):21-29.

QIN Kang, DONG Xin-min, CHEN Yong. Huber-based robust high-degree cubature Kalman filter algorithm[J]. Computer Engineering and Applications, 2017,53(7):21-29. (in Chinese)

[5] 王小剛, 路菲, 崔乃剛. Huber-based濾波及其在相對(duì)導(dǎo)航問(wèn)題中的應(yīng)用[J]. 控制與決策, 2014, 25(2):287-290.

WANG Xiao-gang, LU Fei, CUI Nai-gang. Huber-based filter and its application to relative navigation[J]. Control and Decision, 2014, 25(2):287-290. (in Chinese)

[6] 崔乃剛, 張龍, 王小剛, 等. 自適應(yīng)高階容積卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用[J]. 航空學(xué)報(bào), 2015, 36(12):3885-3895.

CUI Nai-gang, ZHANG Long, WANG Xiao-gang, et al. Application of adaptive high-degree cubature Kalman filter in target tracking[J]. Acta Aeronautica et Astronautica Sinica, 2015, 36(12):3885-3895.(in Chinese)

[7] 高敬東, 李開(kāi)龍, 常路賓. 基于Huber的改進(jìn)魯棒濾波算[J]. 系統(tǒng)仿真學(xué)報(bào), 2014, 26(8):1769-1774.

GAO Jing-dong, LI Kai-long, CHANG Lu-bin. Huber-based modified robust filter algorithm[J]. Journal of System Simulation, 2014, 26(8):1769-1774. (in Chinese)

[8] 王潔, 熊智, 邢麗, 等. 基于新息自適應(yīng)濾波的慣性測(cè)量單元誤差在線標(biāo)定方法研究[J]. 兵工學(xué)報(bào), 2016, 37(7):1203-1213.

WANG Jie, XIONG Zhi, XING Li, et al. Online calibration of IMU errors of inertial navigation system based on innovation-based adaptive filtering[J]. Acta Armamentarii, 2016, 37(7):1203-1213. (in Chinese)

[9] 丁家琳, 肖建. 基于極大后驗(yàn)估計(jì)的自適應(yīng)容積卡爾曼濾波器[J]. 控制與決策, 2014, 29(2):327- 334.

DING Jia-lin, XIAO Jian. Design of adaptive cubature Kalman filter based on maximum a posteriori estimation [J]. Control and Decision, 2014, 29(2):327-334. (in Chinese)

[10] Karlgaard C D, Schaub H. Adaptive nonlinear Huber-based navigation for rendezvous in elliptical orbit[J]. Journal of Guidance, Control, and Dynamics, 2011, 34(2):388-402.

[11] 于浛, 魏喜慶, 宋申民, 等. 基于自適應(yīng)容積卡爾曼濾波的非合作航天器相對(duì)運(yùn)動(dòng)估計(jì)[J]. 航空學(xué)報(bào), 2014, 35(8):2251-2260.

YU Han, WEI Xi-qing, SONG Shen-min, et al. Relative motion estimation of non-cooperative spacecraft based on adaptive CKF[J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(8):2251-2260. (in Chinese)

[12] 王小剛, 郭繼峰, 崔乃剛. 一種魯棒Sigma-point濾波算法及其在相對(duì)導(dǎo)航中的應(yīng)用[J]. 航空學(xué)報(bào), 2010, 31(5): 1024-1029.

WANG Xiao-gang, GUO Ji-feng, CUI Nai-gang. Robust sigma-point filtering and its application to relative navigation[J]. Acta Aeronautica et Astronautica Sinica, 2010, 31(5):1024-1029.(in Chinese)

[13] 王小剛. 非線性濾波方法在無(wú)人機(jī)相對(duì)導(dǎo)航上的應(yīng)用[D]. 哈爾濱：哈爾濱工業(yè)大學(xué), 2010:18-33.

WANG Xiao-gang. The application of nonlinear filtering to UAV relative navigation[D]. Harbin: Harbin Institute of Technology, 2010:18-33. (in Chinese)

[14] 曲法義, 王小剛, 崔乃剛, 等. 基于慣導(dǎo)/GPS/視覺(jué)的無(wú)人機(jī)容錯(cuò)相對(duì)導(dǎo)航方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 21(6): 781-785.

QU Fa-yi, WANG Xiao-gang, CUI Nai-gang, et al. Fault-tolerance relative navigation approach based on SIN/GPS/Vision for UVA[J]. Journal of Inertial Technology, 2010, 21(6):781-785.(in Chinese)