基于擴展卡爾曼濾波器的移動機器人SLAM研究

1、第20卷第1期電子設計工程2012年1月Jan.2012基于擴展卡爾曼濾波器的移動機器人SLAM研究時也，吳懷宇，徐文霞，彭晟遠（武漢科技大學信息科學與工程學院，湖北武漢430081）摘要：文中闡述一種移動機器人SLAM問題的解決方法，首先利用激光測距儀得到環(huán)境中障礙物的監(jiān)測圖表，然后增量的構建全局地圖。利用擴展卡爾曼濾波器（EKF）創(chuàng)建移動機器人定位計算的有界估量；最后通過仿真和物理實驗驗證了該方法的正確性。可為解決機器人在未知環(huán)境下的地圖創(chuàng)建與定位問題提供理論依據(jù)，具有實際意義。關鍵詞：移動機器人；激光測距儀；擴展卡爾曼濾波器；SLAM中圖分類號：TP242.6文獻標識碼：A文章編號：16

2、746236（2012）01-0104-03AmethodofmobilerobotsSLAMbasedonextendedKalmanfilterSHIYe，WUHuai-yu，XUWen-xia，PENGSheng-yuan（CollegeofInformationScienceandEngineering，WuhanUniversityofScienceandTechnology，Wuhan430081，China）Abstract:AmethodoftheSLAMalgorithmforamobilerobotisproposed.Usinglaserrangefinder，canre

3、lativeobstacleobservationprofile，amapoftheworldcanbebuildaccordingtotheproposedboservations.simultaneously，aboundedestimateofmobilerobotlocationbytheextendedKalmanfilter，F(xiàn)inally，thesimulationandexperimentresultsverifiedthecorrectnessoftheforwardSLAMalgorithmlandtheinverseSLAMsolution.Theresultcanbef

4、urtherusedfornavigationaboutunknownenvironmentandhaveapracticalsignificance.Keywords:mobilerobot；laserrangefinder；extendedKalmanfilter；simultaneouslocalizationandmapping在當今社會，移動機器人應具備在未知環(huán)境下的自主導航能力，才有其實際存在價值1。近年來，一些特大自然災害的發(fā)生，更使人們注重機器人的實用性和準確性。機器人實現(xiàn)自主導航的核心問題是地圖構建與定位（SLAM），解決并使用地圖。最后通過MATLAB仿真算法和在未知環(huán)境下

5、實驗，使移動機器人從起始點繞過不定數(shù)目的障礙物后再返回到起始點，驗證理論的正確性和算法的精確性。SLAM問題才可以使機器人在未知環(huán)境中的工作更高效和更科學。移動機器人的SLAM算法研究已是機器人研究領域中的最熱門、最豐富、最具開拓性的課題之一2-3。移動機器人的地圖創(chuàng)建與定位問題是機器人領域一個基礎而關鍵的問題。文獻4針對缺少先驗地圖支持的室內環(huán)境，提出利用激光測距儀和單目視覺的信息融合來解決室內環(huán)境中移動機器人的SLAM問題。文獻5提出基于單目視覺和里程計的SLAM方法，采用尺度不變特征變換算法提取特征，并用擴展卡爾曼濾波更新地圖。文獻7提出基于概率的移動機器人SLAM算法框架，應用貝葉斯規(guī)

6、則作為理論基礎，建立移動機器人SLAM算法的概率表示模型。文獻8-9針對1機器人地圖創(chuàng)建與定位目前廣泛使用卡爾曼濾波器或粒子濾波器來解決SLAM問題。粒子濾波雖然能夠比較精確地表達基于觀測量和控制量的后驗概率分布，但其計算量大而精度低。擴展卡爾曼濾波器可以用在非線性系統(tǒng)上，且計算量相比粒子濾波少，因此選擇擴展卡爾曼濾波器算法。項目組的移動機器人MT-R裝有：雙目傳感器、聲納傳感器、激光測距儀、里程計，機器人可裝載機械臂。激光測距儀是移動機器人的基本感知裝置，用于獲得環(huán)境中障礙物距離信息，這些信息可用于辨識障礙物，并用障礙物作為路標。根據(jù)路標之間的關聯(lián)，計算路標參數(shù)的狀態(tài)方程以及觀測方程，并使之

7、系統(tǒng)化。建立障礙物與路標之間的關聯(lián)方程，由此可得一個基于障礙物作為路標的全局地圖。SLAM問題的高維特性，提出了FastSLAM解決方案。文中以課題組的移動機器人MT-R為研究對象，給定路標的參數(shù)值，利用激光測距儀計算路標之間的關系方程。重點研究用障礙物之間的空間邏輯關系，采用障礙物作為機器人的路標并以此來代替固有路標，估算出機器人在全局坐標系中移動的距離，使機器人在線構造地圖的同時，能夠提取收稿日期：2011-10-16稿件編號：20111007122.1基于擴展卡爾曼濾波器的SLAM算法狀態(tài)方程首先用Fx，F(xiàn)y表示全局坐標系，Gx，Gy表示局部坐標系來建立機器人的空間狀態(tài)方程。為了構建全局

