薛永勝 王 姮 張 華 霍建文

(西南科技大學(xué)特殊環(huán)境機器人技術(shù)四川省重點實驗室 四川綿陽 621010)

?

EKF-SLAM算法的改進及其在Turtlebot的實現(xiàn)

薛永勝 王 姮 張 華 霍建文

(西南科技大學(xué)特殊環(huán)境機器人技術(shù)四川省重點實驗室 四川綿陽 621010)

針對標準擴展卡爾曼濾波(EKF)在移動機器人同時定位與地圖構(gòu)建(SLAM)過程中計算量大、實時性差、精度低、易受干擾等問題，結(jié)合平滑算法和奇異值分解運算，提出了一種基于EKF-SLAM算法的改進ERTSS-SLAM算法。改進ERTSS-SLAM算法使用前向EKF濾波對移動機器人里程計和陀螺儀的位姿信息進行最優(yōu)估計，再使用標準ERTS平滑器進行后向遞推避免發(fā)散，同時使用奇異值分解法避免標準EKF濾波產(chǎn)生的求逆運算，有效提高了系統(tǒng)實時性，增強了系統(tǒng)的魯棒性和定位精度。Turtlebot移動機器人的實驗效果證明了該算法在SLAM應(yīng)用中的高效性和穩(wěn)定性。

同時定位與地圖構(gòu)建 平滑算法 奇異值分解 Turtlebot

伴隨工業(yè)4.0概念的提出和世界范圍內(nèi)機器人技術(shù)的蓬勃發(fā)展，智能機器人的同時定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)問題已成為移動機器人領(lǐng)域備受關(guān)注的研究方向[1-2]，也是移動機器人智能化發(fā)展的重要支撐。

目前，解決SLAM問題中的EKF-SLAM，PF-SLAM，F(xiàn)astSLAM及UKF-SLAM等經(jīng)典算法[3]中，EKF-SLAM算法最先被提出、最為流行。但EKF中的非線性對象線性化過程引入了較大誤差，且雅克比矩陣計算較復(fù)雜，使得其精度和實時性難以滿足實際應(yīng)用需求。針對EKF的缺點，Gurkan Tuna[4]等人通過對標準EKF、壓縮卡爾曼濾波(CEKF)、無跡卡爾曼濾波(UKF)3種算法占用的計算資源和耗時進行測評，發(fā)現(xiàn)CEKF更適合室外SLAM的應(yīng)用，并對CEFKF的應(yīng)用進行了仿真實驗，證明了其有效性；Andrea Griffo[5]等人在EKF-SLAM的基礎(chǔ)上提出雙EKF濾波的傳感器解決方法，一個EKF用于路徑預(yù)測，一個用于路標預(yù)測，提高了精度，但是計算復(fù)雜；張毅[6]等提出了修正迭代擴展卡爾曼濾波算法(MIEKF)，降低了對系統(tǒng)傳感器精度的依賴度，減小了線性化誤差。Wang Daobin[7]等人通過對不同移動機器人的動態(tài)模型改進，提出了一種適用于多種移動機器人平臺的改進EKF-SLAM算法，降低了計算復(fù)雜度，提高了精度。杜航原[8]等提出了一種改進的模糊自適應(yīng)卡爾曼濾波算法，改變系統(tǒng)對觀測信息的依賴和利用程度，使濾波器最終趨于穩(wěn)定，保證了實時性和精度。

現(xiàn)有的多種對EKF-SLAM算法的改進在一定程度上可以單獨改善實時性或者精確度，但無法同時保證其魯棒性和計算效率，因此難以滿足實際應(yīng)用需求。為解決這些問題，本文提出一種基于EKF-SLAM的改進ERTSS-SLAM算法。

1 EKF-SLAM算法

移動機器人的自定位過程是一個非線性化的過程，從統(tǒng)計的角度看，SLAM問題是一個濾波問題，在系統(tǒng)噪聲和測量噪聲服從正態(tài)分布的情況下EKF可以很好地去除系統(tǒng)中的噪聲干擾實現(xiàn)最優(yōu)估計，因此Smith和Cheeseman提出使用EKF來估計 SLAM問題的EKF-SLAM算法，該算法流程框圖如圖1。

圖1 EKF-SLAM算法流程Fig.1 EKF-SLAM algorithm flow

1.1 EKF原理

擴展卡爾曼濾波算法是一種常用的非線性濾波方法，其原理是在預(yù)測點xk|k-1將系統(tǒng)狀態(tài)方程和測量方程進行泰勒展開，并忽略所有非線性展開高階項來實現(xiàn)對非線性方程的線性化近似[9]。常用的非線性離散隨機系統(tǒng)模型如下：

xk+1=f(xk,uk)+w(t)

(1)

zk=h(xk)+v(t)

