李雅梅， 康璐璐

(遼寧工程技術(shù)大學(xué) 電氣與控制學(xué)院,遼寧 葫蘆島 125000)

多新息抗差—自適應(yīng)卡爾曼濾波定位算法研究*

李雅梅， 康璐璐

(遼寧工程技術(shù)大學(xué) 電氣與控制學(xué)院,遼寧 葫蘆島 125000)

針對移動機(jī)器人在定位過程中，由傳感器測量誤差和機(jī)器人模型引起的位姿誤差導(dǎo)致系統(tǒng)定位精度急劇下降的問題，提出了一種多新息卡爾曼濾波算法。在標(biāo)準(zhǔn)卡爾曼濾波的基礎(chǔ)上，當(dāng)傳感器測量值存在誤差時，引入抗差權(quán)因子，通過改變誤差測量值的權(quán)值提高濾波器的估計(jì)精度；當(dāng)機(jī)器人位姿存在誤差時，引入自適應(yīng)因子，通過調(diào)整狀態(tài)協(xié)方差矩陣的大小抵制位姿誤差引起的濾波發(fā)散。同時，引入了多新息，即多個時刻的新息向量，進(jìn)一步提高此非線性系統(tǒng)的精度。實(shí)驗(yàn)表明：當(dāng)存在測量誤差和位姿誤差時，該濾波算法能有效提高定位精度。

抗差濾波; 自適應(yīng)卡爾曼濾波; 權(quán)因子; 自適應(yīng)因子; 多新息

0 引 言

移動機(jī)器人的定位指機(jī)器人在運(yùn)動過程中根據(jù)所處環(huán)境的特征(路標(biāo))確定所處的位置[1]。解決定位誤差問題最早最常用的方法就是擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)算法。針對傳統(tǒng)EKF算法的多種缺陷，近年來,提出了很多改進(jìn)算法。比如精密單點(diǎn)定位的抗差卡爾曼濾波根據(jù)精密單點(diǎn)定位的內(nèi)部可靠性和觀測值的隨機(jī)性來改進(jìn)定位精度[2]；無跡粒子濾波能較好地利用觀測值提供的信息[3]。

根據(jù)定位過程中產(chǎn)生異常值的2個方面：機(jī)器人位置誤差和姿勢誤差，提出了多新息抗差—自適應(yīng)卡爾曼濾波。由于機(jī)器人的運(yùn)動和傳感器測量的過程中均存在不可避免的噪聲，其中兩個重要的噪聲就是測量粗差和位姿誤差[4]。為減小誤差的影響，在標(biāo)準(zhǔn)卡爾曼濾波的基礎(chǔ)上，引入了基于最小二乘權(quán)因子抗差估計(jì)和基于假設(shè)檢驗(yàn)的自適應(yīng)因子，構(gòu)造抗差—自適應(yīng)卡爾曼濾波，對系統(tǒng)進(jìn)行了更精確估計(jì)。機(jī)器人自主定位系統(tǒng)很明顯是一個非線性系統(tǒng)，因此，本文在以前單新息理論的基礎(chǔ)上引入多新息理論，實(shí)現(xiàn)在強(qiáng)非線性系統(tǒng)中的精確定位[5]。

1 濾波算法

1.1 卡爾曼濾波

標(biāo)準(zhǔn)卡爾曼濾波的公式如下

Xk,k-1=φk,k-1Xk-1

(1)

(2)

(3)

Pk=(I-kkHk)Pk,k-1

(4)

Xk=Xk,k-1+kk(Zk-HkXk,k-1)

(5)

式中Xk-1為k-1時刻的狀態(tài)估計(jì);Xk,k-1為k-1時刻的狀態(tài)預(yù)報(bào)值;Pk-1為k-1時刻的狀態(tài)協(xié)方差矩陣;Pk,k-1為Xk,k-1的狀態(tài)協(xié)方差矩陣;Kk為k時刻的卡爾曼濾波增益矩陣;Pk為k時刻測量更新的狀態(tài)協(xié)方差矩陣;Xk為k時刻測量更新的狀態(tài)估值。

