SLAM问题的解决方案

最新推荐文章于 2022-10-21 07:00:24 发布

_IRONMAN_

最新推荐文章于 2022-10-21 07:00:24 发布

阅读量1.6k

点赞数 2

分类专栏：水下机器人文章标签： slam SLAM的实现方法 EKF-SLAM FastSLAM

本文链接：https://blog.csdn.net/hj199404182515/article/details/112065169

版权

概率SLAM问题的解决方案是设计找到一个合适的对观察模型(2)和运动模型(3)的陈述，使得能够高效连续的计算式(4)和(5)的先验和后验概率分布。到目前为止，最常见的表述是在状态空间模型中的一种形式，其带有加性高斯噪声，进而导致了采用扩展卡尔曼滤波器(EKF)来解决SLAM问题。另外一种重要的表述是将(3)中的机器人运动模型的描述成一系列样本点，这些样本点更具有更一般性，是非高斯概率分布。这导致了使用Rao-Blackwellized粒子滤波器，或者叫FastSLAM算法，来解决SLAM问题。当然EKF-SLAM和FastSLAM都是两种很重要的解决方法，更新的方法，也具有潜力的，也已经被提了出来，包括使用信息状态形式的[S. Thrun, Y. Liu, D. Koller, A. Ng, and H. Durrant-Whyte, “Simultaneous localisation and mapping with sparse extended information filters,” Int. J. Robot. Res., vol. 23, no. 7–8, pp. 693–716, 2004]。这些将在本教材的Part II进一步的讨论。

EKF-SLAM

EKF-SLAM方法的基础是描述机器人运动成如下形式

$P(\mathbf{x}_{k}|\mathbf{x}_{k-1},\mathbf{u}_{k})\Leftrightarrow \mathbf{x}_{k}=\mathbf{f}(\mathbf{x}_{k-1},\mathbf{u}_{k})+\mathbf{w}_{k}$ (6)

这里 $\mathbf{f}(\cdot )$ 代表机器人的运动学模型， $\mathbf{w}_{k}$ 具有0均值方差为 $\mathbf{Q}_{k}$ 的加性扰动向量，其为相互不相关的高斯噪声。观测模型表示如下：

$P(\mathbf{z}_{k}|\mathbf{x}_{k},\mathbf{m})\Leftrightarrow \mathbf{z}_{k}=\mathbf{h}(\mathbf{x}_{k},\mathbf{m})+\mathbf{v}_{k}$ (7)

这里 $\mathbf{h}(\cdot )$ 表示观测函数， $\mathbf{v}_{k}$ 具有0均值方差为 $\mathbf{R}_{k}$ 的加性观测噪声向量，其为相互不相关的高斯噪声。

有了这些定义，标准的EKF模型[G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark, and M. Csobra, “A solution to the simultaneous localisation and mapping (SLAM) problem,” IEEE Trans. Robot. Automat., vol. 17, no. 3, pp. 229–241, 2001][P.S. Maybeck, Stochastic Models, Estimaton and Control, vol. I. New York: Academic 1979.]可以被用于计算下面的均值

$\begin{bmatrix} \mathbf{\hat{x}}_{k|k}\\ \mathbf{\hat{m}}_{k} \end{bmatrix}=E\begin{bmatrix} \begin{matrix} \mathbf{x}_{k}\\ \mathbf{m} \end{matrix}|\mathbf{Z}_{0:k} \end{bmatrix}$

协方差，其为 $P(\mathbf{x}_{k},\mathbf{m}|\mathbf{Z}_{0:k},\mathbf{U}_{0:k},\mathbf{x}_{0})$ 的联合后验概率分布

$\mathbf{P}_{k|k}=\begin{bmatrix} \mathbf{P}_{xx} &\mathbf{P}_{xm} \\ \mathbf{P}_{xm}^{T}&\mathbf{P}_{mm} \end{bmatrix}_{k|k}=\begin{bmatrix} \begin{pmatrix} \mathbf{x}_{k}-\mathbf{\hat{x}}_{k}\\ \mathbf{m}-\mathbf{\hat{m}}_{k} \end{pmatrix}\begin{pmatrix} \mathbf{x}_{k}-\mathbf{\hat{x}}_{k}\\ \mathbf{m}-\mathbf{\hat{m}}_{k} \end{pmatrix}^{T}|\mathbf{Z}_{0:k} \end{bmatrix}$

时间更新

$\mathbf{\hat{x}}_{k|k-1}=\mathbf{f}(\mathbf{\hat{x}}_{k-1|k-1},\mathbf{u}_{k})$ (8)

$\mathbf{P}_{xx,k|k-1}= \bigtriangledown \mathbf{f}\mathbf{P}_{xx,k-1|k-1}\bigtriangledown \mathbf{f}^{T}+\mathbf{Q}_{k}$ (9)

这里 $\bigtriangledown \mathbf{f}$ 是 $\mathbf{f}$ 函数在 $\mathbf{\hat{x}}_{k-1|k-1}$ 的雅可比矩阵。一般来说静态的路标是不需要执行时间更新的。（但是，对于运动的路标是必须进行时间更新的[C.C. Wang, C. Thorpe, and S. Thrun, “On-line simultaneous localisation and mapping with detection and tracking of moving objects,” in

最低0.47元/天解锁文章

_IRONMAN_

关注

2
点赞
踩
4

收藏

觉得还不错? 一键收藏
打赏
0
评论
SLAM问题的解决方案

概率SLAM问题的解决方案是设计找到一个合适的对观察模型(2)和运动模型(3)的陈述，使得能够高效连续的计算式(4)和(5)的先验和后验概率分布。到目前为止，最常见的表述是在状态空间模型中的一种形式，其带有加性高斯噪声，进而导致了采用扩展卡尔曼滤波器(EKF)来解决SLAM问题。另外一种重要的表述是将(3)中的机器人运动模型的描述成一系列样本点，这些样本点更具有更一般性，是非高斯概率分布。这导致了使用Rao-Blackwellized粒子滤波器，或者叫FastSLAM算法，来解决SLAM问题...
复制链接

扫一扫