标签：cos 概率 模型 机器人 bmatrix alpha theta omega sin
机器人运动
概率机器人学将运动方程推广：由控制噪声或者未建模的外源性影响，控制输出是不确定的。
基础概念
运动学构型
运动学（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），则如图所示：
标签：cos,概率,模型,机器人,bmatrix,alpha,theta,omega,sin 来源： https://blog.csdn.net/weixin_45929038/article/details/122761173
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。