移动机器人的同时定位与地图创建的算法研究

王芳 刘晋

摘 要：移动机器人的同时定位与地图创建是当前机器人领域研究的基本问题与热点，也是实现智能机器人实现自主导航和控制决策的关键。本文从基于贝叶斯滤波器模型和基于图优化平滑模型两个方面对移动机器人同时定位与地图创建（SLAM）的研究进行了综述，介绍了这两种模型的基本框架以及关键技术，阐述了模型的各种不同实现形式，分析了这些算法的性能，并探讨了这些算法的优缺点以及相关难点的解决思路。最后对基于滤波器模型和基于平滑模型的同时定位与地图创建的未来发展进行展望。

关键词：同时定位与地图创建；贝叶斯；滤波器模型；图优化；平滑模型

中图分类号：TP 文献标识号：A 文章编号：2095-2163（2015-）02-

Research of Simultaneous Location and Mapping of the Mobile Robot

Liu Jin1， Wang Fang2

（1 Unit 261251， Qinhuangdao Hebei， 066102， China；2 Weather Bureaus of Changdu in Tibet， Changdu Xizang， 854000， China）

Abstract： Simultaneous Location and Mapping of the mobile robot are the basic problems and hot pots in the field of robotics，and they are also the key to realize autonomous navigation and control decision. This paper introduces two algorithms：Filter based on Bayes and Smoothing based on graph-based， reviews the basic framework and the key technologies， and elaborates the various forms of achieving. After that， the paper analyzes the performance of these algorithms， and lists the advantages and disadvantages， as well as the way of dealing with difficulties.Finally， the potential future issues and research trends are also explored.

Keywords： SLAM； Bayes； Filter； Graph-Based； Smoothing

0 引言

为了有效地解决移动机器人能夠自主完成许多预想的任务，例如运输、搜索、救援、自动吸尘等现实需求，机器人需要一个准确的工作环境地图。地图的准确性可以使所设计的系统在复杂的环境下，仅依赖自身携带的传感器来完成相应的指令，而不需要依赖像GPS等外部参考系统，尤其是在室内环境下GPS还不能使用。地图创建是指建立机器人所处环境的不同物体如障碍物、路标等的准确的空间位置以及特征描述，定位是指移动机器人在其所处的工作环境中根据一些已知特征来确定自身位置的过程。机器人依据已建立的环境地图信息进行自身的定位，同时又根据机器人定位的效果来更新环境地图，这两者相互依赖，将两者结合起来进行研究，称为移动机器人的同时定位与地图创建（Simultaneous Location and Mapping，SLAM），有时也称为并发定位与建图（Concurrent Localization and Mapping，CLM）。SLAM可以使用很多种方法来实现，这些方法整体上可以概括为以下两类：滤波器方法和平滑方法。

滤波器方法的主要思想是：运用动态贝叶斯理论，根据前面时刻机器人的位姿、控制输入信息以及观测信息这些已知的先验信息，来估计当前系统状态（机器人当前位姿以及地图特征位置）的后验概率。根据先验概率的不同假设，运用不同的求解方式来计算系统的后验概率，就可以得到不同的基于滤波器的算法。常见的滤波器算法有：卡尔曼滤波器（KF）和扩展卡尔曼滤波器（EKF）[1-2]、粒子滤波器（PF）[3-5]和信息滤波器（IF）[6-7]，滤波器主要强调的是时间以及增量特性，此方法也被称为在线SLAM。

相反，平滑方法的主要思想是：根据机器人在运动过程中所有的控制信息和观测信息来优化机器人完整的运功轨迹，并在此基础上创建环境地图，因此也被称为完全SLAM方法[8-10]。这类方法可以使用位姿图来构造SLAM，位姿图中的图节点代表了机器人在不同时刻的不同位置信息，图中的边对应两个位姿之间的约束信息，其中约束通常是指机器人在某时刻的观测信息或者是运动过程中的控制输入信息。将位姿图构建好之后，对图中的节点进行优化，即对机器人的位姿进行调整，使得位姿之间的关系更好地满足节点之间的约束，在优化完成之后得到的图对应机器人的运动轨迹。此方法又称为图优化的SLAM。

1 基于滤波器的SLAM算法

1.1 Bayes滤波模型

