基于卡尔曼滤波与粒子滤波的SLAM研究综述

孙海波 童紫原 唐守锋 童敏明 纪玉明

摘要：同时定位与地图构建（SLAM）问题在移动机器人研究领域受到了广泛关注，其是机器人能否实现完全自主运动功能的关键。首先阐述了SLAM系统相关模型，并介绍了经典卡尔曼滤波相关知识;其次介绍基于扩展卡尔曼滤波、无迹卡尔曼滤波与粒子滤波的SLAM算法如何解决现实世界的非线性、非高斯问题，并总结了各算法优缺点;最后，展望了基于卡尔曼滤波的SLAM算法发展趋势。

关键词关键词：SLAM;卡尔曼滤波;扩展卡尔曼滤波;无迹卡尔曼滤波;粒子滤波

SLAM Research Based on Kalman Filter and Particle Filter

SUN Hai bo TONG Zi yuan TANG Shou feng TONG Min ming JI Yu ming

（1. School of Information and Control Engineering， China University of Mining and Technology，Xuzhou 221000，China;

2.School of Information and Electrical Engineering，University of New South Wales， Sydney2052，Australia;

3.Xuzhou Hanlin Technology Co. LTD，Xuzhou 221000，China）

Abstract：The problem of simultaneous localization and mapping （SLAM） in the field of mobile robotics is getting more and more attention， which is regarded as the key technology to the robot's ability to achieve fully autonomous motion functions. first of all ，this article elaborates the relevant models of SLAM system， and introduces the related knowledge of classical Kalman filter. Secondly， the SLAM algorithm based on extended Kalman filter， unscented Kalman filter and particle filter is introduced to solve the nonlinear and non Gaussian problems in the real world. The advantages and disadvantages of each algorithm are summarized. Finally， the development trend of SLAM algorithm based on Kalman filter is forecasted.

Key Words：SLAM; Kalman filter ; unscented Kalman filter; particle filter

0 引言

目前，基于SLAM技術的机器人定位技术有激光仪定位、红外线定位、超声波定位、机器视觉定位等[1]。SLAM问题是由概率机器人学演变而来的，其原理是将模型方法与行为方法相结合，处理机器人在未知环境中的自主移动问题，是实现移动机器人避障、路径规划以及目标跟踪等复杂任务的前提与基础。同时，卡尔曼滤波从系统状态切入，解决了非平稳随机过程等动态系统的信号处理问题。卡尔曼滤波只需要k时刻的测量量与k-1时刻的估计量，并通过不断预估与修正，即可实时估计信号状态，该方式节省了大量储存与运算空间。现阶段，卡尔曼滤波已演变出不同版本的算法形式，从而大大地提高了算法的精度与稳定性，并成为定位与导航系统中的研究热点。

1 SLAM系统数学模型

SLAM（Simultaneous Localization and Mapping）是由Smith、Self等[2]在1988年提出的，SLAM问题可以描述为： 机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计与地图进行自身定位，并在自身定位的基础上建造增量式地图，以实现机器人的自主定位及导航[3]。SLAM 问题是在概率论理论基础上建立相应概率模型，如西安工程大学的王帆等[4]提出的运动概率模型、观测概率模型与SLAM问题整体概率模型是研究SLAM问题最重要的3个基础模型。

1.1 运动概率模型

运动概率模型是经典的SLAM模型，可以 用条件概率pX v kX v k-1，U k加以描述。其表示的具体含义为移动机器人在运动过程中输入信息U k，在其作用下推导出上一时刻位姿X v k-1运动到当前时刻X v k的 条件概率。

1.2 观测概率模型

用条件 概率pZ kX v k，N描述观测概率模型，其含义是在已知k时刻移动机器人位姿X v k与N（环境特征）的条件下，求取K时刻测量Z k的 条件概率。

1.3 SLAM问题概率模型

用联合后验 pX kZ 1→k，U 1→k，X v 0 描述整个SLAM问题的概率模型，该模型描述为：在给定移动机器人初始位 姿X v 0、已知所有控制输入信息U 1-k且获得所有观测信息Z 1-k的前提下，推导出k时刻 机器人位姿与环境特征组合的联合状态向量的条件概率。

综上所述，SLAM问题涉及两个关键方面：①利用传感器采集周围环境信息，并对信息进行数据处理，从而构建与环境特征匹配的SLAM地图;②根据环境特征信息进行装置的位姿估计[5]。SLAM算法既是对移动机器人位姿及环境特征位置估计的贝叶斯状态评估问题，又是在运动概率模型基础上，通过输入观测数据，预测并更新无人机位姿及路标位置的导航问题[6]。SLAM系统数学模型如图1所示。

