李 鵬，榮冬成，向宇翔，凌智琛，夏 珺

(湘潭大學(xué)自動化與電子信息學(xué)院，湖南湘潭 411100)

0 引言

在組合導(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)定性。

1 系統(tǒng)狀態(tài)方程和觀測方程

1.1 超寬帶測量方程

超寬帶(Ultra-Wide Band，UWB)測量方程可表示為

=()+

(1)

其中，=[,,…,]；=[,,]；(·)為觀測方程；=[,1,…,,]是零均值高斯測量誤差噪聲，其協(xié)方差矩陣為。

1.2 慣性導(dǎo)航系統(tǒng)測量方程

慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)測量模型，通過慣性測量單元(Interial Measurement Unit, IMU)傳感器的測量角速度和測量加速度獲取目標(biāo)位姿信息，測量模型如下

=()+

(2)

1.3 UWB/INS融合定位方程

非線性系統(tǒng)模型如下

(3)

2 改進(jìn)自適應(yīng)抗差CKF算法

2.1 新息自適應(yīng)修正

新息是實際值與量測預(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程序正常濾波即可。

2.2 抗差因子

在自適應(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)

2.3 SVD

為了避免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。的列向量為矩陣的左奇異向量，的列向量為矩陣的右奇異向量。

2.4 改進(jìn)的自適應(yīng)抗差CKF算法

改進(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)。

3 實驗分析

本文設(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ù)值情況對比表

4 結(jié)論

本文針對容積卡爾曼濾波在多源融合定位中存在跟蹤能力和自適應(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算法提高了多源融合定位精度。