Bayes滤波原理：对于SLAM问题，根据之前的移动机器人位姿、观测信息以及控制输入信息来求得t时刻机器人位姿x和环境中特征m的联合后验概率：

（1）

SLAM问题可以分为预测、更新两步递归进行：

（1）预测：根据前一时刻状态的后验概率密度，结合状态转移概率来求。数学公式为：

（2）

（2）更新：利用实际的测量值以及由观测模型得到的观测值来更新当前t时刻状态的后验概率分布，计算公式为：

（3）

其中，为标准化因子。

1.2 KF-SLAM

卡尔曼滤波器是一种最优线性递归估计算法，利用线性的系统状态转移方程和观测方程得到一个全局最优的状态估计。在SLAM的应用中，卡尔曼滤波器分为3个步骤：预测、观测、更新。各步的功能实现可做如下描述：

（1） 预测：根据机器人的运动模型和观测模型，预测系统状态的估计值和观测估计值，并计算预测系统估计的误差协方差；

（2） 观测：当机器人在某时刻获取观测量后，将传感器的观测量与之前预测的观测量的值进行比较获得全新信息，同时计算该全新信息产生的协方差；

（3） 更新：利用观测的信息对预测的系统状态估计进行校正，并对系统状态估计误差的协方差进行更新。

首先，用下面的线性方程来表示系统状态转移方程和观测方程：

（4）

（5）

其中，為系统的状态向量，为系统的观测序列，为系统过程噪声序列，为系统观测噪声序列，为状态转移矩阵，为系统输入控制矩阵，为系统输入控制向量，为系统的观测矩阵。假设和vk均服从均值为0方差分别为和的高斯白噪声序列，在k-1时刻已知系统的状态估计值为和协方差矩阵，则卡尔曼滤波算法的具体步骤为：

1、运用前一时刻的状态估计值和协方差矩阵来预测当前时刻的状态估计和，即：

（6）

（7）

2、根据预测的协方差矩阵和观测噪声协方差矩阵来计算卡尔曼滤波器的增益：

（8）

3、根据预测的状态估计和实际观测值修正系统的状态估计，并计算相应的协方差矩阵，即测量更新：

（9）

（10）

卡尔曼滤波器虽然不需要存储系统以前的数据就可以不断地进行系统更新，但是卡尔曼滤波器中，系统的状态模型和传感器模型必须是线性的，然而在实际的应用中，这些模型往往是非线性的，为了适应非线性系统扩展卡尔曼（EKF）以及EKF的改进算法，这些算法的主要思想是：将非线性的运动方程和非线性的观测方程进一步转换为可以用卡尔曼滤波器来解决的线性系统函数，从而将非线性的状态估计问题运用最优的线性卡尔曼滤波器来求解。同时这些算法得到的后验概率分布是类似的高斯分布，对于一些非高斯分布的模型系统仍然存在很大的误差。

1.3 PF-SLAM

粒子滤波是一种基于蒙特卡罗方法和递推贝叶斯估计的统计滤波方法，即依据大数定理采用蒙特卡罗方法来求解贝叶斯估计中的积分运算。其基本思想是：首先依据系统状态向量的经验条件分布在状态空间产生一组随机样本的集合，称这些样本为粒子，然后根据量测不断调整粒子的权重和位置，通过调整后的粒子信息修正最初的经验条件分布。其实质是用由粒子及其权重组成的离散随机测度近似相关的概率分布，并且根据算法递推更新离散随机测度。当样本容量很大时，这种蒙特卡罗描述就近似于状态变量真实的后验概率密度函数。

根据Bayes理论，在已知t时刻之前的观测信息的条件下，所有与系统状态相关的信息都可以从后验概率分布中获得。设有一个带权重的离散粒子的集合，，其中是一个采样或称之为粒子，表示在t时刻系统的一个可能状态；被称为采样或者粒子的权重，且和，N表示粒子的个数。用该采样集合表示系统的后验概率。

粒子滤波算法包括三个主要步骤：

1.3.1 采样

根据前一时刻的样本集构建下一时刻的样本集，即采样或说是从提议分布中抽取样本。

1.3.2 重要性赋权

计算新样本集每一个样本的重要性权值。对应公式为：

（11）

