几种SLAM算法的简要介绍的南京理工大学

( k k
x
k(
1 ) k(
) x
T
k( k

T
1)
（2）
后验估计:
x( k k
)

x
)
) x k k(

)
（3）
已知条件：初始状态估计及估计误差协方差分别为： 时间更新: x( k k 1) Ax( k 1 k 1) Bu( k 1)
x (11) 、 P(11)
（4） 预测状态 （5）预测协方差
P( k k 1) AP( k 1 k 1) AT Q
状态更新
K( k ) P( k k 1) H T HP( k k 1) H T R


1
（6）Kalma增益 （7）修正状态 （8）修正协方差
x( k k ) x( k k 1) K( k ) z( k ) H x( k k 1)
/(n ) W im (n ) 1 / 2
i 0 i 0
（18）
/(n ) 1 2 W ic (n ) 1 / 2
i 0 i 0
（19）
ˆk 1 和 Pk 1 来计算Sigma 点集 xi ,k 1 由时刻k-1的 x ，通过非线性函 （i 0,1..., L） ˆk / k 1 及误差协方差阵 数 f k 1 (· ) qk 1 传播为 xi ,k / k 1 ，由 xi ,k / k 1 可得状态向量预测值 x
P( k k ) I K( k ) H P( k k 1)


卡尔曼滤波适用范围： 准确已知的线性系统和量测方程；系统噪声和量测噪 声为互不相关的零均值高斯白噪声。
南京理工大学《机器人控制理论与技术》课程论文
Initial estimates for
Time Update(“Predict”)
ˆk 1 x0,k 1 x
（15） （16）
ˆk 1 ( (n ) P xi ,k 1 x i , k 1 )i ˆk 1 ( (n ) P xi n,k 1 x i , k 1 )i
（17）
对应的 i 的一阶二阶权系数：
南京理工大学《机器人控制理论与技术》课程论文
(x i ,k 1 ) q k 1
W ic(x i k i
0
T x Q k 1 ˆk / k 1 )(x i ,k / k 1 x ˆk / k 1 )
（22）
ˆk 1 和 Pk 1 按前面的采样策略来计算Sigma 点集 xi ,k 1 同理， 利用 x ， （i 0,1..., L）
ˆk / k 1 及自 ) rk 传播为 i ,k / k 1 ，由 i ,k / k 1 可得输出预测值 z 通过非线性量测函数 hk (·
（3）Update the error covariance
图 1 卡尔曼滤波代码实现流程图
2.1 扩展卡尔曼滤波方法（EKF） 由于卡尔曼滤波算法具有对系统线性的要求以及对系统噪声须符合高斯分 布的依赖性， 这就在某些程度上限制了卡尔曼滤波算法的应用，所以后人在卡尔 曼滤波算法的基础上提出了扩展卡尔曼滤波方法 （Extended Kalman Filter， EKF） 。 EKF 算法的实质是用泰勒展开的一阶近似来表示出非线性系统方程，从而解决 非线性系统模型的估计问题。它的基本思想是：在滤波值附近，应用泰勒展开算 法将非线性系统展开， 对于二阶以上的高阶项全部都省去，从而原系统就变成了 一个线性系统，再利用标准 Kalman 滤波算法的思想对系统线性化模型进行滤 波。
即： 将非线性系统线性化。
（13）状态方程 （14）观测方程
xk 1 =f ( xk , uk )+k wk xk 1 =f ( xk , uk , wk ) zk =g ( xk , uk ) vk zk =g ( xk , uk , k )
图 2 扩展卡尔曼滤波总体流程图
南京理工大学《机器人控制理论与技术》课程论文
二. SLAM 表示方法： 目前各国研究者已经提出了多种表示法，大致可分为三类：栅格表示法、几 何信息表示法和拓扑图表示法，三种方法都有自己的优缺点。 栅格地图表示法即： 将整个环境分为若干相同大小的栅格，对于每个栅格各 指出其中是否存在障碍物。它的优点：创建和维护容易，尽量的保留了整个环境 的各种信息，同时借助于该地图，可以方便地进行自定位和路径规划。 缺点：
南京理工大学《Байду номын сангаас器人控制理论与技术》课程论文
关于 SLAM 技术的综述报告
—几种 SLAM 算法的简要介绍
学号：************，姓名：**
摘要：移动机器人同步地图创建与定位(SLAM)是移动机器人的核心研究课 题．本文对SLAM的研究进展和关键技术进行了综述。基于卡尔曼滤波器的实现方
法进行了分析比较。总结分析SLAM问题实现中卡尔曼滤波及其拓展算法优缺点，
当栅格数量增大时(在大规模环境或对环境划分比较详细时)，对地图的维护行为 将变得困难，同时定位过程中搜索空间很大，如果没有较好的简化算法，实现实 时应用比较困难。 几何信息地图表示法是指： 机器人从收集对环境的感知信息，从中提取更为 抽象的几何特征，如线段或曲线，使用这些几何信息描述环境。这种方法更加紧 凑， 并且便于位置估计和目标识别。几何信息表示法利用卡尔曼滤波在局部区域 内可获得较高精度，且计算量小，但在广域环境中却难以维持精确的坐标信息。 缺点是： 几何信息的提取需要对感知信息作额外处理，需要一定数量的感知数据 才能得到结果。 拓扑地图抽象度高， 特别是当环境大而简单时，这种方法将环境表示为一张 拓扑意义中的图，图中的节点对应于环境中的一个特征状态、地点。如果节点间 存在直接连接的路径则相当于图中连接节点的弧。优点是：(1)有利于进一步的 路径和任务规划，(2)存储和搜索空间都比较小，计算效率高，(3)可以使用很多 现有成熟、高效的搜索和推理算法。缺点是：对拓扑图的使用是建立在对拓扑节 点的识别匹配基础上的。 目前 SLAM 实现方法可分为两类：一是基于概率模型的方法，二是非概率 模型方法。概率模型方法包括许多基于卡尔曼滤波的 SLAM 方法如扩展卡尔曼 滤波方法、 无迹卡尔曼滤波法、 FastSLAM 等。 而非概率模型方法则有 SM-SLAM、 扫描匹配、数据融合(data association)、基于模糊逻辑等。本文将对基于卡尔曼滤 波的 SLAM 实现方法做简要介绍： 2.1 卡尔曼滤波算法 20 世纪 60 年代，匈牙利数学家 R.E.Kalman 在他的博士论文《A New Approach to Linear Filter and Prediction Problems》 中首次提出了卡尔曼滤波算法。
Pk / k 1 。
1,...,L x i ,k / k 1 fk 1(x i ,k 1 ) q k 1 i 0,
（20） （21）
x ˆk / k 1
Pk / k 1
L
i 0
Wim xi ,k / k 1
, / k 1
L
W im fk i
0
L
1
g ( xk , uk ) xk
ˆk xk x
则可得到：
南京理工大学《机器人控制理论与技术》课程论文
ˆ x [ f (x ˆ x ] w ˆk , uk ) xk 1 k k k k k k ˆ x [g(x ˆ x ]v ˆk , uk ) H zk H k k k k k
更好的探讨了其今后发展方向。 关键词：SLAM 问题；卡尔曼滤波；移动机器人。
一. 引言 SLAM 是 simultaneous localization and mapping 的 全 称 , 也 称 为 CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。 SLAM 的研究起源于 1986 年在美国加利福利亚州旧金山举行的 IEEE ICPA 会 议。 议期间，众多学者对地图构建的一致性问题进行了广泛讨论，并且达成一 个共识：一致的、概率的地图构建方法是机器人领域的一个关键问题，其算法和 理论值得重点研究[1]。 在 SLAM 中，机器人能在未知环境中从一个未知位置开始移动，在移动过 程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图。 机器人根 据在运动过程中获取的传感器数据，利用相关地图模型和集成算法，生成关于环 境的地图表示。 机器人地图可以分为两大类：几何地图和拓扑地图。几何地图又可分为栅格 地图和特征地图。 栅格地图将空间环境地图划分为若干规则方格。每一个方格设 定概率值表示该区域是否被障碍物占用， 特征地图由一系列包含位置信息的特征 组成， 拓扑地图由环境中特征位置或区域组成的节点及其连接关系组成。 近年来， SLAM 的研究取得了很大的进展,并已应用于各种不同的环境，如：室内环境、 水下、室外环境。 本文对 SLAM 的研究进展和关键技术进行了综述。基于卡尔曼滤波等实现 方法进行了分析比较。总结分析 SLAM 实现中卡尔曼滤波等方法实现过程中存 在的难题，探讨了今后的发展方向。
x( k ) Ax( k 1) Bu( k 1) n( k 1) z( k ) Hx( k ) v( k )
系统及观测噪声协方差: 先验估计:
n ~ N (0, Q) v ~ N (0, R)
（1）
x( k
k 1)
P( k k 1 ) E x( k ) x P( k k ) E x( k ) x k(k
xk 1 f( x k, u k ) k wk zk g( x k, u k)
ˆk , uk ) f ( xk , uk ) f ( x
（9）状态方程 （10）观测方程
v k
f ( xk , uk ) xk
ˆk xk x ˆk xk x
ˆk ) （11） ( xk x
南京理工大学《机器人控制理论与技术》课程论文
卡尔曼滤波是一种用递归来解决离散数据线性滤波的方法， 它克服了维纳滤波理 论的局限性，提供了一种高效可靠的“预测—实测—修正”的计算方法来估计整 个过程的状态，使估计的均方根误差最小。卡尔曼滤波法是一种最优估计，它可 以处理多维和非平稳的随机信号，逐渐成为基于概率的 SLAM 算法中主流的方 法，并在各个领域得到了广泛应用。 以下为卡尔曼滤波模型简介： 动态系统及其测量模型：
2.3 无迹卡尔曼滤波法（UKF） 但是EKF将非线性系统线性化时会带来计算精度下降的问题， 而且计算雅克 比矩阵也是比较困难的，甚至难以实现。针对 EKF 的不足，牛津大学的 Julier 等人提出了一种基于 UT 变换的新型卡尔曼滤波器，也就是无迹卡尔曼滤波法 （Unscented Kalman Filter，UKF） 。UKF算法不需要计算 Jacobian 矩阵，利于计 算机编程，可以得到高精度的机器人位姿。 UKF法首先要构造Sigma 散点集, 设状态向量为n 维, x ˆk 1 为时刻k-1 的状 态向量估计值, Pk 1 为该时刻状态向量的协方差矩阵, 2n+1维的Sigma 点集可以 表示为:
ˆk , uk ) g ( xk , uk ) g ( x
g ( xk , uk ) xk
ˆk ) ( xk x
（12）
式（11） 、 （12）分别为f(x),g(x)的多元泰勒展开并略去高次项。
ˆ k f ( xk , uk ) 令： xk
ˆk xk x
ˆ ，H k
(1)Project the state ahead
Measurement Update (“Correct”)
（1）Computer the Kalma gain
（2）Update estimate whit measurement
(2)Project the error covariance ahead