1.2 結(jié)合抗差與自適應(yīng)卡爾曼濾波

1.2.1 抗差估計(jì)

當(dāng)觀測值存在粗差時，抗差估計(jì)將粗差歸入隨機(jī)模型，通過調(diào)節(jié)隨機(jī)模型中觀測值方差的大小來調(diào)整粗差影響[6]。設(shè)方差膨脹公式為

(6)

式中H0為觀測值中無誤差；H1為含有粗差。含有粗差的觀測值的方差擴(kuò)大k倍，其相應(yīng)觀測值的權(quán)值也縮小k2倍,則抗差估計(jì)通過減小粗差觀測值的權(quán)重減小誤差。

在卡爾曼濾波中，測量新息定義為狀態(tài)預(yù)估值和當(dāng)前觀測值的函數(shù)，可表示為

ek=Zk-HkXk,k-1

(7)

(8)

令

TD=χ2(nk)

(9)

若rk

(10)

令

β=diag[β1β2…βn],

0<βi≤1,i=1,2,…,n

(11)

于是，相應(yīng)的等價抗差協(xié)方差矩陣為

(12)

則式(3)可改為

(13)

當(dāng)系統(tǒng)存在粗差時，抗差卡爾曼濾波通過增大相應(yīng)觀測噪聲方差，減小觀測值增益，提高了系統(tǒng)的估計(jì)精度。

1.2.2 自適應(yīng)因子

為了抵制動力學(xué)模型異常產(chǎn)生的誤差，在標(biāo)準(zhǔn)卡爾曼濾波的基礎(chǔ)上引入自適應(yīng)因子α，并將α作用于k時刻測量更新的狀態(tài)協(xié)方差矩陣。式(2)可更新為

(14)

為了得到自適應(yīng)因子α的值，作如下計(jì)算：

利用式(7)可得

(15)

由式(8)、式(9)可知,若rk

(16)

將式(15)代入式(16)有

(17)

為了解上式構(gòu)造函數(shù)

(18)

上式在[1,+∞)為一個凸函數(shù)，因此，采用最陡下降法求解αk。

迭代初始選αk=1，當(dāng)αk代入式(18)滿足條件或者達(dá)到迭代次數(shù)，則停止迭代。最后將求得的αk代入式(14)。αk求解方法參照文獻(xiàn)[7]。

2 多新息理論

本文要應(yīng)用多新息理論，要將新息ek擴(kuò)展為新息矩陣E(n,k)。為了適應(yīng)兼容性，同理系統(tǒng)測量值Zk擴(kuò)展為Z(n,k)，測量噪聲rk擴(kuò)展為r(n,k)，濾波增益矩陣kk擴(kuò)展為k(n,k),代入式(5)有

Xk=Xk,k-1+k(n,k)E(n,k)

(19)

多新息卡爾曼濾波引入之前多個時刻的新息量，大大加強(qiáng)了數(shù)據(jù)的利用，實(shí)驗(yàn)表明,強(qiáng)非線性系統(tǒng)中，多新息卡爾曼濾波較單新息卡爾曼濾波有更好的魯棒性和穩(wěn)定性。雖然多新息卡爾曼濾波會比單新息卡爾曼濾波計(jì)算量加大，但相對于精度的提高，增加的計(jì)算量是可以接受的[8]。

3 仿真驗(yàn)證

3.1 系統(tǒng)建模

實(shí)驗(yàn)平臺為Windows7操作系統(tǒng)，CPU為Intel i5—4170，主頻為3.7 GHz。

實(shí)驗(yàn)所模擬的環(huán)境區(qū)域大小為200 m×200 m。有17個導(dǎo)航點(diǎn)、135個路標(biāo)；機(jī)器人配有里程計(jì)、前輪轉(zhuǎn)角傳感器、激光測距儀、光電編碼器獲得機(jī)器人及環(huán)境信息；機(jī)器人速度為v=3 m/s，最大舵角為20°。