若提议分布可以分解为下式：

（12）

（13）

则得到权值为：

（14）

如果，则重要密度函数仅依赖于和，在计算时，仅需存储粒子，而不必关心粒子集和过去量测值。修正后的权值为：

（15）

在标准的粒子滤波算法中选择相对来说容易实现的先验概率密度函数作为提议分布，即 （16）

那么将权重简化为：

（17）

再将权值归一化，即：

（18）

后验概率密度可表示为：

（19）

当时，由大数定理即可保证上式可逼近真实后验概率。

1.3.3 重新采样

根据1.3.2中计算出来的每一个样本的重要性权值，从样本集中重新采样，保证每个样本被抽中的概率与其重要性权值成正比。经过重新采样得到的样本组成新的样本集合。

但是，由于标准粒子滤波算法选择先验概率密度作为重要密度函数，若在对量测精度要求低的场合，这种选取方法能够获得较好的结果。不过，由于没有考虑当前的量测值，从重要性概率密度中取样得到的样本与从真实后验概率密度采样得到的样本有很大偏差。因此，重要性权重的方差随着时间而随机递增，使得粒子的权重集中到少数粒子上，甚至在经过几步的递归之后，可能只有一个粒子有非零权值，其他粒子的权值却都很小，为此忽略不计，从而使得大量的计算工作都浪费在用来更新那些对的估计几乎不起作用的粒子上，结果粒子集无法表达实际的后验概率分布，造成粒子退化。为避免退化问题发生，引入重采样技术，最简单的方法就是减少或剔除小权值粒子，而对大权值粒子则按照其权值大小进行复制，最后将所有的粒子权值设置为，来避免粒子的退化现象。

2基于图优化的SLAM

2.1基本思想

基于图优化的SLAM算法中构建的位姿图，图中的每个节点表示机器人的位姿，即机器人在工作环境下的位置，节点之间的空间约束是通过观测变量或者是控制输入来实现的，同时空间约束服从一定的概率分布。图优化的SLAM算法主要分为两个部分：图构造和图优化。具体来说，图构造是根据机器人传感器的原始数据构造出相应的图，这部分严重依赖于传感器，也称为前端；而图优化则是确定最符合图中边约束的机器人位姿的布局，此部分依赖于抽象的数据，与传感器的数据无关，也称为后端。下面主要介绍了图优化即后端的算法。

2.2基于松弛的優化方法

Howard[11]等提出采用松弛方法来求解移动机器人的同时定位与地图的创建问题。基本思想是：在每一次的迭代过程中，遍历位姿图中的所有节点，遍历的过程中对每一个节点都执行以下的操作，根据其相邻节点的位置及节点之间的约束关系重新计算并更新该节点的位置信息。而在机器人与坐标系的夹角确定的情况下，Duckett 等证明了使用图优化的松弛算法必收敛于最优解。该方法不仅可以用于根据全局的数据来解决SLAM问题的算法中，还可用于增量式的SLAM 中，每当有新的特征出现时，可以直接更新上一轮结果中得到的地图。但算法存在的缺陷是，当机器人两个位姿之间的约束即节点对应的边存在较大的误差时，要想将此误差分配到图中其它的边上，就需要进行多次迭代才能实现，从而加大了计算量，同时而这也是出现环形闭合时所需要解决的问题。随着算法的不断改进，Frese[12]提出一种改进的多层次松弛（MLR）的优化算法，同时结合多重网格方法来求解系统中的偏微分方程，从而在很大程度上提高了出现环形闭合时位姿图中节点的优化效率。

2.3基于随机梯度下降的方法

Olson[10]将神经网络中的随机梯度下降方法（stochastic gradient descent，SGD）应用到基于图优化的SLAM中，在每次迭代的过程中，依次选取位姿图中的任意一条边，并将此条边上对应的约束作为当前约束，计算下降的最快的方向即梯度，然后在该方向上求出最优的目标函数。通过相关的实验可以证明随机梯度下降在执行的过程中，初始值与最优值的误差超出了很大的阈值范围，在不修改此时初始值的情况下，随机梯度下降仍然能够得到较好的收敛效果。因此，随机梯度下降算法可以避免落入局部最优值，对系统初始化的值具有较高的鲁棒性。Grisetti对Olson提出的随机梯度下降方法进行了改进和拓展，采用树型结构来描述二维和三维空间中机器人位姿之间的关系，系统待求解的状态用增量方式来代替，不仅有效地更新了移动机器人的位姿，而且加快了收敛的速率[13]。

