改進(jìn)自適應(yīng)抗差容積卡爾曼濾波多源室內(nèi)定位

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

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

0 引言

在組合導(dǎo)航系統(tǒng)中，高精度的濾波算法對(duì)導(dǎo)航定位的解算精度具有重要的影響。擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)雖然可以用于非線性系統(tǒng)濾波，但是其線性化環(huán)節(jié)會(huì)引入高截?cái)嗾`差，且Jacobi矩陣的計(jì)算會(huì)增加運(yùn)算的難度，在實(shí)際工程應(yīng)用中效果不佳。無(wú)跡卡爾曼濾波(Unscented Kalman Filter，UKF)克服了EKF的局限性，基于無(wú)味變換(Unscented Transformation， UT)構(gòu)建了一系列Sigma點(diǎn)，以逼近狀態(tài)向量的后驗(yàn)概率密度函數(shù)，實(shí)現(xiàn)簡(jiǎn)單且精度遠(yuǎn)高于EKF；但是UKF在系統(tǒng)狀態(tài)量發(fā)生突變的情況下魯棒性較差，其精度容易受到影響。容積卡爾曼濾波(Cubature Kalman Filter，CKF)利用三階球面徑向容積準(zhǔn)則，對(duì)概率密度函數(shù)進(jìn)行近似，相較于以上的濾波方法，CKF的精度和穩(wěn)定性都有所提高，為解決非線性和噪聲不確定問題提供了一個(gè)新的起點(diǎn)。

以上方法均為非線性系統(tǒng)濾波常用的方法，但普遍存在跟蹤能力不強(qiáng)和自適應(yīng)能力差的問題。在系統(tǒng)受到觀測(cè)值異常或者狀態(tài)量突變的影響時(shí)，濾波器容易產(chǎn)生精度下降的問題，甚至出現(xiàn)濾波發(fā)散的情況。文獻(xiàn)[6]提出了自適應(yīng)CKF方法，能夠有效增強(qiáng)系統(tǒng)的跟蹤能力，系統(tǒng)的觀測(cè)值異常或狀態(tài)量突變問題也得到了很好地解決。文獻(xiàn)[8]提出了一種抗差方法，能夠有效減弱波動(dòng)較大的數(shù)據(jù)對(duì)于濾波器穩(wěn)定性的影響。

針對(duì)濾波跟蹤能力不強(qiáng)和自適應(yīng)能力差的問題，提出了一種改進(jìn)自適應(yīng)抗差CKF算法，在自適應(yīng)修正的基礎(chǔ)上使用抗差方法可以更好地減弱異常觀測(cè)值的影響，結(jié)合奇異值分解(Singular Value Decomposition，SVD)方法，可以使得濾波器更好地運(yùn)行，改善了濾波效果，提高了濾波器的穩(wěn)定性。

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

1.1 超寬帶測(cè)量方程

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

=()+

(1)

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

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

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

=()+

(2)

1.3 UWB/INS融合定位方程

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

(3)

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

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

新息是實(shí)際值與量測(cè)預(yù)測(cè)值之差，能比較直觀地反映出預(yù)測(cè)值偏離實(shí)際狀態(tài)的趨勢(shì)與程度。新息一般用于對(duì)系統(tǒng)進(jìn)行判定，并決定是否需要修正以及如何修正。

圖1 自適應(yīng)修正判決門限

(4)

修正系數(shù)()的選取需要遵循適度修正原則，避免過度修正。主要考慮兩方面：一是修正系數(shù)與系統(tǒng)量測(cè)精度有關(guān)；二是與新息+1的維度有關(guān)。如新息中包含位置信息，位置信息對(duì)應(yīng)的修正系數(shù)()可以選取稍大一些，不應(yīng)超過0.01;速度的修正可以通過位置變化傳遞，則可以選取稍小些。

(5)

當(dāng)新息小于判決門限時(shí)，則系統(tǒng)不要修正，按照CKF程序正常濾波即可。

2.2 抗差因子

在自適應(yīng)修正的基礎(chǔ)上，引入抗差因子，減小異常觀測(cè)值對(duì)CKF過程的干擾。當(dāng)觀測(cè)信息精度很高時(shí)，需加大觀測(cè)值在狀態(tài)估計(jì)中的權(quán)值；反之，當(dāng)觀測(cè)信息誤差偏大時(shí)，需降低觀測(cè)值在狀態(tài)估計(jì)中的權(quán)重。

在系統(tǒng)得到新息之后，將新息進(jìn)行標(biāo)準(zhǔn)化得到抗差

=,+1,+1

(6)

其中，,+1為新息的第個(gè)分量；,+1為其標(biāo)準(zhǔn)差。

類似IGGIII等價(jià)權(quán)函數(shù)模型，對(duì)觀測(cè)抗差值進(jìn)行分類，分為3個(gè)等級(jí)進(jìn)行篩選，分別對(duì)應(yīng)三種不同的抗差因子

(7)

式中：、為常值，通常選取=15～20，=30～85；為標(biāo)準(zhǔn)化抗差。需要注意的是，抗差因子不能設(shè)置為0，否則可能會(huì)影響觀測(cè)向量協(xié)方差矩陣的迭代更新。

根據(jù)觀測(cè)抗差值引入抗差因子，對(duì)觀測(cè)噪聲協(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)計(jì)算容積點(diǎn)

(10)

式中，為狀態(tài)量的維數(shù)，為容積點(diǎn)集，如下所示

(11)

式中，[1]代表單位矩陣。

3)傳播容積點(diǎn)

(12)

4)計(jì)算狀態(tài)量預(yù)測(cè)值及誤差協(xié)方差預(yù)測(cè)值

(13)

(14)

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

(15)

6)傳播容積點(diǎn)

(16)

其中，為系統(tǒng)測(cè)量函數(shù)。

7)計(jì)算測(cè)量預(yù)測(cè)值

(17)

8)計(jì)算新息

(18)

9)比較新息中的對(duì)應(yīng)變量與門限值，進(jìn)行狀態(tài)預(yù)測(cè)值修正

|+1|>

(19)

10)計(jì)算觀測(cè)預(yù)測(cè)協(xié)方差

(20)

11)計(jì)算互協(xié)方差

(21)

12)計(jì)算增益更新、狀態(tài)量、誤差協(xié)方差

(22)

(23)

(24)

13)若式(4)執(zhí)行，則需要根據(jù)式(5)估計(jì)誤差協(xié)方差陣進(jìn)行補(bǔ)償；若不執(zhí)行，則跳過式(5)。

3 實(shí)驗(yàn)分析

本文設(shè)計(jì)了基于UWB與INS的融合定位仿真實(shí)驗(yàn)，對(duì)自適應(yīng)抗差CKF融合算法的魯棒性能進(jìn)行評(píng)估。硬件由DWM1000模塊與IMU傳感器ADIS16465組成。實(shí)驗(yàn)場(chǎng)地如圖2(a)所示。

在實(shí)驗(yàn)環(huán)境中，4個(gè)基站高度相同均為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實(shí)際運(yùn)動(dòng)軌跡如圖2(b)所示，運(yùn)行軌跡中設(shè)有障礙物，目的是有效驗(yàn)證非視距處的UWB解算誤差情況。為了驗(yàn)證算法的有效性，采用MATLAB進(jìn)行仿真分析。

(a)

如圖3所示，在仿真過程中，UWB受到障礙物以及實(shí)際環(huán)境中噪聲干擾的影響，其結(jié)果在障礙物附近和起點(diǎn)處數(shù)據(jù)穩(wěn)定性較差，整體穩(wěn)定性較低。

圖3 UWB定位結(jié)果

如圖4所示，將UWB與INS進(jìn)行松耦合，結(jié)合改進(jìn)自適應(yīng)抗差CKF算法進(jìn)行定位，在起點(diǎn)與障礙物處定位軌跡更加平滑、更接近實(shí)際運(yùn)動(dòng)軌跡。由此可以得出，該方法狀態(tài)估計(jì)更穩(wěn)定，系統(tǒng)魯棒性更好。

圖4 UWB/INS融合定位結(jié)果

同一場(chǎng)景下，對(duì)原始數(shù)據(jù)分別用EKF和CKF算法進(jìn)行數(shù)據(jù)融合，并與改進(jìn)自適應(yīng)抗差CKF數(shù)據(jù)融合誤差情況進(jìn)行對(duì)比。傳統(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ù)融合誤差對(duì)比

各個(gè)濾波算法誤差數(shù)值分析如表1所示。可以看到，傳統(tǒng)CKF相較于EKF均值誤差降低了2.0451cm，而改進(jìn)的方法較EKF均值誤差降低了6.8326cm。在此基礎(chǔ)上，所提方法最大誤差值相較于其他方法均有明顯降低，在系統(tǒng)出現(xiàn)異常時(shí)，減少了數(shù)值波動(dòng)，進(jìn)一步提升了系統(tǒng)穩(wěn)定性。

表1 不同濾波方法誤差數(shù)值情況對(duì)比表

4 結(jié)論

本文針對(duì)容積卡爾曼濾波在多源融合定位中存在跟蹤能力和自適應(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)修正和抗差因子減小了異常觀測(cè)值對(duì)傳統(tǒng)CKF的干擾，在障礙物附近處定位結(jié)果良好；用SVD代替CKF中的Cholesky分解，有效地提高了系統(tǒng)數(shù)值計(jì)算穩(wěn)定性，增強(qiáng)了在粗差干擾下的魯棒性。因此，改進(jìn)的自適應(yīng)抗差CKF算法提高了多源融合定位精度。