第五章：机器人运动

5.1 介绍

在本章和下一章中，我们将介绍滤波器中的运动模型和测量模型，本章关注运动模型。这些模型构成了状态转移概率$p(x_t | u_t, x_{t-1})$，它们在贝叶斯滤波器的预测操作过程中有着重要的作用。本章将提供各种应用于实际机器人的运动模型。下一章将描述传感器测量模型$p(z_t | x_t)$，它主要用于测量值更新过程。本章中的内容将是以后各章中算法具体实现的基础。

机器人运动学是本章的核心话题，关于它的研究已经持续了几十年了。但是，它基本上都是以确定性形式处理的。概率机器人学对运动学方程做了一些推广，使之能够处理运动过程中的不确定性问题，运动的不确定性来自于控制噪声和未建模的外界影响。根据本书的主题，我们将使用概率语言来描述它们，用一个后验概率分布来描述控制的结果。如此一来，前面章节中介绍的状态估计技术就可以用到这些模型上了。

我们只关注在平面环境中运动的移动机器人的运动学。这是一种比当前的运动学的所研究的更具体的对象。我们不会提供执行器的运动学模型，也不会讨论机器人的动力学模型。但我们不能认为这些概率学的方法只能用于移动机器人的运动学模型。这里是对目前技术的介绍，概率学的技术在移动机器人上取得了最大的成功。更成熟的概率学模型(比如机器人动力学的概率模型)还有很多领域需要探索。这些探索也可以使用本章所提到的套路，通过添加噪声变量来概率化确定性机器人执行器模型，描述机器人执行过程中存在的不确定性的特征。

理论上，建立一个合适的概率学模型的目标可能是为了正确地为机器人执行和感知过程中不确定性建模。实际上，我们并不关心模型的确切形状，而更关心产生这些不确定性地因素。事实证明，很多成功的模型都过高的估计了不确定性的程度。如此一来，算法将具有更强地鲁棒性，来应对不成立的马尔科夫假设，处理未建模地状态以及算法的近似误差。我们将在后续章节中讨论概率学机器人算法地具体实现时列出相关的研究成果。

5.2 预备知识

5.2.1 运动学配置configuration

运动学是描述控制动作对机器人配置的效果的方程。一个刚性的移动机器人的配置configuration通常用六个变量来描述 —— 它的三维笛卡尔坐标和相对于一个外部坐标系的三个欧拉角(滚转、俯仰、偏航)。本书中列举的材料大多局限于在平面环境中运动的移动机器人，它的运动学状态，或者说是位姿pose，可以用三个变量来表示，如下图1所示。机器人的位姿包括平面上的两维坐标$x$和$y$，(注意不要与状态变量$x_t$搞混了)和它的方向角$\theta$，它们可以用下式表示： $$ \begin{equation}\label{f1} \left( \begin{matrix} x \\ y \\ \theta \end{matrix} \right) \end{equation} $$ 机器人的方向通常被称为方位bearing，或者航向heading direction。如下图5.1所示，我们认为机器人的方位角为$\theta = 0$时，指向$x$轴的正方向，$\theta = 0.5\pi$指向$y$轴的正方向。

图 5.1. 一个坐标系中的机器人位姿示意图

没有方向的位姿称为位置location。在下一章讨论测量并感知机器人环境的时候位置的概念将很重要。为了简化，本书中通常用二维的向量来描述位置，一个物体的$x$-$y$坐标为： $$ \begin{equation}\label{f2} \left( \begin{matrix} x \\ y \end{matrix} \right) \end{equation} $$ 物体在环境中的位置和姿态就构成了机器人环境系统(robot-environment system)的运动学状态$x_t$。

5.2.2 概率运动学

概率运动学模型，或者说是运动模型motion model在移动机器人的状态转移模型中扮演着重要的角色。这个模型实际上就是一个我们熟悉的条件概率密度函数： $$ \begin{equation}\label{f3} p(x_t | u_t, x_{t-1}) \end{equation} $$ 其中，$x_t$和$x_{t-1}$都表示机器人的位姿(不是它的$x$坐标)，$u_t$则是运动指令。这个模型描述了运动学状态的后验概率分布，即机器人在位姿$x_{t-1}$的状态下执行指令$u_t$后位姿为$x_t$的概率。在实现的时候，$u_t$有时由机器人的里程计提供。但是出于概念上的统一，我们称$u_t$为控制量。

下图5.2列举了两个刚性移动机器人在平面环境中的运动学模型的例子。在这两种情况下，机器人的初始位姿都是$x_{t-1}$。分布$p(x_t|u_t, x_{t-1})$由图中的灰色阴影部分表示，颜色越深表示机器人更可能位于该位置下。在这幅图中，位姿的后验概率被映射到了$x$-$y$平面下，缺少了对机器人方向的描述。图5.2的左图表示机器人沿着一个方向直线运动，另一个例子中机器人则沿着一个更为复杂的曲线运动，这就带来了更多的不确定性。


图 5.2. 运动模型:机器人按照图中实现所示的运动指令运行之后姿态的后验分布。颜色越深表示机器人更可能处于该位姿下。这个图是个2D的，原始的密度函数是三维的，需要考虑机器人的方向角$\theta$。

本章将详细描述两个特定的概率运动模型$p(x_t|u_t,x_{t-1})$，它们都用于描述机器人在平面中的运动。这两个模型在处理不同类型的运动信息时有一定的互补性。第一个模型假设运动数据$u_t$表示给机器人电机的转速指令。很多商业移动机器人(比如，差速驱动，同步驱动)都是独立的控制着机器人的位置和方向。第二个模型假定给出的是里程计的信息。大多数商业底盘使用运动学信息(移动的距离,转动角度)来实现里程计。最终用于整合这些信息的概率学模型与速度模型多少有些不同。

里程计模型一般比速度模型更精确，因此大多数商业机器人并不使用速度模型。但是里程计模型具有一定的滞后，因此不能用于运动规划。蔽障的规划算法不得不预测运动的结果。因此，里程计模型通常用于估计，而速度模型用于概率的运动规划。

5.3 速度运动模型

速度运动模型假定我们可以通过两个速度(转动速度和平移速度)来控制机器人。很多商业机器人都提供了控制接口供程序员指定这些速度。驱动系统通常都是以这种方式控制的，包括差分驱动、阿克曼(Ackerman)驱动、同步驱动等。我们的模型并不适用于那些没有完整约束的驱动系统，比如装有麦克纳姆轮或者腿足式的机器人。

用$v_t$表示$t$时刻的线速度，$\omega_t$表示角速度，有： $$ \begin{equation}\label{f4} u_t = \left(\begin{matrix} v_t \\ \omega_t \end{matrix} \right) \end{equation} $$ 我们认定逆时针转动为角速度$\omega_t$的正方向。前进的方向为$v_t$的正方向。

