Motion Model

在位姿 \(\begin{pmatrix} x \\ y \\ \theta \end{pmatrix}\) 中， \(\begin{pmatrix} x \\ y \end{pmatrix}\) 称为 Location，而 \(\theta\) 称为 Bearing 或者 Heading Direction。

首先，考虑没有误差时的理想情况。

已知位姿 \(x_{t-1} = (x \; y \; \theta)^{\mathsf{T}}\) 以及线速度 \(v_t\) 和角速度 \(\omega_t\) ，经过很短的一段时间 \(\Delta t\) ，我们可以推导出机器人的新位姿。

当角速度 \(\omega_t\) 不为零时，机器人绕圆运动，圆的半径可以这样计算： \(r = \left| \frac{v}{\omega} \right|\) （当角速度为零时，是直线运动，可以认为它绕无穷大的圆运动）。圆心的坐标可以这样计算：
\[\begin{aligned} x_c & = x - \frac{v}{\omega} \sin \theta \\ y_c & = y + \frac{v}{\omega} \cos \theta \\ \end{aligned}\]

经过时间 \(\Delta t\) 后，机器人和圆心连线所转过的角位移 \(\Delta \theta\) 为：
\[\Delta \theta = \omega \Delta t\]
从而机器人的新位姿为：
\[\begin{aligned} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} & = \begin{pmatrix} x_c + r \sin(\theta + \Delta \theta) \\ y_c - r \cos(\theta + \Delta \theta) \\ \theta + \Delta \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) \\ \theta + \omega \Delta t \end{pmatrix} \\ \end{aligned}\]
把前面计算的圆心坐标代入上式，可得机器人的新位姿为：
\[\begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \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}\]

现在，我们考虑有误差的情况。
现实中，我们无法知道真实的线速度和角速度，我们得到的值和真实值之间会有误差。假设我们得到的线速度为 \(v\) ，角速度为 \(\omega\) ，真实的线速度和角速度可以这样表达：
\[\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}\]
其中，参数 \(\alpha_1, \cdots, \alpha_4\) 是与机器人相关的误差参数，如果机器人越精确，则这些误差应该设置得越小。 \(\varepsilon_{b^2}\) 是均值为零，方差为 \(b^2\) 的正态分布（也可以用三角形分布来替代）。
相应地，机器人的新位姿为：
\[\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}\]

其实，前面推导中隐含假设 \(\Delta t\) 内速度是不变的（这时轨迹会是圆），但这个假设并不准确。为了使模型更准确，我们一般还要 假设机器人在到达最终位置后，还会有额外的方向旋转。 即：
\[\theta' = \theta + \hat{\omega} \Delta t + \hat{\gamma} \Delta t\]
其中， \(\hat{\gamma} = \varepsilon_{\alpha_5 v^2 + \alpha_6 \omega^2}\) ，参数 \(\alpha_5, \alpha_6\) 也是与机器人相关的参数。

至此，图 2 所示 Velocity Motion Model 抽样算法已经推导完毕。

如何得到 Velocity Motion Model 解析形式解呢？如果系统没有误差，则非常简单。由前面的推导可知当新的位姿为 \(\begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \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}\) 时概率 \(p(x_t \mid x_{t-1}, u_t)\) 为 1，其它情况概率 \(p(x_t \mid x_{t-1}, u_t)\) 为 0。

真实系统是有误差的，这时我们的思路是： 既然线速度、角速度是有误差的，我们将先利用其它信息求解出它们的一个理论估计值，再和测量值一起对误差进行建模。

线速度、角速度的估计值为：
\[\begin{pmatrix} \hat{v} \\ \hat{\omega} \end{pmatrix} = \begin{pmatrix} \frac{\Delta \text{dist}}{\Delta t} \\ \frac{\Delta \theta}{\Delta t} \end{pmatrix}\]
其中， \(\Delta \text{dist}\) 是 \(\Delta t\) 时间内圆弧上的弧线长。弧线长可以这样求解： \(\Delta \text{dist} = r^{*} \Delta \theta\) 。
至此，要求解线速度、角速度的估计值只用求解圆的半径 \(r^{*}\) 以及转向角的变化 \(\Delta \theta\) 。

