机器人运动
概率机器人学将运动方程推广：由控制噪声或者未建模的外源性影响，控制输出是不确定的。
基础概念
运动学构型
运动学（Kinematics）：描述控制行为对机器人构型产生影响的微积分。
- 位姿：能够表示机器人位置和航向的物理量
- 平面环境下通常限定为三维向量： [ X Y θ ] T \begin{bmatrix}X&Y&\theta\end{bmatrix}^T [XYθ]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=[XYθ]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}
vtr=ωt⋅r=ωtvt
过运动圆弧中心
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}
{XCYC==X−ωtvt⋅sinθY−ωtvt⋅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+ωtvtsin(θ+ωtΔt)YC=Y+ωtvtcos(θ+ωtΔt)θ+ωΔt⎦⎤⎣⎡XYθ⎦⎤+⎣⎡−ωtvtsinθ+ωtvtsin(θ+ωtΔt)ωtvtcosθ−ωtvtcos(θ+ω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]+[ϵα1vt2+α2ωt2ϵα3vt2+α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
1exp(−21b2x2)
如图为均值为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θ⎦⎤+⎣⎡−ω^tv^tsinθ+ω^tv^tsin(θ+ωtΔt)ω^tv^tcosθ−ω^tv^tcos(θ+ω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ϵα5vt2+α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θ⎦⎤+⎣⎡−ω^tv^tsinθ+ω^tv^tsin(θ+ωtΔt)ω^tv^tcosθ−ω^tv^tcos(θ+ω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=[XYθ]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)=⎩⎪⎨⎪⎧−101x>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}
ϵα1vt2+α2ωt2(verr)ϵα3vt2+α4ωt2(ωerr)ϵα5vt2+α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)=ϵα1vt2+α2ωt2(verr)⋅ϵα3vt2+α4ωt2(ωerr)⋅ϵα5vt2+α6ωt2(γerr)
算法设计
系统输入: 初始位姿 x t − 1 = [ X Y θ ] T x_{t-1}=\begin{bmatrix}X&Y&\theta\end{bmatrix}^T xt−1=[XYθ]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θ′−θ−ω^treturnprob(vt−v^t,α1vt2+α2ωt2)⋅prob(ωt−ω^t,α3vt2+α4ωt2)⋅prob(γ^t,α5vt2+α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
1exp(−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=[XYθ]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(α1vt2+α2ωt2)ω^t=ωt+sample(α3vt2+α4ωt2)γ^t=sample(α5vt2+α6ωt2)x′=x−ω^tv^tsinθ+ω^tv^tsin(θ+ω^tΔt)y′=y+ω^tv^tcosθ−ω^tv^tcos(θ+ω^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):return21i=1∑12rand(−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），则如图所示：