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

时 也，吴怀宇，徐文霞，彭晟远

（武汉科技大学 信息科学与工程学院，湖北 武汉 430081）

在当今社会，移动机器人应具备在未知环境下的自主导航能力，才有其实际存在价值[1]。近年来，一些特大自然灾害的发生，更使人们注重机器人的实用性和准确性。机器人实现自主导航的核心问题是地图构建与定位 （SLAM），解决SLAM问题才可以使机器人在未知环境中的工作更高效和更科学。移动机器人的SLAM算法研究已是机器人研究领域中的最热门、最丰富、最具开拓性的课题之一[2-3]。

移动机器人的地图创建与定位问题是机器人领域一个基础而关键的问题。文献[4]针对缺少先验地图支持的室内环境，提出利用激光测距仪和单目视觉的信息融合来解决室内环境中移动机器人的SLAM问题。文献[5]提出基于单目视觉和里程计的SLAM方法，采用尺度不变特征变换算法提取特征，并用扩展卡尔曼滤波更新地图。文献[7]提出基于概率的移动机器人SLAM算法框架，应用贝叶斯规则作为理论基础，建立移动机器人SLAM算法的概率表示模型。文献[8-9]针对SLAM问题的高维特性，提出了FastSLAM解决方案。

文中以课题组的移动机器人MT-R为研究对象，给定路标的参数值，利用激光测距仪计算路标之间的关系方程。重点研究用障碍物之间的空间逻辑关系，采用障碍物作为机器人的路标并以此来代替固有路标，估算出机器人在全局坐标系中移动的距离，使机器人在线构造地图的同时，能够提取并使用地图。最后通过MATLAB仿真算法和在未知环境下实验，使移动机器人从起始点绕过不定数目的障碍物后再返回到起始点，验证理论的正确性和算法的精确性。

1 机器人地图创建与定位

目前广泛使用卡尔曼滤波器或粒子滤波器来解决SLAM问题。粒子滤波虽然能够比较精确地表达基于观测量和控制量的后验概率分布，但其计算量大而精度低。扩展卡尔曼滤波器可以用在非线性系统上，且计算量相比粒子滤波少，因此选择扩展卡尔曼滤波器算法。

项目组的移动机器人MT-R装有：双目传感器、声纳传感器、激光测距仪、里程计，机器人可装载机械臂。激光测距仪是移动机器人的基本感知装置，用于获得环境中障碍物距离信息，这些信息可用于辨识障碍物，并用障碍物作为路标。根据路标之间的关联，计算路标参数的状态方程以及观测方程，并使之系统化。建立障碍物与路标之间的关联方程，由此可得一个基于障碍物作为路标的全局地图。

2 基于扩展卡尔曼滤波器的SLAM算法

2.1 状态方程

首先用［Fx，Fy］表示全局坐标系，［Gx，Gy］表示局部坐标系来建立机器人的空间状态方程。为了构建全局地图，扩展卡尔曼滤波器中的状态向量包含移动机器人的状态部分（X）及路标状态部分（G）。 Fx（k）和Fy（k）为机器人在时间k时在全局坐标系中的位置坐标。 ξ（k）=cosθ（k），η（k）=sinθ（k），为状态变量则状态向量。Xu（k）为机器人的状态向量：

机器人的状态向量和路标状态向量应满足公式（2）和公式（3）：

机器人状态转移矩阵FU（k）满足公式（4）：

系统噪声wu（k）满足公式（5）：

假定在全局坐标系中，路标是静止对象，因此，如果［Fxn（k）Fyn（k）］T是Gn，那么机器人的状态方程如下：

在方程（1）中，路标的过程模型是个单位矩阵，因此GN在以及Xu（k）中的状态向量:

2.2 观测方程

在局部坐标系中， 路标m的局部坐标为zm（k）=［Gxm（k）Gym（k）］。 在k时与全局坐标系中路标n的坐标Gn相关联，矩阵Hn（k）定义为：

那么对应状态向量X（k）的观测方程：

2.3 扩展卡尔曼滤波器的应用

扩展卡尔曼滤波器由状态方程及观测方程组成，工作流程为：预测步骤、检测步骤、更新步骤。这3个步骤并不是独立的，而是循序渐进并且循环往复的。

L（k）是2×2矩阵，它 的对角由方 差rΔd（k）和rΔθ（k）定义， rΔd（k）为Δd（k）的预测误差，rΔθ（k）为Δθ（k）的预测误差。