下面通过求解圆心坐标（最终目的是求解线速度、角速度的理论估计值，不能利用线速度、角速度的测量值）来求解圆的半径 \(r^{*}\) 以及转向角的变化 \(\Delta \theta\) 。
假设圆心为 \((x^* \; y^*)^{\mathsf{T}}\) ，由于圆心连线和运行初始方向垂直，有：
\[\begin{pmatrix} x^* \\ y^* \end{pmatrix} = \begin{pmatrix} x \\ y \end{pmatrix} + \begin{pmatrix} -\lambda \sin\theta \\ \lambda \cos \theta \end{pmatrix}\]
又由于圆心在点 \((x \; y)^{\mathsf{T}}\) 和点 \((x' \; y')^{\mathsf{T}}\) 所确定线段的中垂线上，所以有：
\[\begin{pmatrix} x^* \\ y^* \end{pmatrix} = \begin{pmatrix} \frac{x+x'}{2} + \mu (y-y') \\ \frac{y+y'}{2} + \mu(x' - x) \end{pmatrix}\]

联立上面两个等式，可以求得：
\[\mu = \frac{1}{2} \frac{(x-x') \cos\theta + (y-y')\sin\theta}{(y-y')\cos\theta - (x-x')\sin\theta}\]

从而，圆心坐标 \((x^* \; y^*)^{\mathsf{T}}\) 为：
\[\begin{pmatrix} x^* \\ y^* \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}\]

从而，圆的半径 \(r^*\) 为：
\[r^* = \sqrt{(x-x^*)^2 + (y-y^*)^2} = \sqrt{(x'-x^*)^2 + (y' - y^*)^2}\]
转向角变化 \(\Delta \theta\) 为：
\[\Delta \theta = \text{atan2}(y' - y^*, x'-x^*) - \text{atan2}(y-y^*, x-x^*)\]

至此，线速度、角速度的估计值 \(\begin{pmatrix} \hat{v} \\ \hat{\omega} \end{pmatrix}\) 已经可以求出。

误差：
\[\begin{aligned} v_{err} &= v - \hat{v} \\ \omega_{err} &= \omega - \hat{\omega} \\ \end{aligned}\]

和推导抽样算法时类似，对额外的方向旋转也建立误差：
\[\gamma_{err} = \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega}\]

理想情况下，上面 3 个误差都为 0。现实中，我们假设它们相互独立，服从均值为零的正态分布。 \(p(x_t \mid x_{t-1}, u_t)\) 是这三个误差分布的乘积。
\[p(x_t \mid x_{t-1}, u_t) = \text{prob}(v- \hat{v}, \alpha_1 v^2 + \alpha_2 \omega^2) \cdot \text{prob}(\omega- \hat{\omega}, \alpha_3 v^2 + \alpha_4 \omega^2) \cdot \text{prob}(\hat{\gamma}, \alpha_5 v^2 + \alpha_6 \omega^2)\]

其中， \(\text{prob}(a,b)\) 表示使参数 \(a\) 的均值为 0，方差为 \(b\) 的概率分布。

至此，图 5 所示 Velocity Motion Model 算法已经推导完毕。

考虑两轮机器人（为了稳定性，一般是三个轮，但后面那个轮一般不配置动力），如图 7 所示。

设左轮和右轮速度分别为： \(v_l, v_r\) ，左轮和右轮的间距为 \(l\) ，如图 8 所示。下面来推导出线速度 \(v\) 和角速度 \(\omega\) 。

注：图中 ICC 是瞬时曲率中心(Instantaneous Center of Curvature)的缩写，也就是运动时弧线对应的圆心位置。

显然左轮和右轮具有相同的角速度和瞬时曲率中心，从而有：
\[\begin{aligned} \omega (R + l/2) &= v_r \\ \omega (R - l/2) &= v_l \end{aligned}\]

由上面两式，可得：
\[\begin{aligned} R &= \frac{l(v_r + v_l)}{2(v_r - v_l)} \\ \omega &= \frac{v_r - v_l}{l} \end{aligned}\]

进一步，可得线速度 \(v = R \cdot \omega = \frac{v_r + v_l}{2}\)

参考：
Computational Principles of Mobile Robotics, 2nd, 3.1.5 Wheeled Mobile Robots
http://planning.cs.uiuc.edu/node659.html

Car-like Mobile Robot 有两个控制量：油门(Accelerator)和方向盘(Steering Wheel)。假设前后轮距离为 \(L\) ，前进速度为 \(\varsigma\) ，前进方向和车主方向的夹角为 \(\varphi\) ，如图 9 所示。

可知（推导略），线速度和角速度分别为：
\[\begin{aligned} v_t &= \varsigma_t \cos(\varphi_t) \\ \omega_t &= \frac{\varsigma_t}{L} \sin(\varphi_t) \end{aligned}\]

参考：Self-adaptive Markov Localization for Single-Robot and Multi-Robot Systems