5.3.1 Closed Form Calculation

算法5.1中显示了计算概率$p(x_t | u_t, x_{t-1})$的一种可能实现形式。它以初始的位姿$x_{t-1} = (\begin{matrix}x & y & \theta \end{matrix})^T$、控制$u_t = (\begin{matrix}v & \omega \end{matrix})^T$，以及一个假想的后继位姿$x_t = (\begin{matrix}x' & y' & \theta' \end{matrix})^T$作为输入。它的输出是执行了控制量$u_t$后位姿$x_{t-1}$切换到$x_t$的后验概率$p(x_t|u_t,x_{t-1})$，假设控制需要经过$\Delta t$的时间完成。参数$\alpha_1$到$\alpha_6$是与机器人相关的运动误差参数。这个算法首先计算一个没有误差的机器人的控制量，在后面介绍相关的推导过程时，各个参数的意义就会变得很明确。这些参数由$\hat{v}$和$\hat{\omega}$提供。

算法5.1: 根据速度信息计算$p(x_t | u_t, x_{t-1})$的算法。 我们假设向量$(\begin{matrix}x & y & \theta \end{matrix})^T$表示$x_{t-1}$； $x_t$由$(\begin{matrix}x' & y' & \theta' \end{matrix})^T$表示； $u_t$由速度向量$(\begin{matrix}v & \omega \end{matrix})^T$表示。函数$\mathrm{prob}(a,b)$ 计算了参数$a$在一个均值为0方差为$b^2$的分布中的概率。它可能由算法5.2实现。

函数$\mathrm{prob}(a,b)$对运动误差建模。它计算了参数$x$在均值为0方差为$b^2$的随机分布下的概率。算法2给出了两种可能的实现，分别是服从正态分布和三角分布的误差变量。

算法5.2: 均值为0方差为$b^2$的正态分布和三角分布。

图5.3显示了投影在$x$-$y$平面上的速度运动模型。在三个子图中，机器人都具有相同的线速度和角速度，只是误差参数$\alpha_1, \cdots, \alpha_6$不同。其中左图描述的是误差参数$\alpha_1$到$\alpha_6$都比较小的现象，中间的图中角速度误差($\alpha_3, \alpha_4$)较小，而平移误差($\alpha_1, \alpha_2$)较大。右图则相反，角速度误差大平移误差小。

图 5.3: 不同噪声参数下的速度运动模型。

5.3.2 采样算法

粒子滤波算法不用计算给定$x_t,u_t,x_{t-1}$的后验概率，但是需要对运动模型$p(x_t | u_t, x_{t-1})$进行采样。对条件概率密度采样与计算密度函数不同，在采样中，我们已经有了$u_t$和$x_{t-1}$，需要根据运动模型$p(x_t | u_t, x_{t-1})$来生成随机变量$x_t$。而计算概率密度，我们通过其它的方式得到了$x_t$，然后计算它在$p(x_t | u_t, x_{t-1})$下的概率值。

算法5.3给出了一种根据已知的控制量$u_t$和姿态$x_{t-1}$从$p(x_t | u_t, x_{t-1})$采样随机变量的算法。它以$x_{t-1}$和$u_t$作为输入量，根据$p(x_t | u_t, x_{t-1})$生成关于位姿$x_t$的随机变量。第2到4行用运动学模型的误差参数为控制指令引入了噪声。接着在第5到7行又用这些噪声值来生成新的样本姿态。因此，采样过程实现了一个简单的物理机器人运动模型，这个模型以最直接的方式在预测值中融合了控制噪声。图5.4中显示了用算法5.3生成的500个样本点的结果，可以将之与图5.3对比一下。

算法5.3: 根据位姿$x_{t-1} = (\begin{matrix} x & y & \theta \end{matrix})^T$和控制量$u_t = (\begin{matrix} v & w \end{matrix})$采样姿态$x_t = (\begin{matrix} x' & y' & \theta' \end{matrix})^T$的算法。我们通过一个随机项$\hat{\gamma}$来干扰最终的方向，变量$\alpha_1, \cdots, \alpha_6$为运动噪声。函数$\mathrm{sample}(b^2)$生成一个均值为0方差为$b^2$的随机样本。算法5.4给出了可能的实现。

算法5.4: 从均值为0方差为$b^2$的正态分布和三角分布中采样的算法。$\mathrm{rand}(x,y)$是一个生成区间$[x,y]$中的伪随机数的函数。

图 5.4: 使用图5.3中的参数配置，从速度运动模型采了500个样本。

在很多情况下，对$x_t$采样要比给定$x_t$计算密度函数更简单。这是因为采样只需要前向的模拟物理运动模型就可以了。而根据误差参数来计算一个假设中的位姿的概率值，要求计算物理运动模型的逆。从实现的角度来看，依赖于采样的粒子滤波器更有吸引力。

5.3.3 数学推导

下面我们将给出motion_model_velocity和sample_motion_model_velocity的数学推导过程。对于那些不关心数学细节的读者可以在第一次阅读的时候先跳过这一节，从5.4节继续。我们从一个机器人运动的产生式模型开始推导，最终得到给定$x_t, u_t, x_{t-1}$情形下对$p(x_t | u_t, x_{t-1})$进行采样和计算的公式。

精确运动

在分析概率学问题之前，我们先来看一下理想情况下，无噪声的机器人的运动学。令$u_t = \begin{pmatrix} v & w \end{pmatrix}^T$表示$t$时刻的控制量。如果线速度和角速度在整个时间段$(t-1, t]$中都保持一个固定的值，那么机器人将沿着半径为$r$的圆运动。

$$ \begin{equation}\label{f5} r = \left| \frac{v}{w} \right| \end{equation} $$

这点可以通过做半径为$r$的圆周运动的物体的线速度和角速度关系中得到:

$$ \begin{equation}\label{f6} v = w · r \end{equation} $$

式($\ref{f5}$)也描述了没有转动的机器人轨迹，即$w = 0$，这种机器人将沿着一条直线运动。一条直线则对应着一个半径无限长的圆，因此我们注意到$r$的取值可能是无穷大的。

令$x_{t-1} = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$为机器人的初始位姿，并且假设我们在一段时间$\Delta t$内保持速度为$\begin{pmatrix} v & w \end{pmatrix}$。我们可以容易得到圆心坐标:

$$ \begin{align}\label{f7} x_c & = x - \frac{v}{w}\sin \theta \\ y_c & = y + \frac{v}{w}\cos \theta \end{align} $$

我们用$\begin{pmatrix} x_c & y_c \end{pmatrix}^T$表示圆心坐标。运动了$\Delta t$的时间之后，理想情况下机器人将在$x_t = \begin{pmatrix} x' & y' & \theta' \end{pmatrix}^T$上， $$ \begin{equation}\label{f9} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \begin{pmatrix} x_c + \frac{v}{w} \sin (\theta + w \Delta t) \\ y_c - \frac{v}{w} \cos (\theta + w \Delta t) \\ \theta + w \Delta t \end{pmatrix} = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} + \begin{pmatrix} -\frac{v}{w}\sin \theta + \frac{v}{w}\sin (\theta + w \Delta t) \\ \frac{v}{w}\cos \theta - \frac{v}{w}\cos (\theta + w \Delta t) \\ w \Delta t \end{pmatrix} \end{equation} $$ 上式是根据一个简单的几何特性进行推导的。经过$\Delta t$的时间之后，无噪声的机器人将沿着圆周运动$v · \Delta t$弧长。这将导致机器人的方向角转动了$w · \Delta t$。此时，机器人的位置坐标$x,y$可以由运动的圆心坐标$\begin{pmatrix} x_c & y_c \end{pmatrix}^T$，以及从运动圆心坐标到机器人位置的射线段的长度和偏角得到。我们再把式($\ref{f7}$)代入就得到了上式最后的结果。

当然，真实机器人的速度不会发生跳变，在一段较短的时间内其速度可以看作是恒定的。因此在实际的工程实现中，为了得到变速的运动学特性，我们通常都会保证$\Delta t$是一个很小的值，从而保证在每个时间段内机器人的真实速度近似不变。最终的位姿可以通过将各个时间段内运行的圆周轨迹串联起来近似的得到。

真实的运动

在真实的物理世界里，机器人的运动都是有噪声的。实际的速度与指令给出的是不同的，与通过传感器检测到的速度也是不同的。我们可以通过一个零均值、有限方差的随机变量来对此建模。更精确的，我们假设真实的速度可以通过下式描述:

$$ \begin{equation}\label{f10} \begin{pmatrix} \hat{v} \\ \hat{w} \end{pmatrix} = \begin{pmatrix} v \\ w \end{pmatrix} + \begin{pmatrix} \varepsilon_{\alpha_1 v^2 + \alpha_2 w^2} \\ \varepsilon_{\alpha_3 v^2 + \alpha_4 w^2} \\ \end{pmatrix} \end{equation} $$

其中$\varepsilon_{b^2}$是一个零均值方差为$b^2$的误差项。因此，真实的速度就是指令速度加上一个小误差，或者说是噪声。在我们的模型中，误差的标准差与速度指令成正比。参数$\alpha_i ≥ 0, i = 1, \cdots, 4$是与机器人相关的误差参数。它们描述了机器人的准确度，该值越高，机器人的准确度越差。

正态分布和三角分布是误差$\varepsilon_{b^2}$的两种常用的选择。

均值为零方差为$b^2$的正态分布的概率密度函数如下:

$$ \begin{equation}\label{f11} \varepsilon_{b^2}(a) = \frac{1}{\sqrt{2\pi b^2}} e^{-\frac{1}{2}\frac{a^2}{b^2}} \end{equation} $$

下图5.6(a)中给出了方差为$b^2$的正态分布的密度函数。正态分布是连续随机过程中常用的模型噪声。它的支撑集，即$p(a) > 0$的点集，是实数集$\mathcal{R}$。


(a)	(b)
图 5.6. 方差为$b^2$的概率密度函数。(a) 正态分布，(b)三角分布。

均值为零方差为$b^2$的三角分布的概率密度函数为:

$$ \begin{equation}\label{f12} \varepsilon_{b^2}(a) = \max \left\{ 0, \frac{1}{\sqrt{6} b} - \frac{|a|}{6 b^2} \right\} \end{equation} $$

它只在区间$(-\sqrt{6b}, \sqrt{6b})$有非零值，如上图5.6(b)所示，其密度函数很像一个对称的三角形，因而得名。

因此，在位置$x_{t-1} = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$执行运动指令$u_t = \begin{pmatrix} v & w \end{pmatrix}^T$后的真实位置$x_t = \begin{pmatrix} x' & y' & \theta' \end{pmatrix}^T$的模型可以表示如下:

$$ \begin{equation}\label{f13} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} + \begin{pmatrix} -\frac{\hat v}{\hat w}\sin \theta + \frac{\hat v}{\hat w}\sin (\theta + \hat w \Delta t) \\ \frac{\hat v}{\hat w}\cos \theta - \frac{\hat v}{\hat w}\cos (\theta + \hat w \Delta t) \\ \hat w \Delta t \end{pmatrix} \end{equation} $$

该式是用式($\ref{f9}$)中附加了噪声的运动向量$\begin{pmatrix} \hat v & \hat w \end{pmatrix}^T$来替代指令速度$u_t = \begin{pmatrix} v & w \end{pmatrix}^T$得到的。但是这个模型仍然不是很好，我们将在下文中说明。

最终转动

上述的方程准确的描述了机器人沿半径为$r = \frac{\hat v}{\hat w}$的圆周运动的最终位置。但是这个圆弧段的半径和移动距离都会受到控制噪声的影响，以至于实际的运动轨迹并不一个圆弧。圆周运动的假设将带来一个重要的退化特性。密度函数$p(x_t | u_t, x_{t-1})$的支撑集在一个三维空间中，其中有两维用于描述位置。所以，我们只用了两个噪声变量，分别描述了线速度$v$和角速度$w$的误差。不幸的是，当应用贝叶斯滤波器进行状态估计的时候，这一退化特性将产生不可忽略的影响。

实际上，任何有意义的后验分布都比应该是退化的，位姿向量应当是由变量$x,y,\theta$构成的三维空间的向量。相应的，我们进一步的一般化运动模型，我们假设在机器人在到达最终位姿的时候进行了一个转动$\hat \gamma$。因此，我们可以用下式替代式($\ref{f13}$)来计算$\theta'$:

$$ \begin{equation}\label{f14} \theta' = \theta + \hat w \Delta t + \hat \gamma \Delta t \end{equation} $$

其中

$$ \begin{equation}\label{f15} \hat \gamma = \varepsilon_{\alpha_5 v^2 + \alpha_6 w^2} \end{equation} $$

参数$\alpha5,\alpha6$又是两个与机器人相关的参数，它描述了附加转动噪声的方差。因此，最终的运动模型如下:

$$ \begin{equation}\label{f16} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} + \begin{pmatrix} -\frac{\hat v}{\hat w}\sin \theta + \frac{\hat v}{\hat w}\sin (\theta + \hat w \Delta t) \\ \frac{\hat v}{\hat w}\cos \theta - \frac{\hat v}{\hat w}\cos (\theta + \hat w \Delta t) \\ \hat w \Delta t + \hat \gamma \Delta t \end{pmatrix} \end{equation} $$ 计算$p(x_t | u_t, x_{t-1})$

算法5.1中的motion_model_velocity给出了在给定$x_{t-1} = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$, $u_t = \begin{pmatrix} v & w \end{pmatrix}^T$和$x_t = \begin{pmatrix} x' & y' & \theta' \end{pmatrix}^T$下的概率分布$p(x_t | u_t, x_{t-1})$。该算法的推导过程有点复杂，因为它应用了一个逆运动学模型。motion_model_velocity计算了从位姿$x_{t-1}$到$x_t$的运动向量$\hat u_t = \begin{pmatrix} \hat v & \hat w \end{pmatrix}^T$，以及一个合适的最终转动$\hat \gamma$。我们的推导过程将说明为什么需要这个最终的转动：如果没有最后的这个转动，几乎所有的$x_{t-1}, u_t, x_t$的运动概率都是零。

让我们来计算在$\Delta t$的时间内，将机器人从$x_{t-1} = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$带到$x_t = \begin{pmatrix} x' & y' & \theta' \end{pmatrix}$的控制量$u_t = \begin{pmatrix} v & w \end{pmatrix}^T$的概率$p(x_t | u_t, x_{t-1})$。首先，我们计算将机器人从$x_{t-1}$带到位置$\begin{pmatrix} x' & y' \end{pmatrix}$的控制量$\hat u = \begin{pmatrix} \hat v & \hat w \end{pmatrix}^T$，不计较机器人的最终转动。接下来，我们还需要确定最终转动$\hat \gamma$来获得机器人的转向$\theta'$。基于这些计算过程，我们可以很容易地得到概率$p(x_t | u_t, x_{t-1})$。

读者可能注意到，我们的模型假设机器人在$\Delta t$时间内以一个固定的速度运动。对于一个从$x_{t-1} = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$运动到$x_t = \begin{pmatrix} x' & y' \end{pmatrix}^T$的机器人，其轨迹圆心$\begin{pmatrix} x^* & y^* \end{pmatrix}^T$可以计算如下:

$$ \begin{equation}\label{f17} \begin{pmatrix} x^* \\ y^* \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{equation} $$

其中$\lambda, \mu \in \mathcal{R}$。第一个等式表示从圆心出发的射线段与机器人的初始方向正交。第二个等式描述了一个约束，轨迹圆心位与点$\begin{pmatrix} x & y \end{pmatrix}^T$与$\begin{pmatrix} x' & y' \end{pmatrix}$连线的中垂线上。

除了特殊情况$w = 0$的情况，式($\ref{f17}$)有唯一解。当$w = 0$时，轨迹圆心则位与无穷远处。读者可以自行证明，其解为: $$ \begin{equation}\label{f18} \mu = \frac{1}{2} \frac{(x - x')\cos \theta + (y - y')\sin \theta}{(y - y')\cos \theta - (x - x')\sin \theta} \end{equation} $$ 因此 $$ \begin{equation}\label{f19} \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} \end{equation} $$ 轨迹圆的半径可以通过欧式距离计算得到 $$ \begin{equation}\label{f20} r^* = \sqrt{(x - x^*)^2 + (y - y^*)^2} = \sqrt{(x' - x^*)^2 + (y' - y^*)^2} \end{equation} $$ 现在我们可以计算方向角的变动 $$ \begin{equation}\label{f21} \Delta \theta = \mathrm{atan2}(y' - y^*, x' - x^*) - \mathrm{atan2}(y - y^*, x - x^*) \end{equation} $$ 上式中的$\mathrm{atan2}$是求取$y/x$的反正切函数的一种扩展形式，很多程序语言都有提供。 $$ \begin{equation}\label{f22} \mathrm{atan2}(y, x) = \begin{cases} \mathrm{atan}(y/x) & \mathrm{if}\ x > 0 \\ \mathrm{sign}(y)(\pi - \mathrm{atan}(|y/x|) & \mathrm{if}\ x > 0 \\ 0 & \mathrm{if}\ x = y = 0 \\ \mathrm{sign}(y) \pi / 2 & \mathrm{if}\ x = 0, y \neq 0 \end{cases} \end{equation} $$ 因为我们假设机器人沿着一个圆周运动，所以$x_t$到$x_{t-1}$的弧长为: $$ \begin{equation}\label{f23} \Delta \mathrm{dist} = r^* · \Delta \theta \end{equation} $$ 有了$\Delta \mathrm{dist}$和$\Delta \theta$，我们就可以很容易计算速度$\hat v$和$\hat w$ $$ \begin{equation}\label{f24} \hat u_t = \begin{pmatrix} \hat v \\ \hat w \end{pmatrix} = \Delta t^{-1} \begin{pmatrix} \Delta \mathrm{dist} \\ \Delta \theta \end{pmatrix} \end{equation} $$ 最终转动$\hat \gamma$需要根据式($\ref{f14}$)，将$\Delta t$时间中机器人在$\begin{pmatrix} x' & y'\end{pmatrix}$的朝向$\theta'$计算得到: $$ \begin{equation}\label{f25} \hat \gamma = \Delta^{-1}(\theta' - \theta) - \hat w \end{equation} $$ 运动误差motion error是$\hat u_t, \hat \gamma$与速度指令$u_t = \begin{pmatrix} v & w \end{pmatrix}^T$和$\gamma = 0$之间的差： $$ \begin{align}\label{f26} v_{err} & = v - \hat v \\ w_{err} & = w - \hat w \\ \gamma_{err} = \hat \gamma \end{align} $$ 在式($\ref{f10},\ref{f15}$)所描述的的误差模型下，这些误差有如下的概率: $$ \begin{align}\label{f29} \varepsilon_{\alpha_1 v^2 + \alpha_2 w^2}(v_{err}) \\ \varepsilon_{\alpha_3 v^2 + \alpha_4 w^2}(w_{err}) \\ \varepsilon_{\alpha_5 v^2 + \alpha_6 w^2}(\gamma_{err}) \\ \end{align} $$ 其中$\varepsilon_{b^2}$是之前提到的均值为零方差为$b^2$的正态分布或者三角分布。我们假设不同的误差源之间是独立的，那么目标概率$p(x_t | u_t, x_{t-1})$则是各个误差概率的乘积: $$ \begin{equation}\label{f32} p(x_t | u_t, x_{t-1}) = \varepsilon_{\alpha_1 v^2 + \alpha_2 w^2}(v_{err}) · \varepsilon_{\alpha_3 v^2 + \alpha_4 w^2}(w_{err}) · \varepsilon_{\alpha_5 v^2 + \alpha_6 w^2}(\gamma_{err}) \end{equation} $$ 我们可以看到算法5.1中的motion_model_velocity完全是按照这些公式实现的。具体的，第2到9行对应着式($\ref{f18}, \ref{f19}, \ref{f20}, \ref{f21}, \ref{f24}, \ref{f25}$)。第10行则实现了式($\ref{f32}$)，使用式($\ref{f29}$)来替换误差项。

