概率机器人速度运动模型
由于移动机器人考虑跟多的是在空间中的移动，因此，更多的情况是使考虑移动机器人的运动学描述。假设，移动机器人的运动是三维空间的刚体运动，移动机器人与环境的相对位置可以通过二位平面上的位置来反映。它包括相对外部坐标系的二维平面坐标及其方位角。前两个用 $x$ 和 $y$ 表示（不要与状态 $x$ ,混淆），后者用 $\theta$ 表示，机器人的位姿用下面向量描述：
$$
\left(\begin{array}{cccc}
x\\
y\\
\theta
\end{array}\right)
$$
在《概率机器人》的书里面，讲解了2种概率机器人模型：1.假定运动数据 $u_t$ 指定了机器人电动机的速度，称之为速度模型；2.模型假设机器人具有测距信息，称之为里程计模型。下面对这两种模型分别进行个人的解释说明。
速度运动模型
在速度运动模型中，假定机器人是可以通过旋转角速度 $\omega_t$ 和平移速度 $v_t$ 来进行控制。针对机器人的状态空间模型，这两个速度合成的一个向量就是状态空间中的 $u_t$ ，可以用下面的式子表示：
$$
u_t = \left(\begin{array}{cccc}
v_t\\
\omega_t
\end{array}\right)
$$
规定：逆时针旋转为角速度的正方向，向前运动是平移速度的正方向。
速度运动模型的闭式计算
根据书上的顺序，先介绍速度运动模型的闭式计算算法：
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Algorithm motion_model_velocity} (x_t, u_t, x_{t-1})\\
\text{2:} \qquad \qquad & \mu = \frac{1}{2} \frac{(x-x')\cos{\theta}+(y-y')\sin{\theta}}{(y-y')\cos{\theta}-(x-x')\sin{\theta}}\\
\text{3:} \qquad \qquad & x^{ * } = \frac{x+x'}{2} + \mu(y-y')\\
\text{4:} \qquad \qquad & y^{ * } = \frac{y+y'}{2} + \mu(x'-x)\\
\text{5:} \qquad \qquad & r^{ * } = \sqrt{(x-x^{ * })^2 + (y-y^{ * })^2}\\
\text{6:} \qquad \qquad & \Delta \theta = atan2(y'-y^{ * }, x'-x^{ * }) - atan2(y-y^{ * }, x-x^{ * })\\
\text{7:} \qquad \qquad & \hat{v} = \frac{\Delta \theta}{\Delta t}r^{ * }\\
\text{8:} \qquad \qquad & \hat{\omega} = \frac{\Delta \theta}{\Delta t}\\
\text{9:} \qquad \qquad & \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega}\\
\text{10:} \qquad \qquad & \text{return }prob(v-\hat{v}, \alpha_1 v^2 + \alpha_2 \omega) \cdot prob(\omega - \hat{\omega}, \alpha_3 v^2 + \alpha_4 \omega^2) \cdot prob(\hat{\gamma}, \alpha_5 v^2 + \alpha_6 \omega^2)
\end{align}
$$
在上面的算法中， $u_t$用速度向量 $(v \quad \omega)^T$ 表示， $x_t$ 用向量 $(x' \quad y' \quad \theta')^T$ 表示， $x_{t-1}$ 用向量 $(x \quad y \quad \theta)^T$ 表示。在算法最后的输出函数 $prob(\mathbf{\cdot})$ 用来建立运动误差。它计算其参数 $x$ 以零为中心方差为 $b^2$ 的随机变量下的概率，可以由标准正态分布和三角形分布获得这个值。具体的算法，如下表示：
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Algorithm prob_normal_distributiion} (a, b^2):\\
\text{2:} \qquad \qquad & \text{return } \frac{1}{\sqrt{2 \pi b^2}}exp[ -\frac{1}{2} \frac{a^2}{b^2} ]\\
\text{3:} \qquad \qquad & \text{Algorithm prob_triangular_distributiion} (a, b^2):\\
\text{4:} \qquad \qquad & \text{return } max [ 0, \frac{1}{\sqrt{6}b}-\frac{|a|}{6b^2} ]
\end{align}
$$
速度运动模型的采样算法
个人认为，采样算法主要是为了满足粒子滤波算法的需求，从而形成的采样算法。因为，粒子滤波的本质是用一个样本群表示一个概率分布，在粒子滤波的推导过程中，可以发现粒子滤波的第一步概率获取就是获取运动模型的概率。下面直接给出他的采样算法:
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Algorithm sample_motion_model_velocity}(u_t, x_{t-1}):\\
\text{2:} \qquad \qquad & \hat{v} = v + sample(\alpha_1 v^2 + \alpha_2 \omega^2)\\
\text{3:} \qquad \qquad & \hat{\omega} = \omega + sample(\alpha_3 v^2 + \alpha_4 \omega^2)\\
\text{4:} \qquad \qquad & \hat{\gamma} = sample(\alpha_5 v^2 + \alpha_6 \omega^2)\\
\text{5:} \qquad \qquad & x' = x - \frac{\hat{v}}{\hat{\omega}}\sin{\theta} + \frac{\hat{v}}{\hat{\omega}}\sin{(\theta + \hat{\omega} \Delta t)}\\
\text{6:} \qquad \qquad & y' = y + \frac{\hat{v}}{\hat{\omega}}\sin{\theta} - \frac{\hat{v}}{\hat{\omega}}\cos{(\theta + \hat{\omega} \Delta t)}\\
\text{7:} \qquad \qquad & \theta' = \theta + \hat{\omega} \Delta t + \hat{\gamma}\Delta t\\
\text{8:} \qquad \qquad & \text{return } x_t = (x',y',\theta')^T
\end{align}
$$
上面算法的 $sample(\cdot)$ 可以根据下面的采样算法来进行采样：
$$
\begin{align}
\text{1:} \qquad \qquad & \text{Algorithm sample_normal_distribution}(b^2):\\
\text{2:} \qquad \qquad & \text{return} \frac{1}{2} \sum_{i=1}^{12}rand(-b,b)\\
\\
\text{3:} \qquad \qquad & \text{Algorithm sample_triangular_distribution}(b^2):\\
\text{4:} \qquad \qquad & \text{return} \frac{\sqrt{6}}{2} [rand(-b,b) + rand(-b,b)]
\end{align}
$$
速度运动模型的数学推导
理想运动
在转向概率情况前，先从一种理想的无噪声的机器人运动学开始研究。令 $u_t = (v \quad \omega)^T$表示时刻 $t$ 的控制。如果两种速度在整个时间间隔 $[t-1, t]$ 都是固定的值，则机器人以如下半径的圆运动：
$$
r = |\frac{v}{\omega}|
$$
所以，任意半径为 $r$ 圆形轨迹运动的对象在平移速度 $v$ 和旋转速度 $\omega$ 之间都遵循的一般关系：
$$
v = \omega r
$$
令 $x_{t-1} = (x,y,\theta)^T$ 是机器人的初始位姿，并假定在 $\Delta t$ 时间内保持速度 $(v,\omega)^T$ 恒定。使用 $(x_c, y_c)^T$ 表示运动的圆心坐标，可以利用运动时旋转的圆心 $(x_c,y_c)^T$ 作为中间变量，将运动后的姿态 $(x',y',\theta ')^T$ 与运动前的姿态 $(x,y,\theta)^T$ 通过下面的式子联系起来。
$$
\begin{align}
\left(\begin{array}{ccc}
x'\\
y'\\
\theta'
\end{array}\right) & =
\left(\begin{array}{ccc}
x_c + \frac{v}{\omega}\sin{(\theta + \omega \Delta t)}\\
y_c - \frac{v}{\omega}\cos{(\theta + \omega \Delta t)}\\
\theta + \omega \Delta t
\end{array}\right)\\
& =
\left(\begin{array}{ccc}
x\\
y\\
\theta
\end{array}\right) +
\left(\begin{array}{ccc}
-\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{array}\right)
\end{align}
$$
真实运动
实际上，机器人的运动是受噪声影响的。真实的速度与给定的速度（或者测量的速度，在机器人有测量速度的传感器的情况下）是不同的。将这种不同建模为以 $0$ 为中心的具有有限方差的随机变量。更精确的，假定实际的速度由下式给定：
$$
\left(\begin{array}{ccc}
\hat{v}\\
\hat{\omega}
\end{array}\right) =
\left(\begin{array}{ccc}
v\\
\omega
\end{array}\right) +
\left(\begin{array}{ccc}
\varepsilon_{\alpha_1 v^2 + \alpha_2 \omega^2}\\
\varepsilon_{\alpha_3 v^2 + \alpha_4 \omega^2}
\end{array}\right)
$$
参数 $\alpha_1 \sim \alpha_4(\alpha_i \geq 0,i=1,2,\dots,4)$ 是指定的机器人误差参数。其中，$\varepsilon_{b^2}$ 可以从三角分布和正态分布中选择一种来表示误差。
$$
\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|}{6b^2}]
$$
在理想的速度模型情况下，使用 $(\hat{v},\hat{\omega})$ 替换原来的 $(v,\omega)$ ，就可以得到真实情况下的数学模型。
$$
\begin{align}
\left(\begin{array}{ccc}
x'\\
y'\\
\theta'
\end{array}\right) =
\left(\begin{array}{ccc}
x\\
y\\
\theta
\end{array}\right) +
\left(\begin{array}{ccc}
-\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{array}\right)
\end{align}
$$
最终方向
以上给出的两个方程式都精确地描述了机器人真实地在一个半径为 $ r=\frac{\hat{v}}{\hat{\omega}}$ 形轨迹上移动时的最终位置。而这个圆弧段的半径和移动的距离是受到控制噪声的影响的，轨迹是圆这个事实并不存在。为了推广运动模型，假设机器人到达它的最终位姿时，旋转 $ \hat{\gamma} $ 。通过下式将模型修改为：
$$
\theta' = \theta + \hat{\omega} \Delta t + \hat{\gamma} \Delta t \\
\hat{\gamma} = \varepsilon_{\alpha_5} v^2 + \varepsilon_{\alpha_6} \omega^2
$$
此处的 $\alpha_5$ 和 $\alpha_6$ 是确定附加的旋转噪声方差的额外的特定机器人参数。可以得到速度运动模型的最终模型为下式：
$$
\begin{align}
\left(\begin{array}{ccc}
x'\\
y'\\
\theta'
\end{array}\right) =
\left(\begin{array}{ccc}
x\\
y\\
\theta
\end{array}\right) +
\left(\begin{array}{ccc}
-\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{array}\right)
\end{align}
$$
$p(x_t|u_t, x_{t-1})$ 的计算
motion_model_velocity 算法(最开始的闭式运算)实现了对给定 $x_{t-1} = (x,y,\theta)^T$ 、 $u_t = (v,\omega)^T$ 和 $x_t = (x',y',\theta')^T$ 值时 $p(x_t|u_t, x_{t-1})$ 的计算。
计算在时间单元 $\Delta t$ 内，控制作用 $u_t = (v,\omega)^T$ 下将机器人位姿从 $x_{t-1} = (x,y,\theta)^T$ 变成位置 $x_t = (x',y',\theta')^T$ 的概率 $ p(x_t|u_t, x_{t-1}) $ 。接下来，确定机器人要获得方向 $\theta'$ 需要最终旋转的 $\hat{\gamma}$ 。基于这些，然后可很容易计算出想得到的概率 $ p(x_t|u_t, x_{t-1}) $ 。
模型假设机器入在 $\Delta t$ 内移动速度是恒定的，形成一个圆形的轨迹。对一个从 $x_{t-1}=(x,y,\theta)^T$ 移动到 $x_t = (x',y')^T$ 的机器人，圆的中心定义为 $(x^{ * }, y^{ * })^T$ 并由下式给出：
$$
\left(\begin{array}{ccc}
x^{ * } \\
y^{ * }
\end{array}\right) =
\left(\begin{array}{ccc}
x \\
y
\end{array}\right) +
\left(\begin{array}{ccc}
-\lambda \sin{\theta}\\
\lambda \cos{\theta}
\end{array}\right) =
\left(\begin{array}{ccc}
\frac{x+x'}{2} + \mu(y-y')\\
\frac{y+y'}{2} + \mu(x'-x)
\end{array}\right)
$$
利用圆周运动的几何特性，可以解出 $\mu$ 的结果如下式：
$$
\mu = \frac{1}{2} \frac{(x-x')\cos{\theta} + (y-y')\sin{\theta}}{(y-y')\cos{\theta} - (x-x')\sin{\theta}}
$$
圆的半径可由欧拉距离给定：
$$
r^{ * } = \sqrt{(x-x^{ * })^2 + (y-y^{ * })^2} = \sqrt{(x' - x^{ * })^2 + (y' - y^ { * })^2}
$$
并且：
$$
\Delta \theta = atan2(y' - y^{ * }, x' - x^{ * }) - atan2(y-y^{ * }, x-x^{ * })
$$
既然假定机器人遵循一个圆形轨迹，沿该圆在 $x_t$ 和 $x_{t-1}$ 之间的平移距离为
$$
\Delta dist = r^{ * } \Delta \theta
$$
从 $ \Delta dist$ 和 $\Delta \theta$ ，可以很容易地计算速度 $\hat{v}$ 和 $\hat{\omega}$：
$$
\hat{u}_t =
\left( \begin{array}
\hat{v} \\
\hat{\omega}
\end{array} \right) = \Delta t^{-1}
\left( \begin{array}
\Delta dist
\Delta \theta
\end{array} \right)
$$
旋转速度为：
$$
\hat{\gamma} = \Delta t^{-1} (\theta' - \theta) - \hat{\omega}
$$
运动误差由下式定义：
$$
\begin{align}
v_{err} &= v - \hat{v}\\
\omega_{err} &= \omega - \hat{\omega}\\
\gamma_{err} &= \hat{\gamma}
\end{align}
$$
这些误差具有如下的概率：
$$
\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}
$$
最后可以获得概率 $p(x_t|u_t, x_{t-1})$ 如下式：
$$
p(x_t|u_t, x_{t-1}) = \varepsilon_{\alpha_1 v^2 + \alpha_2 \omega^2}(v_{err}) \cdot \varepsilon_{\alpha_3 v^2 + \alpha_4 \omega^2}(\omega_{err}) \cdot \varepsilon_{\alpha_5 v^2 + \alpha_6 \omega^2}(\gamma_{err})
$$
