SLAM问题的处理方法主要分为滤波和图优化两类。
一、滤波方法（定位）
从贝叶斯理论的观点看，机器人定位问题就是要求已知历史的动作
u
k
u_{k}
uk，感知
z
k
z_{k}
zk，利用其两者确定机器人当前的状态
x
k
x_{k}
xk。
据此定义贝叶斯模型：
b
e
l
(
x
t
)
=
p
(
x
t
∣
z
1
:
t
,
u
1
:
t
)
bel(x_{t}) = p(x_t|z_{1:t}, u_{1:t})
bel(xt)=p(xt∣z1:t,u1:t)
利用贝叶斯法则可得：
=
η
p
(
z
t
∣
x
t
,
z
1
:
t
−
1
,
u
1
:
t
)
p
(
x
t
∣
z
1
:
t
−
1
,
u
1
:
t
)
= \eta p(z_t|x_t,z_{1:t-1},u_{1:t})p(x_t|z_{1:t-1},u_{1:t})
=ηp(zt∣xt,z1:t−1,u1:t)p(xt∣z1:t−1,u1:t)
其中
η
=
1
p
(
z
t
∣
z
1
:
t
−
1
,
u
1
:
t
)
\eta = \frac{1}{p(z_t|z_{1:t-1},u_{1:t})}
η=p(zt∣z1:t−1,u1:t)1为归一化常数。
根据Markov假设和全概率公式有：
=
η
p
(
z
t
∣
x
t
)
∫
p
(
x
t
∣
x
t
−
1
,
z
1
:
t
−
1
,
u
1
:
t
)
p
(
x
t
−
1
∣
z
1
:
t
−
1
,
u
1
:
t
)
d
x
t
−
1
=
η
p
(
z
t
∣
x
t
)
∫
p
(
x
t
∣
x
t
−
1
,
u
t
)
p
(
x
t
−
1
∣
z
1
:
t
−
1
,
u
1
:
t
−
1
)
d
x
t
−
1
=
η
p
(
z
t
∣
x
t
)
∫
p
(
x
t
∣
x
t
−
1
,
u
t
)
b
e
l
(
x
t
−
1
)
d
x
t
−
1
= \eta p(z_t|x_t) \int p(x_t|x_{t-1},z_{1:t-1},u_{1:t})p(x_{t-1}|z_{1:t-1},u_{1:t})dx_{t-1} \\ = \eta p(z_t|x_t) \int p(x_t|x_{t-1},u_t)p(x_{t-1}|z_{1:t-1},u_{1:t-1})dx_{t-1} \\ = \eta p(z_t|x_t) \int p(x_t|x_{t-1},u_t)bel(x_{t-1})dx_{t-1}
=ηp(zt∣xt)∫p(xt∣xt−1,z1:t−1,u1:t)p(xt−1∣z1:t−1,u1:t)dxt−1=ηp(zt∣xt)∫p(xt∣xt−1,ut)p(xt−1∣z1:t−1,u1:t−1)dxt−1=ηp(zt∣xt)∫p(xt∣xt−1,ut)bel(xt−1)dxt−1
据此可得出运动模型与观测模型如下：
运动模型
运动模型解决的问题是：
如何在当前位置
x
t
−
1
x_{t-1}
xt−1作出动作
u
t
u_t
ut时，给出机器人所处位置的概率分布。
b
e
l
ˉ
(
x
t
)
=
η
∫
p
(
x
t
∣
x
t
−
1
,
u
t
)
b
e
l
(
x
t
−
1
)
d
x
t
−
1
\bar{bel}(x_{t}) = \eta \int p(x_t|x_{t-1},u_t)bel(x_{t-1})dx_{t-1}
belˉ(xt)=η∫p(xt∣xt−1,ut)bel(xt−1)dxt−1
观测模型
观测模型解决的问题是：
如何在感知到
z
z
z信息的情况下，更新当前的状态
x
x
x。
b
e
l
(
x
t
)
=
p
(
z
t
∣
x
t
)
b
e
l
ˉ
(
x
t
)
bel(x_{t}) = p(z_t|x_t)\bar{bel}(x_{t})
bel(xt)=p(zt∣xt)belˉ(xt)
为求解以上的机器人定位问题，有两种主要的方法：
（1）粒子滤波（离散） （2）卡尔曼滤波（连续）
1、粒子滤波(Monte Carlo Localization)
b
e
l
(
x
t
)
=
η
∫
p
(
z
t
∣
x
t
)
p
(
x
t
∣
x
t
−
1
,
u
t
)
b
e
l
(
x
t
−
1
)
d
x
t
−
1
bel(x_{t}) = \eta \int p(z_t|x_t)p(x_t|x_{t-1},u_t)bel(x_{t-1})dx_{t-1}
bel(xt)=η∫p(zt∣xt)p(xt∣xt−1,ut)bel(xt−1)dxt−1
上面的推导过程中需要用到积分，对于一般的非线性，非高斯系统，很难得到后验概率的解析解。为了解决这个问题，需要引进蒙特卡洛采样，此方法也称为粒子滤波。
知识点：
- 因为
b
e
l
(
x
t
−
1
)
bel(x_{t-1})
bel(xt−1)难以采样，故引入重要性采样的方法去估计
b
e
l
(
x
t
)
bel(x_{t})
bel(xt)。
b e l ( x t ) = η ∫ p ( z t ∣ x t ) p ( x t ∣ x t − 1 , u t ) q ( x t − 1 ) b e l ( x t − 1 ) q ( x t − 1 ) d x t − 1 = η E q [ p ( z t ∣ x t ) p ( x t ∣ x t − 1 , u t ) b e l ( x t − 1 ) q ( x t − 1 ) ] bel(x_{t}) = \eta \int p(z_t|x_t)p(x_t|x_{t-1},u_t)q(x_{t-1})\frac{bel(x_{t-1})}{q(x_{t-1})}dx_{t-1} \\ = \eta \mathbb{E_q} [p(z_t|x_t)p(x_t|x_{t-1},u_t)\frac{bel(x_{t-1})}{q(x_{t-1})}] bel(xt)=η∫p(zt∣xt)p(xt∣xt−1,ut)q(xt−1)q(xt−1)bel(xt−1)dxt−1=ηEq[p(zt∣xt)p(xt∣xt−1,ut)q(xt−1)bel(xt−1)]参考：Particle Filter Tutorial 粒子滤波：从推导到应用（二）
2、卡尔曼滤波（连续）
假设
Move:
p
(
x
k
∣
x
k
−
1
,
u
k
)
p(x_k|x_{k-1},u_{k})
p(xk∣xk−1,uk) ~
N
(
A
k
x
k
−
1
+
B
k
u
k
,
R
t
)
\mathcal N(A_{k}x_{k-1}+B_{k}u_{k},R_{t})
N(Akxk−1+Bkuk,Rt)，
p
(
x
k
−
1
)
p(x_{k-1})
p(xk−1) ~
N
(
μ
k
−
1
,
Σ
k
−
1
)
\mathcal N(\mu_{k-1},\Sigma_{k-1})
N(μk−1,Σk−1)
Sense:
p
(
y
k
∣
x
k
)
p(y_{k}|x_{k})
p(yk∣xk) ~
N
(
C
k
x
k
,
Q
k
)
\mathcal N(C_{k}x_{k},Q_{k})
N(Ckxk,Qk)
p
(
x
k
)
p(x_{k})
p(xk) ~
N
(
μ
ˉ
k
,
Σ
ˉ
k
)
\mathcal N(\bar{\mu}_{k},\bar{\Sigma}_{k})
N(μˉk,Σˉk)
据此可推导
μ
k
\mu_{k}
μk和
Σ
k
\Sigma_{k}
Σk，推导的知识点包括：
- p ( x k ∣ x k − 1 , u k ) p(x_k|x_{k-1},u_{k}) p(xk∣xk−1,uk)与 p ( x k − 1 ) p(x_{k-1}) p(xk−1)的乘积仍然为高斯分布。
- 为了对 x k − 1 x_{k-1} xk−1部分进行积分，可先分离出关于 x k − 1 x_{k-1} xk−1的高斯分布。
- 高斯分布一阶导的零点为均值，二阶导的零点为协方差的逆。
参考：卡尔曼滤波器（THE KALMAN FILTER）的数学原理
二、图优化方法（定位与建图）
Graph SLAM
GraphSLAM背后的思想主要是把机器人或Landmarks的位姿抽象为点，把机器人在不同位姿运动和观测的约束抽象为边。从而构成信息矩阵
Ω
\Omega
Ω和信息向量
ξ
\xi
ξ，并满足
Ω
\Omega
Ω
μ
\mu
μ=
ξ
\xi
ξ，故地图所有点的位姿
μ
\mu
μ可由以下式子求得：
μ
=
Ω
−
1
ξ
\mu=\Omega^{-1}\xi
μ=Ω−1ξ
参考资料
Particle Filter Tutorial 粒子滤波：从推导到应用（一）
卡尔曼滤波器（THE KALMAN FILTER）的数学原理
SLAM笔记三——贝叶斯滤波器
Particle Filter Tutorial 粒子滤波：从推导到应用（二）
【SLAM】（二）Cartographer的原理探究——GraphSLAM理论基础
扫一扫
1226
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