对$p(x' | u, x)$采样

算法5.3中的sample_motion_model_velocity实现了一个前向模型。其中第5到7行对应着式($\ref{f16}$)。在第2到4行中所计算的噪声值，对应着式($\ref{f10}$)和式($\ref{f15}$)。

算法5.4中实现了一个从正态分布进行采样的方法。这个近似引用了中心极限定理，即任何为退化的随机变量的均值都会收敛到一个正态分布。通过对12个均匀分布样本取均值，sample_normal_distribution将生成一个近似正态分布的值；尽管所得到的值受限于区间$[-2b, 2b]$。最后，算法5.4中的sample_triangular_distribution则实现了一个针对三角分布的采样器。

5.4 里程计运动模型

到目前为止，我们讨论的运动模型都是根据机器人的速度计算位姿的后验概率的模型。有时我们可能想用里程计的测量值作为机器人运动的计算基础。里程计通常通过对轮子编码器数据的积分获得，大多数商业机器人以一定的周期积分，进而估计位姿数据。这就是本章要讨论的第二种运动模型——里程计运动模型odometry motion model。这种模型用里程计的测量值来代替控制量。

实际经验告诉我们尽管里程计也存在误差，但它也比速度模型要准确很多。这两种方法都要受到打滑、数据漂移的影响，此外速度模型还受限于实际运动控制器与数学模型之间的差异。但是里程计具有一定的滞后性，只有在机器人运动之后才能够获得。这对于滤波器算法而言不是什么问题，但是对于准确的运动规划和控制就不合适了。

5.4.1 Closed Form Calculation

严格来说，里程计并不是控制量而是传感器的测量值。把里程计建模成测量值的话，贝叶斯滤波器将不得不引入实际的速度作为状态变量，这将提高状态空间的维度。为了保持状态空间在一个较小的维度上，通常直接将里程计的数据看作是控制信号。在本部分中，我们也是这么做的。这样的模型是现如今最好的概率机器人系统的核心。

让我们先来形式化的描述一下控制信息。在时刻$t$，机器人的正确位姿用随机变量$x_t$来描述。机器人的里程计用于估计其位姿，但是因为漂移、打滑等客观因素的存在，以至于机器人内在的里程计坐标系与物理世界的坐标系之间不存在一个固定的坐标变换。实际上，如果我们知道了这个变换关系就解决了机器人的定位问题!

里程计模型使用的是机器人内部里程计测量的相对运动信息relative motion information。更具体的，假设在时间段$(t-1, t]$中，机器人从位姿$x_{t-1}$运动到了$x_t$。里程计给我们反馈的是从$\bar{x}_{t-1} = (\begin{matrix} \bar{x} & \bar{y} & \bar{\theta} \end{matrix})$到 $\bar{x}_t = (\begin{matrix} \bar{x}' & \bar{y}' & \bar{\theta}' \end{matrix})$的相对运动关系。这里的$\bar{\bullet}$表示里程计的测量值，它与机器人内部坐标系有关，并不能反应相对于世界坐标系的关系。在状态估计中，这些信息的核心就是$\bar{x}_{t-1}$和$\bar{x}_t$之间的相对差difference，将之用来估计真实位姿$x_{t-1}$与$x_t$之间的"差"是个不错的选择。因此，运动信息$u_t$定义为 $$ \begin{equation}\label{f33} u_t = \begin{pmatrix} \bar{x}_{t-1} \\ \bar{x}_t \end{pmatrix} \end{equation} $$ 为了抽象描述相对里程计，我们将$u_t$拆分为三个顺序执行的过程：一个转动(rotation)，接着是一个直线运动(translation)，最后又是一个转动。如下图5.7所示，初始的转动记为$\delta_{rot1}$，平移记为$\delta_{trans}$，第二次的转动记为$\delta_{rot2}$。容易验证，每一个位置对($\bar{s},\bar{s}'$)都有一个唯一的参数向量$\begin{pmatrix} \delta_{rot1} & \delta_{trans} & \delta_{rot2} \end{pmatrix}^T$与之对应，它们是重现$\bar{s}$到$\bar{s}'$的相对运动的必要参数。因此，$\delta_{rot1},\delta_{trans},\delta_{rot2}$一起构成了表述相对运动的里程计的必要统计量。这些运动模型参数都是有噪声的。读者可能注意到里程计模型的参数比速度向量模型的多一个参数，因此它不会面临"final rotation"的退化问题(~.~这是个什么问题)。

图 5.7: 里程计模型。机器人在时间区间$(t-1, t]$的运动可以看作是一个转动$\delta_{rot1}$，紧跟着一个平移$\delta_{trans}$和第二个转动$\delta_{rot2}$所组成的序列。其中转动和平移都是有噪声的。

在探讨数学细节之前，我们先以closed form的形式说明一下这种计算概率密度的基本算法。算法5.5描述了根据里程计计算$p(x_t | u_t, x_{t-1})$的算法。该算法以初始位姿$x_{t-1}$，从机器人里程计获得的位姿对$u_t = \begin{pmatrix} \bar{x}_{t-1} & \bar{x}_t \end{pmatrix}^T$，和一个假设状态$x_t$作为输入，输出是概率值$p(x_t | u_t, x_{t-1})$。

算法5.5: 根据里程计信息计算$p(x_t | u_t, x_{t-1})$的算法。其中，$u_t$表示为$\begin{pmatrix} \bar{x}_{t-1} & \bar{x}_t \end{pmatrix}^T$, $\bar{x}_{t-1} = \begin{pmatrix} \bar{x} & \bar{y} & \bar{\theta} \end{pmatrix}^T$, $\bar{x}_{t} = \begin{pmatrix} \bar{x}' & \bar{y}' & \bar{\theta}' \end{pmatrix}^T$

算法5.5在第2到4行中，通过一个逆运动模型inverse motion model，根据里程计读数计算相对运动参数$\begin{pmatrix} \delta_{rot1} & \delta_{trans} & \delta_{rot2} \end{pmatrix}^T$。在第5到7行中计算位姿$x_{t-1}$到$x_t$的相对运动参数$\begin{pmatrix} \hat{\delta}_{rot1} & \hat{\delta}_{trans} & \hat{\delta}_{rot2} \end{pmatrix}$。第8到10行计算特定运动参数下的误差概率。其中函数$\mathrm{prob}(a,b^2)$实现了一个均值为0方差为$b^2$的误差分布。这里所观测的所有角度差异必须位于区间$[-\pi, \pi]$内。因此必须对$\delta_{rot2} - \bar{\delta}_{rot2}$进行截断，这是一个比较常见却又很难定位的bug源。最后，第11行返回了通过相乘各个误差概率$p_1,p_2,p_3$得到的组合误差概率。这最后一个步骤假设不同的误差源之间是独立的。变量$\alpha_1$到$\alpha_4$都是与机器人相关的参数，描述了机器人运动的噪声。

图5.8中列出了误差参数$\alpha_1$到$\alpha_4$取不同值的里程计运动模型的例子。(a)图的误差参数比较小，(b)图和(c)图分别具有大的平移误差和转动误差。与图5.3所描述的场景对比，测量时间越短这两个场景就越相似。因此，如果频繁地更新置信度，那么这两个运动模型的差异就不那么显著了。

图 5.8: 不同噪声参数下的里程计运动模型。

5.4.2 采样算法

如果使用粒子滤波器进行定位，我们就需要一个对$p(x_t | u_t, x_{t-1})$进行采样的算法。粒子滤波器是要对密度函数$p(x_t | u_t, x_{t-1})$进行采样，而不是计算一个closed-form形式的概率值。下面的算法5.6给出了一种采样方法的实现。这个算法以初始位姿$x_{t-1}$和里程计读数$u_t$作为输入，其输出为随机变量$x_t$的分布$p(x_t | u_t, x_{t-1})$。它与之前的算法不同之处在于第5到10行的步骤，它用对位姿$x_t$的猜测，来代替计算给定$x_t$的概率值。这个采样算法在一定程度上比算法5.5更容易实现，因为算法5.5需要一次逆向计算。

算法5.6: 根据里程计信息对$p(x_t | u_t, x_{t-1})$进行采样的算法。 这里$t$时刻的位姿表示为$x_{t-1} = \begin{pmatrix} x & y & \theta \end{pmatrix}^T$。控制量是里程计获得的两个位置估计的可微集合，记为$u_t = \begin{pmatrix} \bar{x}_{t-1} & \bar{x}_t \end{pmatrix}^T$，其中$\bar{x}_{t-1} = \begin{pmatrix} \bar{x} & \bar{y} & \bar{\theta} \end{pmatrix}^T, \bar{x}_t = \begin{pmatrix} \bar{x}' & \bar{y}' & \bar{\theta}' \end{pmatrix}^T$。

图5.9是一个使用算法5.6生成的样本集合，其噪声参数与图5.8一样。下图5.10是对多次运动的叠加之后估计的一个例子。这些数据是通过粒子滤波器生成的，假设机器人的沿着实线运动。这个例子说明随着机器人的移动，其不确定性不断增加，样本点逐渐散落在较大的范围内。

图 5.9: 对里程计运动模型的采样，使用图5.8相同的噪声参数设置，每个图都有500个样本。

图 5.10: 对于一个无传感器机器人的位置置信度的采样近似。其中的实线表示运动轨迹，散点显示了随着时间序列对机器人位置的估计。

5.4.3 数学推导

该算法的推导过程相对比较简单，当然也是可以在第一次阅读的时候跳过。为了构建使用里程计的概率学运动模型，我们将两个位姿之间的相对运动拆分为三个基本的运动：一个转动，一个直线运动，和另一个转动。下面的公式描述了，如何从里程计的读数$u_t = \begin{pmatrix} \bar{x}_{t-1} & \bar{x}_t\end{pmatrix}^T$中计算两个转动量和一个平移量，其中$\bar{x}_{t-1} = \begin{pmatrix} \bar{x} & \bar{y} & \bar{\theta}\end{pmatrix}, \bar{x}_t = \begin{pmatrix} \bar{x}' & \bar{y}' & \bar{\theta}'\end{pmatrix}$。

$$ \begin{align}\label{f34} \delta_{\mathrm{rot1}} & = \mathrm{atan2}(\bar{y}' - \bar{y}, \bar{x}' - \bar{x}) - \bar{\theta} \\ \delta_{\mathrm{trans}} & = \sqrt{(\bar{x} - \bar{x}')^2 + (\bar{y} - \bar{y}')^2} \\ \delta_{\mathrm{rot2}} & = \bar{\theta}' - \bar{\theta} - \delta_{\mathrm{rot1}} \end{align} $$

为了对运动误差建模，我们假设通过对测量值减去一个均值为零方差为$b^2$的独立噪声，来获得转动和平移的真值。

$$ \begin{align}\label{f37} \hat{\delta}_{\mathrm{rot1}} & = \delta_{\mathrm{rot1}} - \varepsilon_{\alpha_1 \delta_{\mathrm{rot1}}^2 + \alpha_2 \delta_{\mathrm{trans}}^2} \\ \hat{\delta}_{\mathrm{trans}} & = \delta_{\mathrm{trans}} - \varepsilon_{\alpha_3 \delta_{\mathrm{trans}}^2 + \alpha_4 \delta_{\mathrm{rot1}}^2 + \alpha_4 \delta_{\mathrm{rot1}}^2 } \\ \hat{\delta}_{\mathrm{rot2}} & = \delta_{\mathrm{rot2}} - \varepsilon_{\alpha_1 \delta_{\mathrm{rot2}}^2 + \alpha_2 \delta_{\mathrm{trans}}^2} \\ \end{align} $$

和之前的章节中描述方式一样，$\varepsilon_{b^2}$是一个均值为零方差为$b^2$的噪声变量。参数$\alpha_i, i= 1,2,3,4$时与机器人相关的误差参数，描述了运动过程中产生的误差。

显然，真实的位姿$x_t$是通过从位姿$x_{t-1}$开始，一次初始转角$\hat{\delta}_{\mathrm{rot1}}$，接着平移地段距离$\hat{\delta}_{\mathrm{trans}}$，最后跟着一个转角$\hat{\delta}_{\mathrm{rot2}}$得到。因此有:

$$ \begin{equation}\label{f40} \begin{pmatrix} x' \\ y' \\ \theta' \end{pmatrix} = \begin {pmatrix} x \\ y \\ \theta \end{pmatrix} + \begin{pmatrix} \hat{\delta}_{\mathrm{trans}} \cos (\theta + \hat{\delta}_{\mathrm{rot1}}) \\ \hat{\delta}_{\mathrm{trans}} \sin (\theta + \hat{\delta}_{\mathrm{rot1}}) \\ \hat{\delta}_{\mathrm{rot1}} + \hat{\delta}_{\mathrm{rot2}} \end{pmatrix} \end{equation} $$

注意算法sample_motion_model_odometry实现了公式($\ref{f34}$)到式($\ref{f40}$)。

通过在第5到7行中计算假定位姿$x_t$相对于初始位姿$x_{t-1}$的运动参数$\hat{\delta}_{\mathrm{rot1}},\hat{\delta}_{\mathrm{trans}},\hat{\delta}_{\mathrm{rot2}}$，就可以得到算法motion_model_odometry。那么

$$ \begin{align}\label{f41} \delta_{\mathrm{rot1}} - \hat{\delta}_{\mathrm{rot1}} \\ \delta_{\mathrm{trans}} - \hat{\delta}_{\mathrm{trans}} \\ \delta_{\mathrm{rot2}} - \hat{\delta}_{\mathrm{rot2}} \end{align} $$

就是里程计的误差，假设$x_t$是真实的最终位置。误差模型($\ref{f37}$)意味着这些误差的概率可以通过下式得到:

$$ \begin{align}\label{f44} p_1 & = \varepsilon_{\alpha_1 \delta_{\mathrm{rot1}}^2 + \alpha_2 \delta_{\mathrm{trans}}^2} (\delta_{\mathrm{rot1}} - \hat{\delta}_{\mathrm{rot1}}) \\ p_2 & = \varepsilon_{\alpha_3 \delta_{\mathrm{trans}}^2 + \alpha_4 \delta_{\mathrm{rot1}}^2 + \alpha_4 \delta_{\mathrm{rot2}}^2} (\delta_{\mathrm{trans}} - \hat{\delta}_{\mathrm{trans}}) \\ p_3 & = \varepsilon_{\alpha_1 \delta_{\mathrm{rot2}}^2 + \alpha_2\delta_{\mathrm{trans}}^2} (\delta_{\mathrm{rot2}} - \hat{\delta}_{\mathrm{rot1}}) \end{align} $$

这些概率在算法motion_model_odometry的第8到10行中计算，因为我们假设这些误差都是独立的，所以联合误差概率就是它们的积$p_1 · p_2 · p_3$。

5.5 运动和地图

为了方便讨论$p(x_t | u_t, x_{t-1})$，我们假定机器人在真空中运动。特别的，这个模型描述了机器人在缺乏任何关于环境信息的情况下的运动。在很多情况下，我们会有一个地图$m$，它可能包含了能否成功导航机器人的地理信息。比如，将在第9章中讨论的占有地图occupancy maps，使用占有occupied和空闲free两种状态来表述地形，而机器人一定在空闲的区域中运动。因此，地图信息$m$能够给我们提供执行控制指令$u_t$前、中、后，关于机器人位姿$x_t$的更多数据。

现在，让我们考虑一种带有地图信息$m$的运动模型，将之写成$p(x_t | u_t, x_{t-1}, m)$，它表示在标准的变量中考虑了地图信息$m$。如果$m$携带了与位姿信息相关的信息，那么就有： $$ \begin{equation}\label{f47} p(x_t | u_t, x_{t-1}) \neq p(x_t | u_t, x_{t-1}, m) \end{equation} $$ 这个运动模型$p(x_t | u_t, x_{t-1}, m)$应当能够给出比没有地图的运动模型$p(x_t | u_t, x_{t-1})$更好的结果。我们称$p(x_t | u_t, x_{t-1}, m)$为基于地图的运动模型，它根据地图信息$m$，控制量$u_t$和位置信息$x_{t-1}$来计算机器人在位置$x_t$的似然度。不幸的是，以closed form形式计算运动模型是比较困难的。要计算机器人在执行了动作$u_t$之后处于状态$x_t$的似然度，我们必须引入从$x_{t-1}$到$x_t$之间可行路径的概率值，这是一个复杂的过程。

幸运的是，我们可以对基于地图的运动模型进行有效近似，当$x_{t-1}$到$x_t$之间的距离很小候，这种近似效果就会很理想。这种近似方法把基于地图的运动模型分成了两个部分： $$ \begin{equation}\label{f48} p(x_t | u_t, x_{t-1}, m) = \eta \frac{p(x_t | u_t, x_{t-1})p(x_t | m)}{p(x_t)} \end{equation} $$ 其中，$\eta$是我们之前提到的归一化算子，通常$p(x_t)$也是一致的，可以将之写入归一化算子中。我们直接将无地图的估计$p(x_t | u_t, x_{t-1})$和第二项$p(x_t | m)$相乘，第二项表达了位姿$x_t$在与地图$m$的关联性。在占用地图的例子中，当且仅当机器人碰撞到地图中已被占用的栅格时，有$p(x_t | m) = 0$，对于其他情况这种关联性都假定是一个常数。通过$p(x_t | m)$和$p(x_t | u_t, x_{t-1})$的相乘，我们得到一个分布，为所有可能与地图相关的位姿$x_{t-1}$赋予了一个概率值。否则就与$p(x_t | u_t, x_{t-1})$具有相同的分布(~.~又是不知所云)。因为$\eta$可以使用归一化的形式计算，基于地图的运动模型的这种近似可以很容易的计算得出，其计算复杂度不会超过无地图运动模型。

算法5.7描述了基于地图的运动模型的计算和采样算法。需要注意的是，采样算法返回的是一个带权重的样本，其重要性因子正比于$p(x_t | m)$。在实现采样算法时需要加以注意，确保内层循环是可终结的。

算法5.7: 利用环境地图$m$计算$p(x_t | u_t, x_{t-1}, m)$的算法。 这个算法使用了之前的运动模型(算法5.1,算法5.3,算法5.5和算法5.6)，并考虑了机器人在地图$m$中的不可能位置。

下图5.11列举了一个运动学模型的例子。其中(a)图描述的是根据速度运动模型计算的概率密度$p(x_t | u_t, x_{t-1})$。现在假设地图$m$中有一个长方形的障碍物，如(b)图所示。所有可能与障碍物碰撞的位置$x_t$的概率$p(x_t | m$都是零。因为我们的机器人是一个圆盘，所以这个碰撞区域需要相当于障碍物膨胀，膨胀大小为机器人的半径，这相当于把障碍物从工作空间workspace映射到机器人的配置空间configuration space，或者说是位姿空间pose space。如此得到的概率$p(x_t | u_t, x_{t-1}, m)$，如(b)图所示，是对$p(x_t | m)$和$p(x_t | u_t, x_{t-1})$之积的归一化。膨胀后障碍物区域的概率为0，其它地方的概率都正比于$p(x_t | u_t, x_{t-1})$。

图 5.11: 无地图的速度运动模型(a)和考虑了地图$m$的速度运动模型(b)。

图5.11也说明了这种近似方法的一个问题——用(*)标注的区域具有非零的似然度。得到这一结果的原因是$p(x_t | u_t, x_{t-1})$和$p(x_t | m)$在这个区域都不为0，看似机器人有一定的概率处于这一区域中。但在物理世界中，机器人一定不能穿墙而过，所以这个区域是不可能到达的。产生这一矛盾的原因是，我们只检查了最终位姿$x_t$与地图的关联度，而没有考察机器人到目标之间的路径。实际上，这样的错误只在较大的运动$u_t$时才会出现，更新频率高到一定程度之后这一问题可以忽略。

为了探查近似方法的本质，我们简单的推导一下。公式($\ref{f48}$)可以通过贝叶斯公式得到： $$ \begin{equation}\label{f49} p(x_t | u_t, x_{t-1}, m) = \eta p(m | x_t, u_t, x_{t-1})p(x_t | u_t, x_{t-1}) \end{equation} $$ 如果我们用$p(m | x_t)$来近似$p(m | x_t, u_t, x_{t-1})$，再次使用贝叶斯公式我们将得到想要的结果： $$ \begin{align}\label{f50} p(x_t | u_t, x_{t-1}, m) & = \eta p(m | x_t) p(x_t | u_t, x_{t-1}) \\ & = \eta \frac{p(x_t | m)p(m)}{p(x_t}p(x_t | u_t, x_{t-1}) \\ & = \eta \frac{p(x_t | m)p(x_t | u_t, x_{t-1})}{p(x_t)} \end{align} $$ 其中$\eta$是归一化算子(注意上式中的每一步$\eta$的取值都不一样，都需要进行归一化运算)。这个置信度分析说明我们的基于地图的模型在如下的假设下是合理的： $$ \begin{equation}\label{f51} p(m | x_t, u_t, x_{t-1}) = p(m | x_t) \end{equation} $$ 显然这个等式是不成立的。在计算$m$的条件概率时，我们省略了两个因素$u_t$和$x_t$。省略这两个因素，说明我们抛弃了运行到$x_t$的路径信息。以至于我们所知的所有信息只有机器人最终的位姿$x_t$。这个省略操作的后果就是，只要初始和结束的位姿不在占据空间中，这种近似方法就可能错误的认为机器人穿墙而过。前面已经提到，提高更新的频率可以在一定程度上缓解这一问题。际上，对于足够高的更新速率，并且运动模型的噪声是有界的，我们可以保证这个近似是紧致的，这种效果不会出现。

这里详细分析了一个算法实现的细节。特别的，我们必须关注更新频率。频繁更新的贝叶斯滤波器可能得到与偶尔更新的滤波器完全不同的结果。

5.6 总结

针对在平面中运动的移动机器人，本章给出了两种主要的概率运动模型。

我们推导了表述概率运动模型$p(x_t | u_t, x_{t-1})$的算法。在这个算法中，机器人在一个固定的时间间隔$\Delta t$中执行了控制量$u_t$， $u_t$由平移速度和旋转速度构成。这个模型的实现有两个控制噪声的参数，一个用于描述平移噪声，另一个用于描述旋转噪声。但它们不足以生成一个满布空间(space-filling)的后验概率。因此我们添加了第三个噪声参数，称为有噪声的最终转动"noisy final rotation"。
我们还展示了使用机器人里程计的运动模型，用三个步骤表述里程计测量值：一个初始转动，跟着是一个平移，最后又是一个转动。这个概率运动模型假设所有的三个步骤都是有噪声的。严格来说，里程计的读数并不是一个控制量，但是将之看作是控制量可以简化估计问题的公式。
对于这两种运动模型，我们列举了两种实现方式：一个是以closed form的形式计算概率值$p(x_t | u_t, x_{t-1})$，另一个是从$p(x_t|u_t,x_{t-1})$生成样本的方法。closed form的形式的方法以$x_t, u_t, x_{t-1}$为输入，输出的是一个概率值。为了计算这个概率值，算法需要高效的计算模型的逆，并于控制参数的实际值比较(to compare the actual with the commanded control parameters)。采样模型方法不需要计算逆，它实现了一个运动模型$p(x_t | u_t, x_{t-1})$的前向模型，以$u_t$和$x_{t-1}$作为输入，输出的是对$p(x_t | u_t, x_{t-1})$的采样。closed form形式的模型需要一些概率学算法。其它方法主要是应用采样模型的粒子滤波器。
最后，我们考虑了环境地图，并对所有的运动模型进行了扩展，在概率$p(x_t | u_t, x_{t-1}, m)$的条件中引入了地图$m$。这一扩展符合人们的直觉，地图信息中包含了从位姿$x_{t-1}$到$x_t$的运动信息。但这种扩展只是一种近似的方法，只检测最终状态的合法性。

这里列举的运动模型只是一些例子，显然机器人执行器的领域是很丰富的，不仅限于在平地上运动的移动机器人。即使是在移动机器人的领域中，也存在着各种各样的设备是这里列举的模型所不能覆盖的。比如说，具有完全约束的可以横向移动的机器人、悬浮汽车等。此外，我们的描述并没有考虑机器人的动力学，但是它对于在高速公路上快速运动的汽车是很重要的。尽管如此，大部分的机器人还是可以以类似的方法建模：简单的描述机器人运动的物理定律，并声明近似噪声参数。对于动力学模型，需要对机器人的状态进行扩展，状态向量中不仅要包含速度信息还需要记录动力学状态，有很多直接的扩展方法。

仅就自主运动而言，很多机器人都倾向于使用一些惯性传感器，将之作为里程计的补充或者替代之，来测量运动。本书致力于设计各种使用惯性传感器的滤波器，当里程计的信息不充分时，我们也鼓励读者使用更丰富的模型和传感器。

5.7 参考文献

Cox and Wilfong 1990使用一个概率要素对移动机器人的基本运动学公式做了扩展。这里的模型涵盖了差分驱动、Ackerman驱动、同步驱动(Borenstein et al. 1996)。我们的模型并没有涉及到非完整约束(Latombe 1991)的机器人，比如说装有麦克纳姆轮的机器人(Ilon 1975)或者腿足式的机器人。在一些前卫的研究工作中Raibert et al. (1986); Raibert (1991); Saranli and Koditschek (2002)可以找到相关的内容。

在机器人学领域中，已经对机器人的运动以及机器人与环境的交互进行了深入的研究。现代介绍移动机器人的运动学和动力学的文章有Murphy (2000c); Dudek and Jenkin (2000); Siegwart and Nourbakhsh (2004); Cox and Wilfong (1990); Kortenkamp et al. (1998)。在Craig (1989); Vukobratovi´c (1989); Paul (1981); and Yoshikawa (1990)中介绍了经典的机器人运动学和动力学。Featherstone (1987)则给出了更现代的机器人动力学方法。 Mason (2001)研究了兼容运动，这是一种与环境交互的方式。Bekker (1956, 1969) and Wong (1989)的开创性论文，研究了轮子与地面的相互作用。 Iagnemma and Dubowsky (2004)中给出了一种现代的轮子地面作用模型。将这些模型扩展到概率学框架下时未来机器人研究的方向。

5.8 练习

暂略


(a)	(b)
图 5.6. 方差为\(b^2\)的概率密度函数。(a) 正态分布，(b)三角分布。