一種基于觀測范圍約束的改進EKF-SLAM算法

,,

(浙江工業(yè)大學(xué) 信息工程學(xué)院，浙江 杭州 310023)

具備可靠的定位功能是移動機器人實現(xiàn)全自主的必備條件.移動機器人的自主定位和地圖構(gòu)建問題一直是機器人領(lǐng)域的研究熱點.已知環(huán)境地圖的機器人定位和已知機器人位置的地圖構(gòu)建作為兩個獨立問題已經(jīng)被廣泛研究，諸多學(xué)者提出了多種有效的解決方法[1-2].將這兩個問題結(jié)合起來研究稱為同時定位與地圖構(gòu)建(Simultaneous localization and mapping, SLAM)問題，由Smith，Self和Cheeseman最初提出[3].同時定位與地圖構(gòu)建問題可以描述為移動機器人在一個完全未知的環(huán)境中，從未知位置開始遞增式的移動，在移動過程中機器人通過自身所攜帶的傳感器獲取的環(huán)境觀測值來構(gòu)建環(huán)境地圖，同時利用構(gòu)建的環(huán)境地圖更新自身在環(huán)境中的位置，即進行自主定位[4].近年來，SLAM問題已受到越來越多機器人領(lǐng)域?qū)W者的關(guān)注，被認為是實現(xiàn)機器人全自主的關(guān)鍵[4-6].SLAM問題主要采用基于概率估計的方法來解決[7-8]，其中基于擴展卡爾曼濾波的SLAM算法(EKF-SLAM)使用最為普遍[9-11].然而標(biāo)準(zhǔn)EKF-SLAM算法的一個明顯缺點是計算量大，面向大規(guī)模復(fù)雜環(huán)境時算法過于復(fù)雜，運行效率低，且線性誤差會隨著觀測路標(biāo)的增多而加大，容易降低定位精度并導(dǎo)致地圖的不一致[12].

筆者提出了一種基于觀測范圍約束的改進EKF-SLAM算法，通過約束移動機器人的觀測范圍，使機器人在任意時刻只對處于約束范圍內(nèi)的路標(biāo)進行運算，同時刪除系統(tǒng)狀態(tài)向量中超出約束范圍的匹配路標(biāo)，因此系統(tǒng)狀態(tài)向量的維數(shù)不再隨觀測路標(biāo)的加入不斷增加，降低了算法計算量并減小了雅可比矩陣計算過程中的線性誤差，從而提高了算法的運行效率和定位精度，通過仿真驗證了本方法能有效提高算法定位精度和運行效率.

1 移動機器人狀態(tài)模型

1.1 移動機器人運動模型

移動機器人在全局坐標(biāo)系的狀態(tài)用位姿(位置和朝向)表示.令機器人在k時刻的位姿向量為Xv,k=(xk,yk,θk)T，其中(xk,yk)為k時刻機器人在全局坐標(biāo)系的位置坐標(biāo)，θk為k時刻移動機器人的前進方向與全局坐標(biāo)系X軸正方向所成的夾角.以雙輪差動驅(qū)動移動機器人為例，如圖1所示，其中L為左輪和右輪之間連接軸的長度,a為傳感器與車輪中心的水平距離,b為傳感器與左輪和右輪之間連接軸中心(即移動機器人質(zhì)心)的垂直距離,C點為移動機器人質(zhì)心的位置.

圖1 雙輪移動機器人

圖2所示為移動機器人運動模型，XwOwYw為全局坐標(biāo)系，xlolyl為傳感器坐標(biāo)系.由于移動機器人相對于環(huán)境中特征點的位置關(guān)系是通過傳感器觀測獲得的距離和方向信息計算而得，為了計算方便，采用傳感器坐標(biāo)系，即以傳感器位置為坐標(biāo)原點，移動機器人前進方向為X軸正方向，垂直X軸方向向上為Y軸正方向建立坐標(biāo)系.機器人在控制輸入uk=(vR,k,vL,k)T的作用下從k-1時刻運行到k時刻，其中的vL,k和vR,k分別表示k時刻機器人左輪和右輪的線速度[12]，因此機器人質(zhì)心在k時刻的瞬時線速度vk和角速度wk公式為

(1)

移動機器人的運動學(xué)方程為

(2)

其中T為采樣周期.

圖2 移動機器人運動模型

1.2 移動機器人觀測模型

mi(k+1)=mi(k)=mi

