李 鵬,榮冬成,向宇翔,凌智琛,夏 珺
(湘潭大學(xué)自動化與電子信息學(xué)院,湖南湘潭 411100)
在組合導(dǎo)航系統(tǒng)中,高精度的濾波算法對導(dǎo)航定位的解算精度具有重要的影響。擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)雖然可以用于非線性系統(tǒng)濾波,但是其線性化環(huán)節(jié)會引入高截斷誤差,且Jacobi矩陣的計算會增加運算的難度,在實際工程應(yīng)用中效果不佳。無跡卡爾曼濾波(Unscented Kalman Filter,UKF)克服了EKF的局限性,基于無味變換(Unscented Transformation, UT)構(gòu)建了一系列Sigma點,以逼近狀態(tài)向量的后驗概率密度函數(shù),實現(xiàn)簡單且精度遠(yuǎn)高于EKF;但是UKF在系統(tǒng)狀態(tài)量發(fā)生突變的情況下魯棒性較差,其精度容易受到影響。容積卡爾曼濾波(Cubature Kalman Filter,CKF)利用三階球面徑向容積準(zhǔn)則,對概率密度函數(shù)進(jìn)行近似,相較于以上的濾波方法,CKF的精度和穩(wěn)定性都有所提高,為解決非線性和噪聲不確定問題提供了一個新的起點。
以上方法均為非線性系統(tǒng)濾波常用的方法,但普遍存在跟蹤能力不強和自適應(yīng)能力差的問題。在系統(tǒng)受到觀測值異常或者狀態(tài)量突變的影響時,濾波器容易產(chǎn)生精度下降的問題,甚至出現(xiàn)濾波發(fā)散的情況。文獻(xiàn)[6]提出了自適應(yīng)CKF方法,能夠有效增強系統(tǒng)的跟蹤能力,系統(tǒng)的觀測值異常或狀態(tài)量突變問題也得到了很好地解決。文獻(xiàn)[8]提出了一種抗差方法,能夠有效減弱波動較大的數(shù)據(jù)對于濾波器穩(wěn)定性的影響。
針對濾波跟蹤能力不強和自適應(yīng)能力差的問題,提出了一種改進(jìn)自適應(yīng)抗差CKF算法,在自適應(yīng)修正的基礎(chǔ)上使用抗差方法可以更好地減弱異常觀測值的影響,結(jié)合奇異值分解(Singular Value Decomposition,SVD)方法,可以使得濾波器更好地運行,改善了濾波效果,提高了濾波器的穩(wěn)定性。
超寬帶(Ultra-Wide Band,UWB)測量方程可表示為
=()+
(1)
其中,=[,,…,];=[,,];(·)為觀測方程;=[,1,…,,]是零均值高斯測量誤差噪聲,其協(xié)方差矩陣為。
慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)測量模型,通過慣性測量單元(Interial Measurement Unit, IMU)傳感器的測量角速度和測量加速度獲取目標(biāo)位姿信息,測量模型如下
=()+
(2)
非線性系統(tǒng)模型如下
(3)
新息是實際值與量測預(yù)測值之差,能比較直觀地反映出預(yù)測值偏離實際狀態(tài)的趨勢與程度。新息一般用于對系統(tǒng)進(jìn)行判定,并決定是否需要修正以及如何修正。
圖1 自適應(yīng)修正判決門限
(4)
修正系數(shù)()的選取需要遵循適度修正原則,避免過度修正。主要考慮兩方面:一是修正系數(shù)與系統(tǒng)量測精度有關(guān);二是與新息+1的維度有關(guān)。如新息中包含位置信息,位置信息對應(yīng)的修正系數(shù)()可以選取稍大一些,不應(yīng)超過0.01;速度的修正可以通過位置變化傳遞,則可以選取稍小些。
(5)
當(dāng)新息小于判決門限時,則系統(tǒng)不要修正,按照CKF程序正常濾波即可。
在自適應(yīng)修正的基礎(chǔ)上,引入抗差因子,減小異常觀測值對CKF過程的干擾。當(dāng)觀測信息精度很高時,需加大觀測值在狀態(tài)估計中的權(quán)值;反之,當(dāng)觀測信息誤差偏大時,需降低觀測值在狀態(tài)估計中的權(quán)重。
在系統(tǒng)得到新息之后,將新息進(jìn)行標(biāo)準(zhǔn)化得到抗差
=,+1,+1
(6)
其中,,+1為新息的第個分量;,+1為其標(biāo)準(zhǔn)差。
類似IGGIII等價權(quán)函數(shù)模型,對觀測抗差值進(jìn)行分類,分為3個等級進(jìn)行篩選,分別對應(yīng)三種不同的抗差因子
(7)
式中:、為常值,通常選取=15~20,=30~85;為標(biāo)準(zhǔn)化抗差。需要注意的是,抗差因子不能設(shè)置為0,否則可能會影響觀測向量協(xié)方差矩陣的迭代更新。
根據(jù)觀測抗差值引入抗差因子,對觀測噪聲協(xié)方差陣進(jìn)行修正,即
(8)
為了避免CKF算法中由于Cholesky分解導(dǎo)致的系統(tǒng)狀態(tài)協(xié)方差矩陣出現(xiàn)非正定的現(xiàn)象,可以采用SVD方式代替?zhèn)鹘y(tǒng)的Cholesky分解。
SVD方法,定義如下:
假定∈×(≥),則矩陣的SVD可以表示為
(9)
式中,∈×;∈×;∈×;=diag(,,…,),≥≥…≥≥0。的列向量為矩陣的左奇異向量,的列向量為矩陣的右奇異向量。
改進(jìn)自適應(yīng)抗差CKF算法步驟如下:
1)狀態(tài)參數(shù)初始化
2)計算容積點
(10)
式中,為狀態(tài)量的維數(shù),為容積點集,如下所示
(11)
式中,[1]代表單位矩陣。
3)傳播容積點
(12)
4)計算狀態(tài)量預(yù)測值及誤差協(xié)方差預(yù)測值
(13)
(14)
5)計算容積點
(15)
6)傳播容積點
(16)
其中,為系統(tǒng)測量函數(shù)。
7)計算測量預(yù)測值
(17)
8)計算新息
(18)
9)比較新息中的對應(yīng)變量與門限值,進(jìn)行狀態(tài)預(yù)測值修正
|+1|>
(19)
10)計算觀測預(yù)測協(xié)方差
(20)
11)計算互協(xié)方差
(21)
12)計算增益更新、狀態(tài)量、誤差協(xié)方差
(22)
(23)
(24)
13)若式(4)執(zhí)行,則需要根據(jù)式(5)估計誤差協(xié)方差陣進(jìn)行補償;若不執(zhí)行,則跳過式(5)。
本文設(shè)計了基于UWB與INS的融合定位仿真實驗,對自適應(yīng)抗差CKF融合算法的魯棒性能進(jìn)行評估。硬件由DWM1000模塊與IMU傳感器ADIS16465組成。實驗場地如圖2(a)所示。
在實驗環(huán)境中,4個基站高度相同均為2.05m?;咀鴺?biāo)為(3,0,2.05),基站坐標(biāo)為(3,3.9,2.05),基站坐標(biāo)為(0,3.9,2.05),基站坐標(biāo)為(0,0,2.05),單位為m。UWB標(biāo)簽與INS實際運動軌跡如圖2(b)所示,運行軌跡中設(shè)有障礙物,目的是有效驗證非視距處的UWB解算誤差情況。為了驗證算法的有效性,采用MATLAB進(jìn)行仿真分析。
(a)
如圖3所示,在仿真過程中,UWB受到障礙物以及實際環(huán)境中噪聲干擾的影響,其結(jié)果在障礙物附近和起點處數(shù)據(jù)穩(wěn)定性較差,整體穩(wěn)定性較低。
圖3 UWB定位結(jié)果
如圖4所示,將UWB與INS進(jìn)行松耦合,結(jié)合改進(jìn)自適應(yīng)抗差CKF算法進(jìn)行定位,在起點與障礙物處定位軌跡更加平滑、更接近實際運動軌跡。由此可以得出,該方法狀態(tài)估計更穩(wěn)定,系統(tǒng)魯棒性更好。
圖4 UWB/INS融合定位結(jié)果
同一場景下,對原始數(shù)據(jù)分別用EKF和CKF算法進(jìn)行數(shù)據(jù)融合,并與改進(jìn)自適應(yīng)抗差CKF數(shù)據(jù)融合誤差情況進(jìn)行對比。傳統(tǒng)EKF與CKF均降低了原始數(shù)據(jù)的整體誤差,平滑了誤差毛刺,提升了系統(tǒng)穩(wěn)定性。如圖5所示,改進(jìn)自適應(yīng)抗差CKF算法與傳統(tǒng)EKF和CKF相比,進(jìn)一步降低了系統(tǒng)整體誤差值,系統(tǒng)在0~100s和150~300s區(qū)間內(nèi)效果最明顯。改進(jìn)自適應(yīng)抗差CKF與其他幾種濾波方法相比,精度更優(yōu),系統(tǒng)穩(wěn)定性更好。
圖5 不同濾波方法數(shù)據(jù)融合誤差對比
各個濾波算法誤差數(shù)值分析如表1所示??梢钥吹剑瑐鹘y(tǒng)CKF相較于EKF均值誤差降低了2.0451cm,而改進(jìn)的方法較EKF均值誤差降低了6.8326cm。在此基礎(chǔ)上,所提方法最大誤差值相較于其他方法均有明顯降低,在系統(tǒng)出現(xiàn)異常時,減少了數(shù)值波動,進(jìn)一步提升了系統(tǒng)穩(wěn)定性。
表1 不同濾波方法誤差數(shù)值情況對比表
本文針對容積卡爾曼濾波在多源融合定位中存在跟蹤能力和自適應(yīng)能力差的問題,采用改進(jìn)的自適應(yīng)抗差CKF算法進(jìn)行仿真分析,并得到以下結(jié)論:
1)仿真結(jié)果表明,該算法能夠保持較高的濾波精度和數(shù)值穩(wěn)定性。
2)改進(jìn)的自適應(yīng)抗差CKF算法相較于傳統(tǒng)EKF算法平均誤差降低了28.42%;相較于傳統(tǒng)CKF算法平均誤差降低了23.268%。加入的自適應(yīng)修正和抗差因子減小了異常觀測值對傳統(tǒng)CKF的干擾,在障礙物附近處定位結(jié)果良好;用SVD代替CKF中的Cholesky分解,有效地提高了系統(tǒng)數(shù)值計算穩(wěn)定性,增強了在粗差干擾下的魯棒性。因此,改進(jìn)的自適應(yīng)抗差CKF算法提高了多源融合定位精度。