8、地圖，擴展卡爾曼濾波器中的狀態(tài)向量包含移動機器人的狀態(tài)部分（X）及基金項目：國家自然科學基金項目（61075087）作者簡介：時也（1987），男，安徽宿州人，碩士研究生。研究方向：電路與系統(tǒng)、智能自動化。104時也，等基于擴展卡爾曼濾波器的移動機器人SLAM研究路標狀態(tài)部分（G）。Fx（k）和Fy（k）為機器人在時間k時在全局坐標系中的位置坐標。（k）=cos（k），（k）=sin（k），為狀態(tài)變量則狀態(tài)向量。Xu（k）為機器人的狀態(tài)向量：Xu（k）=Fx（k）Fy（k）（k）（k）T（1）機器人的狀態(tài)向量和路標狀態(tài)向量應滿足公式（2）和公式（3）：kX）kkkkkkW）00kkku（k+1

9、u（k+1kkkkXu（k）kkkkkFu（k）kkkkkkGkkkkkkkk1kkn0kkG1kk1kkkkkkkkkkk=0Ikkkk+Okkkkkkkkkk（2）kkkkkkkkkkkkkkkkkkGkNkkk00IkNkkkGkNkkkOkkkkkkkkNkkX（k+1）=W（k+1）X（k）+F（k）（3）機器人狀態(tài)轉移矩陣FU（k）滿足公式（4）：kk+1）-d（k+1）sin（k+1）kkkk10d（k+1）·cos（·kkkkkFk01d（k+1）·sin（k+1）d（k+1）·cos（k+1）kU（k+1）=k00cos（k+1）-si

10、n（k+1）kkkkkkkkk00sin（k+1）cos（k+1）kkkk（4）系統(tǒng)噪聲wu（k）滿足公式（5）：kkk（k）-d（k+1）·（k）kkkkkkkwk（k）d（k+1）·（k）ku（k）=kkkkkkknd（k）（）（k+1）·Nv（k）（5）k0-（k）vknkk=Bkkk（k）kkk假定在全局坐標系中，路標是靜止對象，因此，如果Fxn（k）Fyn（k）T是Gn，那么機器人的狀態(tài)方程如下：Gn（k+1）=Gn（k）=Gn（6）在方程（1）中，路標的過程模型是個單位矩陣，因此GN在以及Xu（k）中的狀態(tài)向量:X（k）=XTV（k）G1T，GnT，G

11、NTT（7）2.2觀測方程在局部坐標系中，路標m的局部坐標為zm（k）=Gxm（k）Gym（k）。在k時與全局坐標系中路標n的坐標Gn相關聯(lián)，矩陣Hn（k）定義為：H（）（）00（k）-（k）00n（k）=k-kk-（k）-（k）00（k）（k）00k（8）那么對應狀態(tài)向量X（k）的觀測方程：軌Zm（k）=Hn（k）X（k）（9）觀測矩陣Hn（k）動態(tài)的改變了全局坐標系和局部坐標系中的路標之間的關系，在H軌（k）（k）n（k）中的參數(shù)-（k）-（k）k會依據(jù)路標的改變而改變。2.3擴展卡爾曼濾波器的應用擴展卡爾曼濾波器由狀態(tài)方程及觀測方程組成，工作流程為：預測步驟、檢測步驟、更新步驟。這3個步

12、驟并不是獨立的，而是循序漸進并且循環(huán)往復的。1）預測步驟在時間k-1，狀態(tài)向量的預測值X（k-1）為軒X（k-1k-1），協(xié)方差矩陣為P（k-1k-1），則在時間k時，X軒（kk-1）=F（k）軒X（k-1k-1）。X軒（kk-1）的估計誤差的協(xié)方差矩陣為：P（kk-1）=W（k）P（k-1k-1）W（k）T+B（k）L（k）B（k）T（10）L（k）是2×2矩陣，它的對角由方差rd（k）和r（k）定義，rd（k）為d（k）的預測誤差，r（k）為（k）的預測誤差。2）檢測步驟在時間k，當全局坐標系中的路標n的坐標值GTN與局部坐標系中的路標m的坐標值關聯(lián)時，則基于軒X（kk-1）的觀

13、測值Z軌m（kk-1）為：軌Zm（kk-1）=Hn（k）X軒（kk-1）（11）R為觀測誤差的方差矩陣，得到Dn（k）在相關的n點路標被定義為：（Gxm（k）Gym（k）TDn（k）=P（kk-1）Hn（k）THn（k）P（kk-1）Hn（k）T+R）-1（12）3）更新步驟激光測距儀檢測的局部坐標（Gxm（k）Gym（k）T中的路標的位置為Zm（k），估量值軒X（kk-1）更新為：X贊（kk）=X贊（kk-1）+Kn（k）（Zm（k）-Z贊m（kk-1）（13）協(xié)方差矩陣P（kk-1）更新為：P（kk）=P（kk-1）-Kn（k）Hn（k）P（kk-1）（14）由檢測和更新步驟可得到全局坐標