(2)

其中ω，v代表了附加在非線性系統(tǒng)運行過程中的0均值高斯噪聲。將系統(tǒng)方程進行線性化表示為：

(3)

(4)

(5)

通過式(6)-式(8)迭代運算可進行預(yù)測更新，通過式(9)-式(12)迭代運算可進行狀態(tài)更新。

xk+1|k=f(xk|k,uk)

(6)

Pk+1|k=JPk|kJT+Qk

(7)

(8)

(9)

?k+1=zk+1-h(xk+1|k)

(10)

xk+1|k+1=xk+1|k+Wk+1?k+1

(11)

(12)

1.2 EKF-SLAM算法

基于EKF的SLAM算法包含運動更新和觀測更新兩個過程，這兩個過程分別是對系統(tǒng)的運動模型和觀測模型的描述[10]。運動更新過程是用各傳感器數(shù)據(jù)作為輸入控制量，對機器人當(dāng)前位置進行預(yù)測估計。觀測更新是當(dāng)移動機器人觀測到當(dāng)前環(huán)境中的特征點并與局部地圖匹配，同時更新機器人在地圖中的位置的過程。通常機器人SLAM將位姿信息xv和地圖特征mi(xi,yi)存儲在一個獨立的狀態(tài)向量xml中：

(13)

(14)

整個地圖信息為MB=(xml,Pml)。

EKF-SLAM具有定義明確、算法編程實現(xiàn)容易等特點，最大的缺陷就是假設(shè)系統(tǒng)中的各種不確定性參數(shù)符合高斯分布，這在嚴重情況下，會造成數(shù)據(jù)關(guān)聯(lián)的不準確，從而導(dǎo)致算法發(fā)散。而且，隨著環(huán)境特征點個數(shù)的增加還會造成輸出協(xié)方差陣中的元素以平方倍數(shù)成倍增加，導(dǎo)致運算量增加[11]。

2 改進ERTSS-SLAM算法

2.1 ERTSS-SLAM算法

ERTS平滑算法能對某一時間區(qū)間內(nèi)的所有量測信息和狀態(tài)進行估計的離線處理，能夠獲得優(yōu)于濾波的估計精度，廣泛應(yīng)用于聲音信號處理、目標跟蹤和導(dǎo)航制導(dǎo)等領(lǐng)域[12]，其原理框圖如圖2所示。圖中上標“+”和“-”分別表示濾波的更新過程和預(yù)測過程，“F”和“S”分別表示前向濾波過程和后向遞推平滑過程。

圖2 ERTS平滑算法原理圖Fig.2 ERTS smoothing algorithm schematics

前向EKF濾波過程利用固定時間區(qū)間[0N]內(nèi)所有采樣值進行，并存儲式(6)-式(12)中的狀態(tài)估計xk+1|k+1、后驗估計協(xié)方差誤差陣Pk+1|k+1和先驗估計協(xié)方差誤差陣Pk+1|k。其后向遞推公式[13]如下：

(15)

(16)

其中后向遞推的平滑增益矩陣：

(17)

ERTSS-SLAM算法具有EKF的優(yōu)點，能夠在線遞推定位和建圖，同時也能夠離線估計進行定位和建圖，避免估計的突變，但計算中協(xié)方差陣中的元素隨著環(huán)境特征點數(shù)量呈指數(shù)增長，下文提出了相應(yīng)的改進算法。

2.2 算法改進

ERTS平滑算法中存在大量n×n矩陣相乘，為簡少運算過程的存儲和計算量，采用奇異值分解法[14]對Ps,k和Ps,k+1進行定義得：

(18)

(19)

定義Mk滿足：

(20)

將上式帶入式(16)化簡為：

(21)

為進行奇異值分解運算，求Mk的逆得：

(22)

(23)

對上式兩邊分別左乘各自轉(zhuǎn)置陣后得到：

(24)

由式(21)和式(23)得到：

(25)

同理可實現(xiàn)式(18)的奇異值分解運算得到：

(26)

將式(24)和式(25)帶入式(16)和式(17)可得改進后的后向遞推的平滑增益矩陣：

(27)

配合前向EKF濾波過程產(chǎn)生協(xié)方差矩陣，將式(25)-式(27)帶入式(15)-式(17)即為本文改進的ERTSS-SLAM算法，其流程如圖3所示。

圖3 改進ERTSS-SLAM算法流程圖Fig.3 Improved ERTSS-SLAM algorithm flowchart

3 實驗及分析

3.1 實驗平臺

Turtlebot移動機器人平臺是一款基于ROS操作系統(tǒng)的移動機器人[15]，其底盤采用韓國Kobuki平臺，該底盤所自帶的傳感器類型及參數(shù)如表1所示。

