机器人运动
概率机器人学将运动方程推广：由控制噪声或者未建模的外源性影响，控制输出是不确定的。
基础概念
运动学构型
运动学（Kinematics）：描述控制行为对机器人构型产生影响的微积分。
- 位姿：能够表示机器人位置和航向的物理量
- 平面环境下通常限定为三维向量： [ X Y θ ] T \begin{bmatrix}X&Y&\theta\end{bmatrix}^T [X?Y?θ?]T
- 航向：也称方位，用于表示机器人的朝向。
- 位置：除航向外的部分，称为位置。
- 位姿和环境中对象的位置构成了机器人-环境系统的运动学状态 x t x_t xt?
概率运动学
概率运动学模型在移动机器人中起到状态变换模型的作用，即 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 ? 1 x_{t-1} xt?1?在当前时刻得到机器人位姿 x t x_t xt?的概率。为对 x t ? 1 x_{t-1} xt?1?施加控制 u t u_t ut?后，机器人取得的运动学状态的后验分布。实际控制中， u t u_t ut?有时由机器人里程计提供。
速度运动模型
速度运动模型（Velocity Motion Model）认为可通过一个旋转速度和一个平移速度来控制机器人。差分驱动、阿克曼驱动、同步驱动通常使用该方案进行控制。
速度运动模型被用于概率运动规划，用
v
t
v_t
vt?表示t时刻机器人的平移速度（Translational Velocity），用
ω
t
\omega_t
ωt?表示旋转速度（Rotational Velocity），有：
u
t
=
[
v
t
ω
t
]
u_t=\begin{bmatrix}v_t\\\omega_t\end{bmatrix}
ut?=[vt?ωt??]
规定，机器人逆时针旋转为旋转正方向（
ω
t
>
0
\omega_t>0
ωt?>0），机器人前向运动为平移正方向（
v
t
>
0
v_t>0
vt?>0）。
模型建立
理想模型
首先，考虑无噪声的理想情况并做出如下假定：
- 时刻t的控制量 u t = [ v t ω t ] T u_t=\begin{bmatrix}v_t&\omega_t\end{bmatrix}^T ut?=[vt??ωt??]T
- 机器人初始位姿： x t ? 1 = [ X Y θ ] T x_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^T xt?1?=[X?Y?θ?]T
- 在时间间隔 Δ t \Delta t Δt间，机器人速度恒定不变。也即机器人的控制量不变
- 运动 Δ t \Delta t Δt时间后，机器人的位姿： x t = [ X ′ Y ′ θ ′ ] T x_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^T xt?=[X′?Y′?θ′?]T
则在世界坐标系
X
O
Y
XOY
XOY中，机器人作半径为
r
r
r的圆弧运动，圆弧的中心点为
C
C
C。过机器人中心作其运动方向的切线，交坐标系得航向角
θ
\theta
θ
机器人平移速度与旋转速度间存在等式：
v
t
=
ω
t
?
r
r
=
v
t
ω
t
\begin{aligned} v_t&=\omega_t\cdot r\\ r&=\frac{v_t}{\omega_t} \end{aligned}
vt?r?=ωt??r=ωt?vt???
过运动圆弧中心
C
C
C做坐标轴平行线，可由三角函数得：
X
C
=
X
?
r
?
cos
?
(
θ
?
90
°
)
=
X
?
r
?
sin
?
θ
Y
C
=
Y
?
r
?
sin
?
(
θ
?
90
°
)
=
Y
?
r
?
cos
?
θ
X_C=X-r\cdot\cos(\theta-90\degree)=X-r\cdot\sin\theta\\ Y_C=Y-r\cdot\sin(\theta-90\degree)=Y-r\cdot\cos\theta
XC?=X?r?cos(θ?90°)=X?r?sinθYC?=Y?r?sin(θ?90°)=Y?r?cosθ
将转弯半径带入简化等式：
{
X
C
=
X
?
v
t
ω
t
?
sin
?
θ
Y
C
=
Y
?
v
t
ω
t
?
cos
?
θ
\begin{cases} X_C&=&X-\frac{v_t}{\omega_t}\cdot\sin\theta\\ Y_C&=&Y-\frac{v_t}{\omega_t}\cdot\cos\theta \end{cases}
{XC?YC??==?X?ωt?vt???sinθY?ωt?vt???cosθ?
如图所示，机器人运动
Δ
t
\Delta t
Δt时间后，沿着运动方向前进
v
t
Δ
t
v_t\Delta t
vt?Δt，并导致其航向旋转了
ω
t
Δ
t
\omega_t\Delta t
ωt?Δt。
此时，圆弧中心坐标不变，在此基础上采用三角函数，计算得到当前时刻的位姿：
[
X
′
Y
′
θ
′
]
=
[
X
C
=
X
+
v
t
ω
t
sin
?
(
θ
+
ω
t
Δ
t
)
Y
C
=
Y
+
v
t
ω
t
cos
?
(
θ
+
ω
t
Δ
t
)
θ
+
ω
Δ
t
]
[
X
′
Y
′
θ
′
]
=
[
X
Y
θ
]
+
[
?
v
t
ω
t
sin
?
θ
+
v
t
ω
t
sin
?
(
θ
+
ω
t
Δ
t
)
v
t
ω
t
cos
?
θ
?
v
t
ω
t
cos
?
(
θ
+
ω
t
Δ
t
)
ω
t
Δ
t
]
\begin{aligned} \begin{bmatrix}X^{'}\\Y^{'}\\\theta^{'}\\\end{bmatrix}&=& \begin{bmatrix} X_C=X+\frac{v_t}{\omega_t}\sin(\theta+\omega_t\Delta t)\\ Y_C=Y+\frac{v_t}{\omega_t}\cos(\theta+\omega_t\Delta t)\\ \theta+\omega\Delta t\end{bmatrix}\\ \begin{bmatrix}X^{'}\\Y^{'}\\\theta^{'}\\\end{bmatrix}&=& \begin{bmatrix}X\\Y\\\theta\\\end{bmatrix}+\begin{bmatrix} -\frac{v_t}{\omega_t}\sin\theta+\frac{v_t}{\omega_t}\sin(\theta+\omega_t\Delta t)\\ \frac{v_t}{\omega_t}\cos\theta-\frac{v_t}{\omega_t}\cos(\theta+\omega_t\Delta t)\\ \omega_t\Delta t\end{bmatrix} \end{aligned}
???X′Y′θ′???????X′Y′θ′?????==????XC?=X+ωt?vt??sin(θ+ωt?Δt)YC?=Y+ωt?vt??cos(θ+ωt?Δt)θ+ωΔt???????XYθ????+????ωt?vt??sinθ+ωt?vt??sin(θ+ωt?Δt)ωt?vt??cosθ?ωt?vt??cos(θ+ωt?Δt)ωt?Δt?????
噪声模型
实际运动中，机器人将受到噪声影响。实际运行速度与期望速度存在一定的偏差。对控制量加入噪声：
[
v
^
t
ω
^
t
]
=
[
v
t
ω
t
]
+
[
?
α
1
v
t
2
+
α
2
ω
t
2
?
α
3
v
t
2
+
α
4
ω
t
2
]
\begin{bmatrix}\hat v_t\\\hat\omega_t\end{bmatrix}=\begin{bmatrix} v_t\\\omega_t\end{bmatrix}+\begin{bmatrix}\epsilon_{\alpha_1v_t^2+\alpha_2\omega_t^2}\\\epsilon_{\alpha_3v_t^2+\alpha_4\omega_t^2}\end{bmatrix}
[v^t?ω^t??]=[vt?ωt??]+[?α1?vt2?+α2?ωt2???α3?vt2?+α4?ωt2???]
式中，噪声
?
b
2
\epsilon_{b^2}
?b2?表示均值为0，方差为
b
2
b^2
b2的误差参量，参数
α
1
\alpha_1
α1?~
α
4
\alpha_4
α4?表示机器人里程计的误差参数，用于确定机器人的精确度。机器人越不精确，参数越大。
噪声可用正态分布进行描述：
?
b
2
(
x
)
=
1
2
π
b
2
e
x
p
(
?
1
2
x
2
b
2
)
\epsilon_{b^2}(x)=\frac{1}{\sqrt{2\pi b^2}}exp(-\frac{1}{2}\frac{x^2}{b^2})
?b2?(x)=2πb2?1?exp(?21?b2x2?)
如图为均值为0，方差为
b
2
b^2
b2的正态分布曲线。正态分布通常用于连续随机过程的噪声建立模型。
噪声也可以用三角形分布进行描述：
?
b
2
(
x
)
=
max
?
{
0
,
1
6
b
?
∣
x
∣
6
b
2
}
\epsilon_{b^2}(x)=\max\{0,\frac{1}{\sqrt 6b}-\frac{|x|}{6b^2}\}
?b2?(x)=max{0,6?b1??6b2∣x∣?}
如图为三角形分布曲线，它仅在
(
?
6
b
,
6
b
)
(-\sqrt{6b},\sqrt{6b})
(?6b?,6b?)区间内大于零。
由此，得到噪声下的机器人位姿：
[
X
′
Y
′
θ
′
]
=
[
X
Y
θ
]
+
[
?
v
^
t
ω
^
t
sin
?
θ
+
v
^
t
ω
^
t
sin
?
(
θ
+
ω
t
Δ
t
)
v
^
t
ω
^
t
cos
?
θ
?
v
^
t
ω
^
t
cos
?
(
θ
+
ω
t
Δ
t
)
ω
^
t
Δ
t
]
\begin{bmatrix} X^{'}\\Y^{'}\\\theta^{'}\\ \end{bmatrix}=\begin{bmatrix} X\\Y\\\theta \end{bmatrix}+\begin{bmatrix} -\frac{\hat v_t}{\hat \omega_t}\sin\theta+\frac{\hat v_t}{\hat \omega_t}\sin(\theta+\omega_t\Delta t)\\ \frac{\hat v_t}{\hat \omega_t}\cos\theta-\frac{\hat v_t}{\hat \omega_t}\cos(\theta+\omega_t\Delta t)\\ \hat\omega_t\Delta t \end{bmatrix}
???X′Y′θ′????=???XYθ????+????ω^t?v^t??sinθ+ω^t?v^t??sin(θ+ωt?Δt)ω^t?v^t??cosθ?ω^t?v^t??cos(θ+ωt?Δt)ω^t?Δt????
最终航向
上述模型建立在机器人围绕半径为
r
r
r的圆弧运动进行建模，但由于环境噪声原因，实际运动轨迹并非圆形。为推广模型，假设机器人抵达最终位姿时，旋转
γ
^
t
\hat\gamma_t
γ^?t?:
θ
′
=
θ
+
ω
^
t
Δ
t
+
γ
^
t
Δ
t
γ
^
=
?
α
5
v
t
2
+
α
6
ω
t
2
\begin{aligned} \theta^{'}&=&\theta+\hat\omega_t\Delta t+\hat\gamma_t\Delta t\\ \hat\gamma&=&\epsilon_{\alpha_5v_t^2+\alpha_6\omega_t^2} \end{aligned}
θ′γ^??==?θ+ω^t?Δt+γ^?t?Δt?α5?vt2?+α6?ωt2???
参量
α
5
\alpha_5
α5?和
α
6
\alpha_6
α6?为机器人里程计噪声参数，由此得到最终的运动模型：
[
X
′
Y
′
θ
′
]
=
[
X
Y
θ
]
+
[
?
v
^
t
ω
^
t
sin
?
θ
+
v
^
t
ω
^
t
sin
?
(
θ
+
ω
t
Δ
t
)
v
^
t
ω
^
t
cos
?
θ
?
v
^
t
ω
^
t
cos
?
(
θ
+
ω
t
Δ
t
)
ω
^
t
Δ
t
+
γ
^
t
Δ
t
]
\begin{bmatrix} X^{'}\\Y^{'}\\\theta^{'}\\ \end{bmatrix}=\begin{bmatrix} X\\Y\\\theta \end{bmatrix}+\begin{bmatrix} -\frac{\hat v_t}{\hat \omega_t}\sin\theta+\frac{\hat v_t}{\hat \omega_t}\sin(\theta+\omega_t\Delta t)\\ \frac{\hat v_t}{\hat \omega_t}\cos\theta-\frac{\hat v_t}{\hat \omega_t}\cos(\theta+\omega_t\Delta t)\\ \hat\omega_t\Delta t+\hat\gamma_t\Delta t \end{bmatrix}
???X′Y′θ′????=???XYθ????+????ω^t?v^t??sinθ+ω^t?v^t??sin(θ+ωt?Δt)ω^t?v^t??cosθ?ω^t?v^t??cos(θ+ωt?Δt)ω^t?Δt+γ^?t?Δt????
闭式算法
算法推导
计算在时间间隔 Δ t \Delta t Δt内，在控制量 u t = [ v t ω t ] T u_t=\begin{bmatrix}v_t&\omega_t\end{bmatrix}^T ut?=[vt??ωt??]T的作用下，机器人位姿从初始位姿 x t ? 1 = [ X Y θ ] T x_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^T xt?1?=[X?Y?θ?]T变为期望位姿 x t = [ X ′ Y ′ θ ′ ] T x_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^T xt?=[X′?Y′?θ′?]T的概率密度 p ( x t ∣ u t , x t ? 1 ) p(x_t | u_t,x_{t-1}) p(xt?∣ut?,xt?1?)
首先，计算将机器人从位姿 x t ? 1 x_{t-1} xt?1?变换至位置 [ X ′ Y ′ ] T \begin{bmatrix}X^{'}&Y^{'}\end{bmatrix}^T [X′?Y′?]T所需的控制量
模型假设机器人在
Δ
t
\Delta t
Δt时间内速度恒定，运动轨迹形成圆弧。则对于圆弧圆心，其与机器人的初始位姿正交，其中
λ
\lambda
λ为未知量：
[
X
?
Y
?
]
=
[
X
Y
]
+
[
?
λ
sin
?
θ
λ
cos
?
θ
]
\begin{bmatrix}X^*\\Y^*\end{bmatrix}=\begin{bmatrix}X\\Y\end{bmatrix}+\begin{bmatrix}-\lambda\sin\theta\\\lambda\cos\theta\end{bmatrix}
[X?Y??]=[XY?]+[?λsinθλcosθ?]
同时，圆弧圆心位于点
(
X
,
Y
)
(X,Y)
(X,Y)和点
(
X
′
,
Y
′
)
(X^{'},Y^{'})
(X′,Y′)的垂直平分线上，其中
μ
\mu
μ为未知量：
[
X
?
Y
?
]
=
[
X
+
X
′
2
+
μ
(
Y
?
Y
′
)
Y
+
Y
′
2
+
μ
(
X
′
?
X
)
]
\begin{bmatrix}X^*\\Y^*\end{bmatrix}=\begin{bmatrix} \frac{X+X^{'}}{2}+\mu(Y-Y^{'})\\ \frac{Y+Y^{'}}{2}+\mu(X^{'}-X) \end{bmatrix}
[X?Y??]=[2X+X′?+μ(Y?Y′)2Y+Y′?+μ(X′?X)?]
当
ω
t
=
0
\omega_t=0
ωt?=0时，圆心位于无穷远处，机器人近似直线运动。；当
w
t
=?
0
w_t\not=0
wt??=0时联立方程，得唯一解：
μ
=
1
2
(
X
?
X
′
)
cos
?
θ
+
(
Y
?
Y
′
)
sin
?
θ
(
Y
?
Y
′
)
cos
?
θ
?
(
X
?
X
′
)
sin
?
θ
\mu=\frac{1}{2}\frac{(X-X^{'})\cos\theta+(Y-Y^{'})\sin\theta}{(Y-Y^{'})\cos\theta-(X-X^{'})\sin\theta}
μ=21?(Y?Y′)cosθ?(X?X′)sinθ(X?X′)cosθ+(Y?Y′)sinθ?
带入等式，得到圆心坐标表达式：
[
X
?
Y
?
]
=
[
1
2
(
X
+
X
′
)
+
1
2
(
X
?
X
′
)
cos
?
θ
+
(
Y
?
Y
′
)
sin
?
θ
(
Y
?
Y
′
)
cos
?
θ
?
(
X
?
X
′
)
sin
?
θ
(
Y
?
Y
′
)
1
2
(
Y
+
Y
′
)
+
1
2
(
X
?
X
′
)
cos
?
θ
+
(
Y
?
Y
′
)
sin
?
θ
(
Y
?
Y
′
)
cos
?
θ
?
(
X
?
X
′
)
sin
?
θ
(
X
′
?
X
)
]
\begin{bmatrix}X^{*}\\Y^{*}\end{bmatrix}=\begin{bmatrix} \frac{1}{2}(X+X^{'})+\frac{1}{2}\frac{(X-X^{'})\cos\theta+(Y-Y^{'})\sin\theta}{(Y-Y^{'})\cos\theta-(X-X^{'})\sin\theta}(Y-Y^{'})\\ \frac{1}{2}(Y+Y^{'})+\frac{1}{2}\frac{(X-X^{'})\cos\theta+(Y-Y^{'})\sin\theta}{(Y-Y^{'})\cos\theta-(X-X^{'})\sin\theta}(X^{'}-X) \end{bmatrix}
[X?Y??]=???21?(X+X′)+21?(Y?Y′)cosθ?(X?X′)sinθ(X?X′)cosθ+(Y?Y′)sinθ?(Y?Y′)21?(Y+Y′)+21?(Y?Y′)cosθ?(X?X′)sinθ(X?X′)cosθ+(Y?Y′)sinθ?(X′?X)????
圆弧半径可以采用欧拉距离公式计算得到：
r
?
=
(
X
?
X
?
)
2
+
(
Y
?
Y
?
)
2
r^{*}=\sqrt{(X-X^{*})^2+(Y-Y^{*})^2}
r?=(X?X?)2+(Y?Y?)2?
由此，得到机器人航向的变化量计算表达式：
Δ
θ
=
a
t
a
n
2
(
Y
′
?
Y
?
,
X
′
?
X
?
)
?
a
t
a
n
2
(
Y
?
Y
?
,
X
?
X
?
)
\Delta\theta=atan2(Y^{'}-Y^*,X^{'}-X^*)-atan2(Y-Y^*,X-X^*)
Δθ=atan2(Y′?Y?,X′?X?)?atan2(Y?Y?,X?X?)
函数
a
t
a
n
2
(
y
,
x
)
atan2(y,x)
atan2(y,x)表达式如下，大多数语言（如Python中math库）提供了对应API（math.atan2(y,x)）：
a
t
a
n
2
(
y
,
x
)
=
{
arctan
?
(
y
/
x
)
x
>
0
s
i
g
n
(
y
)
(
π
?
arctan
?
∣
y
x
∣
)
x
<
0
0
x
=
0
,
y
=
0
s
i
g
n
(
y
)
π
2
x
=
0
,
y
=?
0
atan2(y,x)=\begin{cases} \arctan(y/x)&x>0\\ sign(y)(\pi-\arctan|{\frac{y}{x}}|)&x<0\\ 0&x=0,y=0\\ sign(y)\frac{\pi}{2}&x=0,y\not=0 \end{cases}
atan2(y,x)=??????????arctan(y/x)sign(y)(π?arctan∣xy?∣)0sign(y)2π??x>0x<0x=0,y=0x=0,y?=0?
式中，函数
s
i
g
n
(
x
)
sign(x)
sign(x)为符号函数，表达式如下：
s
i
g
n
(
x
)
=
{
1
x
>
0
0
x
=
0
?
1
x
<
0
sign(x)=\begin{cases} &1&x>0\\ &0&x=0\\ -&1&x<0 \end{cases}
sign(x)=????????101?x>0x=0x<0?
从而，得到机器人的平移距离：
Δ
d
i
s
t
=
r
?
Δ
θ
\Delta dist=r^*\Delta\theta
Δdist=r?Δθ
进而得到控制量的表达式：
u
^
t
=
[
v
^
t
ω
^
t
]
=
Δ
t
?
1
[
Δ
d
i
s
t
Δ
θ
]
\hat u_t=\begin{bmatrix}\hat v_t\\ \hat \omega_t\end{bmatrix}=\Delta t^{-1}\begin{bmatrix}\Delta dist\\ \Delta \theta\end{bmatrix}
u^t?=[v^t?ω^t??]=Δt?1[ΔdistΔθ?]
其次，计算机器人抵达最终航向
θ
′
\theta^{'}
θ′所需的旋转
γ
^
t
\hat\gamma_t
γ^?t?
γ
^
t
=
Δ
t
?
1
(
θ
′
?
θ
)
?
ω
^
t
\hat \gamma_t=\Delta t^{-1}(\theta^{'}-\theta)-\hat \omega_t
γ^?t?=Δt?1(θ′?θ)?ω^t?
最后，计算机器人的概率密度 p ( x t ∣ u t , x t ? 1 ) p(x_t | u_t,x_{t-1}) p(xt?∣ut?,xt?1?)。
定义运动误差参量用于表示
u
^
t
\hat u_t
u^t?和
γ
^
t
\hat \gamma_t
γ^?t?相对控制量
u
t
=
[
v
t
ω
t
]
T
u_t=\begin{bmatrix}v_t&\omega_t\end{bmatrix}^T
ut?=[vt??ωt??]T和旋转
γ
t
=
0
\gamma_t=0
γt?=0的偏差值。
v
e
r
r
=
v
t
?
v
^
t
ω
e
r
r
=
ω
t
?
ω
^
t
γ
e
r
r
=
γ
^
t
\begin{aligned} v_{err}&=v_t-\hat v_t\\ \omega_{err}&=\omega_t-\hat \omega_t\\ \gamma_{err}&=\hat \gamma_t\\ \end{aligned}
verr?ωerr?γerr??=vt??v^t?=ωt??ω^t?=γ^?t??
由此，得到噪声误差：
?
α
1
v
t
2
+
α
2
ω
t
2
(
v
e
r
r
)
?
α
3
v
t
2
+
α
4
ω
t
2
(
ω
e
r
r
)
?
α
5
v
t
2
+
α
6
ω
t
2
(
γ
e
r
r
)
\begin{aligned} \epsilon_{\alpha_1v_t^2+\alpha_2\omega_t^2}(v_{err})\\ \epsilon_{\alpha_3v_t^2+\alpha_4\omega_t^2}(\omega_{err})\\ \epsilon_{\alpha_5v_t^2+\alpha_6\omega_t^2}(\gamma_{err})\\ \end{aligned}
?α1?vt2?+α2?ωt2??(verr?)?α3?vt2?+α4?ωt2??(ωerr?)?α5?vt2?+α6?ωt2??(γerr?)?
假定，不同来源的误差噪声相互独立，则得到概率密度：
p
(
x
t
∣
u
t
,
x
t
?
1
)
=
?
α
1
v
t
2
+
α
2
ω
t
2
(
v
e
r
r
)
?
?
α
3
v
t
2
+
α
4
ω
t
2
(
ω
e
r
r
)
?
?
α
5
v
t
2
+
α
6
ω
t
2
(
γ
e
r
r
)
p(x_t | u_t,x_{t-1})=\epsilon_{\alpha_1v_t^2+\alpha_2\omega_t^2}(v_{err})\cdot\epsilon_{\alpha_3v_t^2+\alpha_4\omega_t^2}(\omega_{err})\cdot\epsilon_{\alpha_5v_t^2+\alpha_6\omega_t^2}(\gamma_{err})
p(xt?∣ut?,xt?1?)=?α1?vt2?+α2?ωt2??(verr?)??α3?vt2?+α4?ωt2??(ωerr?)??α5?vt2?+α6?ωt2??(γerr?)
算法设计
系统输入: 初始位姿 x t ? 1 = [ X Y θ ] T x_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^T xt?1?=[X?Y?θ?]T、控制量 u t = [ v t ω t ] T u_t=\begin{bmatrix}v_t&\omega_t\end{bmatrix}^T ut?=[vt??ωt??]T、估计后继姿态 x t = [ X ′ Y ′ θ ′ ] T x_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^T xt?=[X′?Y′?θ′?]T
系统输出： 后继姿态为 x t x_t xt?的概率 p ( x t ∣ u t , x t ? 1 ) p(x_t | u_t,x_{t-1}) p(xt?∣ut?,xt?1?)
假设条件： 控制算法以固定时间间隔 Δ t \Delta t Δt执行。
用参数 α 1 \alpha_1 α1? ~ α 6 \alpha_6 α6?表示机器人里程计运动噪声系数：
- 里程计平移噪声
- 参数 α 1 \alpha_1 α1?：平移运动中平移分量的噪声
- 参数 α 2 \alpha_2 α2?：平移运动中旋转分量的噪声
- 里程计旋转噪声
- 参数 α 3 \alpha_3 α3?：旋转运动中平移分量的噪声
- 参数 α 4 \alpha_4 α4?：旋转运动中旋转分量的噪声
- 里程计航向噪声
- 参数 α 5 \alpha_5 α5?：最终航向中平移分量的噪声
- 参数 α 6 \alpha_6 α6?：最终航向中旋转分量的噪声
算法实现逻辑如下：
A
l
g
o
r
i
t
h
m
m
o
t
i
o
n
_
m
o
d
e
l
_
v
e
l
o
c
i
t
y
(
x
t
,
u
t
,
x
t
?
1
)
:
1
:
μ
=
1
2
(
X
?
X
′
)
cos
?
θ
+
(
Y
?
Y
′
)
sin
?
θ
(
Y
?
Y
′
)
cos
?
θ
?
(
X
?
X
′
)
sin
?
θ
2
:
X
?
=
1
2
(
X
+
X
′
)
+
μ
(
Y
?
Y
′
)
3
:
Y
?
=
1
2
(
Y
+
Y
′
)
+
μ
(
X
′
?
X
)
4
:
r
?
=
(
X
?
X
?
)
2
+
(
Y
?
Y
?
)
2
5
:
Δ
θ
=
a
t
a
n
2
(
Y
′
?
Y
?
,
X
′
?
X
?
)
?
a
t
a
n
2
(
Y
?
Y
?
,
X
?
X
?
)
6
:
v
^
t
=
Δ
θ
Δ
t
?
r
?
7
:
ω
^
t
=
Δ
θ
Δ
t
8
:
γ
^
t
=
θ
′
?
θ
Δ
t
?
ω
^
t
9
:
r
e
t
u
r
n
p
r
o
b
(
v
t
?
v
^
t
,
α
1
v
t
2
+
α
2
ω
t
2
)
?
p
r
o
b
(
ω
t
?
ω
^
t
,
α
3
v
t
2
+
α
4
ω
t
2
)
?
p
r
o
b
(
γ
^
t
,
α
5
v
t
2
+
α
6
ω
t
2
)
\begin{aligned} &Algorithm\quad motion\_model\_velocity(x_t,u_t,x_{t-1}): \\ 1:&\quad\quad \mu=\frac{1}{2}\frac{(X-X^{'})\cos\theta+(Y-Y^{'})\sin\theta}{(Y-Y^{'})\cos\theta-(X-X^{'})\sin\theta}\\ 2:&\quad\quad X^{*}=\frac{1}{2}(X+X^{'})+\mu(Y-Y^{'})\\ 3:&\quad\quad Y^{*}=\frac{1}{2}(Y+Y^{'})+\mu(X^{'}-X)\\ 4:&\quad\quad r^{*}=\sqrt{(X-X^{*})^2+(Y-Y^{*})^2}\\ 5:&\quad\quad \Delta\theta=atan2(Y^{'}-Y^*,X^{'}-X^*)-atan2(Y-Y^*,X-X^*)\\ 6:&\quad\quad \hat v_t=\frac{\Delta\theta}{\Delta t}\cdot r^*\\ 7:&\quad\quad \hat \omega_t=\frac{\Delta\theta}{\Delta t}\\ 8:&\quad\quad \hat \gamma_t=\frac{\theta^{'}-\theta}{\Delta t}-\hat \omega_t\\ 9:&\quad\quad return\quad prob(v_t-\hat v_t,\alpha_1v^2_t+\alpha_2\omega^2_t)\cdot prob(\omega_t-\hat \omega_t,\alpha_3v^2_t+\alpha_4\omega^2_t)\cdot prob(\hat\gamma_t,\alpha_5v^2_t+\alpha_6\omega^2_t)\end{aligned}
1:2:3:4:5:6:7:8:9:?Algorithmmotion_model_velocity(xt?,ut?,xt?1?):μ=21?(Y?Y′)cosθ?(X?X′)sinθ(X?X′)cosθ+(Y?Y′)sinθ?X?=21?(X+X′)+μ(Y?Y′)Y?=21?(Y+Y′)+μ(X′?X)r?=(X?X?)2+(Y?Y?)2?Δθ=atan2(Y′?Y?,X′?X?)?atan2(Y?Y?,X?X?)v^t?=ΔtΔθ??r?ω^t?=ΔtΔθ?γ^?t?=Δtθ′?θ??ω^t?returnprob(vt??v^t?,α1?vt2?+α2?ωt2?)?prob(ωt??ω^t?,α3?vt2?+α4?ωt2?)?prob(γ^?t?,α5?vt2?+α6?ωt2?)?
函数
p
r
o
b
(
x
,
b
2
)
prob(x,b^2)
prob(x,b2)用于计算参量
x
x
x在均值为0、方差为
b
2
b^2
b2下的概率。
可使用正太分布：
A
l
g
o
r
i
t
h
m
p
r
o
b
_
n
o
r
m
a
l
_
d
i
s
t
r
i
b
u
t
i
o
n
(
x
,
b
2
)
:
r
e
t
u
r
n
1
2
π
b
2
exp
?
(
?
1
2
?
x
2
b
2
)
\begin{aligned} &Algorithm\quad prob\_normal\_distribution(x,b^2):\\ &\quad\quad return\quad \frac{1}{\sqrt{2\pi b^2}}\exp({-\frac{1}{2}\cdot\frac{x^2}{b^2}}) \end{aligned}
?Algorithmprob_normal_distribution(x,b2):return2πb2?1?exp(?21??b2x2?)?
或三角形分布：
A
l
g
o
r
i
t
h
m
p
r
o
b
_
t
r
i
a
n
g
u
l
a
r
_
d
i
s
t
r
i
b
u
t
i
o
n
(
x
,
b
2
)
:
r
e
t
u
r
n
max
?
{
0
,
1
6
b
?
∣
x
∣
6
b
2
}
\begin{aligned} &Algorithm\quad prob\_triangular\_distribution(x,b^2):\\ &\quad\quad return\quad \max\{0,\frac{1}{\sqrt 6b}-\frac{|x|}{6b^2}\} \end{aligned}
?Algorithmprob_triangular_distribution(x,b2):returnmax{0,6?b1??6b2∣x∣?}?
算法效果
在平面空间XOY下，设置机器人具有相同的初始姿态 x t ? 1 x_{t-1} xt?1?和控制量 u t u_t ut?，则在不同里程计噪声参数下，模型具有不同效果。
当机器人具有中等误差（参数
α
1
\alpha_1
α1? ~
α
6
\alpha_6
α6?），则如图所示：
当机器人具有较大的平移误差（参数 α 1 、 α 2 \alpha_1、\alpha_2 α1?、α2?），但具有较小的角度误差（参数 α 3 、 α 4 \alpha_3、\alpha_4 α3?、α4?），则如图所示：
当机器人具有较小的平移误差（参数
α
1
、
α
2
\alpha_1、\alpha_2
α1?、α2?），但具有较大的角度误差（参数
α
3
、
α
4
\alpha_3、\alpha_4
α3?、α4?），则如图所示：
采样算法
算法推导
针对速度运动模型，采用粒子滤波计算噪声项并带入公式计算即可得到预测位姿。
算法设计
系统输入： 初始位姿 x t ? 1 = [ X Y θ ] T x_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^T xt?1?=[X?Y?θ?]T、控制量 u t = [ v t ω t ] T u_t=\begin{bmatrix}v_t&\omega_t\end{bmatrix}^T ut?=[vt??ωt??]T
系统输出： 估计后继姿态 x t = [ X ′ Y ′ θ ′ ] T x_t=\begin{bmatrix}X^{'}&Y^{'}&\theta^{'}\end{bmatrix}^T xt?=[X′?Y′?θ′?]T
假设条件： 控制算法以固定时间间隔 Δ t \Delta t Δt执行。
采样算法采用粒子滤波进行，旨在根据给定的控制量
u
t
u_t
ut?和初始位姿
x
t
?
1
x_{t-1}
xt?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?，算法的设计流程如下：
A
l
g
o
r
i
t
h
m
s
a
m
p
l
e
_
m
o
t
i
o
n
_
m
o
d
e
l
_
v
e
l
o
c
i
t
y
(
u
t
,
x
t
?
1
)
:
1
:
v
^
t
=
v
t
+
s
a
m
p
l
e
(
α
1
v
t
2
+
α
2
ω
t
2
)
2
:
ω
^
t
=
ω
t
+
s
a
m
p
l
e
(
α
3
v
t
2
+
α
4
ω
t
2
)
3
:
γ
^
t
=
s
a
m
p
l
e
(
α
5
v
t
2
+
α
6
ω
t
2
)
4
:
x
′
=
x
?
v
^
t
ω
^
t
sin
?
θ
+
v
^
t
ω
^
t
sin
?
(
θ
+
ω
^
t
Δ
t
)
5
:
y
′
=
y
+
v
^
t
ω
^
t
cos
?
θ
?
v
^
t
ω
^
t
cos
?
(
θ
+
ω
^
t
Δ
t
)
6
:
θ
′
=
θ
+
ω
^
t
Δ
t
+
γ
^
t
Δ
t
7
:
r
e
t
u
r
n
x
t
=
[
x
′
y
′
θ
′
]
T
\begin{aligned} &Algorithm\quad sample\_motion\_model\_velocity(u_t,x_{t-1}):\\ 1:&\quad\quad \hat v_t=v_t+sample(\alpha_1 v^2_t+\alpha_2\omega^2_t) \\ 2:&\quad\quad \hat \omega_t=\omega_t+sample(\alpha_3 v^2_t+\alpha_4\omega^2_t) \\ 3:&\quad\quad \hat \gamma_t=sample(\alpha_5 v^2_t+\alpha_6\omega^2_t) \\ 4:&\quad\quad x^{'}=x-\frac{\hat v_t}{\hat \omega_t}\sin\theta+\frac{\hat v_t}{\hat \omega_t}\sin(\theta+\hat\omega_t\Delta t) \\ 5:&\quad\quad y^{'}=y+\frac{\hat v_t}{\hat \omega_t}\cos\theta-\frac{\hat v_t}{\hat \omega_t}\cos(\theta+\hat\omega_t\Delta t) \\ 6:&\quad\quad \theta^{'}=\theta+\hat\omega_t\Delta t+\hat\gamma_t\Delta t \\ 7:&\quad\quad return\quad x_t=\begin{bmatrix}x^{'}&y^{'}&\theta^{'}\end{bmatrix}^T \end{aligned}
1:2:3:4:5:6:7:?Algorithmsample_motion_model_velocity(ut?,xt?1?):v^t?=vt?+sample(α1?vt2?+α2?ωt2?)ω^t?=ωt?+sample(α3?vt2?+α4?ωt2?)γ^?t?=sample(α5?vt2?+α6?ωt2?)x′=x?ω^t?v^t??sinθ+ω^t?v^t??sin(θ+ω^t?Δt)y′=y+ω^t?v^t??cosθ?ω^t?v^t??cos(θ+ω^t?Δt)θ′=θ+ω^t?Δt+γ^?t?Δtreturnxt?=[x′?y′?θ′?]T?
首先，1~3行采用运动学模型误差产生控制量的噪声；随后，4~6行使用噪声生成新的机器人位姿。式中函数 s a m p l e ( b 2 ) sample(b^2) sample(b2)用于产生以0为均值， b 2 b^2 b2为方差的随机样本。
可使用（近似）正态分布实现函数：
A
l
g
o
r
i
t
h
m
s
a
m
p
l
e
_
n
o
r
m
a
l
_
d
i
s
t
r
i
b
u
t
i
o
n
(
b
2
)
:
r
e
t
u
r
n
1
2
∑
i
=
1
12
r
a
n
d
(
?
b
,
b
)
\begin{aligned} &Algorithm\quad sample\_normal\_distribution(b^2):\\ &\quad\quad return\quad \frac{1}{2}\sum_{i=1}^{12}rand(-b,b) \end{aligned}
?Algorithmsample_normal_distribution(b2):return21?i=1∑12?rand(?b,b)?
或使用三角形分布实现函数：
A
l
g
o
r
i
t
h
m
s
a
m
p
l
e
_
t
r
i
a
n
g
u
l
a
r
_
d
i
s
t
r
i
b
u
t
i
o
n
(
b
2
)
:
r
e
t
u
r
n
6
2
[
r
a
n
d
(
?
b
,
b
)
+
r
a
n
d
(
?
b
,
b
)
]
\begin{aligned} &Algorithm\quad sample\_triangular\_distribution(b^2):\\ &\quad\quad return\quad \frac{\sqrt6}{2}[rand(-b,b)+rand(-b,b)] \end{aligned}
?Algorithmsample_triangular_distribution(b2):return26??[rand(?b,b)+rand(?b,b)]?
式中，函数
r
a
n
d
(
x
,
y
)
rand(x,y)
rand(x,y)用于在
x
x
x到
y
y
y之间生成一个均匀分布的伪随机数产生器。
算法效果
在平面空间XOY下，设置机器人具有相同的初始姿态 x t ? 1 x_{t-1} xt?1?和控制量 u t u_t ut?，且控制量与初始位姿同闭式计算中相同，则在不同不同里程计噪声参数下，具有不同效果。
当机器人具有中等误差（参数 α 1 \alpha_1 α1? ~ α 6 \alpha_6 α6?），则如图所示：
当机器人具有较大的平移误差（参数 α 1 、 α 2 \alpha_1、\alpha_2 α1?、α2?），但具有较小的角度误差（参数 α 3 、 α 4 \alpha_3、\alpha_4 α3?、α4?），则如图所示：
当机器人具有较小的平移误差（参数
α
1
、
α
2
\alpha_1、\alpha_2
α1?、α2?），但具有较大的角度误差（参数
α
3
、
α
4
\alpha_3、\alpha_4
α3?、α4?），则如图所示：