R为观测误差的方差矩阵，得到Dn（k）在相关的n点路标被定义为：（Gxm（k）Gym（k））T

协方差矩阵P（k｜k-1）更新为：

由检测和更新步骤可得到全局坐标系中的路标坐标Gn和局部坐标系中坐标Zm（k）的转换公式，当有m个路标被侦测，则检测步骤和步骤更新重复m次。

2.4 不同坐标下参数的转换

图1 不同坐标系下的参数转换Fig.1 Parameters coversion between different coordinates

3 实验及分析

使移动机器人在环境中行走一个闭合的环路。机器人的移动速度约为0.56 m/s，光电编码器的采样周期为5 ms，激光测距仪的采样周期为0.5 s。整个过程持续90.2 s。激光测距仪的扫描范围为0°～180°，角度分辨率为0.5°，每扫描一次产生361个数据。

图2是机器人的位姿对比曲线，激光测距仪的最大感知距离为8 m，范围不确定度为0.03 m。图中虚线表示在死区地带的机器人位姿曲线，实线为经过EKF校正的机器人位姿曲线。

图2 机器人的位姿对比曲线Fig.2 Curves of robot pose

图3是机器人的位姿误差曲线，可以看出：在前90次的定位中，的校正量保持在±0.9 cm之内，的校正量保持在±2.0 cm之内，的校正量在±0.8°之内。

图3 机器人的位姿误差曲线Fig.3 The error curve of robot pose

4 结束语

文中针对移动机器人的地图创建与定位问题，采用基于扩展卡尔曼滤波器法，利用激光测距仪建立起参考坐标系，以障碍物作为路标，从而使机器人辨识自身位置，使用扩展卡尔曼滤波器来代替粒子滤波器以获得实时地图。最后通过仿真和物理实验验证了准确性。机器人的定位与地图创建问题，是机器人领域的关键技术之一，为后续的解决机器人的SLAM导航和死区定位的精度研究提供了理论依据。

[1]蔡自兴，贺汉根，陈虹.未知环境中移动机器人导航控制研究的若干问题[J].控制与决策，2002，17（4）:385-390.

CAI Zi-xing，HE Han-gen，CHEN Hong.Some issues for mobile robots navigation under unknown environments[J].Control and Decision，2002，17（4）:385-390.

[2]赵冬斌，易建强.全方位移动机器人导论[M].北京:科学出版社，2010.

[3]汤晓.基于激光测距仪的移动机器人同时定位和地图创建[D].济南：山东大学，2007.

[4]庄严，王伟.移动机器人基于激光测距和单目视觉的室内同时定位和地图构建[J].自动化学报，2005，31（6）:925-933.

ZHUANG Yan，WANG Wei.Mobile robot based on laser range finder and monocular vision indoor simultaneous localization and map building[J].Acta Automatica Sinica，2005，31（6）:925-933.

[5]方正，佟国峰，徐心和.基于贝叶斯滤波理论的自主机器人自定位方法研究[J].控制与决策，2006，21（8）：841-846，862.

FANG Zheng，TONG Guo-feng，XU Xin-he.Study of autonomous robot self-locatization methods based on bayesian filter theory[J].Control and Decision，2006，21（8）：841-846，862.

[6]王彭林，石守东，洪小伟.基于单目视觉和里程计的SLAM算法研究[J].计算机仿真，2008，25（10）：172-175.

WANG Peng-lin，SHI Shou-dong，HONG Xiao-wei.A SLAM alogorithm based on monocular vision and adometer[J].Computer Simulation，2008，25（10）：172-175.

[7]石杏喜，赵春霞.基于概率的移动机器人SLAM算法框架[J].计算机工程，2010，36（2）：31-32.

SHI Xing-xi，ZHAO Chun-xia.SLAM algorithm framework of mobile robot based on probability[J].Computer Engineering，2010，36（2）：31-32.

[8]Newman P，Leonard J，Tardós J D，et al.Explore and return:experimental validation of real time concurrent mapping and localization[C].Proceedings of ICRA，2002.

[9]Welch G，Bishop G.An introduction to the kalman filter[R].Chapel Hill：University of North Carolina，2001.

[10]陈卫东，张飞.移动机器人的同步自定位与地图创建研究进展[J].控制理论与应用，2005，22（3）：455-460.

CHEN Wei-dong，ZHANG Fei.Review on the achievements in simultaneous localization and mapbuilding for mobile robot[J].Control Theory and Applications，2005，22（3）：455-460.