迭代的平方根容積卡爾曼濾波SLAM算法

陶 明，凌有鑄，陳孟元，戴雪梅

（安徽工程大學(xué)安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽蕪湖241000）

迭代的平方根容積卡爾曼濾波SLAM算法

陶 明，凌有鑄，陳孟元，戴雪梅

（安徽工程大學(xué)安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽蕪湖241000）

平方根容積卡爾曼濾波算法在移動(dòng)機(jī)器人同步定位與地圖創(chuàng)建問(wèn)題中，存在隨著地圖特征點(diǎn)增多、容積點(diǎn)偏離理想軌跡、狀態(tài)估計(jì)產(chǎn)生較大誤差的缺陷。為此，提出一種改進(jìn)的平方根容積卡爾曼濾波算法。該算法引入迭代測(cè)量更新的方法，在更新階段利用估計(jì)值和平方根因子重新確定采樣的容積點(diǎn)，使得采樣點(diǎn)在高度非線性環(huán)境下保持較小失真，進(jìn)一步提高精度。仿真結(jié)果表明，與平方根容積卡爾曼濾波算法相比，該算法能提高機(jī)器人位姿精度。

迭代；同步定位與地圖創(chuàng)建；采樣；擴(kuò)展卡爾曼濾波；加權(quán)處理

1 概述

同步定位與地圖創(chuàng)建（Simultaneous Localization and Mapping，SLAM）可以表述為：在未知環(huán)境下，移動(dòng)機(jī)器人利用傳感器所收集的信息創(chuàng)建特征地圖，移動(dòng)同時(shí)不斷對(duì)自身的位姿進(jìn)行修正的過(guò)程［1］。SLAM關(guān)乎移動(dòng)機(jī)器人能否自主運(yùn)動(dòng)，是該領(lǐng)域的研究熱點(diǎn)。

經(jīng)典的擴(kuò)展卡爾曼濾波（Extended Kalman Filtering，EKF）［2-3］是應(yīng)用于SLAM領(lǐng)域的主流算法，其實(shí)質(zhì)是對(duì)非線性的系統(tǒng)模型和觀測(cè)模型一階泰勒級(jí)數(shù)展開(kāi)，將模型線性化后通過(guò)標(biāo)準(zhǔn)的卡爾曼濾波處理。在EKF-SLAM的基礎(chǔ)之上，許多學(xué)者都提出了改進(jìn)的新型算法。文獻(xiàn)［4］提出迭代的擴(kuò)展卡爾曼濾波（Iterated Extended Kalman Filtering，IEKF）算法，該算法依托環(huán)境特征的迭代與EKF算法的融合，修正了機(jī)器人的非線性誤差，提高了定位的精度。文獻(xiàn)［5］提出了一種基于外部干擾檢測(cè)對(duì)比，膨脹定位誤差的抗干擾EKF-SLAM算法，提高了算法的魯棒性。但是EKF在線性化的過(guò)程中僅把展開(kāi)的二階以上的高次項(xiàng)做簡(jiǎn)單的舍棄處理，在定位要求非常精確的場(chǎng)合下，不容易達(dá)到理想的效果。且線性化過(guò)程中需要計(jì)算雅可比矩陣，會(huì)造成大幅增加計(jì)算量，不適用于大地圖環(huán)境。

近年來(lái)比較流行的無(wú)跡卡爾曼濾波（Unscented Kalman Filtering，UKF）［6-7］通過(guò)選取不同權(quán)值的采樣點(diǎn)（Sigma點(diǎn)）隨非線性函數(shù)傳遞，經(jīng)過(guò)無(wú)跡變換（Unscented Transformation，UT）得到隨機(jī)變量的統(tǒng)計(jì)特征。UKF在卡爾曼濾波的框架下，減少了非線性方程線性化產(chǎn)生的誤差，能夠使定位精度在同等情況下達(dá)到二階。文獻(xiàn)［8］在數(shù)學(xué)上推導(dǎo)出用協(xié)方差矩陣的平方根代替協(xié)方差矩陣傳遞，理論上能有效解決了這一問(wèn)題，并能很好地應(yīng)用于SLAM問(wèn)題中。文獻(xiàn)［9］在平方根UKF的基礎(chǔ)之上，改變了Sigma點(diǎn)采樣的策略，提出一種比例最小偏度單行采樣的算法，減少了算法的計(jì)算量，提高了實(shí)時(shí)性。然而UKF算法在濾波過(guò)程可能會(huì)導(dǎo)致協(xié)方差矩陣非正定，影響濾波器的性能。