表1 Turtlebot位姿傳感器參數(shù)Table 1 The Parameters of Turtlebot Position and orientation sensor

圖4 Turtlebot機器人及其簡化模型Fig.4 Turtlebot robot and its simplified model

三軸數(shù)字陀螺儀采用意法半導(dǎo)體推出的高精度陀螺儀L3G4200D，具有低功耗、高精度、使用靈活的特點，實際應(yīng)用中僅用單軸進行航向參數(shù)測量和慣性導(dǎo)航，而里程計精度高、體積小，是位置反饋的重要設(shè)備。Turtlebot移動機器人模型簡化后如圖4。

定義輸入向量u=[Vα]T，加入過程噪聲向量v=[vεvβ]T和檢測噪聲向量ω=[ωrωθ]T，對系統(tǒng)進行離散化計算得移動機器人的運動方程及觀測方程[16-17]如下：

(28)

(29)

3.2 結(jié)果比較

為對比分析EKF-SLAM、ERTSS-SLAM和改進ERTSS-SLAM算法，選取分段數(shù)N=10，在MATLAB R2012a中建立11個路標(十字坐標點)的20 m×20 m地圖(如圖5)，并設(shè)置不規(guī)則障礙物(點線)和任意路徑(圓線)。設(shè)置仿真時間500 s,采樣周期T=10 ms，Wk為[-1.0,1.0]的白噪聲，Vk為[-0.02，0.02]的白噪聲信號，傳感器感應(yīng)距離10 m，對3種SLAM算法進行仿真對比。

圖5 SLAM仿真地圖Fig.5 SLAM simulation map

3種算法在SLAM定位過程中的定位誤差結(jié)果如圖6，由圖6可知本文改進算法能有效提升移動機器人定位精度和穩(wěn)定性。

圖6 不同SLAM算法定位誤差Fig.6 Different SLAM algorithms location error

3種SLAM算法在MATLAB R2012a中耗時及誤差對比如表2所示。

表2 不同SLAM算法性能比較Table 2 Performances of different smoothing algorithms

由表2分析知3種SLAM算法均能有效的抑制噪聲信號，單獨的EKF-SLAM算法穩(wěn)定性較差；標準的ERTSS-SLAM算法在EKF基礎(chǔ)上添加了ERTS平滑算法，多耗費0.118 s，但能提高精度58.8%；改進后的ERTSS-SLAM算法定位精度相較于其他兩者分別提高了73.9%，36.7%，在耗時相近的情況下大幅提升了系統(tǒng)的精度和穩(wěn)定性。

為了驗證該算法在實際應(yīng)用中的可行性及穩(wěn)定性，本文在Turtlebot移動機器人進行了實際測試分析，其驗證結(jié)果如表3。

表3 Turtlebot機器人平臺算法驗證結(jié)果Table 3 Turtlebot robot platform algorithm validation results

3.3 抗干擾分析

現(xiàn)代控制對系統(tǒng)的魯棒性要求越來越高，為驗證本文改進算法在系統(tǒng)隨機噪聲干擾下的穩(wěn)定性，分別增強測量噪聲和系統(tǒng)噪聲對3種算法進行定位誤差比較，結(jié)果如圖7。

定位誤差

定位誤差

圖7 不同Wk和Vk擾動下的跟蹤結(jié)果Fig.7 The location results with different Wk and Vk value

由圖7分析可知該算法可以較好地穩(wěn)定系統(tǒng)的輸出，提高系統(tǒng)的定位精度，為SLAM中的地圖構(gòu)建提供可靠支撐。

4 結(jié)語

本文利用ERTS平滑算法的離線估計功能和奇異值分解法避免逆運算的優(yōu)點，對標準EKF-SLAM進行了改進，提出了一種改進的EKF-SLAM算法。文中利用Turtlebot移動機器人平臺的里程計和陀螺儀采集到的位姿信息進行前向擴展卡爾曼濾波器初步濾波后，通過奇異值分解法避免了EKF的求逆運算，并使用后向ERTS平滑算法離線估計，最終獲得位置和姿態(tài)最優(yōu)估計。仿真結(jié)果和實驗證明該算法比標準的EKF-SLAM算法和ERTSS-SLAM算法具有更好的精確性和抗干擾能力。

[1] HUGH-DURRANT W, TIM Bailey. Simultaneous Localisation and Mapping (SLAM):Part I The Essential Algorithms[C]. 2010 International Conference on Intelligent Robots and Systems (IROS), 2010: 1-9.

[2] 楊樂,謝俐. 移動機器人SLAM問題新解[J]. 制造業(yè)自動化, 2013, 35(11): 43-46.

[3] BRADLEY H. Introduction to SLAM(Simultaneous Localization And Mapping)[R]. Las Vegas: Computer Science education, 2010.

