概率运动学
前面的博客中讨论了机器人学中的空间描述与坐标变换，其中讲的是普适性的机器人学，现在我们来讨论简化的情况：平面移动机器人，它仅需2个位置参数和1个姿态参数即可。
假设有平面上移动的机器人，位姿通过二维坐标及其方位角表示：
$$
\begin{equation}
\begin{pmatrix}
x \\
y \\
\theta
\end{pmatrix}
\end{equation}
$$
机器人的方向经常称为方位(bearing) 或航向(heading direction) 。机器人方向 $\theta=0$ 的方向为$x$轴，$\theta = \frac{\pi}{2}$ 方向为 $y$轴方向。
对于机器人所处的环境而言，有时仅需要位置参数即可：
$$ \begin{align}
\begin{pmatrix}
x \\
y
\end{pmatrix}
\end{align} $$
概率运动学模型就是条件概率，在对 $\boldsymbol{x}_{t-1}$ 状态下的机器人执行动作 $\boldsymbol{u}_t$ 后，机器人的运动学状态的后验分布为：
$$ \begin{align}
p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1})
\end{align} $$
注意到，这里的 $\boldsymbol{x}_t$ 和 $\boldsymbol{x}_{t-1}$ 都是机器人位姿矢量， $\boldsymbol{u}_t$ 是运动控制，即机器人动作。实际应用中， $\boldsymbol{u}_t$ 有时由机器人里程计提供。
下面介绍两种特定的概率运动学模型。
速度运动模型
速度运动模型(velocity motion model) 认为可以通过两个速度（旋转速度和平移速度）来控制机器人。许多商业机器人会提供这样的控制接口。
用 $v_t$ 表示 $t$ 时刻的平移速度（translational velocity），用 $\omega_t$ 表示旋转速度（rotational velocity），有：
$$ \begin{align}
u_t = \begin{pmatrix}
v_t \\ \omega_t
\end{pmatrix}
\end{align} $$
规定：逆时针（左转）， $\omega_t$ 为正，向前运动的 $v_t$ 为正。
下面来做纯数学的推导。
先像高中数学/物理一样，假设机器人运动时无噪声，令 $u_t = (v \space \omega)^T$ 表示$t$ 时刻的控制，如果在 $[t-1,t]$ 时间内输入的 $u_t$ 中平移速度和旋转速度都是常量，则机器人运动的轨迹半径为：
$$ \begin{align}
r = \mid\frac{v}{w} \mid
\end{align} $$
旋转速度为0时，轨迹半径为无穷大。我们都知道：
$$ \begin{align}
v=\omega r
\end{align} $$
令机器人初始位姿为 $x_{t-1} = (x \quad y \quad \theta)^T$ ，假设在 $\Delta t$ 时间内保持速度 $(v \quad \omega)^T$ 恒定。机器人轨迹圆弧中心为 $(x_c \enspace y_c)^T$ 。运动 $\Delta t$ 时间后，理想情况下机器人状态为 $x_{t} = (x' \quad y' \quad \theta')^T$ 有：
运动学分析示意图$$ \begin{align}
x_c & = x - \frac{v}{\omega} \sin \theta \\
y_c &= y + \frac{v}{\omega} \cos \theta \\
\begin{pmatrix}
x' \\ y' \\ \theta '
\end{pmatrix} &= \begin{pmatrix} x_c + \frac{v}{\omega} \sin (\theta + \omega \Delta t) \\ y_c - \frac{v}{\omega} \cos (\theta + \omega \Delta t) \\ \omega \Delta t \end{pmatrix} \notag \\
&=\begin{pmatrix}
x \\
y \\
\theta
\end{pmatrix} + \begin{pmatrix} - \frac{v}{\omega} \sin \theta + \frac{v}{\omega} \sin (\theta + \omega \Delta t) \\ \frac{v}{\omega} \cos \theta - \frac{v}{\omega} \cos (\theta + \omega \Delta t) \\ \omega \Delta t \end{pmatrix}
\end{align} $$
现在，我们来考虑引入噪声的情况，假设考虑噪声的实际速度为：
$$ \begin{align}
\begin{pmatrix}
\hat v \\ \hat \omega
\end{pmatrix} = \begin{pmatrix} v \\ \omega \end{pmatrix}
+\begin{pmatrix}
\varepsilon_{\alpha _1 v^2 + \alpha_2 \omega ^2} \\ \varepsilon_{\alpha _3 v^2 + \alpha_4 \omega ^2}
\end{pmatrix}
\end{align} $$
其中， $\varepsilon_{b^2}$ 是一个方差为 $b^2$ 均值为0的误差变量，参数 $\alpha_1 \sim \alpha_4$ 是指定机器人的特定的误差参数。误差 $\varepsilon_{b^2}$ 可以有正态分布和三角分布两种：
$$ \begin{align}
\varepsilon_{b^2}(a)=\frac{1}{\sqrt{2 \pi b^2}} \exp \{- \frac{1}{2} \frac{a^2}{b^2} \} \\
\varepsilon_{b^2}(a) = max \{ 0,\frac{1}{\sqrt{6} b} - \frac{|a|}{6 b^2} \}
\end{align} $$
两种分布的均值都是0，方差都是 $b^2$ ，分布图如下图所示：
正态分布与三角分布.png
因此，在 $x_{t-1} = (x \quad y \quad \theta)^T$ 执行完运动指令 $u_t = (v \space \omega)^T$ 后真实位姿 $x_{t} = (x' \quad y' \quad \theta')^T$ 较好的模型为：
$$ \begin{align}
\begin{pmatrix}
x' \\ y' \\ \theta '
\end{pmatrix} =\begin{pmatrix}
x \\
y \\
\theta
\end{pmatrix} + \begin{pmatrix} - \frac{\hat v}{\hat \omega} \sin \theta + \frac{\hat v}{\hat \omega} \sin (\theta + \hat \omega \Delta t) \\ \frac{\hat v}{\hat\omega} \cos \theta - \frac{\hat v}{\hat\omega} \cos (\theta + \hat \omega \Delta t) \\ \hat \omega \Delta t \end{pmatrix}
\end{align} $$
上式中，我们虽然考虑了噪声，但是仍然有一个假设：机器人运动轨迹为标准的圆弧，这在现实世界中显然是不可能的，这个假设在后面的贝叶斯滤波的状态估计时，可能会产生较大的影响，因此我们要找到不使后验分布退化的描述，假设机器人达到最终位姿时，旋转 $\hat \gamma$ ，不再根据上面的（13）式计算 $\theta '$ ：
$$ \begin{align}
\theta ' &= \theta + \hat \omega \Delta t + \hat\gamma \Delta t \\
\text{其中} \hat \gamma &= \varepsilon_{\alpha_5 v^2 + \alpha_6 \omega^2}
\end{align} $$
这里的 $\alpha_5$ 和 $\alpha_6$ 确定附加的旋转噪声方差的额外的特定机器人参数。
运动模型为：
$$ \begin{align}
\begin{pmatrix}
x' \\ y' \\ \theta '
\end{pmatrix} =\begin{pmatrix}
x \\
y \\
\theta
\end{pmatrix} + \begin{pmatrix} - \frac{\hat v}{\hat \omega} \sin \theta + \frac{\hat v}{\hat \omega} \sin (\theta + \hat \omega \Delta t) \\ \frac{\hat v}{\hat\omega} \cos \theta - \frac{\hat v}{\hat\omega} \cos (\theta + \hat \omega \Delta t) \\ \hat \omega \Delta t + \hat\gamma \Delta t \end{pmatrix}
\end{align} $$
现在我们不考虑最终的姿态 $\theta '$ ，机器人从 $x_{t-1} = (x \quad y \quad \theta)^T$ 移动到 $x_t = (x' \quad y')^T$ ，假设轨迹圆心为 $(x^{\ast} \quad y^{\ast})^T$ 并由下式给出：
$$ \begin{align}
\begin{pmatrix}
x^{\ast} \\
y^{\ast}
\end{pmatrix} = \begin{pmatrix} x \\ y \end{pmatrix} + \begin{pmatrix} -\lambda \sin \theta \\ \lambda \cos \theta \end{pmatrix} =\begin{pmatrix} \frac{x+x'}{2} +\mu (y-y') \\ \frac{y + y'}{2} + \mu (x'-x) \end{pmatrix}
\end{align} $$
对未知的 $\lambda ,\mu \isin \Re$ 第一个等式表示是圆心与机器人初始方向正交，第二个等式是约束圆心在起点和终点线段的中垂线上。上式可以求出解析解：
$$ \begin{align}
\mu &= \frac{1}{2} \frac{(x-x') \cos \theta + (y - y') \sin \theta }{(y - y') \cos \theta - (x- x') \sin \theta } \\
&\Downarrow \notag \\
\begin{pmatrix}
x^{\ast} \\
y^{\ast}
\end{pmatrix} & = \begin{pmatrix} \frac{x+x'}{2} +\frac{1}{2} \frac{(x-x') \cos \theta + (y - y') \sin \theta }{(y - y') \cos \theta - (x- x') \sin \theta } (y-y') \\ \frac{y + y'}{2} + \frac{1}{2} \frac{(x-x') \cos \theta + (y - y') \sin \theta }{(y - y') \cos \theta - (x- x') \sin \theta } (x'-x) \end{pmatrix} \\
\end{align} $$
圆的半径有：
$$ \begin{align}
r^{\ast} &=\sqrt{(x-x^{\ast} )^2 + (y - y^{\ast} )^2}=\sqrt{(x'-x^{\ast} )^2 + (y' - y^{\ast} )^2} \\
\Delta \theta& = atan2 (y' - y^{\ast} ,x' - x^{\ast}) - atan2 (y- y^{\ast},x-x^{\ast}) \\
atan2(y,x) &= \begin{cases}
atan(y/x) &\text{if } x > 0 \\
sign(y) (\pi-atan(|y/x|)) &\text{if } x < 0 \\
0 &\text{if } x=y=0 \\
sign(y) \pi /2 &\text{if } x=0,y \not= 0
\end{cases}
\end{align} $$
机器人移动距离为：
$$ \begin{align}
\Delta dist = r^{\ast} \Delta \theta
\end{align} $$
现在由 $\Delta dist$ 和 $\Delta \theta$ 可以容易地计算出线速度 $\hat v$ 和角速度 $\hat \omega$ ：
$$ \begin{align}
\hat u_t = \begin{pmatrix}
\hat v \\ \hat \omega
\end{pmatrix} = \Delta t ^{-1} \begin{pmatrix}
\Delta dist \\ \Delta \theta
\end{pmatrix}
\end{align} $$
由上面的式（14）、（15）得到旋转速度：
$$ \begin{align}
\hat \gamma = \Delta t^{-1} (\theta ' \quad \theta ) - \hat \omega
\end{align} $$
得到运动误差
$$ \begin{align}
v_{err} &= v-\hat v \\
\omega _{err} &= \omega -\hat \omega \\
\gamma _{err} &= \hat \gamma
\end{align} $$
再由上面的式（10）、（15）的误差模型，得到误差的概率：
$$ \begin{align}
\varepsilon_{\alpha _1 v^2 + \alpha_2 \omega ^2} (v_{err})\\ \varepsilon_{\alpha _3 v^2 + \alpha_4 \omega ^2}(\omega _{err}) \\
\varepsilon_{\alpha_5 v^2 + \alpha_6 \omega^2} (\gamma _{err})
\end{align} $$
这里不同来源的误差是彼此独立的，于是我们需要的概率就有：
$$ \begin{align}
p(x_t | u_t ,x_{t-1}) = \varepsilon_{\alpha _1 v^2 + \alpha_2 \omega ^2} (v_{err}) \varepsilon_{\alpha _3 v^2 + \alpha_4 \omega ^2}(\omega _{err}) \varepsilon_{\alpha_5 v^2 + \alpha_6 \omega^2} (\gamma _{err})
\end{align} $$
现在我们来看算法流程：
速度运动模型算法.png
计算两种不同的密度分布的算法关于采样，下图的采样算法sample_motion_model_velocity 实现了前向模型。
sample_motion_model_velocity.png下图的sample_normal_distribution 算法实现了对一个正态分布采样的通用近似。这个近似利用中心极限定理，即任意非退化随机变量的平均都收敛到一个正态分布。通过将12个均匀分布求平均，算法产生近似正态分布的值；尽管技术上，得到的值总是位于[ - 2b, 2b] 。最后的sample_triangular_distribution 算法实现一个三角形分布的采样。
sample_normal_distribution.png里程计运动模型
里程通常可通过整合轮子的编码信息来得到；许多商业机器人在固定的时间间隔（如0. ls） 产生这样的积分位姿估计。这就引出了本章讨论的第二个运动模型，里程计运动模型(odometry motion model) 。里程计运动模型用距离测量代替控制。实际经验表明，里程计运动模型虽然仍存在误差，但通常比速度运动模型更精确。
里程计模型使用相对运动信息(relative motion information) , 该信息由机器人内部里程计测量。更具体地，在时间间隔 $(t-1,t]$ 内，机器人位姿由$x_{t-1}$ 前进到 $x_t$ ，里程计反馈了从 $\bar{x}_{t-1} = (\bar{x} \medspace \bar{y} \medspace \bar{\theta})^T$ 到 $\bar{x}_{t} = (\bar{x}' \medspace \bar{y}' \medspace \bar{\theta}')^T$ 的相对前进，不过该信息是机器人内部坐标系的信息，该坐标系与世界坐标系的关系是未知的。运动信息定义为：
$$ \begin{align}
u_t = \begin{pmatrix} \bar{x}_{t-1} \\ \bar{x}_t \end{pmatrix}
\end{align} $$
为了提取出相对距离信息，现在将 $u_t$ 分解：旋转、直线运动（平移）和另一个旋转，初始旋转 $\delta _{rot1}$ 平移 $\delta _{trans}$ 和第二次旋转 $\delta _{rot2}$ ，显然每对状态信息都具有唯一的分解，这是易证的。
在进行数学推导之前，我们先看一下算法流程：
基于里程计信息的算法算法的输入信息是初始位姿$x_{t-1}$ 、从里程计获取到的一对位姿 $u_t = (\bar{x}_{t-1} \medspace \bar{x})^T$ 和一个假定的最终姿态 $x_t$ ，输出是数值概率 $p(x_t|u_t,x_{t-1})$ 。如果使用粒子滤波进行采样，下图是一种采样算法：
sample_motion_model_odometry数学推导
从里程计的读数进行分解：
$$ \begin{align}
\delta _{rot1} &= atan2 (\bar{y} '-\bar{y}, \bar{x}' - \bar{x}) - \bar{\theta} \\
\delta _{trans} &= \sqrt{(\bar{x} - \bar{x}')^2 + (\bar{y} -\bar{y}')^2} \\
\delta _{rot2} &= \bar{\theta}' - \bar{\theta} - \delta _{rot1}
\end{align} $$
为了建立运动误差的模型，假定旋转和平移的“真实值”是用测量值减去均值为 $0$ 、方差为 $b^2$ 的独立噪声 $\varepsilon_{b^2}$ 获得，
$$ \begin{align}
\bar{\delta}_{rot1} &= \delta _{rot1} - \varepsilon_{\alpha _1 \delta^2_{rot1} + \alpha_2 \delta ^2_{trans}} \\
\bar{\delta}_{trans} &= \delta _{trans} - \varepsilon_{\alpha _3 \delta^2_{trans} + \alpha_4 \delta ^2_{rot1} + \alpha_4 \delta ^2_{rot2}} \\
\bar{\delta}_{rot2} &= \delta _{rot2} -\varepsilon_{\alpha _1 \delta^2_{rot2} + \alpha_2 \delta ^2_{trans}}
\end{align} $$
参数 $\alpha_1 \sim \alpha_4$ 是机器人的误差参数，它们指定运动的累计误差。
$$ \begin{align}
\begin{pmatrix}
x' \\ y' \\ \theta '
\end{pmatrix} =\begin{pmatrix}
x \\
y \\
\theta
\end{pmatrix} + \begin{pmatrix} \bar{\delta}_{trans} \cos (\theta + \bar{\delta}_{rot1}) \\ \bar{\delta}_{trans} \sin (\theta+\bar{\delta}_{rot2}) \\ \bar{\delta}_{rot1} +\bar{\delta}_{rot2} \end{pmatrix}
\end{align} $$
算法sample_motion_model_odometry实现了上面的式（34）~式（40），对于算法motion_model_odometry的第5-7行，假设 $x_t$ 是最终的实际位姿，那么里程计误差为：
$$ \begin{align}
\delta _{rot1} - \bar{\delta}_{rot1} \\
\delta _{trans} - \bar{\delta}_{trans} \\
\delta _{rot2} - \bar{\delta}_{rot2}
\end{align} $$
前面式（37）-（39）的误差模型意味着这些误差的概率可以由上面定义的分布 $\varepsilon$ 给定：
$$ \begin{align}
p_1 &= \varepsilon_{\alpha _1 \delta^2_{rot1} + \alpha_2 \delta ^2_{trans}} (\delta _{rot1} - \bar{\delta}_{rot1}) \\
p_2 &= \varepsilon_{\alpha _3 \delta^2_{trans} + \alpha_4 \delta ^2_{rot1} + \alpha_4 \delta ^2_{rot2}} (\delta _{trans} - \bar{\delta}_{trans}) \\
p_3 &= \varepsilon_{\alpha _1 \delta^2_{rot2} + \alpha_2 \delta ^2_{trans}} (\delta _{rot2} - \bar{\delta}_{rot2})
\end{align} $$
因为误差彼此之间相互独立，因此联合误差的概率为 $p_1 p_2 p_3$ 。
地图信息
许多情况下，总是给定一个地图 $\boldsymbol{m}$ ，它可能包括关于机器人能或不能通过的空间的信息。地图信息能给机器人位姿提供更多约束条件。此时模型表示为 $p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1},\boldsymbol{m})$ ，如果地图 $\boldsymbol{m}$ 携带有关位姿估计的信息，有：
$$ \begin{align}
p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1}) \not = p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1},\boldsymbol{m})
\end{align} $$
基于地图的运动模型计算是指，被放置在地图 $\boldsymbol{m}$ 所表示的环境里的机器人在位姿 $\boldsymbol{x}_{t-1}$ 处执行动作 $\boldsymbol{u}_t$ 达到位姿 $\boldsymbol{x}_t$ 的可能性。闭式计算该运动模型是很困难的。
幸运的是，存在一种基于地图运动模型的高效近似，如果 $\boldsymbol{x}_{t-1}$ 和 $\boldsymbol{x}_t$ 之间距离很小（如比机器人直径的一半还小）时，该近似的效果很好，这种近似将基于地图的运动模型分解成两部分：
$$ \begin{align}
p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1},\boldsymbol{m}) = \eta \frac{p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1})p(\boldsymbol{x}_t|\boldsymbol{m})} {p(\boldsymbol{x}_t)}
\end{align} $$
通常 $p(x_t)$ 也是均匀的，可以将它归入前面的 $\eta$ 中，然后用无地图估计 $p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1})$ 简单地乘以机器人在地图某位置的概率，如果地图是占用栅格地图，对应位置为占用栅格时，$p(\boldsymbol{x}_t|\boldsymbol{m}) =0$ ，否则为一个常数。下图是这种算法的流程图：
motion_model_with_map$$ \begin{align} \begin{split}
p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1},\boldsymbol{m}) = \eta p(\boldsymbol{m} |\boldsymbol{x}_t,\boldsymbol{u}_t,\boldsymbol{x}_{t-1}) p(\boldsymbol{x}_t| \boldsymbol{u}_t,\boldsymbol{x}_{t-1})
\end{split} \end{align} $$
如果用 $p(\boldsymbol{m}|\boldsymbol{x}_t)$ 近似 $p(\boldsymbol{m} |\boldsymbol{x}_t,\boldsymbol{u}_t,\boldsymbol{x}_{t-1})$ ，并且注意到 $p(\boldsymbol{m})$ 相当于要求的后验是常数，则：
$$ \begin{align} \begin{split}
p(\boldsymbol{x}_t | \boldsymbol{u}_t, \boldsymbol{x}_{t-1},\boldsymbol{m}) &= \eta p(\boldsymbol{m}|\boldsymbol{x}_t) p(\boldsymbol{x}_t| \boldsymbol{u}_t,\boldsymbol{x}_{t-1}) \\
&=\eta \frac{p(\boldsymbol{x}_t| \boldsymbol{m}) p(\boldsymbol{m})}{p(\boldsymbol{x}_t)} p(\boldsymbol{x}_t| \boldsymbol{u}_t,\boldsymbol{x}_{t-1}) \\
& = \eta \frac{p(\boldsymbol{x}_t| \boldsymbol{m}) p(\boldsymbol{x}_t| \boldsymbol{u}_t,\boldsymbol{x}_{t-1})}{p(\boldsymbol{x}_t)}
\end{split} \end{align} $$
在使用近似时，忽略了 $\boldsymbol{u}_t$ 和 $\boldsymbol{x}_{t-1}$ 两项，丢弃了去往 $\boldsymbol{x}_t$ 的机器人路径相关的信息，所以需要使 $\boldsymbol{x}_{t-1}$ 和 $\boldsymbol{x}_t$ 之间距离很小，算法的更新频率要比较高。
这个分析说明了有关算法实现的一个微妙之处，要特别注意更新频率。与那些只是偶尔更新的贝叶斯滤波相比，经常更新的贝叶斯滤波可能会产生出根本不同的结果。