首先,利用一個非線性函數(shù)f(x)對狀態(tài)轉(zhuǎn)移進(jìn)行建模，同時，狀態(tài)的測量值由一個非線性函數(shù)h(x)建模

Xk+1=f(xk,uk)+qk

(20)

Zk+1=h(xk,uk)+rk+1

(21)

為了與多新息一致，將式(21)改為

(22)

狀態(tài)矢量X由小車狀態(tài)Xv和路標(biāo)(特征)狀態(tài)Xm組成。因此

X=[Xv,Xm]

(23)

(24)

系統(tǒng)使用激光傳感器作為測速設(shè)備，使用光電編碼器作為系統(tǒng)的速度計(jì)算，重力傳感器用來確定系統(tǒng)的姿態(tài)。根據(jù)以上靜態(tài)特征的位置估計(jì)定義地圖。假設(shè)系統(tǒng)初始時沒有觀測特征，直角坐標(biāo)系與小車的起始位姿一致，且小車的起始位姿確定。即，X=Xv=0和P=Pv=0。隨后小車開始運(yùn)動，Xv和Pv變?yōu)榉橇阒?。隨著小車運(yùn)動，進(jìn)行第一次觀測，則會產(chǎn)生新的特征，并開始出現(xiàn)Xm和Pm。相應(yīng)的X和P也會隨之增加[7]。隨著小車運(yùn)動，不斷進(jìn)行以上操作，進(jìn)行定位。具體流程如圖1。

圖1 系統(tǒng)建模流程

3.2 仿真結(jié)果

機(jī)器人在由包含17個導(dǎo)航點(diǎn)、135個路標(biāo)點(diǎn)確定的路徑上運(yùn)動。仿真結(jié)果如圖2。實(shí)線為小車路徑，虛線為小車在EKF下的真實(shí)路徑；點(diǎn)線為小車在本文應(yīng)用的濾波(MR-AKF)下的真實(shí)路徑。星形線(*)為路標(biāo)的實(shí)際位置；圓圈(o)為EKF仿真的路標(biāo)的估計(jì)位置；十字線(+)為MR-AKF仿真的路標(biāo)的估計(jì)位置。路標(biāo)的估計(jì)位置和路線越接近實(shí)際位置說明系統(tǒng)的性能越好。

圖2 仿真路線

圖2給出了EKF和本文濾波(MR-AKF)的實(shí)際運(yùn)動軌跡；圖3給出了2種濾波算法與實(shí)際軌跡的整體誤差；圖4給出了2種濾波算法的運(yùn)動模型定位誤差；圖5給出了兩種濾波算法的測量誤差。機(jī)器人在由包含15個導(dǎo)航點(diǎn)、135個路標(biāo)的路徑上運(yùn)動。從圖2～圖4可以看出,機(jī)器人在轉(zhuǎn)向角小于120°時，仿真路徑和真實(shí)路徑會有明顯差異,這是由于轉(zhuǎn)向角較小時，系統(tǒng)動力學(xué)模型的運(yùn)行速度和朝向角度會很難估計(jì)[9]，容易引起誤差,可以看出本文的算法較EKF有顯著改善；從圖2、圖3、圖5可以看出,系統(tǒng)剛開始運(yùn)行，2種濾波算法均較精確，隨著系統(tǒng)的運(yùn)行誤差也會越來越大，這是由于測量誤差的累積，導(dǎo)致系統(tǒng)穩(wěn)定性下降[10]；本文采用的算法也較EKF有明顯改善；在系統(tǒng)運(yùn)行到后半段時2種濾波算法均有所下降，一個很重要的因素是系統(tǒng)非線性的增加，本文采用的多新息抗差—自適應(yīng)卡爾曼濾波較EKF效果好很多。從圖2可以看出本文的濾波算法基本可以實(shí)現(xiàn)對系統(tǒng)的精確定位。

圖3 EKF和MR-AKF整體誤差對比

圖4 EKF和MR-AKF運(yùn)動模型誤差對比

圖5 EKF和MR-AKF測量誤差對比

4 結(jié)束語