[4] GURKAN T,Gulez Kayhan,Mumcu T.Veli. Evaluations of Different Simultaneous Localization and Mapping (SLAM) Algorithms[C].2012 IEEE ransactions on Robotics and Automation,2012: 2693-2698.

[5] LUIGI D, ANDREA G, PIETRO M. A SLAM algorithm for indoor mobile robot localization using an Extended Kalman Filter and a segment based environment mapping[C] Proceedings of the 2012 IEEE International Conference, 2013: 17-23.

[6] 張毅,趙黎明,羅元. 基于MIEKF的移動機器人同時定位與地圖構(gòu)建研究[J]. 計算機應(yīng)用研究, 2011, 28(3): 902-904.

[7] WANG Dao-bin, LIANG Hua-wei,MEI Tao. Lidar Scan Matching EKF-SLAM Using the Differential Model of Vehicle Motion[C] 2013 IEEE Intelligent Vehicles Symposium (IV), 2013: 908-912.

[8] 杜航原,郝燕玲,趙玉新. 基于模糊自適應(yīng)卡爾曼濾波的SLAM算法[J]. 華中科技大學(xué)學(xué)報:自然科學(xué)版, 2012, 40(1): 58-62.

[9] 石杏喜,趙春霞,郭劍輝. 基于PF/CUKF/EKF的移動機器人SLAM框架算法[J]. 電子學(xué)報, 2009, 37(8): 1865-1868.

[10] 王宏健,王晶,邊信黔,等. 基于組合EKF的自主水下航行器SLAM[J]. 機器人, 2012, 34(1): 56-64.

[11] 武二永,項志宇,沈敏一,等. 大規(guī)模環(huán)境下基于激光雷達的機器人SLAM算法[J]. 浙江大學(xué)學(xué)報:工學(xué)版, 2007, 41(12): 1982-1986.

[12] 宮曉琳,張蓉,房建成. 固定區(qū)間平滑算法及其在組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 2012, 20(6): 687-693.

[13] Razali Saifudin-Bin. Smoothing Techniques and a Spherical Simplex Unscented Transformation in Solving a SLAM Problem[D]. Okayama : Okayama University, 2012:44-50.

[14] 宮曉琳,房建成. 基于SVD的R_T_S最優(yōu)平滑在機載SAR運動補償POS系統(tǒng)中的應(yīng)用[J]. 航空學(xué)報, 2009, 30(2): 311-318.

[15] SERGIO M, PAOLA L, LAURA S, et al. Localization and Mapping Approximation for Autonomous Ground Platforms, Implementing SLAM Algorithms[C].2014 IEEE,2014: 139-145.

[16] ZHU J H, ZHENG N N, YUAN Z J, et al.A SLAM Algorithm Based On The Central Difference Kalman Filter [C].2009 Intelligent Vehicles Symposium IEEE，2009：123-128.

[17] ZHANG H T, RONG J, ZHONG X C. The Performance Comparison And Algorithm Analysis Of First/Second Order EKF And Smoother For GPS/DR Navigation [C]. 11th IEEE Inter. Conf. on Comm. Technology. 2008: 432-437.

Improved EKF-SLAM Method and Its Implementation on the Turtlebot

XUE Yong-sheng, WANG Heng, ZHANG Hua, HUO Jian-wen

(KeyLaboratoriesforSpecialEnvironmentRobotTechnologyofSichuanProvince,SouthwestUniversityofScienceandTechnology,Mianyang621010,Sichuan,China)

Targeting to tackle those problems like too massive calculation, weak timeliness, low accuracy and robustness, etc., during the process of the mobile robot’s simultaneous localization and mapping (SLAM) by using the way of the standard extended Kalman filter (EKF), combining the smoothing algorithm and singular value decomposition algorithm, a new SLAM algorithm (ERTSS-SLAM) is proposed in this paper. By the forward EKF method, the improved ERTSS-SLAM algorithm conducts the optical pose estimation of the mobile robot’s odometer and gyroscopes pose information and then avoid dispersion with the standard ERTS smoother, and at the same time, the inverse calculation is prevented by using the singular value decomposition algorithm, so as to enhance the system’s timeliness, robustness and accuracy accordingly. Experimental results on the Turtlebot show that the new ERTSS-SLAM algorithm is very efficient and stable during the SLAM application.

Simultaneous localization and mapping; Smoothing algorithm; Singular Value Decomposition; Turtlebot

2014-10-10

四川省科技廳科技支撐計劃項目(2014RZ0049)；2014四川省科技支撐計劃項目(2014GZ0021)。

薛永勝(1989—)，男，碩士研究生，研究方向為工業(yè)智能控制、智能檢測技術(shù)。E-mail:xys1989225@163.com

TP273+.2

A

1671-8755(2015)01-0054-06