基于扩展卡尔曼滤波器的移动机器人SLAM研究

1、第20卷第1期电子设计工程2012年1月Jan.2012基于扩展卡尔曼滤波器的移动机器人SLAM研究时也，吴怀宇，徐文霞，彭晟远（武汉科技大学信息科学与工程学院，湖北武汉430081）摘要：文中阐述一种移动机器人SLAM问题的解决方法，首先利用激光测距仪得到环境中障碍物的监测图表，然后增量的构建全局地图。利用扩展卡尔曼滤波器（EKF）创建移动机器人定位计算的有界估量；最后通过仿真和物理实验验证了该方法的正确性。可为解决机器人在未知环境下的地图创建与定位问题提供理论依据，具有实际意义。关键词：移动机器人；激光测距仪；扩展卡尔曼滤波器；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，Finally，thesimulationandexperimentresultsverifiedthecorrectnessoftheforwardSLAMalgorithmlandtheinverseSLAMsolution.Theresultcanbef

4、urtherusedfornavigationaboutunknownenvironmentandhaveapracticalsignificance.Keywords:mobilerobot；laserrangefinder；extendedKalmanfilter；simultaneouslocalizationandmapping在当今社会，移动机器人应具备在未知环境下的自主导航能力，才有其实际存在价值1。近年来，一些特大自然灾害的发生，更使人们注重机器人的实用性和准确性。机器人实现自主导航的核心问题是地图构建与定位（SLAM），解决并使用地图。最后通过MATLAB仿真算法和在未知环境下

5、实验，使移动机器人从起始点绕过不定数目的障碍物后再返回到起始点，验证理论的正确性和算法的精确性。SLAM问题才可以使机器人在未知环境中的工作更高效和更科学。移动机器人的SLAM算法研究已是机器人研究领域中的最热门、最丰富、最具开拓性的课题之一2-3。移动机器人的地图创建与定位问题是机器人领域一个基础而关键的问题。文献4针对缺少先验地图支持的室内环境，提出利用激光测距仪和单目视觉的信息融合来解决室内环境中移动机器人的SLAM问题。文献5提出基于单目视觉和里程计的SLAM方法，采用尺度不变特征变换算法提取特征，并用扩展卡尔曼滤波更新地图。文献7提出基于概率的移动机器人SLAM算法框架，应用贝叶斯规

6、则作为理论基础，建立移动机器人SLAM算法的概率表示模型。文献8-9针对1机器人地图创建与定位目前广泛使用卡尔曼滤波器或粒子滤波器来解决SLAM问题。粒子滤波虽然能够比较精确地表达基于观测量和控制量的后验概率分布，但其计算量大而精度低。扩展卡尔曼滤波器可以用在非线性系统上，且计算量相比粒子滤波少，因此选择扩展卡尔曼滤波器算法。项目组的移动机器人MT-R装有：双目传感器、声纳传感器、激光测距仪、里程计，机器人可装载机械臂。激光测距仪是移动机器人的基本感知装置，用于获得环境中障碍物距离信息，这些信息可用于辨识障碍物，并用障碍物作为路标。根据路标之间的关联，计算路标参数的状态方程以及观测方程，并使之

7、系统化。建立障碍物与路标之间的关联方程，由此可得一个基于障碍物作为路标的全局地图。SLAM问题的高维特性，提出了FastSLAM解决方案。文中以课题组的移动机器人MT-R为研究对象，给定路标的参数值，利用激光测距仪计算路标之间的关系方程。重点研究用障碍物之间的空间逻辑关系，采用障碍物作为机器人的路标并以此来代替固有路标，估算出机器人在全局坐标系中移动的距离，使机器人在线构造地图的同时，能够提取收稿日期：2011-10-16稿件编号：20111007122.1基于扩展卡尔曼滤波器的SLAM算法状态方程首先用Fx，Fy表示全局坐标系，Gx，Gy表示局部坐标系来建立机器人的空间状态方程。为了构建全局

8、地图，扩展卡尔曼滤波器中的状态向量包含移动机器人的状态部分（X）及基金项目：国家自然科学基金项目（61075087）作者简介：时也（1987），男，安徽宿州人，硕士研究生。研究方向：电路与系统、智能自动化。104时也，等基于扩展卡尔曼滤波器的移动机器人SLAM研究路标状态部分（G）。Fx（k）和Fy（k）为机器人在时间k时在全局坐标系中的位置坐标。（k）=cos（k），（k）=sin（k），为状态变量则状态向量。Xu（k）为机器人的状态向量：Xu（k）=Fx（k）Fy（k）（k）（k）T（1）机器人的状态向量和路标状态向量应满足公式（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）机器人状态转移矩阵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）系统噪声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，那么机器人的状态方程如下：Gn（k+1）=Gn（k）=Gn（6）在方程（1）中，路标的过程模型是个单位矩阵，因此GN在以及Xu（k）中的状态向量:X（k）=XTV（k）G1T，GnT，G