研究了在系統(tǒng)存在測量誤差和動力模型誤差時，應(yīng)用多新息抗差—自適應(yīng)卡爾曼濾波對測量誤差和動力學(xué)模型誤差進(jìn)行更正，引入的多新息概念使系統(tǒng)即使處于強(qiáng)非線性狀態(tài)也能很精確的實(shí)現(xiàn)定位，定位過程中均有較強(qiáng)的魯棒性，基本達(dá)到精確定位的目的。

[1] 高云峰，周 倫，呂明睿,等.自主移動機(jī)器人室內(nèi)定位方法研究綜述[J].傳感器與微系統(tǒng)，2013,32(12)：1-5.

[2] 許長輝，高井祥，胡 洪,等. 精密單點(diǎn)定位的抗差卡爾曼濾波研究[J].中國礦業(yè)大學(xué)學(xué)報(bào)，2014,33(12)：38-41.

[3] 毛 鈴，李振波，張大偉,等.基于紅外傳感器的移動機(jī)器人定位研究[J].傳感器與微系統(tǒng)，2009,26(2)：159-162.

[4] 季秀才,鄭志強(qiáng)，張 輝.SLAM問題中機(jī)器人定位誤差分析與控制[J].自動化學(xué)報(bào)，2008,34(3)：323-330.

[5] 呂國宏，秦品樂，苗啟廣,等.基于多新息理論的EKF算法研究研究 [J].小型微型計(jì)算機(jī)系統(tǒng)，2016,3(3)：576-58.

[6] 苗岳旺.SINS/GPS組合導(dǎo)航數(shù)據(jù)處理方法研究[D].鄭州:信息工程大學(xué),2013.

[7] 李 剛，蔡成林，李思敏,等.抗差與自適應(yīng)組合的卡爾曼濾波算法在動態(tài)導(dǎo)航中的研究 [J].重慶郵電大學(xué)學(xué)報(bào),2015,27(1)：37-43.

[8] Amitava Chatterjee.Vision-based autonomous robot naviga-tion[M].連曉鋒,譯.北京:機(jī)械工業(yè)出版社,2014.

[9] 雷 創(chuàng).基于自適應(yīng)EKF的相對導(dǎo)航算法研究[J].現(xiàn)代導(dǎo)航，2014,4(2):113-116.

[10] 陳航科，張東升，盛曉超,等.基于Kalman濾波算法的姿態(tài)傳感器信號融合技術(shù)研究[J].傳感器與微系統(tǒng)，2013,32(12):82-85.

李雅梅(1966-),碩士,副教授,主要從事計(jì)算機(jī)控制與應(yīng)用、智能檢測、數(shù)字信號處理等領(lǐng)域研究工作。

康璐璐(1991-),通訊作者，碩士研究生,主要研究方向?yàn)闄z測技術(shù)與自動化裝置研究，E—mail:1980265487@qq.com。

Research on multi-innovation robust adaptive Kalman filtering positioning algorithm*

LI Ya-mei, KANG Lu-lu

(College of Electrical and Control，Liaoning Technical University，Huludao 125000,China)

In the positioning process, the positioning accuracy of mobile robot drops sharply because of the sensor measurement error and the pose error caused by the robot model.In view of this phenomenon,a new algorithm based on multi-innovation Kalman filtering(MR-AKF)is proposed.Based on the standard Kalman filter,when the measured value of the sensor is error,the robust weighting factor is introduced,and the estimation accuracy of the filter is improved by changing the weight of the error measurement value;when the robot pose is error,an adaptive factor is introduced to resist the filtering divergence caused by the pose error by adjusting the size of the state covariance matrix.At the same time,introduce multi-innovation which is the innovation vector of multiple moments to further improve the accuracy of the nonlinear system.The experimental results show that the filtering algorithm can effectively improve the positioning accuracy in the presence of measurement errors and pose errors.

robust filtering; adaptive Kalman filtering; weight factor; adaptive factor; multi-innovation

10.13873/J.1000—9787(2017)09—0038—03

2016—10—17

國家自然科學(xué)基金資助項(xiàng)目(51274118)

TP 31

A

1000—9787(2017)09—0038—03