2. A Thesis Applying for Master Degree of Shanghai Jiao Tong University
MOBILE ROBOT SIMULTANEOUS LOCALIZATION
AND MAPPING IN LARGE AREA
Supported by the Natural Science Foundation of China under grant
60475032 and Program for New Century Excellent Talents in University
Department: Department of Automation
Major: Control Theory and Control Engineering
Author: Dong Haiwei
Advisor: Assoc. Researcher Li Ning
Prof. Chen Weidong
Shanghai Jiao Tong University
February, 2008
5. 上海交通大学硕士学位论文 摘要
第 I 页
大范围环境下移动机器人同步定位和地图创建研究
摘 要
随着 SLAM 技术的不断发展，计算效率已经成为制约 SLAM 发展的主
要因素。目前，在 SLAM 领域中越来越多的学者从稀疏化信息矩阵的角度
提高 SLAM 算法的效率。在这些研究工作中，对于稀疏化信息矩阵问题，
大都从 Bayes 网络的拓扑结构入手，通过构造合理的拓扑结构来实现规
范化信息矩阵的稀疏化。而在具体算法中则根据网络拓扑结构特性对路
标进行分类处理，这样做虽然会使信息矩阵在未经任何处理的情况下自
然稀疏化，从而提高 SLAM 估计结果的精度，但由于算法中需要对路标进
行分类，因此算法的时间复杂度也会随之增加。本文提出的算法是在
Eustice 扩展信息滤波 SLAM 算法上的改进，通过对信息矩阵结构特征的
分析，使得改进后的算法不仅解决了 Thrun 算法中直接稀疏化信息矩阵
导致的非一致问题，而且实时性和实用性也大为提高了。另外，本文提
出的算法与上述通过改进 Bayes 网络拓扑结构得到的算法相比，估计结
果相当，因而具有很高的理论和实际意义。
针对 SLAM 领域中信息矩阵稀疏化的问题，本文从扩展信息滤波 SLAM
算法的几何意义和相关公式两个角度进行分析，不仅得到了规范化信息
矩阵具有稀疏性的结论，而且首次提出了扩展信息滤波 SLAM 算法中信息
矩阵的结构特征。根据信息矩阵的结构特征，本文提出了一种扩展信息
滤波 SLAM（Extended Information Filter SLAM，EIF-SLAM）的改进算
法：通过直接对信息矩阵进行合理地稀疏化，并结合稀疏矩阵高效的运
6. 上海交通大学硕士学位论文 摘要
第 II 页
算规则，显著降低了原算法的计算复杂度；借助环闭合（或重定位）技
术，有效抑制了由于稀疏化所导致的误差，实现了在保证 SLAM 估计结果
一致准确前提下的高效性。最后，稀疏化与 SLAM 求解精度的关系也从理
论上给予了证明。
仿真针对一个大规模环境下的 SLAM 问题，讨论了直接稀疏化信息矩
阵扩展信息滤波 SLAM 算法的应用。在设定运动模型和观测模型参数之后，
通过仿真结果重点对信息矩阵结构特征、算法效率、环闭合以及误差和
协方差四个关键问题进行了分析。实验分别就室内具有摄像头的两轮机
器人和室外具有激光雷达的四轮机器人的情况进行了讨论。两个实验各
具特色：实验一在实验室内通过自主研制的 Frontier-Ⅱ机器人完成。实
验中将平面图片作为路标，通过尺度不变特征变换（SIFT）算法实现数
据关联；实验二采用 SLAM 领域中标准的停车场数据集，其实验环境为室
外停车场，机器人为具有激光雷达的四轮汽车。实验中采用圆柱形物体
作为路标，根据自身兼容最近邻居算法（ICNN）进行数据关联。仿真与
实验结果有力的验证了本文所提出的算法的有效性。
关键词：移动机器人，SLAM，扩展信息滤波，信息矩阵，稀疏化
7. 上海交通大学硕士学位论文 ABSTRACT
第 III 页
MOBILE ROBOT SIMULTANEOUS LOCALIZATION
AND MAPPING IN LAGRE AREA
ABSTRACT
Computational efficiency has become quite crucial while SLAM
technology becomes much more mature. Recent research concerning
information sparsification for Simultaneous Localization and Mapping
(SLAM) has become quite popular. By classifying landmarks into different
types, various Bayes topological network structures are built which could
make information matrix exactly sparse. However, although the accuracy
remains, the efficiency of SLAM algorithm is ruined during landmark
classification. Compared to the research work shown before, our new
algorithm which is based on Eustice's work is much more efficient and
practical while maintaining high accuracy.
Focusing on sparsing information matrix, geometrical meaning of
Extended Information Filter SLAM (EIF-SLAM) and related formula are
analyzed respectively. The conclusion is drawn that normalized information is
sparse. Moreover the structure of information matrix in EIF-SLAM is put
forward for the first time. According to the structure of information matrix, an
improved algorithm based on EIF is introduced. Owning to the high
efficiency of the special rules in sparse operation, the new algorithm solves
SLAM very efficient by sparsing information matrix directly. The errors that
come from sparsification decrease apparently due to loop-closure. In a word,
the new algorithm realizes efficiency with consistent estimate of SLAM. At
last, the relationship between sparsification and SLAM accuracy is analyzed
theoretically.
The new algorithm is simulated in a large scale environment.
Information matrix sparsification, algorithm efficiency, relocalization, error
8. 上海交通大学硕士学位论文 ABSTRACT
第 IV 页
and covariance are analyzed respectively after setting up parameters of
motion model and observation model. Indoor two-wheel robot with camera
and outdoor four-wheel robot with laser are taken into account in experiments.
The two experiments are of great difference. Experiment one accomplishes by
Frontier-II robot which is developed by our lab. Pictures represent landmarks
that are associated by Scale Invariant Feature Transform (SIFT). Experiment
two adopts standard Car Park Dataset which is famous in SLAM field. The
environment of experiment two is outdoor car park and the robot is a
four-wheel mobile car mounted with laser sensor. Cylindrical objects
represent landmarks that are associated by Individual Compatibility Nearest
Neighbor (ICNN). The results of simulation in large scale environment as
well as indoor and outdoor experiments testify validity of the algorithm.
Keywords: Mobile robotics，SLAM，Extended information filter，Information
matrix，Sparsification
13. 上海交通大学硕士学位论文 目录
第 IX 页
插图、表格英文题注列表
FIG. 2.1 DENSITY COMPARISON OF VARIANCE MATRIX & INFORMATION MATRIX................................................. 7
FIG. 2.2 ALGORITHM DIAGRAM OF INFORMATION FILTER SLAM ........................................................................ 7
FIG. 2.3 PRINCIPLE OF DATAASSOCIATION ........................................................................................................ 10
FIG. 2.4 RESULTS OF ICNN DATAASSOCIATION ................................................................................................ 14
FIG. 2.5 RESULTS OF JCBB DATAASSOCIATION................................................................................................. 16
FIG. 2.6 SIMULATION ENVIRONMENT OF DATAASSOCIATION ............................................................................. 17
FIG. 2.7 ANALYSIS OF ICNN DATAASSOCIATION............................................................................................... 18
FIG. 2.8 ANALYSIS OF JCBB DATAASSOCIATION............................................................................................... 19
FIG. 3.1 SIMULATION RESULTS OF EIF-SLAM IN 1-D, 1-DOF CASE................................................................. 26
FIG. 3.2 SIMULATION RESULTS OF EIF-SLAM IN 2-D, 2-DOF CASE................................................................. 27
FIG. 3.3 STATISTIC HISTOGRAM OF ELEMENTS DISTRIBUTION IN NORMALIZED INFORMATION MATRIX.............. 29
FIG. 3.4 LINK STRENGTH BETWEEN ROBOT & FEATURES................................................................................... 29
FIG. 3.5 STATISTIC ANALYSIS OF EIF-SLAM ALGORITHM ON COMPUTATION TIME............................................ 30
FIG. 3.6 ERROR AND COVARIANCE OF ESTIMATION IN 2-D, 2DOF CASE............................................................ 31
FIG. 3.7 MAP ERROR ......................................................................................................................................... 31
FIG. 3.8 COMPARISON OF MOTION TRAJECTORY BETWEEN 2DOF & 3DOF.......................................................... 33
FIG. 3.9 GRAPHICAL USER INTERFACE OF SLAM SIMULATION ENVIRONMENT.................................................. 33
FIG. 3.10 ESTIMATION RESULTS OF 2WB, 2-D, 3-DOF SLAM BEFORE AND AFTER LOOP CLOSURE .................. 35
FIG. 3.11 ERROR AND COVARIANCE OF ESTIMATION IN 2WB, 2-D, 3-DOF CASE............................................... 37
FIG. 4.1 EVOLUTION OF BAYES NETWORK AND INFORMATION MATRIX IN PROJECTION ..................................... 38
FIG. 4.2 EVOLUTION OF BAYES NETWORK AND INFORMATION MATRIX IN ADDING FEATURES ........................... 40
FIG. 4.3 EVOLUTION OF BAYES NETWORK AND INFORMATION MATRIX IN UPDATE ............................................ 40
FIG. 4.4 ELEMENT DISTRIBUTION OF INFORMATION MATRIX ............................................................................. 42
FIG. 4.5 RESULTS OF INFORMATION MATRIX SPARSIFICATION............................................................................ 44
FIG. 4.6 RELATIONSHIP BETWEEN SPARSIFICATION RATIO & THRESHOLD.......................................................... 45
FIG. 4.7 EFFICIENCY COMPARISON OF MATRIX INVERSION BETWEEN DENSE & SPARSE METHOD....................... 46
FIG. 4.8 RELATIONSHIP BETWEEN SPARSE RATIO & EFFICIENCY IN MATRIX INVERSION ..................................... 47
FIG. 5.1 SIMULATION ENVIRONMENT ................................................................................................................ 51
FIG. 5.2 LOG STATISTIC HISTOGRAM OF ELEMENT DISTRIBUTION IN INFORMATION MATRIX .............................. 54
FIG. 5.3 EFFICIENCY COMPARISON BETWEEN EIF-SLAM & IMPROVED EIF-SLAM......................................... 55
FIG. 5.4 EIF-SLAM ESTIMATION RESULTS BEFORE & AFTER LOOP-CLOSURE ................................................... 57
FIG. 5.5 IMPROVED EIF-SLAM ESTIMATION RESULTS BEFORE & AFTER LOOP-CLOSURE.................................. 58
FIG. 5.6 COMPARISON OF ERROR AND COVARIANCE BETWEEN EIF-SLAM & IMPROVED EIF-SLAM............... 59
FIG. 6.1 FRONTIER-Ⅱ AUTONOMOUS MOBILE ROBOT ...................................................................................... 60
FIG. 6.2 CORRIDOR EXPERIMENT ENVIRONMENT .............................................................................................. 61
14. 上海交通大学硕士学位论文 目录
第 X 页
FIG. 6.3 CORRIDOR EXPERIMENT RESULTS ........................................................................................................ 63
FIG. 6.4 LOCALIZATION ERROR OF ROBOT IN CORRIDOR EXPERIMENT .............................................................. 63
FIG. 6.5 CAR PARK EXPERIMENT RESULTS......................................................................................................... 64
FIG. 6.6 LOCALIZATION ERROR OF ROBOT IN CAR PARK EXPERIMENT............................................................... 65
FIG. 7.1 COMPARISON OF MATRIX STORAGE RULES ........................................................................................... 69
FIG. 7.2 INVOKING RELATION OF VC++ PROGRAMS.......................................................................................... 70
TAB. 2.1
2
,d DISTRIBUTION .......................................................................................................................... 12
TAB. 2.2
2
,d DISTRIBUTION .......................................................................................................................... 12
TAB. 3.1 MARK NOTATION IN NORMALIZED INFORMATION MATRIX .................................................................. 26
TAB. 4.1 STRUCTURE ANALYSIS OF INFORMATION MATRIX（EIF FORMULA） ................................................. 42
TAB. 4.2 STRUCTURE ANALYSIS OF INFORMATION MATRIX（BAYES NET）...................................................... 43
TAB. 7.1 ILLUSTRATION OF DATA TYPES IN MKL.............................................................................................. 67
TAB. 7.2 ILLUSTRATION OF MATRIX TYPES IN MKL.......................................................................................... 67
15. 上海交通大学硕士学位论文 第一章 引言
第 1 页
1 第一章 引言
1.1 背景
在移动机器人领域中，各种条件下的定位和地图创建问题已经得到了较完满的解
决，技术也比较成熟了。但在很多情况下，环境的信息是部分、甚至是完全未知的，
此时必须将定位和地图创建结合起来，使得机器人利用对自身的定位来创建地图，同
时利用自身创建的地图来定位，这就是移动机器人同步定位和地图创建问题
（Simultaneous Localization And Mapping，SLAM），最早是由 Smith、Self 和 Cheeseman
提出来的[1, 2]
。由于 SLAM 是移动机器人实现真正自主的核心问题，因此 SLAM 被视
为移动机器人领域的“圣杯”。
不确定性的表示以及概率地图的提出为 SLAM 问题的深入研究提供了理论基础，
而 Smith 等人将扩展卡尔曼滤波（Extended Kalman Filter, EKF）成功的应用于 SLAM
领域，则标志着从概念和理论的角度，SLAM 问题已经得以解决。然而，由于
EKF-SLAM 的计算量与环境地图中路标数目的二次方成正比，即 EKF-SLAM 的实时
性很差，因而 EKF-SLAM 应用于大规模实际环境受到了极大的限制。
在过去的十年中，科研人员将主要精力投向，在保证对地图和机器人位置一致准
确估计的前提下，改进 SLAM 算法的计算效率。当然，也有不少研究工作集中在诸
如消除线性化误差、数据关联和路标特征提取等问题上。经过十多年的研究和发展，
产生了许多 SLAM 算法，以下从环境地图的类型和模型类型两个角度对 SLAM 算法
进行分类。
根据 SLAM 问题中环境地图的类型，可以将 SLAM 算法分为
基于特征地图的 SLAM 算法（Feature-based SLAM）[3, 4]
基于栅格地图的 SLAM 算法（Grid-based SLAM）[3, 4]
基于拓扑-栅格地图 SLAM 算法（Topological-Metric SLAM）[5-7]
基于密度地图的 SLAM 算法（Dense SLAM）[8, 9]
16. 上海交通大学硕士学位论文 第一章 引言
第 2 页
其中，特征地图是 SLAM 问题中最经典也是应用最为广泛的环境地图描述形式；
计算量大和所需存储量空间大是栅格地图的突出问题；拓扑-栅格地图较栅格地图大
大增强了算法的鲁棒性；而密度地图在栅格地图的基础上增加了许多辅助信息，是一
种高级的地图形式。
根据 SLAM 问题中模型的描述不同，可以将 SLAM 算法分为
基于状态空间描述的一类算法，如扩展卡尔曼滤波（Extended Kalman Filter,
EKF）[10, 11]
、压缩扩展卡尔曼滤波（Compressed Extended Kalman Filter, CEKF）
[12]
等
基于样本集描述的一类算法，如 Rao-Blackwellized 粒子滤波 SLAM[13, 14]
，
快速 SLAM（FastSLAM）[15]
、DP-SLAM[16, 17]
等
基于信息空间描述的一类算法，如扩展信息滤波（Extended Information Filter,
EIF）、稀疏连接-树滤波（Thin Junction-Tree Filter, TJTF）[18]
等
基于差异描述的一类算法，如扫描匹配（Scan Matching）[19-21]
其中，基于状态空间的描述是控制领域的经典描述方法，在 SLAM 中的应用非
常广泛；基于样本集的描述方法可以有效解决非线性的问题，但样本数目与估计效果
的关系目前还不清楚，即选取多少样本合适还是目前热门的研究方向；基于信息空间
的描述特别适合 SLAM 问题，其实时性特别好，是最具发展潜力的描述形式；基于
差异描述的算法，没有对 SLAM 问题的不确定性进行表达，算法的收敛性及一致性
无法分析，因而被许多学者所置疑。
在上述 SLAM 算法中，扩展信息滤波（EIF）算法是扩展卡尔曼滤波（EKF）算
法的对称形式，在保留 EKF-SLAM 数学严谨、估计准确特点的同时，其算法结构又
特别适合 SLAM 问题，因此越来越多的科研人员开始关注 EIF-SLAM 算法[22-28]
。
1.2 国内外研究现状
目前，对 SLAM 研究仍然是移动机器人领域中的热点。由于本文的研究基础是
扩展信息滤波 SLAM，因此以下主要讨论扩展信息滤波 SLAM 的国内外研究现状。
最早将扩展信息滤波算法应用于 SLAM 领域是在上世纪七十年代末。与卡尔曼
滤波算法利用稠密的协方差矩阵和均值向量描述 SLAM 问题不同，EIF 则通过信息矩
阵和信息向量来描述 SLAM 问题中的不确定性。由于信息矩阵与协方差矩阵互为逆
矩阵，因此 EIF-SLAM 算法的更新步骤效率特别高，与路标数量的二次方成正比。
17. 上海交通大学硕士学位论文 第一章 引言
第 3 页
然而，从信息矩阵和信息向量恢复均值向量的计算量较大，因此，当时扩展信息滤波
的应用受到了极大的限制。
Thrun 等以及 Frese 和 Hiringer 分别在对 EIF-SLAM 研究中发现，规范化后的信
息矩阵是几乎自然稀疏的，即绝大多数非主对角线上的元素都接近与零[25, 29]
。通过
直接将上述接近于零的元素置为零，使得算法的运算效率大为提高。然而后来的研究
表明，Thrun 算法中稀疏化所导致的误差有可能使 SLAM 估计结果非一致。
从几何意义上看，规范化信息矩阵可以通过高斯马尔可夫随机域（Gaussian
Markov Random Field，GMRF）来表示[30]
，即规范化信息矩阵每个元素的大小都与
Bayes 网络中连接的强度一一对应。通过断开 Bayes 网络中的弱连接，Paskin 等将基
于 Bayes 网络的图模型转化为稀疏树的结构。针对上述树形 SLAM 问题结构，Paskin
和 Frese 分别提出了稀疏连接树滤波（Thin Junction Tree Filter，TJTF）[18]
和树图滤波
（Treemap Filter）[31-34]
，其计算效率分别为 ( )O n 和 (log )O n 。
在过去的十年中，一些科研人员将机器人的运动的全部轨迹和传感器观测的所有
信息根据极大似然估计（Maximum Likelihood Estimate，MLE）统筹考虑，统称其为
批算法（Batch Algorithm）[35-39]
。目前，将扩展信息滤波算法与批算法结合形成了两
个主流的发展方向，分别为基于轨迹的 SLAM（Trajectory-Oritened SLAM）和延迟地
图 SLAM（Delayed Mapping SLAM）。前者非常适合环境路标不容易识别，而直接获
取传感器数据却很方便的环境[38, 40, 41]
；后者则借助大量的传感器信息使路标位置逼
近高斯分布，从而提高了 SLAM 的估计精度[42]
。
另外，Wang 将 SLAM 中定位和地图创建分别处理，得到解耦合 SLAM 算法
（Decoupled SLAM，D-SLAM）[17, 43-45]
。算法利用传感器观测到的路标之间的相对
位置来描述地图的分布。去除机器人位置信息可以使得信息矩阵自然稀疏（naturally
sparse），然而机器人需要不停的通过观测路标来对自身进行定位。
1.3 本文所做的工作及内容安排
综上所述，扩展信息滤波 SLAM 算法已经成为 SLAM 领域中主流的方法之一。
随着 SLAM 技术的不断发展，计算效率已经成为制约 SLAM 发展的主要因素。在最
近的五年中，越来越多的学者从稀疏化信息矩阵的角度提高扩展信息滤波 SLAM 算
法的效率。其中，最具代表性的工作包括 Thrun 等首次解决较大规模环境（维多利亚
公园）的 SLAM 问题[25]
，Eustice 等在深海中建立泰坦尼克沉船地图[46]
等。在这些研
18. 上海交通大学硕士学位论文 第一章 引言
第 4 页
究工作中，对于稀疏化信息矩阵问题，大都从 Bayes 网络的拓扑结构入手，通过构造
合理的拓扑结构来实现规范化信息矩阵的稀疏化[22, 41, 46, 47]
。而在具体算法中则根据
网络拓扑结构特性对路标进行分类处理，这样做虽然会使信息矩阵在未经任何处理的
情况下自然稀疏化，从而提高 SLAM 估计结果的精度，但由于算法中需要对路标进
行分类，因此算法的时间复杂度也会随之增加。本文提出的算法是在 Eustice 扩展信
息滤波-SLAM 算法[41]
上的改进，通过对信息矩阵结构特征的分析，使得改进后的算
法不仅解决了 Thrun 算法中直接稀疏化信息矩阵导致的非一致的问题，而且实时性和
实用性也大为提高了。另外，本文提出的算法与上述通过改进 Bayes 网络拓扑结构得
到的算法相比，估计结果相当，因而具有很高的理论和实际意义。
针对 SLAM 领域中信息矩阵稀疏化的问题，本文通过揭示信息矩阵的结构特征，
提出了一种扩展信息滤波 SLAM（Extended Information Filter SLAM，EIF-SLAM）的
改进算法：通过直接对信息矩阵进行合理地稀疏化，显著降低了原算法的计算复杂度，
并借助环闭合（或重定位）技术，有效抑制了由于稀疏化所导致的误差。稀疏化与
SLAM 求解精度的关系也从理论上给予了证明。仿真对信息矩阵稀疏化、算法效率、
环闭合以及误差和协方差四个关键问题进行了分析。实验分别就室内具有摄像头的两
轮机器人和室外具有激光雷达的四轮机器人的情况进行了讨论。仿真与实验结果有力
的验证了本文所提出的算法的有效性。
本文共分八章，具体来说，各章内容安排如下：
第一章提出了移动机器人同步定位和地图创建（Simultaneous Localization and
Mapping，SLAM）问题。针对目前 SLAM 领域中的众多算法，分别从环境地图的类
型和模型类型两个角度对 SLAM 算法进行了分类。在讨论了扩展信息滤波 SLAM 国
内外研究现状的基础上，通过与 Eustice 和 Thrun 研究工作的对比，阐述了本文的主
要研究工作。
第二章介绍了扩展信息滤波 SLAM 算法的基本思想，并通过对相关文献的整理
和修改，得到了移动、增加路标和更新三个基本步骤的数学表达式。针对 SLAM 算
法中的数据关联问题，在引入数据关联问题模型的基础上，介绍了 SLAM 领域中两
种最重要的数据关联算法：自身兼容最近邻居法（ICNN）与枝和范围联合兼容方法
（JCBB）。仿真针对一个路标间距较小的环境,比较了 ICNN 和 JCBB 在数据关联鲁棒
性上的差异。
第三章总结了二自由度机器人、二轮三自由度机器人和四轮三自由度机器人的运
动模型，并分析了全局坐标类传感器和距离方向类传感器的观测模型。通过设置
19. 上海交通大学硕士学位论文 第一章 引言
第 5 页
SLAM 算法的仿真参数，分别对一维一自由度、二维二自由度和二维三自由度的情况
进行了仿真。通过分析仿真结果，阐释了规范化信息矩阵、环闭合以及 Bayes 网络连
接强度等关键概念和方法。
第四章分别从扩展信息滤波 SLAM 算法的几何意义和相关公式两个角度进行分
析，不仅得到了规范化信息矩阵具有稀疏性的结论，而且首次提出了扩展信息滤波
SLAM 算法中信息矩阵的结构特征。通过建立合理的稀疏化算法，并结合稀疏矩阵高
效的运算规则，实现了在保证 SLAM 估计结果一致准确前提下的高效性。最后，定
量的分析了稀疏化与求解精度的关系。
第五章针对一个大规模环境下的 SLAM 问题，讨论了直接稀疏化信息矩阵扩展
信息滤波 SLAM 算法的应用。在设定运动模型和观测模型参数之后，通过仿真结果
重点对信息矩阵结构特征、算法效率、环闭合以及误差和协方差四个关键问题进行了
分析。
第六章分别就室内具有摄像头的两轮机器人和室外具有激光雷达的四轮机器人
的情况进行了讨论。两个实验各具特色：实验一在实验室内通过自主研制的 Frontier-
Ⅱ机器人完成。实验中将平面图片作为路标，通过尺度不变特征变换（SIFT）算法实
现数据关联；实验二采用 SLAM 领域中标准的停车场数据集，其实验环境为室外停
车场，机器人为具有激光雷达的四轮汽车。实验中采用圆柱形物体作为路标，根据自
身兼容最近邻居算法进行数据关联。
第七章在介绍 Intel Math Kernel Library（MKL）基本功能的基础上，总结了 MKL
中的函数命名规则、数据类型和矩阵类型，为编写 SLAM Math Library 奠定了坚实的
基础。通过阐释矩阵存储格式、矩阵存储规则和模块调用关系三个主题明晰了本文
VC++程序的基本软件结构。
第八章对本文提出的直接稀疏化信息矩阵扩展信息滤波 SLAM 算法进行了全面
总结，并着重从稀疏化信息矩阵的角度提出了本文的贡献。另外，对于未来 SLAM
的研究，指出了多元信息下的 SLAM、复杂环境下的 SLAM 和 SLAM 模型未建模动
态三个 SLAM 领域中全新的问题。
20. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 6 页
2 第二章 扩展信息滤波 SLAM 解决方案
EQUATION SECTION 2
2.1 基本原理
在t 时刻，机器人和路标位置的分布满足
1
1
( , | , ) , ,
( , ) ( , )
t t t t t tt t
t t
x x x M x x x Mx xt t
t
Mx MM Mx MMM M
t t t t
p x M z u N N
N N
(2.1)
其中， tx ：状态向量，表示机器人的位置和姿态
M ：路标的集合
t
z ：路标观测值
t
u ：控制向量
式(2.1)指出，在给定机器人控制量 t
u 以及观测量 t
z 下， tx 和 M 的联合条件概率
密度，在时域中满足均值为 t ，协方差为 t 的正态分布；在信息域中满足均值为 t ，
协方差为 t 的逆正态分布。
扩展信息滤波器就是通过不断更新信息向量 t 和信息矩阵 t 来实现对机器人位
置和环境路标位置的估计[41]
。然而，由于信息向量和信息矩阵并不具有明显的物理
意义，因此必须将其恢复为具有明显物理意义的均值向量 t 和协方差矩阵 t 。具体
来说，两种形式的转换关系式为
1
t t t t t
(2.2)
对于正数来说，数越大其倒数越小。与此类似，通过式(2.2)转换得到的信息矩阵
t 较协方差矩阵 t ，其矩阵稠密度大大降低，如图 2.1所示。通常，SLAM 问题中的
协方差矩阵都是稠密的，但其逆矩阵（即信息矩阵）却是几乎稀疏的。换句话说，信
息矩阵中值较大的元素都集中在主对角线上，而其余位置的元素值相对很小（但不为
0）。扩展信息滤波 SLAM 算法就是利用上述信息矩阵的结构特征来提高运算效率的。
21. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 7 页
（a）协方差矩阵 t （b）信息矩阵 t
图2.1 协方差矩阵和信息矩阵稠密性对比
Fig. 2.1 Density comparison of variance matrix & information matrix
Time Projection
Get Observation
Data Association
New Feature?Add Features Do Update
Y N
图2.2 扩展信息滤波原理图
Fig. 2.2 Algorithm diagram of information filter SLAM
22. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 8 页
基于扩展信息滤波的移动机器人同步定位和地图创建算法，包括三个基本步骤：
移动、增加路标和更新。机器人在每个运动控制周期，通过移动改变自身的位置，同
时在新的位置观察周围的环境特征；在更新地图周期，根据数据关联的算法，区分所
观察到的环境特征是否是环境地图中的路标，如果是所建环境地图中的路标，则进行
更新步骤，否则，进行增加地图路标步骤，程序流程如图 2.2所示。通过对扩展信息
滤波相关文献[22, 23, 41, 46, 47]
的整理和修改，扩展信息滤波的三个主要步骤可以表述如
下：
a. 移动
机器人运动模型和一阶泰勒展开式为
1 1
1
( , )
( , ) ( )t t
t t t
x t t x
x f x u
f u F x
(2.3)
其中， F 是 f 在 tx 点处的雅克比式， (0, )N Q 是白噪声。移动后，信息矩阵
和信息向量的改变如下。
1 1
1 1 1
1 11 1
1 11 1 1 1 1
( , | , ) ( , )
,
tt
t t t t t
t t
t t t
xx M
t tT T
Mx MM Mx x M M Mx x
p x M z u N
Q F uQ F
F Q F Q u
(2.4)
其中，
1
1
1
1
1( , )
t t
t t
t t
T
x x
T
x x
T
x t x
Q F F
F Q F
u F Q f u F
b. 增加路标
增加路标过程中，机器人的观测模型和一阶泰勒展开式为
( , )
( ) ( )t t
t t t
x t x
l g x z
g G x
(2.5)
其中， tl 是路标所在位置的全局坐标， G 是 g 在 tx 点处的雅克比展开式，
(0, )N R 是高斯白噪声。增加路标后，信息矩阵和信息向量的改变如下。
23. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 9 页
1 1
1 1 1
1 1 1
1 1
11 1
( , , | , ) ( , )
0 ,
0
( , )
t t t t
t
t t
t t
t t t t
T T T
x x x M x
t Mx MM t M
x t x
p x x M z u N
G R G G R G R u
R uR G R
u g z G
(2.6)
c. 更新
更新过程中，机器人的观测模型和一阶泰勒展开式为
( )
( ) ( )
t t
t t t
z h
h H
(2.7)
其中， t 为估计的环境向量，它满足
1
( , ) ( , )t t t t tN N
， (0, )N R 是
白噪声， H 是 h 在 t 点处的雅克比式。更新后，信息矩阵和信息向量的改变如下。
1
1
1
1
( )
T
t t
T
t t
t
H R H
H R u
u z h H
(2.8)
移动、增加路标和更新三个步骤是扩展信息滤波中最重要的三个步骤，同时也是
计算量最大的三个步骤，关于信息矩阵稀疏化的讨论将围绕上述三个步骤展开。
2.2 数据关联
在 SLAM 的实际应用中，数据关联是一个至关重要的问题。在将环境测量信息
与已有的地图信息融合产生新的地图后，先前的数据关联将不能再修改。换句话说，
数据关联是不能动态调整的。然而，一个错误的数据关联就有可能导致全局地图估计
的较大误差，因此 SLAM 算法必须保证系统进行的所有数据关联都正确无误。另外，
当机器人经过一段时间的移动回到一个曾经建立过地图的区域时，即环闭合，数据关
联在此时更是重要。
假设机器人通过传感器检测环境中的路标 1{ , , }mE E ，得到传感器观测值集合
1{ , , }mz z z 。数据关联算法的基本原理就是建立一个假设集合 1{ , , }m mH j j ，使
得每一个环境路标 iE 都能与地图路标 ijF 相对应（如果是 iz 不与任何地图路标相对应，
则 0ij ）。传感器观测集合与地图路标的数据关联问题可以通过一个树形结构进行说
24. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 10 页
明，如图 2.3所示。
图2.3 数据关联原理
Fig. 2.3 Principle of data association
其中，对于第i 层，该层的所有节点都可能是与环境路标 iE 关联的地图路标，星
号表示不存在任何地图路标与环境路标 iE 相关联。因此，对于地图路标为 1{ , , }nF F
的情况，则每个节点都有 1n 个分枝。数据关联算法就是通过具体的树的搜索算法，
从( 1)m
n 种数据关联可能中，迅速找出合适的路标匹配对。目前，在 SLAM 领域中
产生了许多的数据关联算法，其中最为重要的两种方法包括自身兼容最近邻居法
（Individual Compatibility Nearest Neighbor，ICNN）与枝和范围的联合兼容方法（Joint
Compatibility Branch and Bound，JCBB），下面分别说明[48, 49]
。
2.2.1 自身兼容最近邻居法
自身兼容最近邻居法通过比较地图路标位置与环境路标观测位置是否足够接近
（通常用阈值来衡量）来判定该地图路标是否与此环境路标关联。
假设向量 x 为机器人 R 和地图路标集合 1{ , , }nF F 的真实位置，ˆx 是 x 的估计，即
为机器人位姿和环境路标位置的估计向量， P 为估计误差的协方差矩阵。
1
ˆ
ˆ
ˆ
ˆ n
R
F
F
x
x
x
x
(2.9)
25. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 11 页
1
1 1 1
1
n
n
n n n
R RF RF
T
RF F F F
T T
RF F F F
P P P
P P P
P
P P P
(2.10)
设环境中存在 m 个环境路标 1{ , , }mE E ，机器人通过传感器得到上述环境路标的
理论观测值 y 和实际观测值 ˆy 的关系为
ˆy y u (2.11)
其中，观测误差u 是均值为 0，协方差为 S 的 Gaussian 白噪声，即 (0, )u N S ，
具体来说，
1
ˆ
ˆ
ˆ m
E
E
y
y
y
(2.12)
1 1
1
m
m m
E E E
T
E E E
S S
S
S S
(2.13)
环境路标 iE 的观测值和对应的地图路标 ijF 满足下面的观测方程[49]
( , ) 0iijf x y (2.14)
通常，观测方程(2.14)是非线性的，在当前估计值处对其进行线性化处理
ˆ ˆ( , ) ( ) ( )i i i iij ij ij ijf x y h H x x G y y (2.15)
其中，
ˆ ˆ ˆ ˆ( , ) ( , )
ˆ ˆ( , ); ;i i
i i i i
ij ij
ij ij ij ij
x y x y
f f
h f x y H G
x y
向量 iijh 表示 iE 与 ijF 配对后所产生的误差，根据式(2.14)和(2.15)可以得到观测误
差的协方差
ˆ ˆ( ) ( )i i i i i i i i i
T T T T
ij ij ij ij ij ij ij ij ijC H Cov x x H G Cov y y G H PH G SG (2.16)
通过计算 Mahalanobis 距离
2
iijD
2 1
i i i i
T
ij ij ij ijD h C h
(2.17)
27. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 13 页
置为GATE_REJECT=4.0，GATE_AUGMENT=16.0
最近邻居（Nearest Neighbor，NN）是最简单的数据关联准则。其选择的路标对
满足以下两个标准：第一，路标对满足自身兼容性测试；第二，所选路标对具有最小
的 Mahalanobis 距离。在 SLAM 领域中应用最为广泛的自身兼容最近邻居算法（如算
法 2.1）就是基于上述 NN 关联准则而产生的。该算法主要用于环境路标间距离相距
较远的情况，其运算效率特别高，运算复杂度为 ( )O mn 。
算法2.1 Individual Compatibility Nearest Neighbor ( ICNN )
2
min 1
2
2 2 2
min
1 { }
2( , )
0
0
2 { }
2( , )
( GATE_REJECT) & ( ) {withint the gate, store nearest neighbor}
i
i
j
ij i j
ij ij
for i to m do measurement E
D mahalonobis E F
nearest
outer
for j to n do measurement F
D mahalanobis E F
if D D D then
nearest j
2 2
min
2
2
0
GATE_AUGMENT { too far to associate, but far enough to be a new feature}
0
ij
ij
ij
i
i
i
i
D D
else if outer D
outer D
end if
end for
if nearest
H nearest
else if outer E
H
end if
end for
return H
在实际应用中，由于自身兼容最近邻居数据关联算法并没有对环境路标的整体信
息进行统筹考虑，因此其只能保证在局部匹配中的正确性，如图 2.4所示。其中，（a）、
（b）和（c）分别表示三种典型情况下 ICNN 数据关联的结果，箭头起点处的十字表
28. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 14 页
示环境路标的位置，箭头终点处的十字表示地图路标的位置，箭头表示数据关联的方
向（由环境路标指向地图路标），大的椭圆表示进行数据关联的范围，其值由
GATE_REJECT 决定。当环境路标处于数据关联范围之内（即在大的椭圆之内）时，
就建立环境路标与地图路标的匹配对。不难看出，当两个环境路标同时距离同一个地
图路标很近的时候，即两个环境路标与该地图路标距离之和小于椭圆长半轴时就会出
现误匹配的错误。因此，ICNN 虽然算法简单、运算效率高，但在环境路标点非常密
集的情况下会出现误匹配的现象。另一方面，当机器人位置具有很大的不确定性时，
ICNN 判别数据关联的方法也不可靠。总之，只有在环境路标比较稀疏的结构化环境
中，简单阈值判别方法才可行。
（a） （b） （c）
图2.4 ICNN 数据关联结果
Fig. 2.4 Results of ICNN data association
2.2.2 枝和范围的联合兼容方法
与自身兼容性测试类似，联合兼容方法（Joint Compatibility，JC）所建立的假设
集合 1{ , , }i iH j j 应满足隐式关系 ( , ) 0iHf x y 。在点 ˆ ˆ( , )x y 处进行线性化，可以得到
一阶 Taylor 展开式
1
1
2
1
2
( , )
( , ) ( , )
ˆ ˆ( , ) ( ) ( )
( , )
( , )
Hi
i i i i
i
i
j
j
H H H H
ij
ij
f x y
f x y f x y
f x y h H x x G y y
f x y
f x y
(2.19)
29. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 15 页
1
1 2
1
2
ˆ ˆ( , )
i
i i
i
i
j
H j
H H
ij
ij
h
h h
h f x y
h
h
(2.20)
1 1
1 12 2
1 1
2 2
ˆ ˆ ˆ ˆ( , ) ( , )
; ;
i ii i
i i
i i
i i
j j
H Hj jH H
H H
ij ijx y x y
ij ij
H G
H GH Gf f
H G
H Gx y
H G
(2.21)
iHh 表示环境路标 (1 )mE m i 与地图路标 jF 数据关联后的线性误差向量。 iHH 和
iHG 分别表示在点 ˆ ˆ( , )x y 处 x 和 y 方向上的 Jacobian 式。与 ICNN 类似，根据估计误差
的协方差 P 和观测误差的协方差 S 可以计算在假设集 iH 下配对误差协方差 iHC
i i i i i
T T
H H H H HC H PH G SG (2.22)
通过计算基于假设集合 iH 的 Mahalanobis 距离
2
iHD
2 1 2
,i i i i
T
H H H H dD h C h
(2.23)
得到环境路标 iE 和地图路标 jF 的联合兼容性测设
2 2
,1true
false
iH d
i
D
H
else
(2.24)
其中， 是期望的置信度， d 表示问题的规模 dim( )iHd f 。
如前所述，数据关联问题可以抽象为树的搜索问题，而枝和范围（Branch and
Bound，BB）就是一种基于树的搜索方法。枝和范围的搜索实际上就是通过合理的纵
向和横向搜索实现树的遍历。BB 数据关联准则就是实现最多的路标匹配对。将联合
兼容性测试与枝和范围搜索方法结合，就得到了枝和范围联合兼容数据关联算法
（Joint Compatibility Branch and Bound，JCBB），如算法 2.2所示。
算法2.2 Joint Compatibility Branch and Bound ( JCBB )
[ ]
JCBB([ ],1) {recursive function}
Best
return Best
30. 上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 16 页
JCBB( , ) { }
( ) ( ) { ?}
1
individual_compatibility( , ) & joint_compatibility( , , ) {( , ) accept
i
i j
H i find pairings for observation E
if i m then
if pairings H pairings Best then the last landmark
Best H
end if
else
for j to n do
if i j H i j then E F
1
ed}
JCBB([ ], 1) {recursive recall function JCBB towards }
JCBB([ 0], 1) {no features can paired with }
i
i
H j i E
end if
end for
H i E
end if
由于 JCBB 数据关联准则是获得最多的路标匹配对，因此其匹配正确率为百分之
百，如图 2.5所示。其中，（a）、（b）和（c）分别表示三种典型情况下 JCBB 数据关
联的结果，箭头起点的十字为环境路标的位置，箭头终点的十字为地图路标的位置，
箭头表示数据关联的方向（由环境路标指向地图路标），椭圆表示进行数据关联的范
围。当环境路标处于地图路标的数据关联范围之内时就建立上述两个路标的的匹配。
（a） （b） （c）
图2.5 JCBB 数据关联结果
Fig. 2.5 Results of JCBB data association
34. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 20 页
3 第三章 扩展信息滤波 SLAM 仿真
EQUATION SECTION 3
3.1 机器人运动模型
本论文中所讨论的机器人都是陆地环境中的轮式机器人，具体来说，下面将分别
介绍二自由度机器人、二轮三自由度机器人以及四轮三自由度机器人的运动模型。
a. 二自由度机器人运动模型
二自由度机器人的运动环境为平面环境。其运动模型为
( )( 1)
( )( 1)
v xv
v yv
x k V dtx k
f
y k V dty k
(3.1)
其中，[ ( 1) ( 1)]T
v vx k y k 和[ ( ) ( )]T
v vx k y k 分别表示机器人在 1k 时刻和 k 时刻
所在位置的坐标，而 xV 与 yV 分别表示机器人沿平面正交坐标轴 x 和 y 方向的速度，dt
是运动控制周期， 是均值为 0，方差为 2
的高斯白噪声。不难看出，上述机器人运
动模型的 Jacobian 矩阵为
1 0
0 1v
f
F
x
(3.2)
b. 二轮、三自由度机器人运动模型
二轮机器人是实验用机器人中最常见的类型，其驱动方式为双轮差速。考虑到机
器人在运动过程中的姿态，其运动模型可以表示为
( 1) ( ) cos( ( ))
( 1) ( ) sin( ( ))
( 1) ( )
v v v
v v v
v v
x k x k Vdt k
f y k y k Vdt k
k k wdt
(3.3)
其中，[ ( 1) ( 1) ( 1)]T
v v vx k y k k 表示机器人在 1k 时刻的位置和姿态，而
[ ( ) ( ) ( )]T
v v vx k y k k 表示机器人在 k 时刻的位置和姿态，V 和 w 分别表示机器人在时
35. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 21 页
刻 k 时的线速度和角速度， dt 是控制周期， 是均值为 0，方差为 2
的高斯白噪声。
简单计算后，得到上述机器人的 Jacobian 矩阵为
1 0 sin( ( ))
0 1 cos( ( ))
0 0 1
v
v
v
Vdt k
f
F Vdt k
x
(3.4)
c. 四轮、三自由度机器人运动模型
室外机器人最常见机器人就是有四轮汽车改装的四轮机器人。通常改装的车辆是
后轮驱动，通过改变前轮的方向角来改变运动方向。其运动模型可以表示为
( 1) ( ) cos( ( ))
( 1) ( ) sin( ( ))
( 1) sin( )
( )
v v v
v v v
v
v
x k x k Vdt G k
f y k y k Vdt G k
k Vdt G
k
WB
(3.5)
其中，[ ( 1) ( 1) ( 1)]T
v v vx k y k k 表示机器人在 1k 时刻的位置和姿态，而
[ ( ) ( ) ( )]T
v v vx k y k k 表示机器人在 k 时刻的位置和姿态，V 是在 k 时刻后轮的前进速
度，G 是在 k 时刻前轮方向变化的角速度，WB 是后轮之间的距离，dt 是运动控制周
期， 是均值为 0，方差为 2
的高斯白噪声。上述机器人运动模型的 Jacobian 矩阵为
1 0 sin( ( ))
0 1 cos( ( ))
0 0 1
v
v
v
Vdt G k
f
F Vdt G k
x
(3.6)
3.2 传感器观测模型
本文所用的传感器主要包括两类：一类是全局坐标类传感器，另一类是距离方向
类传感器，下面分别给出上述两类传感器的观测模型和 Jacobian 矩阵。
a. 全局坐标类传感器观测模型
全局坐标类传感器属于全视角的传感器，在任意时刻它都可以观察周围 360°范
围內的环境，其观察值为路标相对于目前机器人所在坐标系位置的偏差。在本文中，
这种传感器只在仿真中与二自由度机器人配合使用。具体来说，其观测模型为
36. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 22 页
v xi
v yi
x zx
g
y zy
(3.7)
其中，[ ]T
i ix y 是路标在全局坐标系中的位置向量，[ ]T
v vx y 为机器人在全局坐标
系中的位置向量，[ ]T
x yz z 是传感器观测值， 是传感器观测误差，它是均值为 0，
方差为 2
的高斯白噪声。上述传感器观测模型的 Jabobian 矩阵为
1 0
0 1v
g
G
x
(3.8)
在信息滤波 SLAM 算法观测更新步骤中，需要用到观测模型(3.7)的对称形式
x i v
y i v
z x x
h
z y y
(3.9)
其相应的 Jacobian 矩阵为
1 0 1 0
0 1 0 1
h
H
(3.10)
b. 距离方向类传感器观测模型
距离方向类传感器是最常见的传感器类型，它采用极坐标系，将观测值用距离和
方向角两个参数来表示。目前机器人领域中最常使用的摄像头和激光雷达都属于距离
方向类传感器，其观测模型可以表示为
cos( )
sin( )
i v v
i v v
x x r
g
y y r
(3.11)
其中，[ ]T
i ix y 是目标所在位置的全局坐标，而[ ]T
v v vx y 是机器人在当前时刻
的位置和姿态，[ ]T
r 是传感器的测量值，分别是目标距离传感器的距离和方向，
是均值为 0，方差为 2
的高斯白噪声。观测模型(3.11)的 Jacobian 矩阵为
1 0 sin( )
0 1 cos( )
v
vv
rg
G
rx
(3.12)
如前所述，在本文提出的 SLAM 算法中还会用到观测模型(3.11)的对称形式
2 2
( ( )) ( ( ))
( )
atan 2
( )
i v i v
i v
v
i v
x x k y y k
r
h y y k
x x k
(3.13)
37. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 23 页
该形式是将机器人的位置和姿态以及观测目标的全局坐标作为已知，反过来求解
传感器的观测值，其 Jacobian 矩阵为
2 2 2 2
( ) ( ) ( ) ( )
0
( ) ( ) ( ) ( )
1
i v i v i v i v
i v i v i v i v
x x k y y k x x k y y k
h r r r r
H
y y k x x k y y k x x k
r r r r
(3.14)
3.3 仿真参数设定
仿真需要设定的参数包括：机器人运动模型的误差和协方差、观测模型的误差和
协方差、机器人本体物理参数等。这些参数的设置需要根据具体情况合理的设置。本
节讨论的参数主要指算法中线性误差输入量，即移动、增加路标和更新中的线性误差
量。
a. 移动
对于不同的机器人运动模型，线性化误差输入量u 的取值也不同：
两轮两自由度机器人
1
1 0
( , )
0 1t t
x x x x
x t x
y y y y
V dt V
u f u F
V dt V
(3.15)
四轮三自由度机器人
1( , )
cos( ) 1 0 sin( )
sin( ) 0 1 cos( )
sin( ) 0 0 1
cos( ) sin( )
sin( ) cos( )
sin( )
t tx t x
x x
y y
u f u F
Vdt G Vdt G
Vdt G Vdt G
Vdt G
WB
Vdt G G
Vdt G G
Vdt G
WB
(3.16)
38. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 24 页
两轮三自由度机器人
1( , )
cos( ) 1 0 sin( )
sin( ) 0 1 cos( )
0 0 1
cos( ) sin( )
sin( ) cos( )
t tx t x
x x
y y
u f u F
Vdt Vdt
Vdt Vdt
wdt
Vdt
Vdt
wdt
(3.17)
b. 增加路标
对于不同的传感器观测模型，线性化误差输入量u 的取值也不同：
全局坐标类传感器
1 0
( , )
0 1t t
x x x x
x t x
y y y y
z z
u g z G
z z
(3.18)
距离和方向类传感器
( , )
cos( ) 1 0 sin( ) cos( ) sin( )
sin( ) 0 1 cos( ) sin( ) cos( )
t tx t x
x
x
y
y
u g z G
r r
r
r r
(3.19)
c. 更新
对于不同的传感器观测模型，线性化误差输入量u 的取值也不同：
全局坐标类传感器
( )
1 0 1 0
0 1 0 1
i
ii
i
t
x
y
fx xx x
fxy yfy y
fy
u z h H
z z
z z
(3.20)
39. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 25 页
距离和方向类传感器
2 2
2 2 2 2
( )
( ) ( )
atan 2
0
1
i i
i
i
i i i i
i i i i
i
i
t
fx x fy y
fy y
fx x
x
y
fx x fy y fx x fy y
fy y fx x fy y fx x
fx
fy
u z h H
r
r r r r
r r r r
(3.21)
3.4 仿真结果分析
3.4.1 一维、一自由度（1-D, 1-DOF）情况
一维、一自由度（1-D, 1-DOF）虽然是 EIF-SLAM 应用中最简单的情况，但也清
楚的说明了 EIF-SLAM 的基本思想和方法。在仿真中，机器人从左向右在一维横轴
上运动的同时观察环境中的路标，并实时的建立环境地图，仿真结果如图 3.1（a）所
示。其中，（a）中实心三角表示当前机器人的位置，实心圆点表示环境中的路标，叉
号表示机器人对环境路标的估计位置，圆圈所圈定的路标表示机器人当前所观察到的
路标。仿真中设置的传感器观测范围为[ 2 2] ，因此每一时刻传感器都会观察到两个
不同的路标。从 SLAM 估计的结果上看，EIF-SLAM 在一维估计上的精度还是很高
的。
归一化后的信息矩阵如图 3.1（b）所示。所谓归一化就是指将最小和最大的元素
分别置为 0 和 1，其它元素按照线性映射到[0, 1]中的某一正数。归一化信息矩阵中
的元素表示机器人与路标或路标与路标之间的连接强度，其主对角线上的元素是机器
人或路标关于自身的连接强度，因此其连接强度最强。
42. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 28 页
似机器人的运动曲线。因此，需要设置一个接近阈值来确认是否已经到达目标点，如
果机器人与目标点的距离小于该参数就认为机器人已经到达路标点，机器人从此刻起
向下一个路标点运动。值得注意的是，如果接近阈值设置过大，机器人运动的轨迹与
理想轨迹的偏差就会很大，反之，机器人很难进入以路径点为中心的领域之内，从而
引起机器人运动振荡。
（2） 对于信息矩阵的处理，一维仿真用到了归一化信息矩阵，二维仿真则采
用了规范化信息矩阵（normalized information matrix）。规范化包括以下三个基本步骤：
步骤 1 根据协方差矩阵C 得到相关系数矩阵 R
( , )
( , )
( , ) ( , )
C i j
R i j
C i i C j j
(3.22)
步骤 2 由于连接强度与相关系数矩阵中相应 2 2 子矩阵的行列式成正比，因此
对于二自由度机器人，可以利用式(3.23)得到连接强度矩阵 I 。
(1,1) (1,2) (1,3) (1,4) (1, 1) (1, )
(2,1) (2,2) (2,3) (2,4) (2, 1) (2, )
(3,1) (3,2) (3,3) (3,4) (3, 1) (3, )
(4,1) (4,2) (4,3) (4,4) (4, 1) (4, )
(1,1) (1,2)
(2,1) (
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R
R R
(1,3) (1,4) (1, 1) (1, )
2,2) (2,3) (2,4) (2, 1) (2, )
(3,1) (3,2) (3,3) (3,4) (3, 1) (3, )
(4,1) (4,2) (4,3) (4,4) (4, 1) (4, )
R R R n R n
R R R n R n
R R R R R n R n
I
R R R R R n R n
(3.23)
步骤 3 对连接强度矩阵 I 进行归一化处理，具体处理方法如 3.4.1 中所述。
经过上述的规范化处理的信息矩阵是 Bayes 网络节点连接强度的直观体现。特别
地，在一维仿真中，归一化信息矩阵就是规范化信息矩阵。在二维仿真中，规范化信
息矩阵同样满足一维中信息矩阵的四个基本特征，如图 3.2（b）所示。另外，还可以
得到如下的仿真结果：
（1） 如果将规范化信息矩阵中元素的值在区间[0,1]上进行统计，就得到了规
范化信息矩阵元素分布统计图，如图 3.3所示。由此可以看出，连接强度趋于 0 的元
素有很多，即信息矩阵所代表的 Bayes 网络中的许多连接都属于弱连接关系。
43. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 29 页
-0.2 0 0.2 0.4 0.6 0.8 1 1.2
0
50
100
150
200
250
300
350
Range of distribution
Statisticresult
图3.3 规范化信息矩阵元素分布统计直方图
Fig. 3.3 Statistic histogram of elements distribution in normalized information matrix
（2） 当机器人观察到路标时，机器人与路标的连接强度最大，而当机器人不
能观察到路标后，机器人与路标的连接强度会随着时间的推移而逐渐减小，如图 3.4
所示。
0 100 200 300 400 500 600
10
-80
10
-60
10
-40
10
-20
10
0
Steps
Linkstrength
landmark 2 landmark 3 landmark 6
图3.4 机器人与路标连接强度
Fig. 3.4 Link strength between robot & features
44. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 30 页
其中，横轴表示时间，纵轴表示连接强度。图中的三条曲线分别反映了机器人与
2 号、3 号、6 号路标连接强度的变化情况，三条曲线的变化规律基本一致。不失一
般性，取机器人与 3 号路标的连接变化进行说明。由于仿真中设定机器人逆时针运动
三周，因此在每次重新看到 3 号路标的时候，机器人与 3 号路标的连接强度都会突然
增加（几乎是跳变），而当机器人逐渐远离 3 号路标后，连接强度逐渐减小（近似为
线性）。
（3） 由于仿真中的路标较少，因此对算法效率的测试只能反映出算法在路标
数目不太多情况下的时间复杂度，如图 3.5所示。横轴表示状态向量的维数，纵轴表
示计算时间，星号表示移动步骤的时间，叉号表示更新步骤的时间。不难看出，在环
境规模较小的情况下，扩展信息滤波 SLAM 算法的运算效率较高。
0 5 10 15 20 25 30 35 40
0
0.002
0.004
0.006
0.008
0.01
0.012
0.014
0.016
Current size of state vector
Computationtime（s）
Projection time Update time
图3.5 EIF-SLAM 算法计算时间统计分析
Fig. 3.5 Statistic analysis of EIF-SLAM algorithm on computation time
（4） SLAM 的估计估计效果可以从有效性和精确性两个方面进行考察，具体
来说，可以从误差和协方差两个指标进行衡量。当误差绝对值小于协方差绝对值时，
得到的 SLAM 估计结果是一致的，即该估计结果有效；误差和协方差越小，SLAM
估计结果越准确。机器人在二维环境下的 SLAM 估计误差和协方差如图 3.6所示。其
中，横轴表示算法的步数，纵轴表示误差或协方差的大小，虚线和实现分别表示机器
人的误差和协方差曲线。不难看出，机器人的误差绝对值始终小于协方差绝对值，因
45. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 31 页
此，该 SLAM 估计结果具有一致性。值得注意的是，误差和协方差在大约 200 步和
400 步时突然减小，这是由于环闭合（或重定位）所引起的，即在此时机器人刚好运
动一周，又重新观察到了地图中的路标。
0 100 200 300 400 500 600
-5
0
5
x
0 100 200 300 400 500 600
-5
0
5
y
Steps
Error Covariance＋ Covariance－
图3.6 2-D, 2-DOF SLAM 估计误差和协方差
Fig. 3.6 Error and covariance of estimation in 2-D, 2DOF case
0 100 200 300 400 500 600
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
Steps
Maperror
图3.7 地图误差
Fig. 3.7 Map error
46. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 32 页
（5） 如果将地图中所有路标的误差绝对值求和，得到地图误差 ME
| |M i
i M
E e
(3.24)
其中，M 是地图路标集合， ie 是第i 个路标的误差。重定位前，随着地图中环境
路标的增加，地图误差 ME 增加迅速，而在大约 200 步时，误差突然减小，如图 3.7
所示。
上述现象是由于环闭合所引起的，这与（4）中分析一致。值得说明的是，对于
SLAM 估计效果来说，第一次环闭合的效果最为明显，而以后的环闭合（如第二次、
第三次环闭合等）对 SLAM 估计结果的影响会越来越小。
3.4.3 二维、三自由度（2-D, 3-DOF）情况
二维、三自由度仿真包括两轮机器人和四轮机器人两种情况。两轮和四轮机器人
的差异主要体现在运动模型的不同。因此，不失一般性，本节主要研究两轮机器人
SLAM 仿真问题。
相对于二自由度的情况，三自由度仿真需要同时估计机器人位置和姿态，如图
3.8所示。SLAM 估计结果的准确性对于姿态很敏感，在姿态上微小的偏差都会导致
最终 SLAM 结果很大的误差。因此，从硬件的角度，应该对驱动和运动装置进行校
正，尽量消除运动系统误差；从算法的角度，要设计合理的标定实验，根据数据统计
的结果合理估计机器人运动时位置和姿态的不确定性。
-100 -50 0 50 100
-100
-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（m）
（a）二自由度
47. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 33 页
-100 -50 0 50 100
-100
-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（m）
（b）三自由度
图3.8 二自由度和三自由度机器人运动轨迹对比
Fig. 3.8 Comparison of motion trajectory between 2dof & 3dof
对于二维、三自由度的仿真，可以通过 SLAM 仿真环境图形用户界面（Graphical
user interface，GUI）设置和修改路标（landmark）和期望路径点（waypoint）的位置，
如图 3.9所示。
图3.9 SLAM 仿真环境图形用户界面
Fig. 3.9 Graphical user interface of SLAM simulation environment
48. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 34 页
具体来说，对路标和路径点的操作包括添加、移动和删除。假设设置后的仿真环
境中有 p 个路标点和 q 个路径点，则 GUI 会将仿真环境保存为具有下面数据结构的
“.mat”文件。
1, 2, , 1, 2, ,
1, 2, , 1, 2, ,
,
x x p x x x q x
y y p y y y q y
L L L W W W
lm wp
L L L W W W
(3.25)
其中，lm 和 wp 分别表示路标点和路径点集合， , ,[ ] (1 )T
i x i yL L i p 为第i 个路
标点的全局坐标，而 , ,[ ] (1 )T
j x j yW W j q 为第 j 个路径点的全局坐标。
不失一般性，下面分析二轮、二维、三自由度情况下 SLAM 环闭合问题。与3.4.2
节中环闭合的结果相似，机器人在第一圈随着移动距离的不断增加，对自身和环境路
标位置的估计协方差就会越来越大；而当机器人在第二圈，观察到地图中最早建立的
路标时，机器人对自身和环境路标位置的估计协方差就会突然减小，如图 3.10所示。
其中，“星号”表示路标的真实位置，“椭圆”表示路标或机器人位置的 3 不确定性，
椭圆中心的“十字”表示机器人和路标的估计位置。上述环闭合现象的产生是由扩展
信息滤波 SLAM 算法的更新步骤所引起的。直观上看，则是因为算法校正了误差最
大的（环境路标，地图路标）对，即（第二圈第一个环境路标，第一圈第一个地图路
标）对所引起的。
需要说明的是，对于三自由度情况下的扩展信息滤波 SLAM 算法，在对信息矩
阵进行规范化时，需要对3.4.2节中的规范化步骤 2 作如式(3.26)的修改。由此计算得
到的规范化信息矩阵只反映机器人位置与各路标位置之间的连接关系。
(1,1) (1,2) (1,3) (1,4) (1, 1) (1, )
(2,1) (2,2) (2,3) (2,4) (2, 1) (2, )
(3,1) (3,2) (3,3) (3,4) (3, 1) (3, )
(4,1) (4,2) (4,3) (4,4) (4, 1) (4, )
(5,1) (5,2) (5,3) (5,4) (5, 1) (5, )
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R R R R n R n
(1,1) (1,2) (1,3) (1,4) (1, 1) (1, )
(2,1) (2,2) (2,3) (2,4) (2, 1) (2, )
(4,1) (4,2) (4,3) (4,4) (4, 1) (4, )
(5,1) (5,2) (5,3) (5,4) (5, 1) (5, )
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R R R R n R n
I
(3.26)
49. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 35 页
-100 -50 0 50 100
-80
-60
-40
-20
0
20
40
60
80
X（ m）
Y（m）
（a）环闭合前
-100 -50 0 50 100
-80
-60
-40
-20
0
20
40
60
80
X（ m）
Y（m）
（b）环闭合后
图3.10 环闭合前后 2WB, 2-D, 3-DOF SLAM 估计结果
Fig. 3.10 Estimation results of 2WB, 2-D, 3-DOF SLAM before and after loop closure
50. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 36 页
与3.4.2节讨论类似，机器人在 x 、y 和 方向上的误差和协方差验证了 SLAM 算
法的估计结果具有一致性。另外，950 步误差和协方差的突然减小是环闭合现象所导
致的。
0 200 400 600 800 1000 1200 1400 1600 1800
-30
-20
-10
0
10
20
30
Steps
X
Error
Covariance
（a） x 方向
0 200 400 600 800 1000 1200 1400 1600 1800
-40
-30
-20
-10
0
10
20
30
40
Steps
Y
Error
Covariance
（b） y 方向
51. 上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 37 页
0 200 400 600 800 1000 1200 1400 1600 1800
-0.25
-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2
0.25
Steps
Error
Covariance
（c） 方向
图3.11 2WB, 2-D, 3-DOF SLAM 估计误差和协方差
Fig. 3.11 Error and covariance of estimation in 2WB, 2-D, 3-DOF case
53. 上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 39 页
初始时，有四个路标与机器人起始状态 tx 直接连接，分别为 1m 、 2m 、 3m 和 4m ，
间接连接的路标为 5m ，此时的信息矩阵是稀疏的，如图 4.1（a）所示。当机器人移
动后，由于只增加了新状态 1tx 与 tx 的连接，因此信息矩阵的稀疏性并没有受到影响，
如图 4.1（b）所示。然而，当消去中间状态 tx 时，会产生一次全连接。参与全连接的
节点为 1tx 以及曾经与中间状态 tx 直接连接的节点。在图 4.1（c）中，产生的了由节
点 1tx 、 1m 、 2m 、 3m 和 4m 构成的全连接。值得注意的是，在产生全连接的同时，这
些连接的强度也被弱化了。
从定量的角度，可以从移动过程的具体公式进行分析。从式(4.1)中 1(2,2)t 中看
出，每移动一步，信息矩阵中 MM 的值都会减少，即 Bayes 网络中连接强度减小了。
这就是信息矩阵几乎稀疏的主要原因。
1 1
1 1 1 1
t
t t t
x M
t T
Mx MM Mx x M
Q F
F Q
(4.1)
其中，
1
1
1
1
1( , )
t t
t t
t t
T
x x
T
x x
T
x t x
Q F F
F Q F
u F Q f u F
综上所述，移动过程会增加信息矩阵的连接数量，但同时会减弱连接的强度。换
句话说，从连接数量增加的角度看，移动过程是信息矩阵稠密化过程；但从连接强度
的角度看，移动过程使得信息矩阵稀疏化。
b. 增加路标
增加路标类似于移动过程中增加状态的过程，不同的是增加的状态是地图中新的
路标，并且该状态也不会被消去。图 4.2表明增加路标会使得信息矩阵稀疏化。增加
路标的公式(4.2)中， 1 1(2,3) (3,2) 0t t 定量的说明了稀疏化的过程。
1 1
1
1 1
0
0
t t t
t
T T
x x x M
t Mx MM
G R G G R
R G R
(4.2)
综上所述，随着增加路标过程的进行，信息矩阵 稀疏化的程度不断加强。
54. 上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 40 页
图4.2 增加路标中 Bayes 网络与信息矩阵演化过程
Fig. 4.2 Evolution of Bayes network and information matrix in adding features
c. 更新
更新过程增加了与节点 tx 直接关联连接的强度，如图 4.3所示。在更新过程后，
连接 1tx m 、 2tx m 、 3tx m 和 4tx m 的强度增强了。由于只有局部连接被加强，因此并不会
对信息矩阵的稀疏性产生大的影响。
图4.3 更新中 Bayes 网络与信息矩阵演化过程
Fig. 4.3 Evolution of Bayes network and information matrix in update
更新公式(4.3)中， H 是稀疏矩阵，因此 1T
H R H
中只有与机器人位置姿态
[ ]T
v v vx y 和待更新路标[ ]i i
T
f fx y 相关的位置不为零。更进一步，更新过程只加强
了机器人自身和待更新路标之间的连接强度。
1
1
T
t t H R H
(4.3)
其中，
i iv v v f f
h h h h h h
H
x y x y
tx tx
1m
2m
3m
4m
5m
1m
2m
3m
4m
5m
tx 1m 2m 3m 4m
tx
1m
2m
3m
4m
tx 1m 2m 3m 4m 5m
tx
1m
2m
3m
4m
5m
tx
1m
2m
3m
4m
5m
tx 1m 2m 3m 4m 5m
tx
1m
2m
3m
4m
5m
tx 1m 2m 3m 4m 5m
tx
1m
2m
3m
4m
5m
56. 上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 42 页
20
40
60
80
20
40
60
80
-2
0
2
4
row
column
value
（b）去掉主对角线和副对角线端点领域后的局部图
图4.4 信息矩阵元素分布图
Fig. 4.4 Element distribution of information matrix
表 4.1与表 4.2分别从 EIF 相关公式和信息矩阵的几何意义（即 Bayes 网络）两个
角度，具体分析信息矩阵特征形成的原因。
表4.1 信息矩阵结构特征分析（EIF 公式）
Tab. 4.1 Structure analysis of information matrix（EIF formula）
特征 分析
1 信息矩阵是正定对称的 Hermit 矩阵[27, 41, 47]
。
2 式(4.1)中 1(2,2)t 项，每移动一步，信息矩阵中 MM 中的元素值都会减小。
3
更新公式(4.3)中， H 是稀疏矩阵，因此 1T
H R H
中只有与机器人位置姿态
[ ]T
v v vx y 和待更新路标[ ]i i
T
f fx y 相关的位置不为零。更进一步，更新过程加强
了表示待更新路标位置元素的值。
57. 上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 43 页
表4.2 信息矩阵结构特征分析（Bayes 网络）
Tab. 4.2 Structure analysis of information matrix（Bayes net）
特征 分析
1 以主对角线对称的两个元素表示相同的两个节点的连接强弱。
2 两个节点间最短路越长，其在信息矩阵中所代表的元素离主对角线越远。
3
副对角线端点表示机器人状态 tx 和目前正与其直接连接的路标的连接强度，因此，
该元素值较大。
规范化信息矩阵通过从信息矩阵提取机器人和路标间的连接信息，建立机器人和
各路标的 Bayes 网络。规范化信息矩阵具有明显的几何意义，因而对其几何特征及意
义等的研究也较充分。下面通过信息矩阵规范化的步骤检验信息矩阵与规范化信息矩
阵的结构特征具有统一性，从而验证本节提出的信息矩阵的结构特征。信息矩阵规范
化需要进行如下三个基本步骤：
步骤 1 根据协方差矩阵C 得到相关系数矩阵 R 。
( , )
( , )
( , ) ( , )
C i j
R i j
C i i C j j
(4.4)
其中， ( , )C i j 和 ( , )R i j 分别表示协方差矩阵C 和相关系数矩阵 R 中第i 行、第 j 列
的元素。
步骤 2 由相关系数矩阵 R 计算连接强度矩阵 I 。由于连接强度与相关系数矩阵
中相应 2 2 子矩阵的行列式成正比，因此不失一般性，对于二自由度机器人，连接强
度矩阵 I 可以计算如下。
(1,1) (1,2) (1,3) (1,4) (1, 1) (1, )
(2,1) (2,2) (2,3) (2,4) (2, 1) (2, )
(3,1) (3,2) (3,3) (3,4) (3, 1) (3, )
(4,1) (4,2) (4,3) (4,4) (4, 1) (4, )
(1,1) (1,2)
(2,1) (
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R R R R n R n
R R
R R
(1,3) (1,4) (1, 1) (1, )
2,2) (2,3) (2,4) (2, 1) (2, )
(3,1) (3,2) (3,3) (3,4) (3, 1) (3, )
(4,1) (4,2) (4,3) (4,4) (4, 1) (4, )
R R R n R n
R R R n R n
R R R R R n R n
I
R R R R R n R n
(4.5)
