1、南京理工大学 机器人控制理论与技术课程论文 关于 SLAM 技术的 综述报告 几种 SLAM 算法 的简要介绍 学号： *，姓名： * 摘要： 移动机器人同步地图创建与定位 (SLAM)是移动机器人的核心研究课题本文对 SLAM的 研究进展和关键技术进行了综述。 基于卡尔曼滤波器的实现方法 进行了分析比较 。总结分析 SLAM问题 实现中卡尔曼滤波 及其拓展算法优缺点，更好的 探讨了 其今后 发展方向。 关键词： SLAM 问题；卡尔曼滤波；移动机器人 。 一 . 引言 SLAM 是 simultaneous localization and mapping 的全称 ,也称为 CML (Con
2、current Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM 的研究起源于 1986 年在美国加利福利亚州旧金山举行的 IEEE ICPA 会议。 议期间，众多学者对地图构建的一致性问题进行了广泛讨论，并且达成一个共识：一致的、概率的地图构建方法是机器人领域的一个关键问题，其算法和理论值得重点研究 1。 在 SLAM 中 ， 机器人 能 在未知环境中从一个未知位置开始移动 ， 在移动过程中根据位置估计和传感器数据进行自身定位 ， 同时建造增量式地图。 机器人根据在运动过程中获取的传感器数据，利用相关地图模型和集成算法，生成关于环境的地图表
3、示。 机器人地图可以分为 两大类：几何地图和拓扑地图。几何地图又可分为栅格地图和特征地图。 栅格地图将空间环境地图划分为若干规则方格。每一个方格设定概率值表示 该区域是否被障碍物占用，特征地图由一系列包含位置信息的特征组成， 拓扑地图由环境中特征位置或区域组成的节点及其连接关系组成。 近 年来 ，SLAM 的研究取得了很大的进展 ,并已应用于各种不同的环境 ， 如 ： 室内环境、水下、室外环境 。 本文对 SLAM 的 研究进展和关键技术进行了综述。 基于卡尔曼滤波等实现方法 进行了分析比较 。总结分析 SLAM 实现中卡尔曼滤波 等方法 实现过程中 存在的难题，探讨了今后的发展方向。 南京理
4、工大学 机器人控制理论与技术课程论文 二 . SLAM 表示 方法 ： 目前各国研究者已经提出了多种表示法，大致可分为三类：栅格表示 法 、几何信息表示 法 和拓扑图表示 法，三 种方法都有自己的优缺点。 栅格地图表示法即 ： 将整个环境分为若干相同大小的栅格，对于每个栅格各指出其中是否存在障碍物。 它的优点： 创建和维护容易，尽量的保留了整个环境的各种 信息，同时借助于该地图，可以方便地进行自定位和路径规划。 缺点 ：当栅格数量增大时 (在大规模环境或对环境划分比较详细时 )，对地图的维护行为将变得困难，同时定位过程中搜索空间很大，如果没有较好的简化算法，实现实时应用比较困难。 几何信息地图
5、表示 法是指 ： 机器人 从 收集对环境的感知信息，从中提取更为抽象的几何特征， 如线段或曲线，使用这些几何信息描述环境。这种方法更加 紧凑， 并 且便于位置估计和目标识别。几何 信息表示 法利用卡尔曼滤波在局部区域内可获得 较高精度，且计算量小，但在广域 环境中却难以维持精确的坐标信息。缺点是：几何信息的提取需要对感知信息作额外处理， 需要一定数量的感知数据才能得到结果。 拓扑地图抽象度高，特别是当环境大而简单时， 这种方法将环境表示为一张拓扑意义中的图，图中的节点对应于环境中的一个特征状态 、地点。如果节点间存在直接连接的路径则相当于图中连接节点的弧。 优点是： (1)有利于进一步的路径和
6、任务规划， (2)存储和搜索空间都比较小，计算效率高， (3)可以使用很多现 有成熟、高效的搜索和推理算法。缺点是：对拓扑图的使用是建立在对拓扑节点的识别匹配基础上的。 目前 SLAM 实现 方法可分为两类：一是基于概率模型的方法，二是 非概率模型方法。概率模型方法 包括 许多基于卡尔曼滤波的 SLAM 方法如 扩展卡尔曼滤波方法 、 无迹卡尔曼滤波法 、 FastSLAM 等 。 而 非概率模型方法 则 有 SM-SLAM、扫描匹配、数据融合 (data association)、基于模糊逻辑等。 本文将对基于卡尔曼滤波的 SLAM 实现方法做简要介绍： 2.1 卡尔曼滤波算法 20 世纪
7、60 年代，匈牙利数学家 R.E.Kalman 在他的博士论文 A New Approach to Linear Filter and Prediction Problems中首次提出了卡尔曼滤波算法。南京理工大学 机器人控制理论与技术课程论文 卡尔曼滤波是一种用递归来解决离散数据线性滤波的方法，它克服了维纳滤波理论的局限性，提供了一种高效可靠的“预测 实测 修正”的计算方法来估计整个过程的状态，使估计的均方根误差最小。卡尔曼滤波法是一种最优估计，它可以处理多维和非平稳的随机信号，逐渐成为基于概率的 SLAM 算法中主流的方法，并在各个领域得到了广泛应用 。 以下 为卡尔曼滤波模型简介： 动态
8、系统及其测量模型： ( ) ( 1 ) ( 1 ) ( 1 )( ) ( ) ( )k k k kk k kx A x B u nz H x v （ 1） 系统及观测噪声协方差 : ( 0 , ) ( 0 , )n N Q v N R 先验估计 : ( 1 ) ( 1 ) ( 1 )( ) ( )( 1 ) Tk k k k k kkkkkx P E x x x x （ 2） 后验估计 : ( ) ( ) ( )( ) ( )() Tk k k k k kkkkkx P E x x x x （ 3） 已知条件：初始状态估计及估计误差协方差 分别为： (11)x 、(11)P时间更新 : ( 1
9、 ) ( 1 1 ) ( 1 )k k k k kx A x B u （ 4） 预测状态 ( 1 ) ( 1 1 ) Tk k k kP A P A Q （ 5） 预测协方差 状态更新 1() ( 1 ) ( 1 )TTk k k k kK P H H P H R （ 6） Kalma增益 ( ) ( 1 ) ( 1 )( ) ( )k k k k k kkkx x K z H x （ 7） 修正状态 ()( ) ( 1 )kk k k kP I K H P （ 8） 修正协方差 卡尔曼滤波 适用范围：准确已知的线性系统和量测方程；系统噪声和量测噪声为互不相关的零均值高斯白噪声。 南京理工大学
10、 机器人控制理论与技术课程论文 T i m e U p d a t e ( “ P r e d i c t ” )( 1 ) P r o j e c t t h e s t a t e a h e a d( 2 ) P r o j e c t t h e e r r o r c o v a r i a n c e a h e a dMeasurement Update ( “ Correct ” )（ 1 ） C o m p u t e r t h e K a l m a g a i n（ 2 ） U p d a t e e s t i m a t e w h i t m e a s u r
11、e m e n t （ 3 ） U p d a t e t h e e r r o r c o v a r i a n c eInitial estimates for图 1卡尔曼滤波代码实现流程图 2.1 扩展卡尔曼滤波方法（ EKF） 由于 卡尔曼滤波 算法具有对系统线性的要求以及对系统噪声须符合高斯分布的依赖性，这就在某些程度上限制了 卡尔曼滤波 算法的应用，所以后人在 卡尔曼滤波 算法的基础上提出了扩展卡尔曼滤波方法（ Extended Kalman Filter， EKF）。EKF 算法的实质是用泰勒展开的一阶近似来表示出非线性系统方程，从而解决非线性系统模型的估计问题。 它的基本思
12、想是：在滤波值附近，应用泰勒展开算法将非线性系统展开，对于二阶以上的高阶项全部都省去，从而原系统就变成了一个线性系统，再利用标准 Kalman 滤波算法的思想对系统线性化模型进行滤波。 1 ( , )k k k k kx f x u w （ 9）状态方程 ( , )k k k kz g x u v （ 10）观测方程 )(),(),(),( kkxxk kkkkkk xxx uxfuxfuxf kk （ 11）)(),(),(),( kkxxk kkkkkk xxx uxguxguxg kk （ 12） 式（ 11）、（ 12）分别为 f(x),g(x)的 多元泰勒展开 并 略去高次项 。 令
13、 ：kk xxk kkk xuxf ),( ，kk xxk kkk xuxgH ),( 则可得到： 南京理工大学 机器人控制理论与技术课程论文 1 ( , ) k k k k k k k k kx x f x u x w （ 13）状态方程 ( , ) k k k k k k k kz H x g x u H x v （ 14）观测方程 即： 11= ( , ) + = ( , , )= ( , ) = ( , , )k k k k k k k k kk k k k k k k kx f x u w x f x u wz g x u v z g x u 将非线性系统线性化。 图 2扩展卡尔曼滤
14、波总体流程图 2.3 无迹卡尔曼滤波法（ UKF） 但是 EKF将非线性系统线性化时会带来计算精度下降的问题，而且计算 雅克比 矩阵也是比较困难的，甚至难以实现 。 针对 EKF 的不足，牛津大学的 Julier 等人提出了一种基于 UT 变换的新型卡尔曼滤波器，也就是无迹卡尔曼滤波法（ Unscented Kalman Filter， UKF）。 UKF算法不需要计算 Jacobian 矩阵，利于计算机编程，可以得到高精度的机器人位姿。 UKF法首先要构造 Sigma 散点集 , 设状态向量为 n 维 , 1kx 为时刻 k-1 的状态向量估计值 , 1kP 为该时刻状态向量的协方差矩阵 ,
15、 2n+1维的 Sigma 点集可以表示为 : 0, 1 1kkxx （ 15） , 1 1 , 1 ( ( ) )i k k i k ix x n P （ 16） , 1 1 , 1 ( ( ) )i n k k i k ix x n P （ 17） 对应的 i 的一阶二阶权系数： 南京理工大学 机器人控制理论与技术课程论文 )(2/1 )/( nnW mi00ii（ 18） )(2/1 1)/( 2 nnW ci 00ii （ 19） 由时刻 k-1的 1kx 和 1kP 来计算 Sigma 点集 1,kix ）（ Li .,1,0 ，通过非线性函数 11 )( kk qf 传播为 1/,
16、 kkix ，由 1/, kkix 可得状态向量预测值 1/ kkx 及误差协方差阵1/ kkP 。 11,11/, )( kkikkki qxfx Li ,.,10 （ 20） 10 1,11/,01/ )( kLi kikmikkiLimikk qxfWxWx （ 21） 10 1/1/,1/1/,1/ )( kLiTkkkkikkkkicikk QxxxxWP （ 22） 同理，利用 1kx 和 1kP 按 前面的采样策略来计算 Sigma 点集 1,kix ）（ Li .,1,0 ，通过非线性量测函数 kk rh )( 传播为 1/, kki ，由 1/, kki 可得输出预测值 1/
17、 kkz 及自协方差阵kzP和互协方差阵kkzxPkkkikkki rxh )( 1/,1/, （ 23） kkkiLiLi kmikkimikk rxhWWz )( 1/,0 01/,1/ （ 24） kLiTkkkkikkkkiciz RzzWPk 0 1/1/,1/1/, )( （ 25） Li Tkkkkikkkkicizx zxxWP kk 0 1/1/,1/1/, )( （ 26） 在获得新的量测后 kz ，进行滤波量测更新 南京理工大学 机器人控制理论与技术课程论文 TkzkkkkzzxkkkkkkkkKPKPPPPKzzKxxkkkk1/11/1/ )(（ 28） 2.4 Sa
18、ge-Husa自适应卡尔曼滤波 实际系统中，系统过程噪声方差矩阵 Q 和量测误差方差阵 R 事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵 H 也不能确切建立。 若建立的模型与实际模型不符可能回引起滤波发散。 自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法 。下面简单介绍一种 系统模型参数已知，而噪声统计参数 Q 和 R 未知情况下的自适应滤波。由于 Q 和 R 等参数最终是通过增益矩阵 K 影响滤波值的，因此进行自适应滤波时，也可以不去估计 Q 和 R 等参数而直接根据量测数据调整 K就可以了。 输出相关法自适应滤波的基本途径就是根据量测数据估计出输出函数序列Ck
19、， 再由 Ck 推算出最佳增益矩阵 K，使得增益矩阵 K 不断地与实际量测数据 Ck 相适应。 kkkkk wxx 11, （ 29） kkkk vxHz （ 30） 0)()(,)()(,)(TjkkjkTkkkkkjkTkkkkvwERvvErvEQwwEqwE（ 31） Sage-Husa 自适应卡尔曼滤波是在利用量测数据进行递推滤波时，通过时变噪声估计估值器，实时估计和修正系统噪声和量测噪声的统计特性，从而达到降低系统模型误差、抑制滤波发散提高哦滤波精度的目的。 Sage-Husa 自适应卡尔曼滤波算法实现 : kkkkk zKxx 1, （ 32） 111,1, kkkkkk qxx
20、 （ 33） 南京理工大学 机器人控制理论与技术课程论文 kkkkkk rxHzz 1, （ 34） 11,1, kTkkkkTkkkk RHPHHPK （ 35） 11,11,1, kT kkkkkkk QPP （ 36） 1,)( kkkkk PHKIP （ 37） 其中， kr 、 KR 、 kq 和 kQ 由以下时变噪声统计估值器获得： )()1( ,1111 kkkkkkkk xHzdrdr （ 38） )()1( 1,11111 TkkkkTkkkkkk HPHzzdRdR （ 39） )()1( ,111 kkkkkkkk xxdqdq （ 40） )()1( ,1,111111
21、1 T kkkkkkTkTkkkkkkk PPKzzKdQdQ （ 41） 式中：111 kk bbd， 10 b 为遗忘因子。 如果系统状态变量的维数 较高，而 Sage-Husa 自适应滤波算法中又增加了对系统噪声统计特性的计算，计算量将大大增加，实时性也将难以得到保证。除此之外，对于阶次较高的系统， Sage-Husa 自适应滤波算法中 kR 和 kQ 的在线估计有时会由于计算发散失去半正定性和正定性而出现滤波发散现象，此时Sage-Husa 自适应滤波算法的稳定性和收敛性不能完全保证。 基于极大似然准则的自适应卡尔曼滤波，通过系统状态方差阵和量测噪声方差阵实时估计系统噪声统计特性的变化
22、，以保证滤波器更好地适应这种变化。极大似然估计从系统量测量出现概率最大的角度估计，其特点是不仅考虑新息的变化，而且考虑新息协方差矩阵 vkC 的变化。它的量测噪声协方差矩阵 R 和系统噪声协方差矩阵 Q 为： Tkkkkvkk HPHCR 1, （ 42） 1,11,11 kkkT kkkTikNki ik PPxxNQ（ 43） kkkkkk vKxxx 1, （ 44） 南京理工大学 机器人控制理论与技术课程论文 kNkiTiivk vvNC11 （ 45） 式中： 1, kkkk zzv ， N 为平滑窗口的宽度。 自适应滤波是一种具有抑制滤波发散作用的滤波方法。在滤波过程中 自适应滤波
23、一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。 2.5 分解法（ FastSLAM） 此外除了 经典卡尔曼滤波算法及其拓展 外， SLAM实现还包括 M.Montemerlo 等人提出 的 一种基于粒子滤波器 (Particle Filter) FastSLAM 方法 。 FastSLAM 将SLAM分解为机器人定位和特征标志的位置估计两个过程 。 粒子滤波器中的每个粒子代表机器人的一条可能运动路径 ， 利用观测信息计算每个粒子的权重 ， 以评价每条路径的好坏 。 对于每个粒子来说 ， 机器人的运动路径是确定的 ， 因此特征标志之间相互独立 ， 特
24、征标志的观测信息只与机器人的位姿有关 ， 每个粒子可以采用 n 个卡尔曼滤波器分别估计地图中 n 个特征的位置 。 假设需要 k个粒子实现 SLAM、 FastSLAM， 总共有 kn个卡尔曼滤波器 。 FastSLAM 的时间复杂度为O(kn)， 通过利用树型的数据结构进行优化 ， 其时间复杂度可以达到 O( klog n)。 FastSLAM算法 另一 主要优点是通过采用粒子滤波器估计机器人的位姿 ， 可以很好地表示机器人的非线性、非高斯运动模型 。 SLAM 分解可以从 SLAM 的路径后验概率中直接推导。用条件概率的定义，SLAM 后验概率可以写成： tttttttttttt nuzs
25、pnuzspnuzsp ,|,|,|, （ 46） 因此，要得到分解的后验概率，对所有的非负值 t 需要以下方程： ),(),(1 nuzsnuzsttttNn ntttt pp （ 47） 这个论断的证据可以从归纳中得到。要得到这个结果，需要两个中间结果第一个量是观测到的环境特征 nt 的概率。这个量可以用贝耶斯定理写成： ),(),( ),(),( 11 1 nuzspnuzszp nuzszpnuzsp ttttnttttttttttttttn ttt n （ 48） 南京理工大学 机器人控制理论与技术课程论文 目前的观测量 仅仅与机器人的状态和被观测到的环境特征有关。在（ 2.13）最
26、右边的项，在没有新的观测量 的情况下，当前的位姿 、当前的控制量 、当前的数据关联 对 没有影响。因此，所有这些变量可以省去得到： ),(),( ),(),( 11111 nuzspnszp nuzszpnuzsp ttttnttnttttttttttn ttt （ 49） 需要的第二个中间结果是 ),( nuzsp ttttnnt ，没有观测到的任意环境特征的概率这很简单，如果没有观测到环境特征，环境特征后验概率不变。因此，环境特征的后验概率在 t 时刻等于 1t 时刻。首先，在 1t 时刻做归纳假设 ： Nn ttttntttt nuzspnuzsp 1 11111111 ),(),( （
27、 50） 归纳的开始情况 0t ,没有加入观测量。因此， 0t ，分解因式是对的。在 0t ,再一次用贝耶斯定理来扩展（ 2.12）的左边可以得到： ),(),(),( nuzspnuzspnuzsp ttttN nn tttttntttttt （ 51） 等于单独的环境特征后验概率的乘积。 ),(),( 1 nuzspnuzsp ttttnNntttt （ 52） 该方法将 SLAM问题分解为对机器人姿态和路标在地图中的位置的递归算法。每一粒粒子都有对应的地图，再将地图估计分解成 N个独立的特征估计，路径估计采用粒子滤波器，地图估计采用扩展卡尔曼滤波器。 FastSLAM算法 有机地将粒子滤
28、波器与卡尔曼滤波器集成在一起，鲁棒性地解决数据关联和多目标跟踪问题，其时间消耗与路标的数量成对数关系，计算量小，用时短。 基于 Rao-Blackwellized粒子滤波器的 FastSLAM算法是一种高效的机器人同步定位和绘制地 图的算法，其具有高效性和准确性，该方法使用提高了机器人地图创建的实时性，增强了避障能力。 三 . 总结 本文着重介绍了 卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、 Sage-Husa自适应卡尔曼滤波和分解法（ FastSLAM） 等 SLAM 实现方法。从这些算法当中总结了各自的优缺点以及适应条件，相信在不断地总结前人经验的基础之上发现算法的缺点如 降低计算复杂度
29、、提高鲁棒性 等。在不断地探索中才能取得更大的成果。 南京理工大学 机器人控制理论与技术课程论文 近年来机器人领域的学者 对 SLAM 进行了大量的研究，是在降低计算复杂度、提高鲁棒性等方面取得了很大的进展。随着研究的深入， 下面几个问题 成为了当前 SLAM 的研究热点：如何更好的将视觉处理与其他传感器结合，提高环境特征提取的精度，减少误差，提高定位和构图的精确性；如何将二维静态环境中的研究与应用扩展到与现实中的环境切合的动态的三维环境；如何将人工智能、智能控制等领域的方法引入到 SLAM 中 ,开发新的更加有效的 SLAM 算法 .； 扩展 SLAM的应用环境： 如何找出更好的地图表达方式
30、，特别是复杂地形和大环境中的地图表达方式； 深入研究基于多机器人协作的 SLAM，提高其应用水平；研究更有效的 SLAM 实现方法 。 参考文献 1. 郭剑辉 . 移动机器人同时定位与地图构建方法研究 D. 博士学位论文 . 南京 :南京理工大学计算机科学与技术学院 , 2008. 2. 陈卫东 , 张飞 . 移动机器人的同步自定位与地图创建研究进展 J. 控制理论与应用 , 2005, (03): 455-460. 3. 周东华 , 席裕庚 , 张钟俊 . 一种带多重次优渐消因子的扩展卡尔曼滤波器 J. 自动化学报 , 1991(6):689-695. 4. 张友民 . 卡尔曼滤波计算方法研
31、究进展 C/ 中国自动化学会青年学术年会 . 1994:529-538. 5. 吴玲 , 卢发兴 , 刘忠 . UKF 算法及其在目标被动跟踪中的应用 J. 系统工程与电子技术 , 2005, 27(1):49-51. 6. 潘泉 , 杨峰 , 叶亮 ,等 . 一类非线性滤波器 UKF 综述 J. 控制與決策 , 2005, 20(5):481-489. 7. 张常云 . 自适应滤波方法研究 J. 航空学报 , 1998, 19(Z1):97-100. 8. 杨元喜 . 自适应动态导航定位 M. 测绘出版社 , 2006. 9. 周武 , 赵春霞 . 一种基于遗传算法的 FastSLAM 2.
32、0 算法 J. 机器人 , 2009, 31(1):25-32. 10. 陶辉 , 吴怀宇 , 程磊 ,等 . 轮式移动机器人 FastSLAM 算法研究 J. 哈尔滨理工大学学报 , 2011, 16(1):42-47. 南京理工大学 机器人控制理论与技术课程论文 四 . 课程论文评分标准表 评价内容 具 体 要 求 分值 评分 查阅、收集资料 查阅一些相关资料，收集素材，进行参考。 10 选题、构思、主见 选题新颖，构思全面，对问题有较深刻的认识，有一定独特见解。 10 逻辑结构 结构合理，层次分明，条理清晰，逻辑性强。 10 撰写质量 格式规范，语句通顺，语言准确，书写工整，达到论文要求的字数。 20 学过知识的运用 结合学过的内容，充分运用掌握的知识，充分表达自己的观点。 20 分析与阐述问题的能力 所阐述问题清楚，突出重点，论文表现出对实际问题有较强的分析能力和概括能力，并所论述的事项有说服力。 30 总分
