前言
本节通过机器人运动模型(motion model)实现状态转移概率 p ( x t ∣ u t , x t − 1 ) p(x_t|u_t,x_{t-1}) p(xt∣ut,xt−1)，即执行运动控制 u t u_t ut后机器人状态的后验分布，其中， x t , x t − 1 x_t,x_{t-1} xt,xt−1代表机器人的位姿： [ x y θ ] \begin{bmatrix} x \\ y \\ \theta \\ \end{bmatrix} ⎣⎡xyθ⎦⎤《概率机器人》假设 θ = 0 \theta =0 θ=0为 x x x轴正方向， θ = π / 2 \theta =\pi/2 θ=π/2为 y y y轴正方向。
一、速度运动模型
速度运动模型的控制输入： u t = [ v t w t ] u_t=\begin{bmatrix} v_t \\ w_t \\ \end{bmatrix} ut=[vtwt]
1.速度运动前向模型
我所理解的前向运动是指跟定上一时刻的位姿
x
t
−
1
x_{t-1}
xt−1和控制输入
u
t
u_t
ut，计算本时刻位姿的可能状态
x
t
x_t
xt。
理想运动
这个模型我们可以认为机器人时时刻刻做旋转运动，直线只是当旋转半径无穷大的一种特例，基于上图，我们可以写出如下公式：
{
v
=
w
⋅
r
x
c
=
x
−
r
⋅
c
o
s
(
θ
−
90
)
y
c
=
y
−
r
⋅
s
i
n
(
θ
−
90
)
\begin{cases}v=w·r&\\ x_c=x-r·cos(\theta-90)&\\y_c=y-r·sin(\theta-90) &\end{cases}
⎩⎪⎨⎪⎧v=w⋅rxc=x−r⋅cos(θ−90)yc=y−r⋅sin(θ−90)整理得：
{
x
c
=
x
+
r
⋅
s
i
n
θ
y
c
=
y
−
r
⋅
c
o
s
θ
\begin{cases}x_c=x+r·sin\theta&\\y_c=y-r·cos\theta &\end{cases}
{xc=x+r⋅sinθyc=y−r⋅cosθ最终得到：
[
x
′
y
′
θ
′
]
=
[
x
c
+
v
w
s
i
n
(
θ
+
w
Δ
t
)
y
c
+
v
w
c
o
s
(
θ
+
w
Δ
t
)
θ
+
w
Δ
t
]
=
[
x
y
θ
]
+
[
−
v
w
s
i
n
θ
+
v
w
s
i
n
(
θ
+
w
Δ
t
)
v
w
c
o
s
θ
−
v
w
c
o
s
(
θ
+
w
Δ
t
)
w
Δ
t
]
\begin{bmatrix} x' \\ y' \\\theta' \end{bmatrix}=\begin{bmatrix} x_c + \frac{v}{w}sin(\theta +w\Delta t) \\y_c + \frac{v}{w}cos(\theta +w\Delta t) \\\theta+w\Delta t \end{bmatrix}=\begin{bmatrix}x \\ y \\ \theta\end{bmatrix}+\begin{bmatrix}-\frac{v}{w}sin\theta+\frac{v}{w}sin(\theta+w\Delta t) \\ \frac{v}{w}cos\theta-\frac{v}{w}cos(\theta+w\Delta t) \\ w\Delta t\end{bmatrix}
⎣⎡x′y′θ′⎦⎤=⎣⎡xc+wvsin(θ+wΔt)yc+wvcos(θ+wΔt)θ+wΔt⎦⎤=⎣⎡xyθ⎦⎤+⎣⎡−wvsinθ+wvsin(θ+wΔt)wvcosθ−wvcos(θ+wΔt)wΔt⎦⎤真实运动
由于真实的速度存在一些与给定速度成比例的误差：
[
v
^
w
^
]
=
[
v
w
]
+
[
ε
α
1
v
2
+
α
2
w
2
ε
α
3
v
2
+
α
4
w
2
]
\begin{bmatrix}\hat v\\\hat w\end{bmatrix}=\begin{bmatrix}v\\w\end{bmatrix}+\begin{bmatrix}\varepsilon_{\alpha_1v^2+\alpha_2w^2}\\\varepsilon_{\alpha_3v^2+\alpha_4w^2}\end{bmatrix}
[v^w^]=[vw]+[εα1v2+α2w2εα3v2+α4w2]并假设当到达最终位姿时，又带来了
γ
^
=
ε
α
5
v
2
+
α
6
w
2
\hat{\gamma}=\varepsilon_{\alpha_5}v^2+\alpha_6w^2
γ^=εα5v2+α6w2误差
真实运动模型(前向模型)为：
[
x
′
y
′
θ
′
]
=
[
x
y
θ
]
+
[
−
v
^
w
^
s
i
n
θ
+
v
^
w
^
s
i
n
(
θ
+
w
^
Δ
t
)
v
^
w
^
c
o
s
θ
−
v
^
w
^
c
o
s
(
θ
+
w
^
Δ
t
)
w
^
Δ
t
+
γ
^
Δ
t
]
\begin{bmatrix} x' \\ y' \\\theta' \end{bmatrix}=\begin{bmatrix}x \\ y \\ \theta\end{bmatrix}+\begin{bmatrix}-\frac{\hat{v}}{\hat{w}}sin\theta+\frac{\hat{v}}{\hat{w}}sin(\theta+\hat{w}\Delta t) \\ \frac{\hat{v}}{\hat{w}}cos\theta-\frac{\hat{v}}{\hat{w}}cos(\theta+\hat{w}\Delta t) \\ \hat{w}\Delta t+\hat{\gamma}\Delta t\end{bmatrix}
⎣⎡x′y′θ′⎦⎤=⎣⎡xyθ⎦⎤+⎣⎡−w^v^sinθ+w^v^sin(θ+w^Δt)w^v^cosθ−w^v^cos(θ+w^Δt)w^Δt+γ^Δt⎦⎤
采样算法
在粒子滤波中，我们不是要计算状态转移后验的具体数值，而是能够从状态转移中采样，使得上一时刻的粒子转变为临时粒子，根据上面推导的公式，有以下算法：
2.速度运动逆向模型
我所理解的速度运动逆向模型是从上一时刻的位姿
x
t
−
1
x_{t-1}
xt−1和本时刻的位姿
x
t
x_t
xt以及旋转误差
γ
^
\hat{\gamma}
γ^，来确定运动参数
u
^
t
\hat{u}_t
u^t。具体推导较为复杂，也没有太看明白╮(╯﹏╰）╭，先这么用，后续补上。
二、里程计运动模型
里程计运动模型的输入：
u
t
=
[
x
‾
t
−
1
x
‾
t
]
u_t=\begin{bmatrix}\overline{x}_{t-1}\\\overline{x}_t\end{bmatrix}
ut=[xt−1xt]是基于机器人内部坐标的相对位置变化。
里程计模型
如上图将
u
t
u_t
ut分解成三个部分：旋转+平移+旋转，根据里程计读数
x
‾
t
−
1
,
x
‾
t
\overline{x}_{t-1},\overline{x}_t
xt−1,xt，可以求出这三个部分：
{
δ
r
o
t
1
=
a
t
a
n
2
(
y
‾
′
−
y
‾
,
x
‾
′
−
x
‾
)
−
θ
‾
δ
t
r
a
n
s
=
(
x
‾
−
x
‾
′
)
2
+
(
y
‾
−
y
‾
′
)
2
δ
r
o
t
2
=
θ
‾
′
−
θ
‾
−
δ
r
o
t
1
\begin{cases}\delta_{rot1}=atan2(\overline{y}'-\overline{y},\overline{x}'-\overline{x})-\overline{\theta}\\\delta_{trans}=\sqrt{(\overline{x}-\overline{x}')^2+(\overline{y}-\overline{y}')^2}\\\delta_{rot2}=\overline{\theta}'-\overline{\theta}-\delta_{rot1}\end{cases}
⎩⎪⎨⎪⎧δrot1=atan2(y′−y,x′−x)−θδtrans=(x−x′)2+(y−y′)2δrot2=θ′−θ−δrot1上面里程计测量模型将作为真实位姿
x
t
−
1
,
x
t
x_{t-1},x_t
xt−1,xt之间变换的估计器使用。
真实模型
真实模型由初始位姿
x
t
−
1
x_{t-1}
xt−1和假设的最终状态
x
t
x_t
xt作为输入：
{
δ
^
r
o
t
1
=
a
t
a
n
2
(
y
′
−
y
,
x
′
−
x
)
−
θ
δ
^
t
r
a
n
s
=
(
x
−
x
′
)
2
+
(
y
−
y
′
)
2
δ
^
r
o
t
2
=
θ
′
−
θ
−
δ
^
r
o
t
1
\begin{cases}\hat{\delta}_{rot1}=atan2(y'-y,x'-x)-\theta\\\hat{\delta}_{trans}=\sqrt{(x-x')^2+(y-y')^2}\\\hat{\delta}_{rot2}=\theta'-\theta-\hat{\delta}_{rot1}\end{cases}
⎩⎪⎨⎪⎧δ^rot1=atan2(y′−y,x′−x)−θδ^trans=(x−x′)2+(y−y′)2δ^rot2=θ′−θ−δ^rot1误差模型
为了建立误差的运动模型，定义里程计模型和真实值模型之间是带有独立噪声的关系：
{
δ
^
r
o
t
1
=
δ
r
o
t
1
−
ε
α
1
δ
r
o
t
1
2
+
α
2
δ
t
r
a
n
s
2
δ
^
t
r
a
n
s
=
δ
t
r
a
n
s
−
ε
α
3
δ
t
r
a
n
s
2
+
α
4
δ
r
o
t
1
2
+
α
4
δ
r
o
t
2
2
δ
^
r
o
t
2
=
δ
r
o
t
2
−
ε
α
1
δ
r
o
t
2
2
+
α
2
δ
t
r
a
n
s
2
\begin{cases}\hat{\delta}_{rot1}=\delta_{rot1}-\varepsilon_{\alpha1\delta_{rot1}^2+\alpha2\delta_{trans}^2}\\\hat{\delta}_{trans}=\delta_{trans}-\varepsilon_{\alpha3\delta_{trans}^2+\alpha4\delta_{rot1}^2+\alpha4\delta_{rot2}^2}\\\hat{\delta}_{rot2}=\delta_{rot2}-\varepsilon_{\alpha1\delta_{rot2}^2+\alpha2\delta_{trans}^2}\end{cases}
⎩⎪⎨⎪⎧δ^rot1=δrot1−εα1δrot12+α2δtrans2δ^trans=δtrans−εα3δtrans2+α4δrot12+α4δrot22δ^rot2=δrot2−εα1δrot22+α2δtrans2
1.采样算法
采样算法不需要计算后验概率
p
(
x
t
∣
u
t
,
x
t
−
1
)
p(x_t|u_t,x_{t-1})
p(xt∣ut,xt−1)，只需要能根据后验概率采样即可。则根据真实模型、里程计模型和误差模型，实际位置
x
t
x_t
xt可由下面公式计算：
[
x
′
y
′
θ
′
]
=
[
x
y
θ
]
+
[
δ
^
t
r
a
n
s
c
o
s
(
θ
+
δ
^
r
o
t
1
)
δ
^
t
r
a
n
s
s
i
n
(
θ
+
δ
^
r
o
t
1
)
δ
^
r
o
t
1
+
δ
^
r
o
t
2
]
\begin{bmatrix}x'\\y'\\\theta'\end{bmatrix}=\begin{bmatrix}x\\y\\\theta\end{bmatrix}+\begin{bmatrix}\hat{\delta}_{trans}cos(\theta+\hat{\delta}_{rot1})\\\hat{\delta}_{trans}sin(\theta+\hat{\delta}_{rot1})\\\hat{\delta}_{rot1}+\hat{\delta}_{rot2}\end{bmatrix}
⎣⎡x′y′θ′⎦⎤=⎣⎡xyθ⎦⎤+⎣⎡δ^transcos(θ+δ^rot1)δ^transsin(θ+δ^rot1)δ^rot1+δ^rot2⎦⎤
得到下面采样算法：
2.闭式算法
假设位置
x
t
x_t
xt为实际位置的后验由闭式算法给出。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