2 基于卡尔曼滤波与粒子滤波的SLAM算法

由于SLAM算法在移动机器人位姿估计中的广泛应用，已提出了半个世纪的卡尔曼滤波至今仍是学者们的研究热点。本部分将介绍经典卡尔曼滤波SLAM算法与几种常用的SLAM算法。

2.1 经典卡尔曼滤波SLAM算法

经典卡尔曼滤波的核心思想是递推求解，在忽略之前数据量的前提下，根据x k-1递推出x k 。经典卡尔曼滤波算法步骤与结构如下：

步骤1：设置初始量 0、p 0。

步骤2：状态预测并推算误差协方差。

步骤3：计算卡尔曼增益。

步骤4：计算误差。

步骤5：更新误差协方差。

由以上步骤得出，經典卡尔曼滤波适用于线性高斯系统。在解决移动机器人与无人机的各种实际导航问题时，系统大多是非线性和非高斯的，因此，拓展卡尔曼滤波适用范围。国内外学者相继提出基于EKF[7]（扩展卡尔曼滤波）的SLAM算法、基于UKF[8]（无迹卡尔曼滤波）的SLAM算法与基于PF[9]（粒子滤波）的SLAM算法。

2.2 基于扩展卡尔曼滤波（EKF）的SLAM算法

扩展卡尔曼滤波算法以卡尔曼滤波为基础，与经典卡尔曼滤波算法的不同之处在于，其仅将卡尔曼滤波里的矩阵A和H用公式函数f与h代替，如公式（1）、（2）所示。

然后采用泰勒展开式的一阶逼近非线性系统，如公式（3）、（4）所示，对其状态进行线性估计，并通过更新系统协方差矩阵对系统状态进行递归更新[10]。

基于EKF的SLAM算法可分为3个步骤：预测过程、更新过程与向量增广，算法结构如图2所示。

EKF忽略了泰勒展开式的高阶项，从而给估计带来误差，所以要求系统的状态方程与广策方程都非常接近线性并且连续，才能得到较好的滤波结果。同时因为状态噪声与观测噪声的协方差矩阵保持不变，所以对其的估计可能不够准确，容易造成误差积累，导致滤波器发散。因此，EKF SLAMY适用于环境较为简单、环境特征较少、定位要求不高的场合。

2.3 基于无迹卡尔曼滤波（UKF）的SLAM算法

为解决EKF中的问题，研究者提出一种基于无迹卡尔曼滤波的SLAM算法。无迹卡尔曼滤波采用无迹变换UT处理非线性均值与协方差，用一组确定的样本点方式预测新时刻的状态，是一种递归式贝叶斯估计方法[11 12]。该组确定性样本点被称为Sigma点，其在非线性函数映射下，得到各点权重。无迹卡尔曼滤波算法步骤如下：

（1）初始化。

（2）Sigma采样。使用对称采样策略，得到当前时刻状态估计的点集。

（3）预测方程。

（4）更新方程。

机器人运动中不断地进行步骤（2）、（4），即可获得机器人各时刻的准确位置。

UKF通过确定后的采样，可近似状态的后验PDF，解决了系统非线性加剧后的滤波发散问题，但是UKF算法在系统后验概率密度为非高斯的情况下，滤波结果依旧不理想。而且由于噪声是随时间变化的，会造成统计特性的不准确，从而使滤波发散与定位误差变大。

2.4 基于粒子滤波（PF）的SLAM算法

粒子滤波是指利用状态空间的一组加权随机样本逼近状态量的后验概率分布。为了有效避免EKF算法因系统线性化近似造成的误差，基于粒子滤波的SLAM算法采用一组加权样本逼近最优估计，从而不受模型线性程度与高斯假设的约束。将粒子滤波算法引入机器人的同时定位与地图构建功能是由Murphy & Doucet[13 14]提出的，并由Montemerla等[15 16]研究者完成粒子滤波算法框架的推广。粒子滤波算法步骤可分为初始化、预测、观测更新、再采样与结果输出几个步骤，如图3所示。

PF SLAM算法的核心思想是将联合状态向量后验概率依据概率公式分解为机器人轨迹部分与以轨迹为条件的特征部分[15]。样本重要性采样是粒子滤波中常用的采样方法，但该方法需要考虑过去时刻的观测向量，从而增加了存储与计算压力，所以研究人员提出了序贯重要性采样方法。其核心思想是利用一系列随机样本的加权与表示所需状态向量的后验概率分布，得到状态向量的估计值。当粒子数目趋于无穷大时，可以用概率理论（大数定理与中心极限定理）加以证明，蒙特卡罗特性与后验概率密度函数表示等价，序贯重要性采样滤波器接近于最优贝叶斯滤波[17]，而该算法在使用过程中还需考虑粒子退化现象[18]。当滤波一段时间后，用预测结果带来的概率分布更新各粒子权重，匹配观测结果的粒子会赋予更高权重，最后少数具有很高权重的粒子将起主导作用，而大多数权重较低的粒子，对结果影响较小，但还要花费大量时间对权重较低的粒子进行更新，所以无法避免粒子的退化现象。因此，要引入一个量度，即有效粒子的采样尺度，当采样尺度小于测量值时需要重新采样，也即再采样。