2009年，A rasaratnam和Haykin提出了容積卡爾曼濾波（Cubature Kalman Filtering，CKF）［10］，提供了一種新的非線性系統(tǒng)狀態(tài)估計(jì)方法。CKF在容積規(guī)則下選取一組相應(yīng)權(quán)值的容積點(diǎn)，帶入非線性函數(shù)傳遞后經(jīng)過(guò)加權(quán)處理得到隨機(jī)變量的統(tǒng)計(jì)特性。CKF與UKF相比，同樣減少了非線性方程線性化產(chǎn)生的誤差，但CKF以其計(jì)算量小、數(shù)值精確度高、濾波穩(wěn)定性強(qiáng)的特點(diǎn)被越來(lái)越多的學(xué)者肯定，成為一種解決各種估值問(wèn)題的有效手段。文獻(xiàn)［11］提出了一種基于容積平方根卡爾曼濾波（Square Root Cubature Kalman Filtering，SR-CKF）的SLAM算法，提升了整個(gè)系統(tǒng)穩(wěn)定性。

以上述研究?jī)?nèi)容為基礎(chǔ)，本文提出一種迭代平方根容積卡爾曼濾波（Iterated Square Root Cubature Kalm an Filtering，ISR-CKF）算法。該算法在SRCKF算法基礎(chǔ)上引入了高斯牛頓迭代方法，汲取了SR-CKF算法的優(yōu)點(diǎn)：采用協(xié)方差的平方根來(lái)傳遞矩陣信息削弱了截?cái)嗾`差對(duì)SLAM的誤差精度影響。同時(shí)通過(guò)增加迭代過(guò)程以利于量測(cè)信息的充分利用。

2 地圖創(chuàng)建與同步定位問(wèn)題描述

SLAM問(wèn)題的實(shí)質(zhì)是：通過(guò)移動(dòng)機(jī)器人內(nèi)部的運(yùn)動(dòng)信息和傳感器外部采集的觀測(cè)信息對(duì)機(jī)器人的位姿和特征地圖進(jìn)行估計(jì)。

機(jī)器人運(yùn)動(dòng)模型為：

機(jī)器人觀測(cè)模型為：

其中，WK服從N（0，QK）的高斯分布；VK服從N（0，RK）的高斯分布［12］。

因此，通過(guò)數(shù)學(xué)語(yǔ)言SLAM問(wèn)題可以表述為：

即已知控制量向量U1：K和觀測(cè)向量Z1：K，估計(jì)當(dāng)前時(shí)刻特征地圖向量和狀態(tài)向量SrK的聯(lián)合概率分布。由式（1）可得求解出每個(gè)時(shí)刻滿足正態(tài)分布向量SK的最優(yōu)解及其協(xié)方差PK成為了解決SLAM問(wèn)題的核心問(wèn)題。

本文所提出的ISR-CKF算法通過(guò)采集到的信息，預(yù)估計(jì)機(jī)器人的位姿和特征地圖信息。

其核心是通過(guò)容積變換公式式（2）計(jì)算條件轉(zhuǎn)移概率密度，進(jìn)而在高斯域下實(shí)現(xiàn)貝葉斯濾波：

其中，f（y）為非線性函數(shù)；N（；）為高斯分布；向量y為ny維，y服從均值為μ；方差為ξi表示互相正交的完全對(duì)稱容積點(diǎn)集。

在式（3）中，ξi有2（ns+nu）列：

ISR-CKF算法根據(jù)貝葉斯濾波的基本原理，其具體的求解過(guò)程分為預(yù)測(cè)、更新2個(gè)步驟，分別到達(dá)求出K時(shí)刻先驗(yàn)概率分布和后驗(yàn)概率分布的目的：

（1）利用K-1時(shí)刻后驗(yàn)概率分布及K時(shí)刻運(yùn)動(dòng)控制量預(yù)測(cè)求出先驗(yàn)概率分布：

（2）利用K時(shí)刻觀測(cè)量與先驗(yàn)概率分布更新后驗(yàn)概率分布：

其中，η為常量。

3 迭代平方根容積卡爾曼濾波

SR-CKF算法是在卡爾曼濾波框架下，利用三階球面的相徑容積規(guī)則，通過(guò)確定的容積點(diǎn)集傳遞后加權(quán)得到非線性函數(shù)的均值和協(xié)方差平方根因子。在傳遞過(guò)程中采用協(xié)方差矩陣平方根（基于QR分解）代替協(xié)方差矩陣信息更新來(lái)解決由于計(jì)算機(jī)舍入誤差可能導(dǎo)致的濾波器發(fā)散問(wèn)題。具體的預(yù)測(cè)階段算法描述如下：

（1）預(yù)測(cè)階段

其中，j為容積點(diǎn)序號(hào)，取值為1，2，…，2（nu+ns）；矩陣包含了K-1時(shí)刻的位姿信息，特征點(diǎn)信息和運(yùn)動(dòng)控制信息。由機(jī)器人狀態(tài)信息SK-1和運(yùn)動(dòng)信息uK增廣為高斯噪聲變量，經(jīng)式（7）處理可得矩陣和矩陣

2）計(jì)算各容積點(diǎn)的先驗(yàn)估計(jì)，以容積點(diǎn)j為例：

每個(gè)容積點(diǎn)經(jīng)非線性運(yùn)動(dòng)方程傳播后可以得到機(jī)器人K-1時(shí)刻位姿信息，和機(jī)器人K時(shí)刻運(yùn)動(dòng)信息，預(yù)測(cè)K時(shí)刻機(jī)器人位姿信息。

3）機(jī)器人位姿狀態(tài)預(yù)估計(jì)和平方根因子預(yù)測(cè)

根據(jù)容積變換公式式（2）可得：

經(jīng)過(guò)QR分解地圖特征誤差向量AK|K-1可以得到平方根因子矩陣CK|K-1用于下一步更新階段：

雖然SR-CKF-SLAM算法能夠很好地提高移動(dòng)機(jī)器人位姿定位精度和系統(tǒng)控制的穩(wěn)定性，但是隨著地圖特征點(diǎn)的增多，系統(tǒng)觀測(cè)的維度會(huì)逐漸增加，導(dǎo)致容積點(diǎn)偏離理想軌跡，從而在狀態(tài)估計(jì)中產(chǎn)生很大的誤差。因此，本文采用迭代方法，在更新階段利用估計(jì)值和平方根因子重新確定采樣的容積點(diǎn)，通過(guò)容積變換得到系統(tǒng)的統(tǒng)計(jì)特性，再結(jié)合預(yù)測(cè)階段估計(jì)的新觀測(cè)值改善系統(tǒng)狀態(tài)估計(jì)，從而提高算法精度。

（2）更新階段

以第i個(gè)特征點(diǎn)為例，假設(shè)迭代初始值分別為SK|K-1和CK|K-1，第l次迭代機(jī)器人位姿信息和平方根因子分別為其K時(shí)刻的觀測(cè)向量其觀測(cè)模型為：

1）計(jì)算迭代容積點(diǎn)

2）計(jì)算第l次迭代卡爾曼增益

4）設(shè)置迭代終止條件

其中，Lmax為迭代最大次數(shù)，是預(yù)先設(shè)置的固定常量。

5）迭代終止，各數(shù)據(jù)更新：

位姿更新：

當(dāng)觀測(cè)到多個(gè)特征點(diǎn)時(shí)，需要重復(fù)式（12）～式（25）。

4 仿真實(shí)驗(yàn)與分析

SLAM算法精度研究在Matlab7.0軟件環(huán)境下，使用主頻3.4 GHz，Core-i3雙核處理器，4 GB內(nèi)存的計(jì)算機(jī)進(jìn)行仿真實(shí)驗(yàn)。在澳大利亞學(xué)者Tim Bailey提供的開(kāi)源仿真實(shí)驗(yàn)平臺(tái)［13］上，分別對(duì)UKFSLAM，SR-CKF-SLAM，ISR-CKF-SLAM 3種算法進(jìn)行仿真實(shí)驗(yàn)，分析各個(gè)算法之間的精度差異。

仿真實(shí)驗(yàn)中的機(jī)器人運(yùn)動(dòng)方程為：

其中，矩陣mK表示機(jī)器人位姿；Ts表示系統(tǒng)內(nèi)部采樣間隔；νK表示機(jī)器人行進(jìn)速度；αK表示轉(zhuǎn)角；L表示輪距。

仿真實(shí)驗(yàn)中的機(jī)器人觀測(cè)矩陣方程為：

其中，lK表示機(jī)器人與觀測(cè)到地圖特征點(diǎn)的距離；βK表示機(jī)器人與觀測(cè)到地圖特征點(diǎn)的夾角；（mχ，i，my，i）為機(jī)器人觀測(cè)到的地圖特征點(diǎn)i的位置。

實(shí)驗(yàn)假設(shè)的地圖環(huán)境為：250 m×200 m的室外環(huán)境，17個(gè)確定的路徑點(diǎn)，35個(gè)地圖特征點(diǎn)。實(shí)驗(yàn)參數(shù)設(shè)置如下：機(jī)器人行進(jìn)速度為3 m/s，輪距為4 m，探測(cè)傳感器探測(cè)范圍為30 m，最大探測(cè)視角為前向180°，系統(tǒng)內(nèi)部采樣間隔為25 ms，速度誤差為0.4 m/s，轉(zhuǎn)角誤差為2°，距離觀測(cè)誤差為0.15 m，角度觀測(cè)誤差為1°，ISR-CKF-SLAM算法設(shè)定迭代次數(shù)為5次。

機(jī)器人的行進(jìn)軌跡仿真結(jié)果如圖1～圖3所示。

圖1 UKF-SLAM算法機(jī)器人的行進(jìn)軌跡

圖2 SR-CKF-SLAM算法機(jī)器人的行進(jìn)軌跡

圖3 ISR-CKF-SLAM算法機(jī)器人的行進(jìn)軌跡

機(jī)器人行進(jìn)完全部軌跡大概需要240 s，經(jīng)過(guò)多次實(shí)驗(yàn)選取平均值。本文取仿真結(jié)果的前180 s進(jìn)行分析。

在行進(jìn)的初始階段3種算法的運(yùn)行軌跡與理想軌跡偏差基本不大，隨著地圖特征點(diǎn)的不斷增多，3種算法的運(yùn)行軌跡相較于理想軌跡都有不同程度的偏移，但是ISR-CKF-SLAM算法與UKF-SLAM算法、SR-CKF-SLAM算法相比更契合理想軌跡的運(yùn)行路線，即意味著位姿估計(jì)更加精確。由圖4可以得出，ISR-CKF-SLAM無(wú)論是在X方向、Y方向、還是位姿角上的估計(jì)誤差一直保持在一個(gè)穩(wěn)定的范圍之內(nèi)，且與UKF-SLAM、SR-CKF-SLAM相比都保持著較高的精度。這是因?yàn)橥ㄟ^(guò)迭代方法的測(cè)量更新，重新采樣的容積點(diǎn)會(huì)減小對(duì)系統(tǒng)模型的線性化誤差，提高了地圖特征點(diǎn)位置估計(jì)的精度，進(jìn)而提高移動(dòng)機(jī)器人的定位精度。

圖4 3種算法估計(jì)誤差對(duì)比

為了更加直觀地表達(dá)3種算法性能差異，采用統(tǒng)計(jì)數(shù)據(jù)列表的方式分析。由表1可知，相比SRCKF-SLAM，ISR-CKF-SLAM在位姿X方向誤差降低了44%，在位姿Y方向誤差降低了10.8%，位姿角誤差降低了37.8%。

表1 3種算法的誤差比較

5 結(jié)束語(yǔ)

本文提出一種迭代的平方根容積卡爾曼濾波SLAM算法（ISR-CKF-SLAM）。通過(guò)高斯牛頓迭代方法對(duì)SR-CKF算法進(jìn)行改進(jìn)，在使用最新更新信息的基礎(chǔ)上，優(yōu)化了容積點(diǎn)采樣規(guī)則，降低了初始誤差和線性化誤差對(duì)狀態(tài)估計(jì)的影響，使得以此產(chǎn)生的后驗(yàn)概率分布更加接近真實(shí)值。仿真結(jié)果表明，該算法的機(jī)器人位姿精度估計(jì)更加準(zhǔn)確，并且更加穩(wěn)定。今后將研究如何優(yōu)化迭代方法，進(jìn)一步提高該算法的實(shí)時(shí)性。

［1］ Durrant-Whyte H，Bailey T.Simultaneous Localization and Mapping：Part I［J］.IEEE Robotics and Automation Magazine，2006，13（2）：99-108.

［2］ Smith R C，Cheeseman P.On the Representation and Estimation of Spatital Uncertainty［J］.Robotics Research，1986，5（4）：231-238.

［3］ Smith R，Self M，Cheeseman P.Estimating Uncertain Spatial Relationships in Robotics［C］//Proceedings of Conference on Uncertainty in Artificial Intelligence. Am sterdam，Holland：［s.n.］，1988：435-461.

［4］ 強(qiáng)敏利，張萬(wàn)緒.IEKF濾波在移動(dòng)機(jī)器人定位中的應(yīng)用［J］.電子技術(shù)與應(yīng)用，2013，39（2）：74-77.

［5］ 呂太之.一種新的抗外部干擾EKF-SLAM算法［J］.計(jì)算機(jī)工程，2012，38（21）：1-4.

［6］ Julier S J，Uhlman JK.Unscented Filtering and Nonliner Estimation［J］.Proceedings of the IEEE，2006，13（2）：99-108.

［7］ Julier S J，Uhlm an J K，Durrant-W hyte H F.A New Method for the Nonlinear Transform ation of M eans and Covariances in Filters and Estimators［J］.IEEE Transactions on Automatic Control，2000，45（3）：477-482.

［8］ Merwe R，Wan E A.The Square-root Unscented Kalman Filter for State and Parameter Estimation［C］//Proceedings of IEEE International Conference on Acoustics，Speech，and Signal Processing.Washington D.C.，USA：IEEE Press，2001：3461-3464.

［9］ 汪貴冬，陳躍東，陳孟元.比例最小偏度單行采樣的平方根UKF-SLAM算法［J］.計(jì)算機(jī)工程與應(yīng)用，2014，50（17）：63-67.

［10］ Arasaratnam I，Haykin S.Cubature Kalman Filters［J］. IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

［11］ 康軼非，宋永端，宋 寧，等.平方根容積卡爾曼濾波在移動(dòng)機(jī)器人SLAM中的應(yīng)用［J］.機(jī)器人，2013，35（2）：186-193.

［12］ 石杏喜，趙春霞.基于概率的移動(dòng)機(jī)器人SLAM算法框架［J］.計(jì)算機(jī)工程，2010，36（2）：31-32，41.

［13］ Tim B.SLAM Simulations［EB/OL］.［2010-10-08］. http：//www-personal.acfr.usyd.edu.au/tbailey/.

編輯 劉 冰

SLAM Algorithm of Iterated Square Root Cubature Kalm an Filtering

TAO M ing，LING Youzhu，CHEN Mengyuan，DAIXuemei
（Anhui Key Laboratory of Electric Drive and Control，Anhui Polytechnic University，Wuhu 241000，China）

The disadvantage of Square Root Cubature Kalman Filtering（SR-CKF）algorithm on the Simultaneous Location and Mapping（SLAM）is that with map feature points increasing，the volume points deviate from the ideal trajectory to cause great defects in state estimation.In order to solve that problem，this paper provides an improved square root cubature Kalman filtering algorithm.The algorithm takes advantage of iterative method of the measurement update，which makes the sampling points less distortion and further im proves the accuracy in the highly nonlinear environment. Simulation results show that，compared with SR-CKF algorithm，this algorithm can effectively improve the accuracy of position and attitude.

iteration；Simultaneous Localization and Mapping（SLAM）；sampling；Extended Kalman Filtering（EKF）；weighted processing

陶 明，凌有鑄，陳孟元，等.迭代的平方根容積卡爾曼濾波SLAM算法［J］.計(jì)算機(jī)工程，2015，41（9）：317-321.

英文引用格式：Tao Ming，Ling Youzhu，Chen Mengyuan，et al.SLAM Algorithm of Iterated Square Root Cubature Kalman Filtering［J］.Computer Engineering，2015，41（9）：317-321.

1000-3428（2015）09-0317-05

A

TP24

10.3969/j.issn.1000-3428.2015.09.058

陶 明（1989-），男，碩士研究生，主研方向：智能計(jì)算，智能控制；凌有鑄（通訊作者），教授；陳孟元，講師、碩士；戴雪梅，碩士研究生。

2014-12-05

2015-02-27 E-m ail：peerless1207@163.com