(3)

或

(4)

移動機器人通過傳感器獲得自身與環(huán)境中第i個路標(biāo)的觀測值zi=(ri,φi)T，其中ri是傳感器與路標(biāo)之間的距離，φi為路標(biāo)和傳感器之間的連線與移動機器人前進方向所成的夾角.由此可得移動機器人觀測模型為

(5)

2 標(biāo)準(zhǔn)EKF-SLAM算法

Xv,k=f(Xv,k-1,uk)+εk-1

(6)

zk=h(Xv,k)+ηk

(7)

其中：εk和ηk是均值等于零、協(xié)方差矩陣分別為Qk和Rk的高斯白噪聲，即εk～N(0,Qk)，ηk～N(0,Rk).EKF-SLAM算法主要包括預(yù)測過程、觀測過程、更新過程和狀態(tài)向量擴展過程[3].

(8)

其中：Pvv為機器人位姿的協(xié)方差矩陣；Pvm和Pmv分別為機器人和路標(biāo)的互協(xié)方差矩陣以及路標(biāo)和機器人的互協(xié)方差矩陣；Pmm為路標(biāo)的協(xié)方差矩陣.由移動機器人運動模型可得k時刻系統(tǒng)狀態(tài)向量的預(yù)測值和協(xié)方差矩陣為

(9)

(10)

(11)

(12)

(13)

(14)

3) 更新過程.利用預(yù)測值和觀測值來更新系統(tǒng)狀態(tài)向量及其協(xié)方差矩陣，步驟為

(15)

(16)

Pk|k=(I-KkHk)Pk|k-1

(17)

其中Kk為卡爾曼增益.

4) 狀態(tài)向量擴展過程.傳感器k時刻觀測到的環(huán)境路標(biāo)可能同時有圖中已有路標(biāo)和新路標(biāo).已有路標(biāo)用來更新狀態(tài)預(yù)測值，新路標(biāo)則通過初始化處理后加入系統(tǒng)狀態(tài)向量.假設(shè)k時刻觀測到第i個路標(biāo)是新路標(biāo)，觀測值為zi,k=(ri,k,φi,k)T，則新路標(biāo)在全局坐標(biāo)系的位置為

(18)

把新路標(biāo)加入系統(tǒng)狀態(tài)向量，得到擴展后的新狀態(tài)向量為

(19)

新狀態(tài)向量的協(xié)方差矩陣為

(20)

(21)

標(biāo)準(zhǔn)EKF-SLAM算法中，系統(tǒng)狀態(tài)向量是一個2n+3維的矢量，其中，n為已觀測到的路標(biāo)數(shù)量，每一路標(biāo)有2個變量，機器人位姿為3個變量.對于含有成千上萬個路標(biāo)的大規(guī)模復(fù)雜環(huán)境，隨著移動機器人所觀測路標(biāo)的增多，系統(tǒng)狀態(tài)向量的維數(shù)不斷增加，協(xié)方差矩陣和雅可比矩陣的計算量急劇加大，同時計算雅可比矩陣的線性誤差隨之增加，從而導(dǎo)致算法運行效率和定位精度的降低.

3 具有觀測范圍約束的EKF-SLAM算法

通常傳感器有一定的觀測范圍，對于遠距離處散落的觀測點較為稀疏，無法判斷觀測到的物體是否為環(huán)境路標(biāo)，易將環(huán)境中某些不確定物體誤認為是路標(biāo)，從而構(gòu)建出錯誤地圖.為了將這些處于傳感器觀測范圍內(nèi)但距離較遠的不確定物體進行剔除，筆者設(shè)定一個最大觀測距離，把傳感器觀測范圍限定在一個合適區(qū)域內(nèi).

(22)

則認為路標(biāo)mi是已觀測到的mj，否則認為是第一次觀測到路標(biāo)mi，其中ε為一個給定的足夠小的標(biāo)量.在標(biāo)準(zhǔn)EKF-SLAM算法的第二步觀測過程中對觀測值添加約束條件，對算法進行改進，具體方法為：

1) 對于非第一次觀測的路標(biāo)mi，如果滿足條件ri,k≤RMaxRange，則通過標(biāo)準(zhǔn)EKF-SLAM算法的第三步更新過程來更新其位置；如果ri,k>RMaxRange，則在系統(tǒng)狀態(tài)向量中刪除路標(biāo)mi，同時標(biāo)準(zhǔn)EKF-SLAM算法的第四步狀態(tài)向量擴展過程變?yōu)闋顟B(tài)向量縮減過程.