3 结论

移动机器人在未知环境下的同时定位与地图创建，是机器人学和智能控制的重要研究领域，同时SLAM也是自主导航以及自动完成其他指令的关键部分，因此近几年来在机器人的研究领域，SLAM已成为研究的重点与热点。与此同时，出现了分别受时间约束的贝叶斯滤波器算法以及受空间约束的平滑算法，前者主要是根据上一时刻机器人的状态、观测信息以及控制输入信息来确定此刻机器人的位姿和地图特征；后者则是根据全局的所有信息来构建以及优化地图。

滤波器算法中需要改进的地方：基于卡尔曼滤波器的基本算法以及改进算法中，不能解决非强高斯和非强线性的模型，基于标准的粒子滤波器算法以及改进算法存在着粒子退化，从而导致样本匮乏的现象。

平滑算法技术中以下方面仍具有较大的发展趋势：对SLAM问题非线性结构的深入研究，增强后端的鲁棒性以及地图的表示和创建形式。

参考文献：

[1] SIMTH R， SELF M， CHEESEMAN P. Estimating uncertain spatial realtion ships in robotics[C]// Autonomous Robot Vehicles， COX I， WILFONG G， Eds. Berlin： Springer-Verlag， 1990：167–193.

[2] CASTELLANOS J A， MONTIEL J M M， NEIRA J， et al. The SPmap： A probabilistic framework for simultaneous localization and map building[J]. IEEE Trans. Robot. Automat.， 1999，15（5）：948–953.

[3] MONTEMERLO M， THRUN S， KOLLER D， et al. FastSLA M： A factored solution to simultaneous localization and mapping[C]//Proc. Nat. Conf. Artificial Intelligence （AAAI）. Edmonton， Canada：AAAI， 2002：593–598.

[4] HAHNEL D， BURGARD W， FOX D， et al. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements[C]//Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems （IROS）. Las Vegas， NV：IEEE， 2003： 206–211.

[5] GRISETTI G， STACHNISS C， BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE Trans.Robot.， 2007， 23（1）： 34–46.

[6] EUSTICE R， SINGH H， LEONARD J J. Exactly sparse delayed-state filters[C]// Proc. IEEE Int. Conf. Robotics and Automation （ICRA）， Barcelona， Spain：IEEE，2005： 2428–2435.

[7] THRUN S， LIU Y， KOLLER D， et al. Simultaneous localization and mapping with sparse extended information filters[J]. Int. J. Robot. Res.， 2004，23（ 7/8）：693–716.

[8] LU F， MILIOS E. Globally consistent range scan alignmen t for environment mapping[J].Autonom. Robots，1997， 4： 333–349.

[9] LAERT D F， KAESS M. Square root SAM： Simultaneous location and mapping via square root information smoothing[J]. Int. J. Robot. Res.， 2006.

[10] OLSON E， LEONARD J， TELLER S. Fast iterative optimization of pose graphs with poor initial estimates[C]// Proc. IEEE Int. Conf. Robotics and Automation （ICRA）.Oralando， Florida：IEEE， 2006： 2262–2269.

[11] HOWARD A， MATARIC' M J， SUKHATME G. Relaxation on a mesh： A formalism for generalized localization[C]// Proc. IEEE/RSJ Int. Conf. Intelligent Robots and Systems （IROS）. Los Alamitos，Calif：IEEE， 2001.

[12] FRESE U， LARSSON P， DUCKETT T. A multilevel relaxation algorithm for simultaneous localisation and mapping[J]. IEEE Trans. Robot.， 2005， 21（ 2）：1–12.

[13] GRISETTI G， STACKNISS C， BURGARD W. Non-linear constraint network optimization for efficient map learning[J]. IEEE Trans. Intell. Transport. Syst.， 2009.

作者簡介：王芳（1984-），女，江苏沭阳人，硕士，助工，主要研究方向：人工智能、无人机等；

刘晋（1987-），男，山西晋城人，本科，主要研究方向： 数学与应用数学。