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

（安徽工程大學安徽省電氣傳動與控制重點實驗室，安徽蕪湖241000）

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

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

（安徽工程大學安徽省電氣傳動與控制重點實驗室，安徽蕪湖241000）

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

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

1 概述

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

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

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

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

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

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

SLAM問題的實質(zhì)是：通過移動機器人內(nèi)部的運動信息和傳感器外部采集的觀測信息對機器人的位姿和特征地圖進行估計。

機器人運動模型為：

機器人觀測模型為：

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

因此，通過數(shù)學語言SLAM問題可以表述為：

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

本文所提出的ISR-CKF算法通過采集到的信息，預估計機器人的位姿和特征地圖信息。

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

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

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

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

（1）利用K-1時刻后驗概率分布及K時刻運動控制量預測求出先驗概率分布：

（2）利用K時刻觀測量與先驗概率分布更新后驗概率分布：

其中，η為常量。

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

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

（1）預測階段

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

2）計算各容積點的先驗估計，以容積點j為例：

每個容積點經(jīng)非線性運動方程傳播后可以得到機器人K-1時刻位姿信息，和機器人K時刻運動信息，預測K時刻機器人位姿信息。

3）機器人位姿狀態(tài)預估計和平方根因子預測

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

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

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

（2）更新階段

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

1）計算迭代容積點

2）計算第l次迭代卡爾曼增益

4）設置迭代終止條件

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

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

位姿更新：

當觀測到多個特征點時，需要重復式（12）～式（25）。

4 仿真實驗與分析

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

仿真實驗中的機器人運動方程為：

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

仿真實驗中的機器人觀測矩陣方程為：

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

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

機器人的行進軌跡仿真結果如圖1～圖3所示。

圖1 UKF-SLAM算法機器人的行進軌跡

圖2 SR-CKF-SLAM算法機器人的行進軌跡

圖3 ISR-CKF-SLAM算法機器人的行進軌跡

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

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

圖4 3種算法估計誤差對比

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

表1 3種算法的誤差比較

5 結束語

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

［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］ 強敏利，張萬緒.IEKF濾波在移動機器人定位中的應用［J］.電子技術與應用，2013，39（2）：74-77.

［5］ 呂太之.一種新的抗外部干擾EKF-SLAM算法［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］.計算機工程與應用，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］ 康軼非，宋永端，宋 寧，等.平方根容積卡爾曼濾波在移動機器人SLAM中的應用［J］.機器人，2013，35（2）：186-193.

［12］ 石杏喜，趙春霞.基于概率的移動機器人SLAM算法框架［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］.計算機工程，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-），男，碩士研究生，主研方向：智能計算，智能控制；凌有鑄（通訊作者），教授；陳孟元，講師、碩士；戴雪梅，碩士研究生。

2014-12-05

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