2) 對于第一次觀測的路標(biāo)mi，如果滿足條件ri,k≤RMaxRange，則按照標(biāo)準(zhǔn)EKF-SLAM算法繼續(xù)更新和向量擴展過程；如果ri,k>RMaxRange，則判斷其為超出該局部地圖的路標(biāo)并予以忽略.

綜上所述，具有觀測范圍約束的EKF-SLAM算法如下：

步驟2對傳感器的觀測范圍添加約束條件，由上述方法對機器人在k時刻觀測到的路標(biāo)進行判斷，對滿足條件的路標(biāo)由式(12,13)計算其新息vk和協(xié)方差矩陣Sk.

步驟4將首次觀測的路標(biāo)加入到狀態(tài)向量，同時刪除非第一次觀測并超出約束范圍的路標(biāo)，由式(19,20)擴展?fàn)顟B(tài)向量及其協(xié)方差矩陣.

步驟5判斷是否構(gòu)建出完整地圖，若是則退出，否則返回步驟1繼續(xù)構(gòu)建地圖.

添加約束條件后，移動機器人任意時刻只對滿足約束條件的路標(biāo)進行運算，系統(tǒng)狀態(tài)向量的維數(shù)不會隨著觀測路標(biāo)的增加而不斷加大，計算量也不會隨著更多路標(biāo)的加入而急劇增加，同時降低了線性誤差，提高了算法的運行效率和定位精度.

4 仿真與結(jié)果分析

用MATLAB建立仿真環(huán)境地圖，地圖區(qū)域大小為100 m×100 m，設(shè)定30個路標(biāo).假定傳感器位于機器人質(zhì)心位置，即a=0,b=0.仿真過程將機器人視為質(zhì)點，初始位置設(shè)在1號路標(biāo)附近為(25,90),速度v=2 m/s為其在路標(biāo)附近建立移動軌跡，傳感器觀測范圍為25 m，180度，約束條件RMaxRange=20 m,ε=0.1 m.讓機器人在該環(huán)境下分別采用標(biāo)準(zhǔn)EKF-SLAM算法和具有約束的EKF-SLAM算法進行仿真實驗，仿真結(jié)果如圖3所示.

圖3 機器人軌跡和路標(biāo)位置仿真結(jié)果

圖3中，實線表示機器人的真實移動軌跡，點線代表標(biāo)準(zhǔn)EKF-SLAM算法估計的機器人移動軌跡，虛線為具有約束的 EKF-SLAM算法估計的機器人移動軌跡，六角形為路標(biāo)真實位置，三角形為標(biāo)準(zhǔn)EKF-SLAM算法估計的路標(biāo)位置，正方形為具有約束的EKF-SLAM算法估計的路標(biāo)位置.由圖3可以看出：筆者提出的具有約束的EKF-SLAM算法比標(biāo)準(zhǔn)EKF-SLAM算法具有更好的估計效果.

由于仿真實驗中的系統(tǒng)過程噪聲和觀測噪聲都是隨機產(chǎn)生的，每一次的仿真結(jié)果都會有所差別，因此不能只用一次仿真實驗的結(jié)果來判斷算法的性能.為了更加精確的比較標(biāo)準(zhǔn)EKF-SLAM算法和具有約束的EKF-SLAM算法的定位精度，在相同仿真環(huán)境下對兩種算法分別進行500 次Monte Carlo實驗，獲取機器人位置和路標(biāo)位置的均方誤差(MSE)曲線.在k時刻x的均方誤差(MSE)值定義為

(23)

圖4,5分別給出了500次Monte Carlo實驗的機器人位置和路標(biāo)位置在X方向和Y方向的均方誤差(MSE)曲線，表1列出了機器人和路標(biāo)位置及其X，Y方向的均方根誤差.圖4中機器人位置X，Y方向的均方誤差曲線都存在兩處突變的峰值，由圖3可知：由于機器人在轉(zhuǎn)角處轉(zhuǎn)彎時車輪偏轉(zhuǎn)角度突然變大，從而使算法中狀態(tài)向量的預(yù)測值與真實值之間的偏差增加，導(dǎo)致估計誤差加大.隨著機器人跨過轉(zhuǎn)角重新回到直線路徑后，誤差又逐漸回歸到正常范圍.由圖4，5和表1可知：具有約束的EKF-SLAM算法比標(biāo)準(zhǔn)EKF-SLAM算法提高了機器人和路標(biāo)的定位精度.

