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

李雅梅， 康璐璐

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

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

李雅梅， 康璐璐

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

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

抗差濾波; 自適應卡爾曼濾波; 權因子; 自適應因子; 多新息

0 引 言

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

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

1 濾波算法

1.1 卡爾曼濾波

標準卡爾曼濾波的公式如下

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)估計;Xk,k-1為k-1時刻的狀態(tài)預報值;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 結合抗差與自適應卡爾曼濾波

1.2.1 抗差估計

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

(6)

式中H0為觀測值中無誤差；H1為含有粗差。含有粗差的觀測值的方差擴大k倍，其相應觀測值的權值也縮小k2倍,則抗差估計通過減小粗差觀測值的權重減小誤差。

在卡爾曼濾波中，測量新息定義為狀態(tài)預估值和當前觀測值的函數(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)

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

(12)

則式(3)可改為

(13)

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

1.2.2 自適應因子

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

(14)

為了得到自適應因子α的值，作如下計算：

利用式(7)可得

(15)

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

(16)

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

(17)

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

(18)

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

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

2 多新息理論

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

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

(19)

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

3 仿真驗證

3.1 系統(tǒng)建模

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

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

首先,利用一個非線性函數(shù)f(x)對狀態(tài)轉移進行建模，同時，狀態(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和路標(特征)狀態(tài)Xm組成。因此

X=[Xv,Xm]

(23)

(24)

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

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

3.2 仿真結果

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

圖2 仿真路線

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

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

圖4 EKF和MR-AKF運動模型誤差對比

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

4 結束語

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

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

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

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

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

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

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

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

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

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

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

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

康璐璐(1991-),通訊作者，碩士研究生,主要研究方向為檢測技術與自動化裝置研究，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

國家自然科學基金資助項目(51274118)

TP 31

A

1000—9787(2017)09—0038—03