大范围环境下移动机器人
DESCRIPTION
TRANSCRIPT
申请上海交通大学硕士学位论文
大范围环境下移动机器人同步定位
和地图创建研究
国家自然科学基金（60475032）和新世纪优秀人才支持计划资助
系 别： 自动化系
专 业： 控制理论与控制工程
学 生： 董海巍
导 师： 李柠 副研究员
陈卫东 教 授
上海交通大学
2008 年 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
上海交通大学
学位论文原创性声明
本人郑重声明：所呈交的学位论文，是本人在导师的指导下，独立进行研究工作所取得
的成果。除文中已经注明引用的内容外，本论文不包含任何其他个人或集体已经发表或撰写
过的作品成果。对本文的研究做出重要贡献的个人和集体，均已在文中以明确方式标明。本
人完全意识到本声明的法律结果由本人承担。
学位论文作者签名：
日期： 年 月 日
上海交通大学
学位论文版权使用授权书
本学位论文作者完全了解学校有关保留、使用学位论文的规定，同意学校保留并向国家
有关部门或机构送交论文的复印件和电子版，允许论文被查阅和借阅。本人授权上海交通大
学可以将本学位论文的全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或扫
描等复制手段保存和汇编本学位论文。
保密□，在 年解密后适用本授权书。
本学位论文属于
不保密□。
（请在以上方框内打“√”）
学位论文作者签名： 指导教师签名：
日期： 年 月 日 日期： 年 月 日
上海交通大学硕士学位论文 摘要
第 I 页
大范围环境下移动机器人同步定位和地图创建研究
摘 要
随着 SLAM 技术的不断发展，计算效率已经成为制约 SLAM 发展的主
要因素。目前，在 SLAM 领域中越来越多的学者从稀疏化信息矩阵的角度
提高 SLAM 算法的效率。在这些研究工作中，对于稀疏化信息矩阵问题，
大都从 Bayes 网络的拓扑结构入手，通过构造合理的拓扑结构来实现规
范化信息矩阵的稀疏化。而在具体算法中则根据网络拓扑结构特性对路
标进行分类处理，这样做虽然会使信息矩阵在未经任何处理的情况下自
然稀疏化，从而提高 SLAM 估计结果的精度，但由于算法中需要对路标进
行分类，因此算法的时间复杂度也会随之增加。本文提出的算法是在
Eustice 扩展信息滤波 SLAM 算法上的改进，通过对信息矩阵结构特征的
分析，使得改进后的算法不仅解决了 Thrun 算法中直接稀疏化信息矩阵
导致的非一致问题，而且实时性和实用性也大为提高了。另外，本文提
出的算法与上述通过改进 Bayes 网络拓扑结构得到的算法相比，估计结
果相当，因而具有很高的理论和实际意义。
针对 SLAM 领域中信息矩阵稀疏化的问题，本文从扩展信息滤波 SLAM
算法的几何意义和相关公式两个角度进行分析，不仅得到了规范化信息
矩阵具有稀疏性的结论，而且首次提出了扩展信息滤波 SLAM 算法中信息
矩阵的结构特征。根据信息矩阵的结构特征，本文提出了一种扩展信息
滤波 SLAM（Extended Information Filter SLAM，EIF-SLAM）的改进算
法：通过直接对信息矩阵进行合理地稀疏化，并结合稀疏矩阵高效的运
上海交通大学硕士学位论文 摘要
第 II 页
算规则，显著降低了原算法的计算复杂度；借助环闭合（或重定位）技
术，有效抑制了由于稀疏化所导致的误差，实现了在保证 SLAM 估计结果
一致准确前提下的高效性。最后，稀疏化与 SLAM求解精度的关系也从理
论上给予了证明。
仿真针对一个大规模环境下的 SLAM 问题，讨论了直接稀疏化信息矩
阵扩展信息滤波 SLAM算法的应用。在设定运动模型和观测模型参数之后，
通过仿真结果重点对信息矩阵结构特征、算法效率、环闭合以及误差和
协方差四个关键问题进行了分析。实验分别就室内具有摄像头的两轮机
器人和室外具有激光雷达的四轮机器人的情况进行了讨论。两个实验各
具特色：实验一在实验室内通过自主研制的 Frontier-Ⅱ机器人完成。实
验中将平面图片作为路标，通过尺度不变特征变换（SIFT）算法实现数
据关联；实验二采用 SLAM 领域中标准的停车场数据集，其实验环境为室
外停车场，机器人为具有激光雷达的四轮汽车。实验中采用圆柱形物体
作为路标，根据自身兼容最近邻居算法（ICNN）进行数据关联。仿真与
实验结果有力的验证了本文所提出的算法的有效性。
关键词：移动机器人，SLAM，扩展信息滤波，信息矩阵，稀疏化
上海交通大学硕士学位论文 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
上海交通大学硕士学位论文 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
上海交通大学硕士学位论文 目录
第 V 页
目 录
摘 要 .................................................................................................................................................................. I
ABSTRACT ................................................................................................................................................... III
插图、表格中文题注列表 ........................................................................................................................... VII
插图、表格英文题注列表 ............................................................................................................................. IX
第一章 引言 ..................................................................................................................................................... 1
1.1 背景 ........................................................................................................................................................ 1
1.2 国内外研究现状 .................................................................................................................................... 2
1.3 本文所做的工作及内容安排 ................................................................................................................ 3
第二章 扩展信息滤波 SLAM 解决方案 ........................................................................................................ 6
2.1 基本原理 ................................................................................................................................................ 6
2.2 数据关联 ................................................................................................................................................ 9
2.2.1 自身兼容最近邻居法 ................................................................................................................... 10
2.2.2 枝和范围的联合兼容方法 ........................................................................................................... 14
2.2.3 仿真 ............................................................................................................................................... 17
第三章 扩展信息滤波 SLAM 仿真 .............................................................................................................. 20
3.1 机器人运动模型 .................................................................................................................................. 20
3.2 传感器观测模型 .................................................................................................................................. 21
3.3 仿真参数设定 ...................................................................................................................................... 23
3.4 仿真结果分析 ...................................................................................................................................... 25
3.4.1 一维、一自由度（1-D, 1-DOF）情况 ....................................................................................... 25
3.4.2 二维、二自由度（2-D, 2-DOF）情况 ....................................................................................... 27
3.4.3 二维、三自由度（2-D, 3-DOF）情况 ....................................................................................... 32
第四章 信息矩阵稀疏化 ............................................................................................................................... 38
4.1 信息矩阵的稀疏性 .............................................................................................................................. 38
4.2 信息矩阵的结构特征 .......................................................................................................................... 41
4.3 稀疏矩阵运算具有高效性 .................................................................................................................. 45
4.4 稀疏化算法 .......................................................................................................................................... 47
4.5 误差分析 .............................................................................................................................................. 48
上海交通大学硕士学位论文 目录
第 VI 页
第五章 仿真与分析 ....................................................................................................................................... 51
5.1 仿真环境 .............................................................................................................................................. 51
5.2 模型和参数设定 .................................................................................................................................. 52
5.3 仿真结果及分析 .................................................................................................................................. 54
第六章 实验 ................................................................................................................................................... 60
6.1 室内走廊 .............................................................................................................................................. 60
6.2 室外停车场 .......................................................................................................................................... 63
第七章 程序设计 ........................................................................................................................................... 66
7.1 INTEL®
MATH KERNEL LIBRARY ............................................................................................................ 66
7.2 软件结构 .............................................................................................................................................. 68
第八章 全文总结 ........................................................................................................................................... 71
8.1 主要结论 .............................................................................................................................................. 71
8.2 研究展望 .............................................................................................................................................. 72
参 考 文 献 ................................................................................................................................................... 74
附录 1 MATLAB 文件说明 ........................................................................................................................... 77
附录 2 VISUAL C++程序说明 ...................................................................................................................... 78
致 谢 ........................................................................................................................................................... 79
攻读硕士学位期间录用的论文 ..................................................................................................................... 80
攻读硕士学位期间参与的项目 ..................................................................................................................... 80
上海交通大学硕士学位论文 目录
第 VII 页
插图、表格中文题注列表
图 2.1 协方差矩阵和信息矩阵稠密性对比 ................................................................................................... 7
图 2.2 扩展信息滤波原理图........................................................................................................................... 7
图 2.3 数据关联原理 .................................................................................................................................... 10
图 2.4 ICNN 数据关联结果 .......................................................................................................................... 14
图 2.5 JCBB 数据关联结果 .......................................................................................................................... 16
图 2.6 数据关联仿真环境............................................................................................................................. 17
图 2.7 ICNN 数据关联分析 .......................................................................................................................... 18
图 2.8 JCBB 数据关联分析 .......................................................................................................................... 19
图 3.1 1-D, 1-DOF 情况 EIF-SLAM 仿真结果 ............................................................................................ 26
图 3.2 2-D, 2-DOF 情况 EIF-SLAM 仿真图 ................................................................................................ 27
图 3.3 规范化信息矩阵元素分布统计直方图 ............................................................................................. 29
图 3.4 机器人与路标连接强度 ..................................................................................................................... 29
图 3.5 EIF-SLAM 算法计算时间统计分析 ................................................................................................. 30
图 3.6 2-D, 2-DOF SLAM 估计误差和协方差............................................................................................. 31
图 3.7 地图误差 ............................................................................................................................................ 31
图 3.8 二自由度和三自由度机器人运动轨迹对比 ..................................................................................... 33
图 3.9 SLAM 仿真环境图形用户界面 ......................................................................................................... 33
图 3.10 环闭合前后 2WB, 2-D, 3-DOF SLAM 估计结果 ........................................................................... 35
图 3.11 2WB, 2-D, 3-DOF SLAM 估计误差和协方差 ................................................................................ 37
图 4.1 移动中 BAYES 网络与信息矩阵演化过程 ........................................................................................ 38
图 4.2 增加路标中 BAYES 网络与信息矩阵演化过程 ................................................................................ 40
图 4.3 更新中 BAYES 网络与信息矩阵演化过程 ........................................................................................ 40
图 4.4 信息矩阵元素分布图......................................................................................................................... 42
图 4.5 信息矩阵稀疏化结果......................................................................................................................... 44
图 4.6 稀疏比与阈值的关系......................................................................................................................... 45
图 4.7 稠密方式和稀疏方式矩阵求逆效率比较图 ..................................................................................... 46
图 4.8 稀疏度与矩阵求逆效率的关系 ......................................................................................................... 47
图 5.1 仿真环境 ............................................................................................................................................ 51
图 5.2 信息矩阵元素分布对数统计直方图 ................................................................................................. 54
图 5.3 扩展信息滤波 SLAM 算法改进前后效率对比 ................................................................................ 55
图 5.4 扩展信息滤波-SLAM 重定位前后估计结果 ................................................................................... 57
图 5.5 直接稀疏化信息矩阵 EIF-SLAM 环闭合前后估计结果 ................................................................. 58
图 5.6 EIF-SLAM 与直接稀疏化信息矩阵 EIF-SLAM 误差和协方差对比 ............................................. 59
图 6.1 FRONTIER-Ⅱ自主移动机器人 ........................................................................................................... 60
图 6.2 走廊实验环境 .................................................................................................................................... 61
上海交通大学硕士学位论文 目录
第 VIII 页
图 6.3 走廊实验结果 .................................................................................................................................... 62
图 6.4 走廊实验机器人定位误差 ................................................................................................................. 63
图 6.5 停车场实验结果 ................................................................................................................................ 64
图 6.6 停车场实验机器人位置误差 ............................................................................................................. 65
图 7.1 矩阵存储规则对比............................................................................................................................. 69
图 7.2 VC++程序调用关系图....................................................................................................................... 70
表 2.1 2
,d 分布表 ........................................................................................................................................ 12
表 2.2 2
2, 分布表 ......................................................................................................................................... 12
表 3.1 规范化后的信息矩阵标记含义对照表 ............................................................................................. 26
表 4.1 信息矩阵结构特征分析（EIF 公式） .............................................................................................. 42
表 4.2 信息矩阵结构特征分析（BAYES 网络） ......................................................................................... 43
表 7.1 MKL 数据类型说明 ........................................................................................................................... 67
表 7.2 MKL 矩阵类型说明 ........................................................................................................................... 67
上海交通大学硕士学位论文 目录
第 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 DATA ASSOCIATION ........................................................................................................ 10
FIG. 2.4 RESULTS OF ICNN DATA ASSOCIATION ................................................................................................ 14
FIG. 2.5 RESULTS OF JCBB DATA ASSOCIATION ................................................................................................. 16
FIG. 2.6 SIMULATION ENVIRONMENT OF DATA ASSOCIATION ............................................................................. 17
FIG. 2.7 ANALYSIS OF ICNN DATA ASSOCIATION ............................................................................................... 18
FIG. 2.8 ANALYSIS OF JCBB DATA ASSOCIATION ............................................................................................... 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
上海交通大学硕士学位论文 目录
第 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
上海交通大学硕士学位论文 第一章 引言
第 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]
上海交通大学硕士学位论文 第一章 引言
第 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 算法的更新步骤效率特别高，与路标数量的二次方成正比。
上海交通大学硕士学位论文 第一章 引言
第 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]等。在这些研
上海交通大学硕士学位论文 第一章 引言
第 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 在数据关联鲁棒
性上的差异。
第三章总结了二自由度机器人、二轮三自由度机器人和四轮三自由度机器人的运
动模型，并分析了全局坐标类传感器和距离方向类传感器的观测模型。通过设置
上海交通大学硕士学位论文 第一章 引言
第 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 领域中全新的问题。
上海交通大学硕士学位论文 第二章 扩展信息滤波 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 ：路标的集合
tz ：路标观测值
tu ：控制向量
式(2.1)指出，在给定机器人控制量 tu 以及观测量 tz 下， 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 算法就是利用上述信息矩阵的结构特征来提高运算效率的。
上海交通大学硕士学位论文 第二章 扩展信息滤波 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 UpdateY N
图2.2 扩展信息滤波原理图
Fig. 2.2 Algorithm diagram of information filter SLAM
上海交通大学硕士学位论文 第二章 扩展信息滤波 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 TMx MM Mx x M M Mx x
p x M z u N
Q F uQ F
F Q F Q u
(2.4)
其中，
11
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 是高斯白噪声。增加路标后，信息矩阵和信息向量的改变如下。
上海交通大学硕士学位论文 第二章 扩展信息滤波 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 都能与地图路标ij
F 相对应（如果是 iz 不与任何地图路标相对应，
则 0ij ）。传感器观测集合与地图路标的数据关联问题可以通过一个树形结构进行说
上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 10 页
明，如图 2.3所示。
图2.3 数据关联原理
Fig. 2.3 Principle of data association
其中，对于第 i 层，该层的所有节点都可能是与环境路标 iE 关联的地图路标，星
号表示不存在任何地图路标与环境路标 iE 相关联。因此，对于地图路标为 1{ , , }nF F
的情况，则每个节点都有 1n 个分枝。数据关联算法就是通过具体的树的搜索算法，
从 ( 1)mn 种数据关联可能中，迅速找出合适的路标匹配对。目前，在 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
xx
x
(2.9)
上海交通大学硕士学位论文 第二章 扩展信息滤波 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 PP
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 的观测值和对应的地图路标ij
F 满足下面的观测方程[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 fh 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)
上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 12 页
将 iE 和 jiF 进行自身兼容性测试（Individual Compatibility Test，IC_Test）
2
,1if
0 else
iij d
i
j DH
(2.18)
其中，2
,d 为参考阈值， dim( )iijd f ， 为期望的置信度。
2
,d 的分布满足 2 分
布，如表 2.1所示。通过查表，就可以得到不同自由度和置信度的数据关联结果。
表2.1 2
,d 分布表
Tab. 2.1 2
,d distribution
2
,d d
1 2 3 4 5 6
50％ 0.45 1.39 2.37 3.35 4.35 5.43
80% 1.64 3.21 4.64 5.99 7.29 8.56
90% 2.71 4.61 6.25 7.78 9.24 10.64
95% 3.84 5.99 7.81 9.49 11.07 12.59
99% 6.63 9.21 11.34 13.28 15.09 16.81
特别地，距离方向类传感器是机器人领域中应用最为广泛的感知设备，其观测空
间为两维。因此，在实际应用中，可以通过查询简化的2
2, 分布表来获取数据关联的
阈值，如表 2.2所示。
表2.2 2
2, 分布表
Tab. 2.2 2
,d distribution
1 2 3 4 2
2, 1 4 9 16
40％ 86％ 99％ 99.9%
值得注意的是，通常在进行 IC_TEST 时，需要设置两个阈值参数GATE_REJECT
和GATE_AUGMENT。 GATE_REJECT 表示可以进行数据关联的最大距离，即当2 GATE_REJECTiijD 时认为匹配成功；GATE_AUGMENT表示建立新的地图路标的
最小距离，即当2 >GATE_AUGMENTiijD 时，观测值 z 与地图路标间的 Mahalanobis 距
离足够远，可以认为是地图中未建立的路标。在本节的仿真中，ICNN 的阈值分别设
上海交通大学硕士学位论文 第二章 扩展信息滤波 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 数据关联的结果，箭头起点处的十字表
上海交通大学硕士学位论文 第二章 扩展信息滤波 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
12
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 yf x y h H x x G y y
f x y
f x y
(2.19)
上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 15 页
1
1 2
1
2ˆ ˆ( , )
i
i i
i
i
j
H j
H H
ij
ij
h
h hh 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 fH 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
DH
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
上海交通大学硕士学位论文 第二章 扩展信息滤波 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
上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 17 页
对于（c）中的情况，与 ICNN 数据关联结果（图 2.4）相比，即使两个环境路标
同时处于同一个地图路标的数据关联范围之内（即都位于同一个椭圆之内）时，也不
会出现误匹配的错误。但iHh 和
iHC 的维数随着假设集合 iH 的维数的增加而增加，因
此随着问题规模的不断增大，JCBB 的运算时间会不断增长。因此，在小规模的环境
中，可以利用 JCBB 算法在较短的时间内得到可靠的数据关联结果。
2.2.3 仿真
下面通过一个具体的仿真实验，对比 ICNN 与 JCBB 两种数据关联算法在匹配鲁
棒性上的差异。实验环境是一个正方形的回廊，如图 2.6所示。其中，三角形轨迹表
示机器人的期望运动轨迹，实心点表示环境中的路标（共 88 个路标点）。为了增加算
法中数据关联的难度，实验中的环境路标两两相邻，成对出现于运动路线的两侧。机
器人通过 SLAM 算法，根据定位信息和所建立的地图沿预定路线运动。
-2 0 2 4 6 8 10 12
0
1
2
3
4
5
6
7
8
9
10
X（ m）
Y（
m）
图2.6 数据关联仿真环境
Fig. 2.6 Simulation environment of data association
由于 ICNN 的数据关联算法没有考虑全局地图路标信息，因此对于上述环境路标
密集的仿真环境，其误匹配的错误难以避免，如图 2.7所示。其中，四副子图的横轴
表示算法步数，纵轴表示数据关联的次数统计。匹配正确包括两个方面的含义，一是
正确建立了环境路标与地图路标的匹配（与GATE_REJECT相关），二是正确将距离
上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 18 页
地图路标足够远的环境路标识别为尚未建立的地图路标（与 GATE_AUGMENT相
关）。上述两种情况分别对应Ture positives和Ture nagatives 中的统计情况。与之类
似，False positives和False nagatives分别表示算法中错误的匹配路标对和错误的建
立新地图路标的次数。容易看出，开始时由于机器人位置估计准确，因此不确定较小，
匹配错误少。随着机器人位置估计误差的不断增大，ICNN 产生的误匹配越多。最终，
在 150 步后，几乎所有的匹配都是错误的。
50 100 1500
0.5
1
True positives
50 100 1500
0.5
1
True negatives
50 100 1500
0.5
1
False positives
50 100 1500
0.5
1
False negatives
图2.7 ICNN 数据关联分析
Fig. 2.7 Analysis of ICNN data association
与 ICNN 相比，JCBB 的数据关联结果全部正确，没有任何误匹配的错误发生，
如图 2.8所示。其中，错误的匹配统计结果False positives和False nagatives都为零。
不难看出，如果将True nagatives的统计图向上翻转180 ，再叠加到True positives图
上就得到了一个黑色统计图，即所有数据关联无一错误，这与2.2.2节的分析一致。
上海交通大学硕士学位论文 第二章 扩展信息滤波 SLAM 解决方案
第 19 页
50 100 1500
0.5
1
True positives
50 100 1500
0.5
1
True negatives
50 100 1500
0.5
1
False positives
50 100 1500
0.5
1
False negatives
图2.8 JCBB 数据关联分析
Fig. 2.8 Analysis of JCBB data association
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 20 页
3 第三章 扩展信息滤波 SLAM 仿真
EQUATION SECTION 3
3.1 机器人运动模型
本论文中所讨论的机器人都是陆地环境中的轮式机器人，具体来说，下面将分别
介绍二自由度机器人、二轮三自由度机器人以及四轮三自由度机器人的运动模型。
a. 二自由度机器人运动模型
二自由度机器人的运动环境为平面环境。其运动模型为
( )( 1)
( )( 1)
v xv
v yv
x k V dtx kf
y k V dty k
(3.1)
其中，[ ( 1) ( 1)]Tv vx k y k 和[ ( ) ( )]T
v vx k y k 分别表示机器人在 1k 时刻和 k 时刻
所在位置的坐标，而 xV 与 yV 分别表示机器人沿平面正交坐标轴 x 和 y 方向的速度，dt
是运动控制周期， 是均值为 0，方差为 2 的高斯白噪声。不难看出，上述机器人运
动模型的 Jacobian 矩阵为
1 0
0 1v
fF
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分别表示机器人在时
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 21 页
刻 k 时的线速度和角速度， dt 是控制周期， 是均值为 0，方差为 2 的高斯白噪声。
简单计算后，得到上述机器人的 Jacobian 矩阵为
1 0 sin( ( ))
0 1 cos( ( ))
0 0 1
v
v
v
Vdt kf
F Vdt kx
(3.4)
c. 四轮、三自由度机器人运动模型
室外机器人最常见机器人就是有四轮汽车改装的四轮机器人。通常改装的车辆是
后轮驱动，通过改变前轮的方向角来改变运动方向。其运动模型可以表示为
( 1) ( ) cos( ( ))
( 1) ( ) sin( ( ))
( 1) sin( )( )
v v v
v v v
vv
x k x k Vdt G k
f y k y k Vdt G k
k Vdt Gk
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 kf
F Vdt G kx
(3.6)
3.2 传感器观测模型
本文所用的传感器主要包括两类：一类是全局坐标类传感器，另一类是距离方向
类传感器，下面分别给出上述两类传感器的观测模型和 Jacobian 矩阵。
a. 全局坐标类传感器观测模型
全局坐标类传感器属于全视角的传感器，在任意时刻它都可以观察周围 360°范
围內的环境，其观察值为路标相对于目前机器人所在坐标系位置的偏差。在本文中，
这种传感器只在仿真中与二自由度机器人配合使用。具体来说，其观测模型为
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 22 页
v xi
v yi
x zxg
y zy
(3.7)
其中，[ ]T
i ix y 是路标在全局坐标系中的位置向量，[ ]T
v vx y 为机器人在全局坐标
系中的位置向量，[ ]T
x yz z 是传感器观测值，是传感器观测误差，它是均值为 0，
方差为 2 的高斯白噪声。上述传感器观测模型的 Jabobian 矩阵为
1 0
0 1v
gG
x
(3.8)
在信息滤波 SLAM 算法观测更新步骤中，需要用到观测模型(3.7)的对称形式
x i v
y i v
z x xh
z y y
(3.9)
其相应的 Jacobian 矩阵为
1 0 1 0
0 1 0 1
hH
(3.10)
b. 距离方向类传感器观测模型
距离方向类传感器是最常见的传感器类型，它采用极坐标系，将观测值用距离和
方向角两个参数来表示。目前机器人领域中最常使用的摄像头和激光雷达都属于距离
方向类传感器，其观测模型可以表示为
cos( )
sin( )
i v v
i v v
x x rg
y y r
(3.11)
其中，[ ]T
i ix y 是目标所在位置的全局坐标，而 [ ]T
v v vx y 是机器人在当前时刻
的位置和姿态，[ ]Tr 是传感器的测量值，分别是目标距离传感器的距离和方向，
是均值为 0，方差为 2 的高斯白噪声。观测模型(3.11)的 Jacobian 矩阵为
1 0 sin( )
0 1 cos( )
v
vv
rgG
rx
(3.12)
如前所述，在本文提出的 SLAM 算法中还会用到观测模型(3.11)的对称形式
2 2( ( )) ( ( ))
( )atan 2
( )
i v i v
i vv
i v
x x k y y kr
h y y k
x x k
(3.13)
上海交通大学硕士学位论文 第三章 扩展信息滤波 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 rH
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 Vu 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)
上海交通大学硕士学位论文 第三章 扩展信息滤波 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 zu 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 rr
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)
上海交通大学硕士学位论文 第三章 扩展信息滤波 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 xfx
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]中的某一正数。归一化信息矩阵中
的元素表示机器人与路标或路标与路标之间的连接强度，其主对角线上的元素是机器
人或路标关于自身的连接强度，因此其连接强度最强。
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 26 页
0 2 4 6 8 10
v+ 1 2 3 4 5 6
v+
1
2
3
4
5
6
（a）实时仿真结果 0 2 4 6 8 10
v+ 1 2 3 4 5 6
v+
1
2
3
4
5
6
（b）归一化信息矩阵
图3.1 1-D, 1-DOF 情况 EIF-SLAM 仿真结果
Fig. 3.1 Simulation results of EIF-SLAM in 1-D, 1-DOF case
归一化信息矩阵的顶部和左侧的标记的具体含义如表 3.1所示。归一化信息矩阵
中每个元素所表示的连接意义就可以通过上述标记来明确。
表3.1 归一化信息矩阵标记含义对照表
（图 3.1（a）中从左向右路标编号顺次递增）
Tab. 3.1 Mark notation in normalized information matrix
标记 V+ 1 2 3 4 5 6
含义 机器人 路标 1 路标 2 路标 3 路标 4 路标 5 路标 6
例如， ( , )v v 表示机器人关于自身的连接强度， ( ,1)v 表示机器人与路标 1
的连接强度。归一化信息矩阵元素的颜色与该取值的大小成正比。1 为黑色，0 为白
色。另外，在所有非零元素的中心位置都会标记一个黑点。从仿真结果中，可以得到
下面四个结论：
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 27 页
（1） 由于连接关系具有对称性，因此信息矩阵是 Hermit 阵；
（2） 机器人以及路标对自身的连接强度最大。因此，归一化信息矩阵的对角
线上的元素为 1（对应元素颜色为黑色）；
（3） 从矩阵元素值的分布来看，归一化信息矩阵中值较大的元素分布在主对
角线领域和副对角线端点领域两处，然而归一化信息矩阵中并没有值为零的元素，因
此，归一化信息矩阵是几乎稀疏的；
（4） 由于在本仿真中设定机器人任意时刻都可以观察到两个路标，因此副对
角线端点领域存在两个值较大的元素。
3.4.2 二维、二自由度（2-D, 2-DOF）情况
机器人在二维环境中，沿逆时针方向运动三周，顺次通过事先标记的路径点
（waypoint），并在移动的过程中，完成 SLAM 估计，实时仿真结果如图 3.2（a）所
示。其中，实心三角型表示机器人当前的位置，实心圆点表示路标点，星号表示环境
中的路标，路标旁的数字为该路标的编号，叉号表示机器人对路标的估计位置。
-150 -100 -50 0 50 100 150-100
-50
0
50
100
X（ m）
Y（
m）
2 3
4 5
6
7
8
9
1011
12
13
14
15
1617
18
19
20
21 22
23
24
25
26 27
28
29
3031
32
33
34
35
36
v+ 2 3 6 7 8 9 10 36 11v+
2
3
6
7
8
9
10
36
11
（a）实时仿真结果 （b）规范化信息矩阵
图3.2 2-D, 2-DOF 情况 EIF-SLAM 仿真图
Fig. 3.2 Simulation results of EIF-SLAM in 2-D, 2-DOF case
与上述一维环境仿真相比，二维环境下 EIF-SLAM 仿真的复杂性大大增加，主
要表现在以下几个方面：
（1） 一维环境仿真中机器人的运动路线是直线，而二维环境中机器人运动的
路线是任意的二维曲线。为了便于操作，将曲线中设置若干的路标点，通过折线来近
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 28 页
似机器人的运动曲线。因此，需要设置一个接近阈值来确认是否已经到达目标点，如
果机器人与目标点的距离小于该参数就认为机器人已经到达路标点，机器人从此刻起
向下一个路标点运动。值得注意的是，如果接近阈值设置过大，机器人运动的轨迹与
理想轨迹的偏差就会很大，反之，机器人很难进入以路径点为中心的领域之内，从而
引起机器人运动振荡。
（2） 对于信息矩阵的处理，一维仿真用到了归一化信息矩阵，二维仿真则采
用了规范化信息矩阵（normalized information matrix）。规范化包括以下三个基本步骤：
步骤 1 根据协方差矩阵C 得到相关系数矩阵 R
( , )
( , )( , ) ( , )
C i jR 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 nI
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 网络中的许多连接都属于弱连接关系。
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 29 页
-0.2 0 0.2 0.4 0.6 0.8 1 1.20
50
100
150
200
250
300
350
Range of distribution
Sta
tistic r
esult
图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
100
Steps
Lin
k s
trength
landmark 2 landmark 3 landmark 6
图3.4 机器人与路标连接强度
Fig. 3.4 Link strength between robot & features
上海交通大学硕士学位论文 第三章 扩展信息滤波 SLAM 仿真
第 30 页
其中，横轴表示时间，纵轴表示连接强度。图中的三条曲线分别反映了机器人与
2 号、3 号、6 号路标连接强度的变化情况，三条曲线的变化规律基本一致。不失一
般性，取机器人与 3 号路标的连接变化进行说明。由于仿真中设定机器人逆时针运动
三周，因此在每次重新看到 3 号路标的时候，机器人与 3 号路标的连接强度都会突然
增加（几乎是跳变），而当机器人逐渐远离 3 号路标后，连接强度逐渐减小（近似为
线性）。
（3） 由于仿真中的路标较少，因此对算法效率的测试只能反映出算法在路标
数目不太多情况下的时间复杂度，如图 3.5所示。横轴表示状态向量的维数，纵轴表
示计算时间，星号表示移动步骤的时间，叉号表示更新步骤的时间。不难看出，在环
境规模较小的情况下，扩展信息滤波 SLAM 算法的运算效率较高。
0 5 10 15 20 25 30 35 400
0.002
0.004
0.006
0.008
0.01
0.012
0.014
0.016
Current size of state vector
Com
puta
tion t
ime（
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所示。其
中，横轴表示算法的步数，纵轴表示误差或协方差的大小，虚线和实现分别表示机器
人的误差和协方差曲线。不难看出，机器人的误差绝对值始终小于协方差绝对值，因
上海交通大学硕士学位论文 第三章 扩展信息滤波 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 6000
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
Steps
Map e
rror
图3.7 地图误差
Fig. 3.7 Map error
上海交通大学硕士学位论文 第三章 扩展信息滤波 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）二自由度
上海交通大学硕士学位论文 第三章 扩展信息滤波 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
上海交通大学硕士学位论文 第三章 扩展信息滤波 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 Wlm 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)
上海交通大学硕士学位论文 第三章 扩展信息滤波 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
上海交通大学硕士学位论文 第三章 扩展信息滤波 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 方向
上海交通大学硕士学位论文 第三章 扩展信息滤波 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
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 38 页
4 第四章 信息矩阵稀疏化
EQUATION SECTION 4
4.1 信息矩阵的稀疏性
下面通过分析扩展信息滤波 SLAM 算法的几何意义，即 Bayes 网络中节点间连
接关系，并结合具体公式，来解释信息矩阵稀疏性的由来。
a. 移动
机器人的移动过程，通常可以分解为增加状态 1tx 和消去状态 tx 两个过程。移动
过程中 Bayes 网络与信息矩阵演化如图 4.1所示。两者的对应关系为：信息矩阵中每
个元素的值正比于其行与列对应的 Bayes 网络中节点的连接强度，且在信息矩阵中元
素颜色越深，则该元素所代表的值越大。
（a） （b） （c）
图4.1 移动中 Bayes 网络与信息矩阵演化过程
Fig. 4.1 Evolution of Bayes network and information matrix in projection
tx
1tx
tx
1tx 1m
2m
3m
4m
5m 1m
2m
3m
4m
5m 1m
2m
3m
4m
5m
tx 1m 2m 4m3m 5m
tx
1m
2m
3m
4m
5m
tx1tx 1m 2m 3m 4m 5m
1tx
1m
2m
tx
3m
4m
5m
1tx 1m 2m 3m 4m 5m
1tx
1m
2m
3m
4m
5m
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 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)
其中，
11
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)
综上所述，随着增加路标过程的进行，信息矩阵稀疏化的程度不断加强。
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 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 是稀疏矩阵，因此 1TH 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 hH
x y x y
tx tx
1m
2m
3m
4m
5m1m
2m
3m
4m
5m
tx 1m 2m 3m 4m
tx
1m
2m
3m
4m
tx 1m 2m 3m4m 5m
tx
1m
2m
3m
4m
5m
tx
1m
2m
3m
4m
5m
tx1m 2m 3m 4m
5m
tx
1m
2m
3m
4m
5m
tx 1m 2m 3m4m 5m
tx
1m
2m
3m
4m
5m
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 41 页
综上所述，更新过程增强了信息矩阵的部分连接，如果增强的连接处于信息矩阵
对角线附近，则更新过程不对信息矩阵的稀疏化产生影响；反之，由于增强连接的数
量很小，因此该过程不会改变信息矩阵几乎稀疏的整体性质。总之，更新过程并不影
响信息矩阵的稀疏性。
4.2 信息矩阵的结构特征
Eustice 等对规范化信息矩阵特征的研究已经比较深入[23, 41, 47]，但信息矩阵规范
化运算量较大，因此实际应用中很少使用规范化信息矩阵。目前还没有直接关于信息
矩阵结构特征的结论。本节分别从扩展信息滤波公式和 Bayes 网络两个角度揭示信息
矩阵的结构特征，并验证信息矩阵特征与规范化信息矩阵特征的一致性。
信息矩阵元素分布的三维图如图 4.4所示，其中 row 与 column 分别表示信息矩阵
的行和列，value 表示信息矩阵元素的值。从元素取值的分布上看，信息矩阵具有以
下三个基本的特征：
（1） 信息矩阵以主对角线为中心对称分布；
（2） 除副对角线端点外，离主对角线越远，元素值越小；
（3） 副对角线端点处形成了峰值。
0
50
100
0
20
40
60
80
100
-600
-400
-200
0
200
400
600
800
rowcolumn
valu
e
（a）全局图
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 42 页
20
40
60
80
20
40
60
80
-2
0
2
4
rowcolumn
valu
e
（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 是稀疏矩阵，因此 1TH R H 中只有与机器人位置姿态
[ ]T
v v vx y 和待更新路标 [ ]i i
T
f fx y 相关的位置不为零。更进一步，更新过程加强
了表示待更新路标位置元素的值。
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 43 页
表4.2 信息矩阵结构特征分析（Bayes 网络）
Tab. 4.2 Structure analysis of information matrix（Bayes net）
特征 分析
1 以主对角线对称的两个元素表示相同的两个节点的连接强弱。
2 两个节点间最短路越长，其在信息矩阵中所代表的元素离主对角线越远。
3 副对角线端点表示机器人状态 tx 和目前正与其直接连接的路标的连接强度，因此，
该元素值较大。
规范化信息矩阵通过从信息矩阵提取机器人和路标间的连接信息，建立机器人和
各路标的 Bayes 网络。规范化信息矩阵具有明显的几何意义，因而对其几何特征及意
义等的研究也较充分。下面通过信息矩阵规范化的步骤检验信息矩阵与规范化信息矩
阵的结构特征具有统一性，从而验证本节提出的信息矩阵的结构特征。信息矩阵规范
化需要进行如下三个基本步骤：
步骤 1 根据协方差矩阵C 得到相关系数矩阵 R 。
( , )
( , )( , ) ( , )
C i jR 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 nI
R R R R R n R n
(4.5)
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 44 页
步骤 3 对连接强度矩阵 I 中的元素进行归一化处理。
根据式(4.4)得到的相关系数矩阵 R 保持信息矩阵的所有结构特征；式(4.5)计算得
到的连接强度矩阵 I 中所有需要计算行列式的子矩阵都是对角占优的，且主对角线的
元素值都很大，因此 I 中主对角线的元素值很大；归一化不会改变连接强度矩阵的几
何特征。因此，根据规范化步骤得到的规范化信息矩阵是对角占优的 Hermit 矩阵，
这与 Eustice 等得到的规范化信息矩阵的结构特征相一致[41]。
根据不同的阈值，将信息矩阵中几乎为零的元素置为零就得到了稀疏化程度不同
的信息矩阵，如图 4.5所示。其中，信息矩阵下方的值表示直接稀疏化阈值，即如果
信息矩阵元素绝对值小于该阈值，就认为该元素为零；上方的图就是经过在上述稀疏
化过程处理后的信息矩阵。一般来说，矩阵稀疏化程度越高，代数运算效率越高。但
与此同时，信息矩阵丢失的信息也越多。综上，随着信息矩阵稀疏化程度的提高运算
效率增高了，同时运算精度降低了。因此，需要找到合适的运算时间和精度的折衷。
图4.5 信息矩阵稀疏化结果
Fig. 4.5 Results of information matrix sparsification
从定量的角度，对于图 4.5中的信息矩阵稀疏化过程，将稀疏比和阈值的数据分
别在对数坐标和十进制坐标中用曲线拟和，结果分别如图 4.6（a）和（b）所示。其
中，横坐标表示阈值，纵坐标表示以该阈值为参数稀疏化后的信息矩阵的稀疏比。不
难看出，在对数横坐标系下元素分布服从线性分布，即信息矩阵在对数阈值区间中的
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 45 页
元素个数相当。与此对应，在十进制坐标系下，元素分布则服从指数分布。上述结论
与信息矩阵的结构特征相一致。
10-11
10-10
10-9
10-8
10-7
10-6
10-5
0.35
0.4
0.45
0.5
0.55
0.6
0.65
Threshold
Spars
ific
ation r
atio
simulation data
（a）对数坐标系
1 2 3 4 5 6 7 8 9 10
x 10-6
10-0.5
10-0.4
10-0.3
Threshold
Spars
ific
ation r
atio
simulation data
（b）十进制坐标系
图4.6 稀疏比与阈值的关系
Fig. 4.6 Relationship between sparsification ratio & threshold
4.3 稀疏矩阵运算具有高效性
由于模型线性化和数据关联的需要，扩展信息滤波在移动、增加路标和更新每个
过程中都需要对信息矩阵进行运算。其中，式(2.2)的求解是其最主要的计算工作，为
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 46 页
了分析方便，我们将其转化为标准的线性方程组求解问题
I
(4.6)
其中， I 为与同维的单位矩阵。
随着地图路标数量的增加，上述线性方程组求解的运算量会大大增加。如前所述，
信息矩阵是几乎稀疏的，如果能合理利用信息矩阵的稀疏性质，必将大大提高扩展信
息滤波的计算效率。
本文采用 Tim Davis 提出的稀疏矩阵运算方法[50]，线性方程组求解的效率明显提
高。为了方便比较，统一采用矩阵求逆时间作为比较标准。图 4.7比较了稀疏度为 0.5
的稀疏矩阵，按照稠密方式和稀疏方式在矩阵求逆过程中的运算效率。横轴表示矩阵
的维数，左纵轴表示稀疏方式求逆运算时间，右纵轴表示稠密方式求逆运算时间。从
图中，可以得到两个结论：第一，当矩阵为稀疏矩阵时，按照稀疏矩阵的特殊运算规
律，可以大大减少运算时间（大约为稠密运算时间的 1/60）；第二，当稀疏度不变时
（本例中为 0.5），稀疏矩阵、稠密矩阵求逆时间与矩阵维数的增加成指数关系。
500 1000 1500 2000 2500 3000 3500 40000
0.1
0.2
0.3
0.4
0.5
0.6
0.7
Tim
e b
ased o
n s
pars
e m
eth
od（
s）
Dimension of matrix
500 1000 1500 2000 2500 3000 3500 40000
5
10
15
20
25
30
35
Tim
e b
ased o
n d
ense m
eth
od（
s）
Sparse
Dense
图4.7 稠密方式和稀疏方式矩阵求逆效率比较图
Fig. 4.7 Efficiency comparison of matrix inversion between dense & sparse method
如果采用稀疏方式对矩阵求逆，则维数相同的矩阵，稀疏度就成了求逆效率的关
键因素，如图 4.8所示。本例中采用的测试矩阵是 2000 维的矩阵，横轴表示矩阵稀疏
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 47 页
度，纵轴表示求逆运算时间。不难看出，对于相同维数的矩阵，矩阵求逆时间与稀疏
度近似为线性关系。
0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 10.12
0.13
0.14
0.15
0.16
0.17
0.18
0.19
0.2
0.21
Sparse ratio
Tim
e（
s）
图4.8 稀疏度与矩阵求逆效率的关系
Fig. 4.8 Relationship between sparse ratio & efficiency in matrix inversion
4.4 稀疏化算法
为了降低扩展信息滤波 SLAM 算法的时间复杂度和空间复杂度，根据上述信息
矩阵的结构特征，构造稀疏化算法，如算法 4.1所示。
算法4.1 稀疏化算法
2 1
1
10 | ( , ) |
dim( )( , )
( , ) others
, 1 ( ) 1 ( )
k
k kk
k
k k
while k N
i ji j
i j
i j i r j c
end
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 48 页
其中，0.5 1 为保守系数， 1N 为稀疏化起始步数， k 为第 k 步信息矩阵，
dim( )k 为信息矩阵 k 的维数， ( )kr 和 ( )kc 分别表示信息矩阵 k 的行数和列数。
N 越大，SLAM 精度越高，实时性越差；越大，保守程度越大。由于只有移动步骤
会使得信息矩阵稠密化[23]，因此在实际应用中只需在每个算法周期，对信息矩阵进
行一次稀疏化即可。
上述信息矩阵稀疏化算法会给信息矩阵带来误差，进而降低 SLAM 估计的精度。
设稀疏化前后的信息矩阵分别表示为和 E ，其中 E 是由于稀疏化而产生的误差
矩阵。不难看出，上述稀疏化算法总可以保证下述关系式成立
1 1E (4.7)
在多种情况的多次仿真中发现，当稀疏化误差 E 满足1 1E 时，SLAM 的估
计结果具有一致性，即估计误差始终在协方差范围之内；当1 1E 时估计结果处
于临界稳定，即估计误差与协方差相当；当1 1E 时 SLAM 估计结果不具有一
致性，即对机器人和路标位置的估计误差超出协方差估计范围。
算法 4.1导致的误差可以通过环闭合（或重定位）来消除。所谓环闭合是指机器
人在移动的过程中发现地图上已经建立的路标，即机器人的运动轨迹是一个环。此时，
根据扩展信息滤波算法[22, 23]，更新步骤对所有地图路标的位置都重新进行修正，因
而 SLAM 估计结果的整体误差将大为减小。在实际应用中，可以通过设计闭环移动
路线有效抑制信息矩阵稀疏化导致的 SLAM 估计误差。
综上所述，算法 4.1在保证 SLAM 估计一致准确的同时，通过稀疏化信息矩阵有
效的提高了信息滤波算法效率。另外，通过环闭合技术极大减小了稀疏化所导致的估
计误差。
4.5 误差分析
定理 4.1 从理论的角度，分析了稀疏化误差与 SLAM 估计精度的关系，定量的给
出了算法 4.1所引入的稀疏化误差对 SLAM 估计结果的影响，即机器人和路标估计位
置和协方差的相对误差率[51]。
定理 4.1 假设信息矩阵 n n 非奇异，和 E 分别满足方程组
1
1( )s E
(4.8)
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 49 页
( ) sE
(4.9)
其中，与 s 分别表示稀疏化前后的协方差矩阵， 与 s 分别表示稀疏化前后
的均值向量， 0 ， E 是信息矩阵稀疏化引入的误差，且 E 适当小，使得11/E ，则
( )
1 ( )( / )
s E
E
(4.10)
( )
1 ( )( / )
s E
E
(4.11)
证明：
（矩阵求逆）
假 设 E 足 够 小 ， 使 得 E 为 可 逆 矩 阵 。 于 是 ， 误 差1 1 1 1 1 1( ) ( )E I E 。如果 1( ) 1E ，则 E 就是可逆矩阵，且
可以把 1 1( )I E 写成 1E 的幂级数
1 1 1 1 1 1 1 1
0 1
( ) ( 1) ( ) ( 1) ( )k k k k
k k
E E E
(4.12)
假设 是给定的矩阵范数，且11/E ，则
1 1 1( ) 1E E E ，
因而特别有 1( ) 1E 和(4.11)成立，于是
1 1 1 1 1
1
1 1
1 1 1
1 11
( ) ( 1) ( )
( / )
1 1 ( / )
k k
k
k
k
E E
E EE
E E
(4.13)
或
1 1
1
1
( ) ( ), 1
1 ( )( / )
E EE
E
如果 (4.14)
其中， ( ) 表示信息矩阵的条件数。
（线性方程组求解）
与矩阵求逆证明类似，如果 E 足够“小”，使得 1( ) 1E ，则根据(4.11)，有
上海交通大学硕士学位论文 第四章 信息矩阵稀疏化
第 50 页
1 1 1 1
1 1 1 1 1
1 1
( ) [ ( ) ]
( 1) ( ) ( 1) ( )
s
k k k k
k k
E E
E E
(4.15)
如果 是矩阵范数，且1 1A E ，又如果 是相容的向量范数，则
1
1
11 1
k
s
k
EE
E
(4.16)
与推导(4.13)的证明类似，如果1 1E ，且向量范数 与矩阵范数 相容，
则相对误差上界为
( )
1 ( )( / )
s E
E
(4.17)
□
定理 4.1给出了由于信息矩阵稀疏化，状态向量均值和协方差相对误差率的上界，
为进一步的理论分析奠定了坚实的基础。
上海交通大学硕士学位论文 第五章 仿真与分析
第 51 页
5 第五章 仿真与分析
EQUATION SECTION 5
5.1 仿真环境
下面根据上述信息矩阵结构特征，针对一个具体的 SLAM 问题，讨论直接稀疏
化信息矩阵扩展信息滤波在 SLAM 问题中的应用。
仿真环境如图 5.1所示，“星号”表示环境中的路标点（共 400 个），“折线”表示
机器人期望走的轨迹。需要解决的问题是，机器人从起始点 (0,0)出发，逆时针运动，
在自身不断定位的同时，绘制环境地图。在本问题中，地图就是路标点的位置坐标。
为了讨论环闭合（或重定位）问题，设定机器人逆时针运动两周。
-100 -50 0 50 100-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（
m）
landmark
waypoint
Start Point
图5.1 仿真环境
Fig. 5.1 Simulation environment
上海交通大学硕士学位论文 第五章 仿真与分析
第 52 页
5.2 模型和参数设定
仿真中的机器人是两轮三自由度的机器人，传感器是距离-方向类传感器（如激
光、摄像头等）。下面以移动、增加路标和更新三个步骤来分别说明仿真模型和参数
的具体设定。
a．移动过程 机器人的运动模型为
( 1) ( ) cos( ( ))
( , ) ( 1) ( ) sin( ( ))
( 1) ( )
v v v
t t v v v
v v
x k x k Vdt k
f x u y k y k Vdt k
k k wdt
(5.1)
其中， [ ( 1) ( 1) ( 1)]T
v v vx k y k k 表示机器人在 1k 时刻的位置和姿态，而
[ ( ) ( ) ( )]T
v v vx k y k k 表示机器人在 k 时刻的位置和姿态，V 和w分别表示机器人在时
刻 k 时的线速度和角速度， dt 是控制周期， 是均值为 0，方差为 2 的高斯白噪声。
运动模型(5.1)的 Jacobian 矩阵为
1 0 sin( ( ))
0 1 cos( ( ))
0 0 1
v
v
v
Vdt kf
F Vdt kx
(5.2)
模型线性化误差输入量u 求解如下：
cos( ) sin( )
( , ) sin( ) cos( )t tx t x
Vdt
u f u F Vdt
wdt
(5.3)
b．增加路标过程 传感器观测模型为
cos( )
( , )sin( )
i v v
t t
i v v
x x rg x z
y y r
(5.4)
其中，[ ]T
i ix y 是路标所在位置的全局坐标，而 [ ]T
v v vx y 是机器人在当前时刻
的位置和姿态，[ ]Tr 是传感器的测量值，分别表示路标相对于传感器的距离和方向，
是均值为 0，方差为 2 的高斯白噪声。
观测模型(5.4)的 Jacobian 矩阵为
上海交通大学硕士学位论文 第五章 仿真与分析
第 53 页
1 0 sin( )
0 1 cos( )
v
vv
rgG
rx
(5.5)
模型线性化误差输入量u 为
cos( ) sin( )
( , )sin( ) cos( )t tx t xu g z G r
(5.6)
c．更新过程 观测模型(5.4)的对称形式为
2 2( ( )) ( ( ))
( )atan 2
( )
i v i v
i vv
i v
x x k y y kr
h y y k
x x k
(5.7)
其中，[ ]T
i ix y 是目标所在位置的全局坐标，而 [ ]T
v v vx y 是机器人在当前时刻
的位置和姿态，[ ]Tr 是传感器的测量值，分别是路标相对于传感器的距离和方向，
是均值为 0，方差为 2 的高斯白噪声。
其 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 rH
y y k x x k y y k x x k
r r r r
(5.8)
模型线性化误差输入量u 为
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 xfx
fy
u z h H
r
r r r r
r r r r
(5.9)
上海交通大学硕士学位论文 第五章 仿真与分析
第 54 页
5.3 仿真结果及分析
应用直接稀疏化信息矩阵 EIF-SLAM 算法得到以下仿真结果，下面重点对信息
矩阵结构特征、算法效率、重定位以及误差和协方差四个关键问题进行分析。
(1) 信息矩阵的结构特征
信息矩阵元素绝对值在对数空间中的分布近似为线性分布，如图 5.2所示。其中，
横坐标表示信息矩阵元素绝对值对数分布范围，纵坐标表示统计数量，即在横坐标所
在范围內的信息矩阵元素个数，直线为参考线。在对数空间，元素分布服从线性分布
等价为欧几里得空间元素分布服从指数分布。因此，仿真中元素分布的特点验证了本
文对信息矩阵结构的分析。
-140 -120 -100 -80 -60 -40 -20 0 200
2
4
6
8
10
12
14x 10
4
Range of distribution in log scale
Sta
tistic r
esult
图5.2 信息矩阵元素分布对数统计直方图
Fig. 5.2 Log statistic histogram of element distribution in information matrix
(2) 算法效率
移动、增加路标和更新是 EIF-SLAM 中最重要的三个步骤，同时也是耗时最多
的三个步骤。因此，可以通过上述三个步骤的效率来反映 SLAM 算法的整体运算效
率。直接稀疏化信息矩阵 EIF-SLAM 算法较 EIF-SLAM 算法实时性大为提高，如图
5.3所示。其中，“星号”和“十字”分别代表原算法和改进后算法三个步骤的耗时。
随着状态向量维数的不断增大，改进后的 EIF-SLAM 算法效率得到了显著提高。
上海交通大学硕士学位论文 第五章 仿真与分析
第 55 页
0 50 100 150 200 250 300 350 4000
0.5
1
1.5
Do p
roje
ction
EIF-SLAM Improved EIF-SLAM
0 50 100 150 200 250 300 350 4000
0.5
1
1.5
Add f
eatu
res
0 50 100 150 200 250 300 350 4000
0.5
1
1.5
Current size of state vector
Do u
pdate
图5.3 扩展信息滤波 SLAM 算法改进前后效率对比
Fig. 5.3 Efficiency comparison between EIF-SLAM & improved EIF-SLAM
(3) 环闭合
当机器人的运动轨迹为一个环时，根据 EIF-SLAM 更新步骤[41]，机器人和路标
的位置估计误差和协方差都会显著减小。EIF-SLAM 和直接稀疏化信息矩阵
EIF-SLAM 算法在环闭合前后的估计结果分别如图 5.4和图 5.5所示。
其中，“星号”表示路标的真实位置，“椭圆”表示路标或机器人位置的 3 不确
定性，椭圆中心的“十字”表示机器人和路标的估计位置。椭圆越小，十字离真实路
标越近，估计效果越好。从仿真结果中，可以直观的看出改进后的算法虽然在环闭合
前（即第一圈）较原算法误差和协方差都偏大，但在环闭合后（即第二圈）误差与协
方差都与原算法相当。换句话说，改进后的算法由于稀疏化损失的估计精度完全可以
通过环闭合技术弥补。
(4) 估计误差和协方差
误差和协方差可以定量的反映 SLAM 估计结果的精确性。下面以误差和协方差
作为指标，对比分析环闭合技术在直接稀疏化信息矩阵 EIF-SLAM 算法中的作用。
EIF-SLAM 与直接稀疏化信息矩阵 EIF-SLAM 误差、协方差曲线分别如图 5.6（a）与
图 5.6（b）所示。其中，“虚线”表示机器人位姿误差，“实线”表示机器人位姿的协
上海交通大学硕士学位论文 第五章 仿真与分析
第 56 页
方差，横坐标表示算法步数。由于在 250 步时误差和协方差突然减小，因此在 250
步时机器人观察到了地图中的路标，即出现了环闭合现象。值得注意的是，图 5.6（b）
中机器人通过环闭合由临界稳定估计（即误差与协方差大致相等）转变为一致稳定估
计。换句话说，由于算法 4.1总可以保证系统的估计为一致估计或临界稳定估计，因
此改进后的 EIF-SLAM 算法可以通过环闭合取得与原算法相同的精确度，这也与（3）
中的分析一致。
上海交通大学硕士学位论文 第五章 仿真与分析
第 57 页
-100 -50 0 50 100
-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（
m）
（a）环闭合前
-100 -50 0 50 100
-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（
m）
（b）环闭合后
图5.4 扩展信息滤波-SLAM 重定位前后估计结果
Fig. 5.4 EIF-SLAM estimation results before & after loop-closure
上海交通大学硕士学位论文 第五章 仿真与分析
第 58 页
-100 -50 0 50 100
-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（
m）
（a）环闭合前
-100 -50 0 50 100
-80
-60
-40
-20
0
20
40
60
80
100
X（ m）
Y（
m）
（b）环闭合后
图5.5 直接稀疏化信息矩阵 EIF-SLAM 环闭合前后估计结果
Fig. 5.5 Improved EIF-SLAM estimation results before & after loop-closure
上海交通大学硕士学位论文 第五章 仿真与分析
第 59 页
0 50 100 150 200 250 300 350 400 450 500-10
0
10
x
Error Covariance
0 50 100 150 200 250 300 350 400 450 500-20
0
20
y
0 50 100 150 200 250 300 350 400 450 500-0.1
0
0.1
Steps
（a）EIF-SLAM
0 50 100 150 200 250 300 350 400 450 500-10
0
10
x
Error Covariance
0 50 100 150 200 250 300 350 400 450 500-20
0
20
y
0 50 100 150 200 250 300 350 400 450 500-0.1
0
0.1
Steps
（b）直接稀疏化信息矩阵 EIF-SLAM
图5.6 EIF-SLAM 与直接稀疏化信息矩阵 EIF-SLAM 误差和协方差对比
Fig. 5.6 Comparison of error and covariance between EIF-SLAM & improved EIF-SLAM
上海交通大学硕士学位论文 第六章 实验
第 60 页
6 第六章 实验
EQUATION SECTION 6
6.1 室内走廊
实验中使用的机器人是由本实验室自行研制开发的 Frontier-Ⅱ自主移动机器人，
该机器人采用双轮差速驱动方式，具有前向视觉、红外测距以及远程控制等功能，如
图 6.1所示。
图6.1 Frontier-Ⅱ自主移动机器人
Fig. 6.1 Frontier-Ⅱ autonomous mobile robot
初始时，机器人以当前位置作为起始点开始运动。实验中，采用摄像头事先拍摄
的图片作为环境中的路标，当发现自身与前方的路标相距 500mm 时左转，如此往复
逆时针运动三周。在此过程中，机器人应用本文提出的算法，完成一个完整的 SLAM
估计过程。实验环境如图 6.2所示，其中，“星号”表示路标的真实位置。
实验中，机器人利用尺度不变特征变换（Scale Invariant Feature Transform, SIFT）
算法实现两个功能：第一，感知环境，得到路标相对于摄像头光心的距离和角度；第
上海交通大学硕士学位论文 第六章 实验
第 61 页
二，数据关联，将摄像头观测到的图像与路标进行匹配。由于 SIFT 算法对平面图片
具有很强的鲁棒性，因此，数据关联中不会出现误匹配的情况。在具体应用本文提出
的算法时，由于 Frontier-Ⅱ是两轮三自由度的机器人，且摄像头属于距离-方向类传
感器，因而算法中使用的运动模型与观测模型与仿真中相同。
图6.2 走廊实验环境
Fig. 6.2 Corridor experiment environment
在选择稀疏化系数 2N ， 0.8 后，里程计实验数据与 SLAM 估计结果分别如
图 6.3（a）和图 6.3（b）所示。其中，“五角星”表示里程计记录的机器人位置，“星
号”表示路标的真实位置，“十字”表示路标或机器人的估计位置。从图 6.3（a）中
可以清晰的看出，机器人运动了三周。值得注意的是，机器人在运动中里程计误差在
不断积累，因而其轨迹偏差越来越大；而 SLAM 估计中，由于机器人每次到达初始
点时都进行了重定位，因而其定位误差在重定位前不断积累（比里程计定位误差小得
多），而重定位后突然减小，总的来说，其误差是不断趋于减小的。
机器人定位误差如图 6.4所示，其中，横轴表示算法运行的步数，纵轴分别表示
机器人在 X 轴与 Y 轴方向上的误差。不难看出，本文提出的 SLAM 算法对路标和机
器人位置的估计都比较精确。
-500 0 500 1000 1500 2000 2500
-500
0
500
1000
1500
2000
2500
X（ mm）
Y（
mm）
Start Point
上海交通大学硕士学位论文 第六章 实验
第 62 页
-1000 -500 0 500 1000 1500 2000 2500 3000
-1000
-500
0
500
1000
1500
2000
2500
3000
X（ mm）
Y（
mm）
Waypoint
Landmark
（a）里程计实验数据
-1000 -500 0 500 1000 1500 2000 2500-1000
-500
0
500
1000
1500
2000
2500
3000
X（ mm）
Y（
mm）
（b）SLAM 估计结果
图6.3 走廊实验结果
上海交通大学硕士学位论文 第六章 实验
第 63 页
Fig. 6.3 Corridor experiment results
0 500 1000 1500 2000 2500 3000 3500-200
-100
0
100
200
x (
mm
)
0 500 1000 1500 2000 2500 3000 3500-200
-100
0
100
200
y (
mm
)
Steps
图6.4 走廊实验机器人定位误差
Fig. 6.4 Localization error of robot in corridor experiment
6.2 室外停车场
停车场是 SLAM 领域中著名的数据集[52]，本实验通过停车场数据集来验证算法
的有效性。其实验环境为室外停车场，机器人由四轮汽车改造而成，机器人安装有
GPS、激光雷达、里程计等传感器。机器人在停车场以逆时针方向运动数周，通过激
光雷达观测环境。本实验通过里程计和激光雷达的数据，利用直接稀疏化信息矩阵
EIF-SLAM 算法完成一个完整的 SLAM 估计过程。与结构化的室内环境相比，室外
环境更加具有一般性。由于环境更加复杂，因而对本文提出的 SLAM 算法的检验也
就更具说服力。另外，在实验中，为了降低算法的空间复杂度，实验中信息矩阵采用
稀疏矩阵格式存储；而在运算中则利用稀疏矩阵独特的算法进一步提高算法的运算效
率[50]。
实验中采用圆柱型的物体（即，标杆和灯柱）作为环境路标，通过个体兼容最近
邻居法（Individual Compatibility Nearest Neighbor, ICNN）进行数据关联，由于环境
中的路标并不非常稠密，因此不会发生误匹配的情况。在具体应用本文提出的算法时，
由于机器人是四轮三自由度的机器人，因此机器人的运动模型为：
上海交通大学硕士学位论文 第六章 实验
第 64 页
( 1) ( ) cos( ( ))
( 1) ( ) sin( ( ))
( 1) sin( )( )
v v v
v v v
vv
x k x k Vdt G k
f y k y k Vdt G k
k Vdt Gk
WB
(6.1)
其中， [ ( 1) ( 1) ( 1)]T
v v vx k y k k 表示机器人在 1k 时刻的位置和姿态，而
[ ( ) ( ) ( )]T
v v vx k y k k 表示机器人在 k 时刻的位置和姿态，V 和G 分别表示机器人在时
刻 k 时的机器人的前进速度和前轮转动的角速度，WB是机器人的车宽，dt 是控制周
期， 是均值为 0，方差为 2 的高斯白噪声。
-25 -20 -15 -10 -5 0 5 10 15
-5
0
5
10
15
Lantitude（ m）
Longtitu
de（
m）
（a）GPS 定位数据
-25 -20 -15 -10 -5 0 5 10 15
-5
0
5
10
15
Latitude（ m）
Longitude（
m）
（b）SLAM 估计结果
图6.5 停车场实验结果
Fig. 6.5 Car Park experiment results
上海交通大学硕士学位论文 第六章 实验
第 65 页
激光雷达属于距离-方向类传感器，因而算法中使用的观测模型与仿真中相同。
需要注意的是，由于激光传感器并没有安装在机器人的几何中心处，因此在应用观测
模型时需对数据进行必要的坐标变换。实验结果如图 6.5所示。
其中，两图中横坐标和纵坐标分别为纬度和经度，（a）中的“圆圈”表示机器人
通过 GPS 得到的位置坐标，（b）中的“三角形”和“十字”分别表示 SLAM 算法对
机器人和路标位置的估计结果，“椭圆”表示路标和机器人位置的3 不确定性。
由于停车场数据集中没有机器人方向的数据，因此不能得到环境路标的真实位
置。但由于 SLAM 算法本质就是将机器人位姿和环境路标位置作为统一的状态向量
来估计，因此机器人位置估计准确性与环境路标位置估计的准确性具有一致性。机器
人位置估计误差如图 6.6所示。其中，横轴表示算法的步数，纵轴分别表示机器人在
经度和纬度上的误差。在本实验中，机器人的估计位置与 GPS 测量结果的误差较小，
因此等价的，环境路标位置的估计也很准确。室外停车场实验充分验证了算法对于复
杂室外环境的有效性。另外，实验还说明，通过合理的应用稀疏矩阵的存储方式和算
法[50]，可以进一步提高算法的实时性。
100 200 300 400 500 600 700 800 900 1000 1100-2
0
2
4
Latitu
de（
m）
100 200 300 400 500 600 700 800 900 1000 1100-6
-4
-2
0
2
Longitude（
m）
Steps
图6.6 停车场实验机器人位置误差
Fig. 6.6 Localization error of robot in Car Park experiment
上海交通大学硕士学位论文 第七章 程序设计
第 66 页
7 第七章 程序设计
EQUATION SECTION 7
7.1 Intel® Math Kernel Library
Intel® Math Kernel Library（MKL）是由 Intel 公司研发的数学核心函数库。它功
能强大、运算效率特别高。其强大的功能与 MATLAB 相当。具体来说，其主要的功
能包括：
基本线性代数运算（包括向量-向量运算、向量-矩阵运算和矩阵-矩阵运算三
种形式）；
关于稀疏矩阵和稀疏向量的基本线性代数运算；
快速 Fourier 变换、离散 Fourier 变换；
求解线性方程组；
求特征值、奇异值；
求解最小二乘问题；
求解大型稀疏线性方程组；
求解偏微分方程组；
求解非线性最优化问题；
与其强大的数学运算功能相对应，MKL 包含有上千个函数。为了快捷、方便的
调用每个功能函数，MKL 将所有的函数按照功能分成了许多子功能库。在调用一个
具体的功能函数时，只需编译与其相关的库文件，并连接相应的动态连接文件。值得
注意的是，动态连接文件的选择要合适，连接所有的动态连接文件可能导致冲突。因
此，正确的调试方法是在添加基本的动态连接文件的基础上根据调试信息增加必要的
动态连接文件。
MKL 中的函数命名具有统一的规范。虽然个别子功能库中的函数命名规则稍有
不同，但其基本语法相同。所有的函数都具有如下的命名规则：
<character code> <name> <mod> ( )
上海交通大学硕士学位论文 第七章 程序设计
第 67 页
其中，<character code>表示该函数所处理数据的类型，如表 7.1所示。<name>
表示函数中矩阵参数的类型，如表 7.2所示。<mod>表示具体的运算名称。例如，对
于两个单精度的普通矩阵相乘的函数为 sgemm，两个双精度的 Hermit 矩阵相乘的函
数为 dhemm。
表7.1 MKL 数据类型说明
Tab. 7.1 Illustration of data types in MKL
数据类型 s c d z
说明
单精度实数
（real, single
precision）
单精度复数
（complex,
single
precision）
双精度实数
（real, double
precision）
双精度复数
（complex,
double
precision）
表7.2 MKL 矩阵类型说明
Tab. 7.2 Illustration of matrix types in MKL
矩阵参数 说明
ge 一般矩阵（general matrix）
gb 一般带形矩阵（general band matrix）
sy 对称矩阵（symmetric matrix）
sp 对称矩阵，压缩存储（symmetric matrix，packed storage）
sb 对称带形矩阵（symmetric band matrix）
he Hermit 矩阵（Hermitian matrix）
hp Hermit 矩阵，压缩存储（Hermitian matrix，packed storage）
hb Hermit 带形矩阵（Hermitian band matrix）
tr 三对角矩阵（triangular matrix）
tp 三对角矩阵，压缩存储（triangular matrix，packed storage）
tb 三对角带形矩阵（triangular band matrix）
虽然 MKL 是由 Fortran 语言编写的，但仍然可以方便的通过 VC++调用其中的函
数。但在调用的过程中，需要注意参数传递规则。与 VC++函数参数传递规则不同，
上海交通大学硕士学位论文 第七章 程序设计
第 68 页
调用 Fortran 函数时参数只能通过地址传递，而不能通过变量传递。即传递参数前需
要首先为变量或数组分别存储空间，然后再对上述存储空间赋值，最后通过引用上述
变量或数组的地址来传递参数。调用 MKL 函数时，对于变量利用“&”提取地址，
而对于数组则直接根据其数组名直接提取地址。例如，假设矩阵 A和 B 是初始化后的
矩阵，且 A coln B rown ，则下面的函数将矩阵 A和 B 相乘。
(& , & , \
& , & );
dgemm A rown A coln A pdata
B rown B coln B pdata
，
， (7.1)
其中，变量 A rown 和 A coln 分别存储矩阵 A的行数和列数，B rown 和
B coln 分别存储矩阵 B 的行数和列数， A pdata 和 B pdata 分别为存储矩阵
A和 B 元素的一维数组。
7.2 软件结构
在前述直接稀疏化信息矩阵 SLAM 算法及其仿真和实验的基础上，本文利用
MKL 得到算法的 VC++实现。下面通过分析矩阵存储格式、矩阵存储规则和模块调
用关系三个问题来阐释程序的软件结构。
(1) 矩阵存储格式
在 MKL 中所有的矩阵都视为具有行数和列数属性的一维数组，为了运算方便，
程序中将矩阵视为由行数、列数和元素三个要素构成的结构体，其形式为
#define SLAM_int int
#define SLAM_double double
struct mat // struct definition of matrix
{
SLAM_int rown;
SLAM_int coln;
SLAM_double *pdata;
}
其中，rown 和 coln 分别表示矩阵的行数和列数，pdata 是指向一维数组的指针，
矩阵的元素按照 C 风格存储于上述一维数组中。针对不同规模的 SLAM 问题和不同
上海交通大学硕士学位论文 第七章 程序设计
第 69 页
的求解精度要求，可以通过改变 SLAM_int 和 SLAM_double 的宏定义来解决。
(2) 矩阵存储规则
如前所述，矩阵是通过具有行数和列数的一维数组来描述的。而一维数组存储矩
阵时有两种存储规则，分别是以行为主的存储格式和以列为主的存储格式。前者在
VC++程序中普遍采用，而后者则是 Fortran 程序的矩阵存储标准格式，如图 7.1所示。
1
3
2
1
432
1
3
2
1
432
（a）行为主的存储规则（C 风格） （b）列为主的存储规则（Fortran 风格）
图7.1 矩阵存储规则对比
Fig. 7.1 Comparison of matrix storage rules
在 MKL 中矩阵以列为主的存储规则存储于一维数组中，而在很多情况下需要从
上述一维数组中一行一行地提取元素。因此，需要建立上述两种存储格式的关系。假
设m n 的矩阵Mat 以列为主的规则存储于数组 Arr 中，则在矩阵Mat 的第 i 行、第 j 列
的元素为
( , ) ( )Mat i j Arr j m i
其中， 1, ,i m ， 1, ,j n 。
(3) 模块调用关系
虽然 MKL 的功能非常强大，但其处理的问题是一般性的数学问题。针对移动机
器人中的 SLAM 问题，在对 MKL 中部分函数进行整合和扩展的基础上，得到 SLAM
数学库（SLAM Math Library）。扩展信息滤波模块（Information Filter Module）和数
据关联模块（Data Association Module）分别调用 SLAM 数学库，如图 7.2所示。
上海交通大学硕士学位论文 第七章 程序设计
第 70 页
Intel Math Kernel library
SLAM Math Library
Information FilterModule
Data AssociationModule
图7.2 VC++程序调用关系图
Fig. 7.2 Invoking relation of VC++ programs
上海交通大学硕士学位论文 第八章 全文总结
第 71 页
8 第八章 全文总结
8.1 主要结论
稀疏性是 EIF-SLAM 算法中信息矩阵的固有特征，而 EIF-SLAM 算法中最主要
的计算量就集中在信息矩阵的运算上。信息矩阵稀疏化的程度是 SLAM 估计结果准
确性与精确性的折衷。合理的稀疏化会在保证 SLAM 一致准确估计的同时极大的提
高算法的计算效率。
本文在提出信息矩阵结构特征的基础上，充分利用信息矩阵稀疏性的特点，提出
了直接稀疏化信息矩阵 EIF-SLAM 算法。通过选择合适的稀疏化起始步数和保守系
数，合理的对信息矩阵进行稀疏化处理，在大大提高原算法运算效率的同时，通过建
立闭合路径，利用环闭合（或重定位）技术，使得估计结果与 EIF-SLAM 算法相当。
换句话说，改进后的算法在减小运算复杂度的同时得到了一致准确的估计结果。最后，
对稀疏化所引起的误差进行了分析。仿真和实验以大量的仿真和实验数据为基础，将
室内和室外环境作为研究对象，应用各类传感器和机器人，充分验证了算法的有效性。
本算法为 SLAM 应用于更大规模的环境提供了有力的算法基础。具体来说，本文的
贡献主要体现在以下三个方面：
（1）首次提出扩展信息滤波 SLAM 算法中信息矩阵的结构特征，并分别从扩展
信息滤波 SLAM 相关公式和 Bayes 网络两个角度验证了信息矩阵的结构特征。通过
对信息矩阵的结构特征的分析，并结合稀疏矩阵运算的特点，提出了直接稀疏化信息
矩阵扩展信息滤波 SLAM 算法。与其它扩展信息滤波 SLAM 算法相比，本算法运算
效率更高、实用性更强。
（2）在多种情况下的多次仿真中发现了保持 SLAM 估计结果一致的稀疏化临界
条件。当稀疏化误差 E 满足1 1E 时，SLAM 的估计结果具有一致性，即估计误
差始终在协方差范围之内；当1 1E 时估计结果处于临界稳定，即估计误差与协
方差相当；当1 1E 时 SLAM 估计结果不具有一致性，即对机器人和路标位置
的估计误差超出协方差估计范围。本文提出的算法满足1 1E ，因此文中提出的
上海交通大学硕士学位论文 第八章 全文总结
第 72 页
稀疏化算法可以保证 SLAM 估计结果具有一致性。
（3）从理论的角度，分析了直接稀疏化信息矩阵引入的稀疏化误差与 SLAM 估
计结果之间的关系。文中通过严格的数学推理，得出了状态向量均值和协方差相对误
差率的上界，为进一步的理论分析奠定了坚实的基础。
总之，本文提出的直接稀疏化信息矩阵 EIF-SLAM 算法在极大提高算法效率的
同时得到一致的 SLAM 估计结果。与其它稀疏化规范信息矩阵的算法相比，效率更
高，可操作性更强。
8.2 研究展望
在过去的十年中，SLAM 技术取得了长足的发展：科研人员们将各个领域中的成
果应用于 SLAM 问题中，产生了众多的算法；SLAM 技术的应用已经从传统的陆地
环境发展到深海探测、航空遥感、甚至是深空探索。人们在逐步发掘 SLAM 技术深
刻的理论意义和广阔的应用前景的同时，也感到了许多 SLAM 中急待解决的有趣问
题：
（1） 多元信息下的 SLAM
人们赖以生存的自然界中的信息是多元化的，人对世界的感知就包括视觉、听觉、
味觉、嗅觉和触觉。当然，如果借助各种传感器就可以得到更多事物的信息。因此，
从广义的角度看，SLAM 估计结果不应该仅仅局限于自身和路标的位置，而应该更多
的反映事物的多重本质，如路标的颜色、温度、硬度等等。这样建立的地图是具有多
维信息的立体地图，由此 SLAM 应用的领域和范围就可以大大拓展。另外，多元的
信息可以大大促进数据关联算法的效率和精确度。Nieto 提出的 Dense SLAM 就是一
个很好的尝试[9]。
（2） 复杂环境下的 SLAM
SLAM 问题中环境的复杂性包括两个方面的内容，一是自然环境具有高非结构特
性，二是环境中存在大量生命体。前者主要体现在 SLAM 算法需要对路标具有很强
的适应性，能够从各种各样的环境中，如森林、戈壁甚至是具有很少先验知识的环境
中提取合适的路标；后者需要在建立环境地图的同时，将环境中移动的生命体，如行
走的人、运动的小动物等从环境中区分出来，这样才能在真实的自然环境中自主、智
能的建立环境地图。目前，对非结构化环境 SLAM 问题，Newman 等已经也取得了
一些初步的进展[11]，而对于具有移动物体的 SLAM 问题还处于起步阶段。
上海交通大学硕士学位论文 第八章 全文总结
第 73 页
（3） SLAM 模型中的未建模动态
许多具有严格理论基础的 SLAM 算法（如扩展卡尔曼滤波 SLAM、扩展信息滤
波 SLAM 等）都需要建立精确的机器人和传感器模型。然而，通常建立的模型都具
有未建模动态，因此理论模型与实际测量值之间的误差并非 Gaussian 白噪声。通过
借助黑箱的辨识方法，对误差进行辨识，可以修正机器人运动模型，从而大大提高
SLAM 的精度。目前还没有相关的文献，作者曾利用自回归-滑动平均模型（Auto
Regression-Moving Average, ARMA）对机器人运动模型误差进行辨识，效果良好。
如今，解决 SLAM 的方法已经达到了一个相对较成熟的阶段。今后的研究将主
要集中在继续提高 SLAM 算法效率和精度的同时，将 SLAM 算法应用于更具一般性
和未知性的大范围自然环境中。
上海交通大学工程硕士学位论文 参考文献
第 74 页
参 考 文 献
[1] Smith, R, M. Self, and P. Chesseman. Estimating uncertain spatial relationships in robotics. in
Proceedings of Conference on Uncertainty in Artificial Intelligence. 1988.
[2] Hugh, D.W. and B. Tim, Simultaneous localization and mapping: partⅠ. IEEE Robotics and
Automation Magazine, 2006. 13(2): p. 99-110.
[3] Choi, Y.H. and S.Y. Oh, Grid-based visual SLAM in complex environments. Journal of Intelligent
and Robotic Systems: Theory and Applications, 2007. 50(3): p. 241-255.
[4] Stachniss, C., D. Hahnel, W. Burgard et al., On actively closing loops in grid-based FastSLAM.
Advanced Robotics, 2005. 19(10 SPEC. ISS.): p. 1059-1079.
[5] Choset, H. and K. Nagatani, Topological simultaneous localization and mapping (SLAM): Toward
exact localization without explicit localization. IEEE Transactions on Robotics and Automation,
2001. 17(2): p. 125-137.
[6] Kouzoubov, K. and D. Austin. Hybrid topological/metric approach to SLAM. in Proceedings - IEEE
International Conference on Robotics and Automation. 2004.
[7] Blanco, J.L., J.A. Fernandez-Madrigal, and J. Gonzalez. A new approach for large-scale localization
and mapping: Hybrid metric-topological SLAM. in Proceedings - IEEE International Conference on
Robotics and Automation. 2007.
[8] Ellekilde, L.P., S. Huang, J.V. Miro et al., Dense 3D map construction for indoor search and rescue.
Journal of Field Robotics, 2007. 24(1-2): p. 71-89.
[9] Nieto, J., J. Guivant, and E. Nebot, DenseSLAM: Simultaneous localization and dense mapping.
International Journal of Robotics Research, 2006. 25(8): p. 711-744.
[10] Castellanos, J.A., J. Neira, and J.D. Tardos, Map Building and SLAM Algorithms. Autonomous
Mobile Robots: Sensing, Control, Decision-Making and Applications, 2006.
[11] Newman, P.M., EKF Based Navigation and SLAM. 2006, Oxford University. p. 46-55.
[12] Folkesson, J. and H. Christensen. Outdoor exploration and SLAM using a compressed filter. 2003.
Taipei, Taiwan: Institute of Electrical and Electronics Engineers Inc.
[13] Li, M.H., B.R. Hong, and R.H. Luo, Improved Rao-Blackwellized particle filters for mobile robot
simultaneous localization and mapping. Jilin Daxue Xuebao (Gongxueban)/Journal of Jilin
University (Engineering and Technology Edition), 2007. 37(2): p. 401-406.
[14] Dong, J.F., W.S. Wijesoma, and A.P. Shacklock. An efficient rao-blackwellized genetic algorithmic
filter for SLAM. in Proceedings - IEEE International Conference on Robotics and Automation.
2007.
[15] Kim, C., R. Sakthivel, and W.K. Chung. Unscented FastSLAM: A robust algorithm for the
simultaneous localization and mapping problem. in Proceedings - IEEE International Conference on
上海交通大学工程硕士学位论文 参考文献
第 75 页
Robotics and Automation. 2007.
[16] Eliazar, A.I. and R. Parr. DP-SLAM 2.0. in Proceedings - IEEE International Conference on
Robotics and Automation. 2004.
[17] Wang, Z., S. Huang, and G. Dissanayake, D-SLAM: A decoupled solution to simultaneous
localization and mapping. International Journal of Robotics Research, 2007. 26(2): p. 187-204.
[18] Paskin, M., Thin junction tree filters for simultaneous localization and mapping. 2002, University of
California, Berkeley.
[19] Bosse, M. and J. Roberts. Histogram matching and global initialization for laser-only SLAM in
large unstructured environments. in Proceedings - IEEE International Conference on Robotics and
Automation. 2007.
[20] Nuchter, A., K. Lingemann, J. Hertzberg et al., 6D SLAM - 3D mapping outdoor environments.
Journal of Field Robotics, 2007. 24(8-9): p. 699-722.
[21] Cole, D.M. and P.M. Newman. Using laser range data for 3D SLAM in outdoor environments. in
Proceedings - IEEE International Conference on Robotics and Automation. 2006.
[22] Walter, M.R., R.M. Eustice, and J.J. Leonard, Exactly sparse extended information filters for
feature-based SLAM. The International Journal of Robotics Research, 2007. 26(4): p. 335-359.
[23] Eustice, R.M., M.R. Walter, and J.J. Leonard, Sparse extended information filters: insight into
sparsification, in IEEE/RSJ International Conference on Intelligent Robots and Systems. 2005. p.
3281-3288.
[24] Thrun, S., D. Koller, Z. Ghahmarani et al, A. Ng, Simultaneous localization and mapping with
sparse extended information filters: theory and initial results. 2002.
[25] Thrun, S., Y. Liu, D. Koller et al, Simultaneous localization and mapping with sparse extended
information filters. International Journal of Robotics Research, 2004. 23(7-8): p. 693-716.
[26] Walter, M.R., R.M. Eustice, and J. Leonard, A provably consistent method for imposing sparsity in
feature-based SLAM information filters, in Springer Tracts in Advanced Robotics. 2007.
[27] Walter, M.R., R.M. Eustice, and J.J. Leonard, Exactly sparse extended information filters for
feature-based SLAM. International Journal of Robotics Research, 2007. 26(4): p. 335-359.
[28] Liu, Y. and S.Thrun. Results for outdoor-SLAM using sparse extended information filters. in
Proceedings of IEEE International Conference Robotics Automation. 2003.
[29] Frese, U. and G. Hirzinger. Simultaneous localization and mapping - a discussion. in Proceedings of
the IJCAI Workshop on Reasoning with Uncertainty in Robotics. 2001.
[30] Speed, T.P. and H.T. Kiiveri, Gaussian Markov distributions over finite graphs. Annals of Statistics,
1986. 14(1): p. 138-150.
[31] Frese, U. Treemap: An O(log n) algorithm for simultaneous localization and mapping. in Lecture
Notes in Artificial Intelligence (Subseries of Lecture Notes in Computer Science). 2005.
[32] Frese, U., Treemap: An O(log n) algorithm for indoor simultaneous localization and mapping.
Autonomous Robots, 2006. 21(2): p. 103-122.
[33] Frese, U. Efficient 6-DOF SLAM with treemap as a generic backend. in Proceedings - IEEE
International Conference on Robotics and Automation. 2007.
[34] Frese, U., Treemap: An O(logn) algorithm for simultaneous localization and mapping, in Spatial
上海交通大学工程硕士学位论文 参考文献
第 76 页
Cognition IV. 2005, Springer Verlag. p. 455-476.
[35] Duckett, T., S. Marsland, and J. Shapiro. Learning globally consistent maps by relaxation. in
Proceedings of the IEEE International Coference on Robotics and Automation (ICRA). 2000.
[36] Folkesson, J. and H. Christensen. Graphical SLAM - a self-correcting map. in Proceedings of the
IEEE International Conference on Robotics and Automation (ICRA). 2004.
[37] Frese, U., P. Larsson, and T. Duckett, A multilevel relaxation algorithm for simultaneous localization
and mapping. IEEE Transactions on Robotics, 2005. 21(2): p. 196-207.
[38] Dellaert, F. Square root SAM. in Proceedings of Robotics: Science and Systems (RSS). 2005.
[39] Thrun, S., W. Burgard, and D. Fox, Probabilistic Robotics. 2005: MIT Press.
[40] Newman, P.M., D.M. Cole, and K. Ho, Outdoor SLAM using visual appearance and laser ranging,
in IEEE International Conference on Robotics and Automation. 2006.
[41] Eustice, R.M., H. Singh, and J.J. Leonard, Exactly sparse delayed-state filters, in IEEE International
Conference on Robotics and Automation. 2005. p. 2428-2435.
[42] Deans, M. and M. Hebert, Experimental comparison of techniques for localization and mapping
using a bearing-only sensor., in International Symposium on Experimental Robotics. 2000.
[43] Huang, S., Z. Wang, and G. Dissanayake. Mapping large scale environments using relative position
information among landmarks. in Proceedings - IEEE International Conference on Robotics and
Automation. 2006.
[44] Wang, Z., S. Huang, and G. Dissanayake, Implementation issues and experimental evaluation of
D-SLAM, in Springer Tracts in Advanced Robotics, P. Corke and Sukkariah, Editors. 2006. p.
155-166.
[45] Wang, Z., S. Huang, and G. Dissanayake, D-SLAM: Decoupled localization and mapping for
autonomous robots, in Springer Tracts in Advanced Robotics. 2007.
[46] Eustice, R.M., S. Hanumant, J.J. Leonard et al, Visually navigating the RMS Titanic with SLAM
information filters. Robotics: Science and Systems, 2005. 25(12): p. 1223-1242.
[47] Eustice, R.M., S. Hanumant, and J.J. Leonard, Exactly sparse delayed-state filters for view-based
SLAM. IEEE Transactions on Robotics, 2006. 22(6): p. 1100-1114.
[48] Cooper, A.J., A Comparison of Data Association Techniques for Simultaneous Localization and
Mapping, in Aerospace Engineering and Mechanics. 2003, University of Minnesota. p. 45-63.
[49] Neira, J. and J.D. Tardos, Data association in stochastic mapping using the joint compatibility test.
IEEE Transactions on Robotics and Automation, 2001. 17(6): p. 890-897.
[50] Davis, T.A., Direct Methods for sparse linear systems. 2006: Society for Industrial and Applied
Mathematics.
[51] Roger, A.H. and R.J. Charles, Matrix Analysis. 1986, Cambridge: Cambridge University Press.
[52] Car park dataset, http://www.acfr.usyd.edu.au/homepages/academic/enebot/experimental_data
/car_park.zip.
上海交通大学工程硕士学位论文 附录 1
第 77 页
附录 1 MATLAB 文件说明
文件名 说明
if_sim.m 扩展信息滤波 SLAM 主程序
configfile.m 仿真参数设置
frontend.fig 仿真环境设置 GUI 背景图
frontend.m 仿真环境设置 GUI 程序
example_webmap.mat 环境地图
information_doprojection.m 扩展信息滤波移动步骤
information_addfeature.m 扩展信息滤波增加路标步骤
information_doupdate.m 扩展信息滤波更新步骤
vehicle_model.m 机器人运动模型
observe_model.m 传感器观测模型
compute_control.m 计算控制量（二轮机器人）
compute_steering.m 计算前轮方向角（四轮机器人）
get_observations.m 获得传感器观测值
get_obs_jacs.m 计算观测模型的 Jacobian 式
do_observation_model.m 由机器人和路标位置反向计算传感器观测量
add_control_noise.m 加入控制噪声
add_observation_noise.m 加入观测噪声
data_associate.m 数据关联（路标配对未知）
data_associate_known.m 数据关联（路标配对已知）
distance_mahalanobis.m 计算 mahalanobis 距离
pi_to_pi.m 将弧度转换到[-Π,Π]
transform_to_global.m 将坐标由局部坐标系转换到全局坐标系
rhomatrix.m 计算协方差矩阵的相关稀疏矩阵
plot_distribution.m 绘制规范化信息矩阵元素分布图
plot_time.m 绘制算法效率图
readm.txt 程序说明
上海交通大学工程硕士学位论文 附录 2
第 78 页
附录 2 Visual C++程序说明
矩
阵
运
算
文件名 说明
mat_int 矩阵初始化
mat_pre 矩阵显示
mat_tran 矩阵转置
mat_add 矩阵相加
mat_sub 矩阵相减
mat_mul 矩阵相乘
mat_inv 矩阵求逆
mat_sol 线性方程组求解
mat_sca 矩阵与常数相乘
mat_sel 从矩阵中选择子矩阵块
mat_ext_right 向右扩展矩阵
mat_ext_down 向下扩展矩阵
mat_ass 矩阵赋值
cre_eye 创建单位矩阵
cre_zeros 创建零矩阵
辅助
函数
pi_to_pi 将弧度转换到[-Π,Π]
get_obs 获得传感器观测值
扩展
信息
滤波
info_do_pro 扩展信息滤波移动步骤
info_add_feat 扩展信息滤波增加路标步骤
info_do_upda 扩展信息滤波更新步骤
数据
关联
data_ass 数据关联
obs_mod 观测模型
comp_ass 计算 Mahalanobis 距离
上海交通大学工程硕士学位论文 致谢
第 79 页
致 谢
在硕士学位论文定稿的日子，我要首先感谢我的导师陈卫东教
授：在论文选题上，陈老师为我选择了机器人同时定位和地图创建
这一既富挑战又十分有趣的课题；在具体艰辛的科研工作中，陈老
师的热情鼓励和悉心指导帮助我克服了一个又一个难题；在点滴生
活中，陈老师的言传身教让我深刻懂得了“做学问、先做人”的道
理。
在此，我还想感谢汪小帆教授、李柠副研究员、卢俊国副教授、
王景川老师和李志军老师。从你们的身上，我不仅学到了许多科学
研究的方法和技巧，也得到了很多宝贵的实际经验。我还想感谢张
飞、徐宁、许俊勇和项彬彬，我们从相识到相知、从相知到熟悉，
在实验室中我们互相学习、共同进步，结下了深厚的友谊。感谢自
主机器人研究组的每位一成员，从进入实验室的第一天起我们一起
生活学习，亲如一家。我会永远珍藏这份美好的回忆！
感谢父亲、母亲为我所做的一切，虽然我不在你们身边，但我
一直都牵挂着你们。我唯有通过自己加倍努力来让你们为我骄傲！
最后，我要感谢所有支持和关心过我的朋友们！
上海交通大学工程硕士学位论文 攻读硕士学位期间录用的论文和参与的项目
第 80 页
攻读硕士学位期间录用的论文
[1] 董海巍, 陈卫东, 基于稀疏化的快速扩展信息滤波-SLAM 算法研究. 机器人（EI）. （已录用）
[2] 董海巍, 陈卫东, 直接稀疏化信息矩阵 EIF-SLAM 算法. 自动化学报（EI）. （在审）
攻读硕士学位期间参与的项目
1、 国家自然科学基金项目：未知环境中自主移动机器人的同步自定位与地图创建研究
(60475032)，参与时间 2006年 9月-2007年 12 月
2、 上海市曙光计划项目：基于视觉的移动机器人自定位与地图创建，参与时间2006年9月-2006
年 12月
3、 国家教育部项目：教育部新世纪优秀人才支持计划，参与时间 2007年 3月-2008年 2月
4、 国家 863 计划重点项目：助老/助残机器人概念样机研究与开发（2006AA040203），参与时
间 2007年 10月-2008年 2月