為了驗證算法運行效率是否有所提高，測試了五組不同路標(biāo)數(shù)量情況下標(biāo)準(zhǔn)EKF-SLAM算法和具有約束EKF-SLAM算法構(gòu)建地圖需要花費的時間，測試結(jié)果列入表1.計算可得5組測試中具有約束EKF-SLAM算法比標(biāo)準(zhǔn)EKF-SLAM算法平均構(gòu)建地圖時間節(jié)省18.42 s，證明了具有約束EKF-SLAM算法提高了算法運行效率.

圖4 500次Monte Carlo實驗的機器人位置X方向和Y方向均方誤差(MSE)曲線

圖5 500次Monte Carlo實驗的路標(biāo)位置X方向和Y方向均方誤差(MSE)曲線

表1 機器人和路標(biāo)的均方根誤差及不同路標(biāo)數(shù)量時構(gòu)建地圖所需時間

5 結(jié) 論

通過對移動機器人的觀測范圍添加約束條件的方法，對標(biāo)準(zhǔn)EKF-SLAM算法存在的計算量大、運行效率和定位精度低等問題進行了改善，提出了一種基于觀測范圍約束的改進EKF-SLAM算法.通過將移動機器人的觀測范圍控制在約束條件內(nèi)，并在機器人移動過程中刪除系統(tǒng)狀態(tài)向量里超出約束范圍的匹配路標(biāo)，使機器人任意時刻只對滿足約束條件的路標(biāo)進行運算，因此系統(tǒng)狀態(tài)向量的維數(shù)不會隨著觀測路標(biāo)的增加而不斷加大，減小算法計算量的同時降低了線性誤差，提高了算法的運行效率和定位精度.通過仿真驗證了所提方法能夠有效提高算法的定位精度和運行效率.

參考文獻：

[1] BORENSTEIN J, EVERETT H R, FENG L, et al. Mobile robot positioning: sensors and techniques[J]. Journal of Robotic Systems, Special Issue on Mobile Robots,1997,14(4):231-249.

[2] MORAVEC H P, ELFES A. High resolution maps from wide angle sonar[C]// Proceedings of the IEEE International Conference on Robotics and Automation. St Louis, MO: IEEE,1985:116-121.

[3] SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[C]// Proceedings of Conference on Uncertainty in Artificial Intelligence. Amsterdam: North-Holland,1988:435-461.

[4] DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: part I[J]. IEEE robotics and Automation Magazine,2006,13(2):99-110.

[5] DISSANAYAKE GAMINI M W M, NEWMAN P, CLARK S, et al. A solution to the simultaneous localization and map building (SLAM) problem[J]. IEEE Transactions on Robotics and Automation,2001,17(3):229-241.

[6] GUIVANT J E, NEBOT E M. Optimization of the simultaneous localization and map-building algorithm for real-time implementation[J]. IEEE Transactions on Robotics and Automation,2001,17(3):242-257.

[7] 陳衛(wèi)東,張飛.移動機器人的同步自定位與地圖創(chuàng)建研究進展[J].控制理論與應(yīng)用,2005,22(3):455-460.

[8] THRUN S, BURGARD W, FOX D. A probabilistic approach to concurrent mapping and localization for mobile robots[J]. Machine Learning,1998,31(5):29-53.

[9] CHOI M Y, SAKTHIVEL R, CHUNG W K. Neural network-aided extended kalman filter for SLAM problem[C]// Proceedings of the IEEE International Conference on Robotics and Automation. Italy: IEEE,2007:1686-1690.

[10] JEONG-GWAN K, SU-YONG A, SE-YOUNG O. Modified neural network aided EKF based SLAM for improving an accuracy of the feature map[C]// Proceedings of the International Joint Conference on Neural Networks. Barcelona: IEEE,2010:1-7.

[11] CHOI K S, LEE S G. Enhanced SLAM for a mobile robot using extended kalman filter and neural networks[J]. International Journal of Precision Engineering and Manufacturing,2010,11(2):255-264.

[12] BAILEY T, NIETO J, GUIVANT J, et al. Consistency of the EKF-SLAM algorithm[C]// Proceedings of the IEEE International Conference on Intelligent Robots and Systems. Beijing: IEEE,2006:3562-3568.