14、系中的路標坐標Gn和局部坐標系中坐標Zm（k）的轉換公式，當有m個路標被偵測，則檢測步驟和步驟更新重復m次。2.4不同坐標下參數(shù)的轉換路標在不同坐標系下的轉換是在動態(tài)改變的，如圖1所示，dmin表示預測路標位置與實際路標位置誤差的最小值，計算出的全局坐標（Fx軇nFy軇n）T和使用擴展卡爾曼濾波器估算出的全局坐標（FxnFyn）應滿足公式（15）：姨（Fxn-Fx軇n）2+（Fyn-Fy軇n）2dmin（15）圖1不同坐標系下的參數(shù)轉換Fig.1Parameterscoversionbetweendifferentcoordinates局部坐標系中的坐標（Gxm（k）Gym（k）T與全局坐標系

15、中坐標（Fx軇n，F(xiàn)y軇n）T應滿足公式（16）：kkkkkkkFx軇N+1kkk=軌（k）-·Gxm（k）kkkkkk-（k）d（k）（k）+Fx（k）（k）d（k）（k）+Fyr（k-1）·kr（kkFy軇kN+1kkk001k-1）kkkkGym（k）kkk（16）kkkk1kkkk1kkk105電子設計工程2012年第1期3實驗及分析使移動機器人在環(huán)境中行走一個閉合的環(huán)路。機器人的移動速度約為0.56m/s，光電編碼器的采樣周期為5ms，激光測距儀的采樣周期為0.5s。整個過程持續(xù)90.2s。激光測距儀的掃描范圍為0°180°，角度分辨率為0.5

16、°，每掃描一次產生361個數(shù)據(jù)。圖2是機器人的位姿對比曲線，激光測距儀的最大感知距離為8m，范圍不確定度為0.03m。圖中虛線表示在死區(qū)地帶的機器人位姿曲線，實線為經過EKF校正的機器人位姿曲線。圖2機器人的位姿對比曲線Fig.2Curvesofrobotpose圖3是機器人的位姿誤差曲線，可以看出：在前90次的定位中，的校正量保持在±0.9cm之內，的校正量保持在±2.0cm之內，的校正量在±0.8°之內。圖3機器人的位姿誤差曲線Fig.3Theerrorcurveofrobotpose4結束語文中針對移動機器人的地圖創(chuàng)建與定位問題，采用基于

17、擴展卡爾曼濾波器法，利用激光測距儀建立起參考坐標系，以障礙物作為路標，從而使機器人辨識自身位置，使用擴展106卡爾曼濾波器來代替粒子濾波器以獲得實時地圖。最后通過仿真和物理實驗驗證了準確性。機器人的定位與地圖創(chuàng)建問題，是機器人領域的關鍵技術之一，為后續(xù)的解決機器人的SLAM導航和死區(qū)定位的精度研究提供了理論依據(jù)。參考文獻：1蔡自興，賀漢根，陳虹.未知環(huán)境中移動機器人導航控制研究的若干問題J.控制與決策，2002，17（4）:385-390.CAIZi-xing，HEHan-gen，CHENHong.Someissuesformobilerobotsnavigationunderunknowne

18、nvironmentsJ.ControlandDecision，2002，17（4）:385-390.2趙冬斌，易建強.全方位移動機器人導論M.北京:科學出版社，2010.3湯曉.基于激光測距儀的移動機器人同時定位和地圖創(chuàng)建D.濟南：山東大學，2007.4莊嚴，王偉.移動機器人基于激光測距和單目視覺的室內同時定位和地圖構建J.自動化學報，2005，31（6）:925-933.ZHUANGYan，WANGWei.Mobilerobotbasedonlaserrangefinderandmonocularvisionindoorsimultaneouslocalizationandmapbuild

19、ingJ.ActaAutomaticaSinica，2005，31（6）:925-933.5方正，佟國峰，徐心和.基于貝葉斯濾波理論的自主機器人自定位方法研究J.控制與決策，2006，21（8）：841-846，862.FANGZheng，TONGGuo-feng，XUXin-he.Studyofautono-mousrobotself-locatizationmethodsbasedonbayesianfiltertheoryJ.ControlandDecision，2006，21（8）：841-846，862.6王彭林，石守東，洪小偉.基于單目視覺和里程計的SLAM算法研究J.計算機仿真，2008，25（10）：172-175.WANGPeng-lin，SHIShou-dong，HONGXiao-wei.ASLAMalogorithmbasedonmonocularvisionandadometerJ.Com