由于PF SLAM独立于系统模型，所以在解决了EKF SLAM与UKF SLAM中由于线性化和噪声非高斯造成的误差后，其适用范围更广，但PF SLAM在决定其状态估计优劣的最优参数分布时很难进行采样。

3 基于卡尔曼滤波的SLAM算法研究趋势

未来定位导航的发展趋势是将多种算法进行融合，单一算法已难以满足需求，因此需要根据不同要求选择不同算法。多传感器的信息融合技术也逐渐成熟，从而提高了算法的实时性与定位精度。随着机器学习的兴起，与神经网络融合的卡尔曼滤波也被提出。

4 结语

针对运动机器人等移动装置的同时定位导航问题，本文在SLAM系统的数学模型基础上，对各种SLAM算法进行阐述与总结，对理论及应用中存在的问题作出详细介绍，并总结了各算法优缺点，最后展望了SLAM算法未来发展趋势。基于卡尔曼滤波与粒子滤波的SLAM算法的应用还有较大提升空间，未来仍需作进一步深入研究。

参考文献参考文献：

[1] 黄玉强.移动机器人在未知环境下的基于视觉系统的地图创建[D].南京：南京大学，2012.

[2] 曹美会，鲜斌，张旭，等.基于视觉的四旋翼无人机自主定位与控制系统[J].信息与控制，2015，44（2）：190 196.

[3] 白志军.四旋翼无人机室内自主导航系统的研究与实现[D].厦门：厦门大学，2014.

[4] 王帆.基于卡尔曼滤波和粒子滤波的移动机器人同时定位与地图创建研究[D].西安：西安工程大学，2012.

[5] 刘书池.面向工业互联网的井下无人机单目视觉SLAM定位方法[D].北京：北京交通大学，2017.

[6] 楊晓晓.室内机器人单目视觉同时定位与地图构建技术研究与实现[D].成都：成都信息工程学院，2014.

[7] 冯少江，徐泽宇，石明全，等.基于改进扩展卡尔曼滤波的姿态解算算法研究[J].计算机科学，2017，44（9）：227 229.

[8] 李志雄，王姮.基于无迹卡尔曼滤波的运动机器人定位研究[J].轻工科技，2015（11）：73 74，76.

[9] 韩同辉，沈超，沈静，等.基于PF算法的移动机器人定位研究[J].机电一体化，2012，18（3）：13 16.

[10] 王聪伟.基于扩展卡尔曼滤波的足式机器人运动速度估计研究[D].哈尔滨：哈尔滨工业大学，2014.

[11] 金峰，蔡鹤皋.机器人IMU与激光扫描测距传感器数据融合[J].机器人，2000（6）：470 473.

[12] 骆云祥.非线性滤波在移动机器人SLAM中的应用[D].南京：南京理工大学，2009.

[13] MURPHY K. Bayesian map learning in dynamic environments[C].Proc of the Conf on Neural Information Processing Systems，NIPS，Denver，USA ，1999：1015 1021.

[14] DOUCET A，FREITAS J DE，MURPHY K，et al.Rao Blackwellized particle filtering for dynamic Bayesion networks[C].Proc of the Conf on Uncertainty in Artifical Intelligence，UAL，Stanford，USA，2000：176 185.

[15] MONTEMERLO M， THRUN S， ROLLER D， et al. FastSLAM 2.0： an improved particle filtering algorithm for simultaneous localization and mapping that provably converges[J]. Proc.int.conf.on Artificial Intelligence， 2003， 133（1）：1151 1156.

[16] MONTEMERLO M， THUN S， KOLLER D， et al. FastSLAM： a factored solution to simultaneous mapping and localization[C].Proceedings of the National Conference on Artificial Intelligence，2002：590 605.

[17] KIM C， SAKTHIVEL R， WAN K C. Unscented FastSLAM： a robust algorithm for the simultaneous localization and mapping problem[C]. IEEE International Conference on Robotics and Automation. IEEE， 2007：2439 2445.

[18] SHOJAIE K，AHMADI K，SHAHRI A M.Effects of iteration in Kalman filters family for improvement of estimation accuracy in SLAM [C] .IEEE International Conference on Advanced Intelligent Mechatronics，2007：1 6.