11、NTT（7）2.2观测方程在局部坐标系中，路标m的局部坐标为zm（k）=Gxm（k）Gym（k）。在k时与全局坐标系中路标n的坐标Gn相关联，矩阵Hn（k）定义为：H（）（）00（k）-（k）00n（k）=k-kk-（k）-（k）00（k）（k）00k（8）那么对应状态向量X（k）的观测方程：軌Zm（k）=Hn（k）X（k）（9）观测矩阵Hn（k）动态的改变了全局坐标系和局部坐标系中的路标之间的关系，在H軌（k）（k）n（k）中的参数-（k）-（k）k会依据路标的改变而改变。2.3扩展卡尔曼滤波器的应用扩展卡尔曼滤波器由状态方程及观测方程组成，工作流程为：预测步骤、检测步骤、更新步骤。这3个步

12、骤并不是独立的，而是循序渐进并且循环往复的。1）预测步骤在时间k-1，状态向量的预测值X（k-1）为軒X（k-1k-1），协方差矩阵为P（k-1k-1），则在时间k时，X軒（kk-1）=F（k）軒X（k-1k-1）。X軒（kk-1）的估计误差的协方差矩阵为：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的坐标值关联时，则基于軒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）协方差矩阵P（kk-1）更新为：P（kk）=P（kk-1）-Kn（k）Hn（k）P（kk-1）（14）由检测和更新步骤可得到全局坐标

14、系中的路标坐标Gn和局部坐标系中坐标Zm（k）的转换公式，当有m个路标被侦测，则检测步骤和步骤更新重复m次。2.4不同坐标下参数的转换路标在不同坐标系下的转换是在动态改变的，如图1所示，dmin表示预测路标位置与实际路标位置误差的最小值，计算出的全局坐标（Fx軇nFy軇n）T和使用扩展卡尔曼滤波器估算出的全局坐标（FxnFyn）应满足公式（15）：姨（Fxn-Fx軇n）2+（Fyn-Fy軇n）2dmin（15）图1不同坐标系下的参数转换Fig.1Parameterscoversionbetweendifferentcoordinates局部坐标系中的坐标（Gxm（k）Gym（k）T与全局坐标系

15、中坐标（Fx軇n，Fy軇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实验及分析使移动机器人在环境中行走一个闭合的环路。机器人的移动速度约为0.56m/s，光电编码器的采样周期为5ms，激光测距仪的采样周期为0.5s。整个过程持续90.2s。激光测距仪的扫描范围为0°180°，角度分辨率为0.5

16、°，每扫描一次产生361个数据。图2是机器人的位姿对比曲线，激光测距仪的最大感知距离为8m，范围不确定度为0.03m。图中虚线表示在死区地带的机器人位姿曲线，实线为经过EKF校正的机器人位姿曲线。图2机器人的位姿对比曲线Fig.2Curvesofrobotpose图3是机器人的位姿误差曲线，可以看出：在前90次的定位中，的校正量保持在±0.9cm之内，的校正量保持在±2.0cm之内，的校正量在±0.8°之内。图3机器人的位姿误差曲线Fig.3Theerrorcurveofrobotpose4结束语文中针对移动机器人的地图创建与定位问题，采用基于

17、扩展卡尔曼滤波器法，利用激光测距仪建立起参考坐标系，以障碍物作为路标，从而使机器人辨识自身位置，使用扩展106卡尔曼滤波器来代替粒子滤波器以获得实时地图。最后通过仿真和物理实验验证了准确性。机器人的定位与地图创建问题，是机器人领域的关键技术之一，为后续的解决机器人的SLAM导航和死区定位的精度研究提供了理论依据。参考文献：1蔡自兴，贺汉根，陈虹.未知环境中移动机器人导航控制研究的若干问题J.控制与决策，2002，17（4）:385-390.CAIZi-xing，HEHan-gen，CHENHong.Someissuesformobilerobotsnavigationunderunknowne

18、nvironmentsJ.ControlandDecision，2002，17（4）:385-390.2赵冬斌，易建强.全方位移动机器人导论M.北京:科学出版社，2010.3汤晓.基于激光测距仪的移动机器人同时定位和地图创建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