移动机器人运动模型（两轮、四轮、麦克纳姆轮和概率运动）

一、理论运动模型

运动机器人通常可以采用轮式和足式进行移动，通过电机驱动轮子移动机器人即轮式机器人，通过仿生式的多足关节移动机器人即多足机器人。由于多足机器人控制非常复杂，所以商业应用较多的是轮式机器人。

1.底盘运动学模型概述

轮式机器人底盘按照转向方式的不同，可以分为；两轮差速模型，四轮差速模型、阿克曼模型、全向模型。日常生活中，常见的就是四轮差速模型，在做小车时通常会用到麦克纳姆轮，而两轮差速模型是最基础最简单的底盘运动模型，这里就对这三种着重介绍一下。

运动受约束模型和运动不受约束模型

在二维平面上运动的底盘，可以用速度向量[v_x,v_y,w_z]底盘的运动，v_x和v_y是底盘线速度在x和y两个坐标轴方向上的分量，w_z是底盘绕z轴在平面旋转的角速度。如果速度向量[v_x,v_y,w_z]中的三个分量之间的取值是相互独立的，这样的底盘运动速度不受约束，也称为不受约束底盘。在两轮差速底盘或四轮差速底盘中，v_x和w_z都由左轮、右轮速度V_L和V_r计算，如式（1-6）所示，由于V_L和V_r的取值是相互独立的，以V_L和V_r作为向量的基，那么v_x和w_z点乘的积为零，也就是说v_x和w_z在向量基V_L和V_r中是正交的，即v_x和w_z是相互独立的，不过由于两轮差速模型或四轮差速底盘无法朝轮子的轴方向运动，即速度分量v_y始终为0，这样的运动底盘运动速度就受到了约束，也称为受约束底盘。

2.底盘的数学模型推导

1. 两轮差速模型

两轮差速模型是最简单的底盘模型，在底盘的左右两边平行安装两个由电机驱动的动力轮，考虑到至少需要三点才能稳定支撑，地盘上还需要安装用于支撑的万向轮。前面一个万向轮和后面两个动力轮组成三轮结构，如下图。

但是三轮结构还存在侧翻的问题，当底盘上的载重分布不均匀时，转弯很容易发车，改进方法是多安装几个万向轮。

1.前向运动学

底盘中的各个动力轮的速度可以由编码器值计算得到，那么通过各个轮子的速度求底盘整体的运动速度，即前向运动学。已知左边动力轮的线速度为V_L,右动力轮的线速度为V_r,动力轮的线速度可以用编码器计数值计算，如(1-1)式所示
$$V=\frac{M\cdot N\cdot 2\pi \cdot R}{P\cdot △t}\text{\hspace{1em}(1-1)}$$
其中：M为采样周期内编码器的计数值；P为编码器的线数，即编码器盘旋转一周触发的总脉冲数；N为电机减速比，为小于1的分数；R为轮子半径；△t为编码器采样周期。此处结合编码器测速公式以及圆周速度计算公式，编码器的公式在作业的编码器篇章有介绍。

三维空间中刚体运动速度为六维向量，对标着六个自由度，底盘在二维空间中运动时，只有向前的线速度v_x和z轴的角速度w_z不为零。根据前向运动学，在已知左动力轮、右动力轮的线速度分别为V_L和V_r的条件下，利用下面所示的几何关系，便可求出底盘的整体速度v_x和w_z。

在很短的时间t内，底盘可以看成绕O点做圆周运动，两个圆周的半径分别为r-l/2和r+l/2，其中l为两个轮子之间的轴距，两个轮子走过的路程分别为s_L和s_r，转过的角度为θ，底盘速度瞬心走过的路程为s，s_L和s_r满足下式：
$$S_r=\theta \cdot (r+\frac{l}{2})=V_r\cdot t\text{\hspace{1em}(1-2)}$$

$$S_L=\theta \cdot (r-\frac{l}{2})=V_L\cdot t\text{\hspace{1em}(1-3)}$$

将上式两个等式相减可以求出θ，利用θ继而可以求出S,结果如下式：
$$\theta =\frac{S_r-S_L}{l}=\frac{V_r-V_L}{l}\cdot t\text{\hspace{1em}(1-4)}$$

$$S=\theta \cdot r=\frac{S_r-S_L}{l}\cdot \frac{(S_r+S_L)l}{2(S_r-S_L)}=\frac{S_r+S_L}{2}=\frac{V_r+V_L}{2}\cdot t\text{\hspace{1em}(1-5)}$$

由于运动时间t很小，利用极限的思想，θ趋近于0，S也趋近于0，那么S可以等效为底盘的运动速度瞬心轨迹圆弧切线方向的一小段位移，用S和θ直接除以t就可以求出底盘的整体速度v_x和w_z了，如式（1-6）所示。
$$\left[\begin{array}{c}{v}_x\\ w_z\end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}& \frac{1}{2}\\ -\frac{1}{l}& \frac{1}{l}\end{array}\right]\left[\begin{array}{c}{V}_L\\ V_r\end{array}\right]\text{\hspace{1em}(1-6)}$$

2.逆运动学

前向运动学是利用各个轮子的速度求解底盘整体的速度，逆向运动学是前向运动学的逆过程，即利用底盘整体的速度求解各个轮子的速度。逆运动学计算就是比较简单了，直接由式（1-6）进行矩阵求逆运算就能得到，如式（1-7）所示。

$$\left[\begin{array}{c}{V}_L\\ V_r\end{array}\right]=\left[\begin{array}{cc}1& -\frac{l}{2}\\ 1& \frac{l}{2}\end{array}\right]\left[\begin{array}{c}{v}_x\\ w_z\end{array}\right]\text{\hspace{1em}(1-6)}$$

3.轮式里程计

机器人主机向底盘发送运动控制量v_x和w_z，经过逆向运动学公式（1-6）解算就可以得到底盘中每个轮子应该运行的目标速度，通过电机控制主板的PID算法可以实现每个轮子转速控制，具体可以参照作业的PID部分。

底盘在运动过程中，轮式编码器可以反馈每个轮子的转速，经过前向运动学公式（1-6）解算就可以得到底盘的整体速度v_x和w_z了，基于v_x和w_z，就可以由前一时刻的底盘的位姿P_k-1推算出当前时刻底盘的位姿P_k，这个推算过程也叫“航迹推演算法”。

轮式里程计其实就是编码器、底盘运动学模型、航迹推演算法等综合作用的产物。也就是说，轮式里程计并不是某种传感器，而是利用轮式编码器计算底盘位姿的一种算法。

轮式里程计利用观测的左右轮转速值得到的速度和角速度，带入移动机器人运动模型中，推演出机器人当前的位置和航向角信息。由于测量误差、执行机构误差、模型误差以及可能发生的侧滑等原因，轮式里程计信息会有一定的误差。由于轮式里程计是利用几何模型不断推演出来的，误差会一直积累在结果里，最终会不可用。具有相同问题的还有IMU，也即惯性测量单元，同样的，他也是采用几何模型推演得到移动机器人的里程计信息。单独的轮式里程计，由于没有定时校准，误差一直积累，最终无法满足应用，实际应用中，轮式里程计与IMU、GPS、视觉里程计（硬件为单目、双目或深度相机）或者激光里程计（硬件为单线或多线激光雷达）等信息融合得到较为准确的定位信息，这一部分属于多传感器融合定位的知识体系。

里程计运动模型

在机器人中，通常采用右手坐标系，底盘的正前方为x轴，底盘的正上方为z轴正方向，底盘的正左方为y轴正方向，航向角θ以x轴为0度角并沿逆时针方向增大。一般在底盘上电时刻，在底盘的位姿处建立里程计坐标系（odom），随着时间推移底盘的实时位姿P₀、P₁、P₂、P₃、· · · ·、P_n连接起来就形成了底盘的航行轨迹。同时，底盘自身的坐标系（robot）建立在其速度瞬心处。

建立起robot坐标系中速度量v_x和w_z与odom坐标系中的底盘位姿P_k之间的关系，就能解算出轮式里程计。在二维平面中移动的底盘，可以用P=[X,Y,θ]^T表示底盘的坐标和航向（即底盘位姿），那么位姿P满足式（1-7）微分方程,在这设v_y=0.
$$\dot{P}=\left[\begin{array}{c}\dot{X}\\ \dot{Y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{V}_x\\ V_y\\ w_z\end{array}\right]\text{\hspace{1em}(1-7)}$$
之后将上述式子在时间t上进行积分，就可以求出底盘实时位姿，也就是里程计信息。实际情况下，需要在离散时间域上进行计算，由于底盘相邻两位姿相隔时间△t很小，积分运算可以用式（1-8）的累加运算来替代。
$$P_k=P_k{-}_1+\dot{P}\cdot △t\text{\hspace{1em}(1-7)}$$

由于里程计提供底盘在odom坐标系中的位姿，而odom坐标系与底盘上电启动时所处的地方有关，也就说里程计中的底盘位姿是一个相对值。另外，里程计随时间的推移存在较大的累计误差，所以一般都是取不同时刻之间的里程计信息差值并应用于其他算法，例如SLAM技术。

2.四轮差速模型

四轮差速模型与两轮差速模型非常相似，并且在承重能力和越野能力上比两轮差速模型更有优势。

1.前向运动模型

在四轮差速模型中，前轮和后轮的速度是同步的，这里以底盘几何中心COG沿y轴方向上的点ICR作为整个底盘进行圆周运动时的圆心，ICR和COG的距离大小与圆周运动角速度大小有关。四个轮子到ICR的距离为d_i，轮子的实际速度v_i是侧向滑动速度v_iy和预设目标速度v_ix的合成速度，这里i=1,2,3,4。

在模型中底盘的速度瞬心在质心COM处，而COM和COG往往是不重合的。我们可以用COM位置处的线速度v_c和角速度w_c表示整个底盘的运动速度，其中COM到ICR的距离为d_c。v_c垂直于ICR-COM线段，v_c不仅有预设目标速度v_cx分量，还有侧向滑动速度v_cy分量。

底盘中左轮、右轮轴距为c，点COM与底盘后端以及前端的距离分别为a和b。

绕圆心做圆周运动的物体，其线速度v、角速度w和圆周半径d满足w=v/d。因此可以建立底盘中的约束关系，式（2-1）。
$$w_c=\frac{v_c}{d_c}\text{\hspace{1em}(2-1)}$$
假设线d_c与y轴的夹角为α_c,可以对式（2-1）进行变形，结果为式（2-2）所示。
$$w_c=\frac{v_c}{d_c}=\frac{v_{c}cos{\alpha }_c}{d_{c}cos{\alpha }_c}=\frac{v_{cx}}{d_{cx}}\text{\hspace{1em}(2-2)}$$

$$w_c=\frac{v_c}{d_c}=\frac{v_{c}sin{\alpha }_c}{d_{c}sin{\alpha }_c}=\frac{v_{cy}}{d_{cy}}\text{\hspace{1em}(2-3)}$$

刚体旋转时，各个位置的角速度与质心处的角速度是一样的，即四个轮子绕ICR旋转的角速度也是w_c。按照式（2-3）推理，同样可以得到约束关系式。
$$w_c=\frac{v_i}{d_i}=\frac{v_{i}cos{\alpha }_i}{d_{i}cos{\alpha }_i}=\frac{v_{ix}}{d_{ix}}\text{\hspace{1em}(2-4)}$$

$$w_c=\frac{v_i}{d_i}=\frac{v_{i}sin{\alpha }_i}{d_{i}sin{\alpha }_i}=\frac{v_{iy}}{d_{iy}}\text{\hspace{1em}(2-5)}$$

将式（2-2）到（2-5）整理得，约束关系式（2-6）
$$w_c=\frac{v_c}{d_c}=\frac{v_{cx}}{d_{cx}}=\frac{v_{cy}}{d_{cy}}=\frac{v_{iy}}{d_{iy}}=\frac{v_{ix}}{d_{ix}}\text{\hspace{1em}(2-6)}$$
同时，d_i与d_c在x轴和y轴上的投影长度满足式（2-7）和（2-8）的关系。
$$d_{1y}=d_{2y}=d_{cy}-\frac{c}{2}\text{\hspace{1em}(2-7)}$$

$$d_{3y}=d_{4y}=d_{cy}+\frac{c}{2}\text{\hspace{1em}(2-8)}$$

当四轮差速底盘设定的左轮、游轮速度分别为V_L和V_r,且前轮、后轮速度严格同步时，课件里如式（2-9）和（2-10）所示关系。
$$V_L=v_{1x}=v_{2x}\text{\hspace{1em}(2-9)}$$

$$V_r=v_{3x}=v_{4x}\text{\hspace{1em}(2-10)}$$

利用式（2-6）到式（2-10）的约束关系，可以推导出式（2-11）和（2-12）的结论。
$$V_L=w_c\cdot (d_{cy}-\frac{c}{2})=w_c\cdot d_{cy}-w_c\cdot \frac{c}{2}=v_{cx}-w_c\cdot \frac{c}{2}\text{\hspace{1em}(2-11)}$$

$$V_r=w_c\cdot (d_{cy}+\frac{c}{2})=w_c\cdot d_{cy}+w_c\cdot \frac{c}{2}=v_{cx}+w_c\cdot \frac{c}{2}\text{\hspace{1em}(2-11)}$$

将上式整理后，得到四轮差速底盘的前向运动学关系，如式（2-12）所示。
$$\left[\begin{array}{c}{v}_{cx}\\ w_z\end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}& \frac{1}{2}\\ -\frac{1}{c}& \frac{1}{c}\end{array}\right]\left[\begin{array}{c}{V}_L\\ V_r\end{array}\right]\text{\hspace{1em}(1-12)}$$

2.逆向运动学

前向运动学是利用各个轮子的速度求解底盘整体的速度，逆向运动学是前向运动学的逆过程，即利用底盘整体的速度求解各个轮子的速度。
$$\left[\begin{array}{c}{V}_L\\ V_r\end{array}\right]=\left[\begin{array}{cc}1& -\frac{c}{2}\\ 1& \frac{c}{2}\end{array}\right]\left[\begin{array}{c}{v}_{cx}\\ w_z\end{array}\right]\text{\hspace{1em}(1-6)}$$

3.轮式里程计

从前向运动学模型和逆运动学模型来看，咱们发现四轮差速底盘和两轮差速底盘的运动公式完全一样。

3.全向模型（四轮麦克纳姆轮模型）

底盘速度收到约束的原因是普通轮子不能侧向移动，只有切向移动一个自由度。如果把轮子换成包含切向和侧向两个自由度的轮子，则底盘速度不受约束，也就成了全向底盘，这种具有两个自由度的轮子称为麦克纳姆轮，其结构是在轮面安装上滚柱，依据滚柱的不同安装方式，麦克纳姆轮又分为不同的型号。以45°安装滚柱的麦克纳姆轮是最常见的型号。

1.前向运动学

全向底盘的运动可以用下图来表示并进行分析，在这咱们用底盘中心处的线速度$\vec{v}$和角速度$\vec{w}$描述底盘的整体运动，其中线速度$\vec{v}$由分量$\overrightarrow{v_x}$和$\overrightarrow{v_y}$合成，$\vec{v}$可以是任何方向，即取值不受约束。底盘中4个轮子的速度分别为$\overrightarrow{v_1}$、$\overrightarrow{v_2}$、$\overrightarrow{v_3}$和$\overrightarrow{v_4}$。底盘前轮、后轮轴距为2a，左轮、右轮轴距为2b。

底盘的整体运动速度由线速度$\vec{v}$和角速度$\vec{w}$组成，那么每个轮子的速度都可以分成两个分量，一个分量对应线速度$\vec{v}$，另一个分量对应底盘的角速度$\vec{w}$，如式（3-1）所示
$$\{\begin{array}{l}\overrightarrow{v_1}=\vec{v}+\overrightarrow{v_{1r}}\\ \overrightarrow{v_2}=\vec{v}+\overrightarrow{v_{2r}}\\ \overrightarrow{v_3}=\vec{v}+\overrightarrow{v_{3r}}\\ \overrightarrow{v_2}=\vec{v}+\overrightarrow{v_{4r}}\end{array}\text{\hspace{1em}(3-1)}$$

以图为例，结合轮子的结构对其速度组成进行具体分析，如下图所示，轮子包括切向和侧向两个自由度，切向速度分量$\overrightarrow{V_1}$由电机提供，侧向速度分量$\overrightarrow{V_{roll}}$是滚柱从动式产生，他不由动力产生，所以无法直接确定其值。那么$\overrightarrow{v_1}$可以用$\overrightarrow{V_1}$和$\overrightarrow{V_{roll}}$表示，如（3-2）所示
$$\begin{gathered} \overrightarrow{v_1}=\vec{V}+\overrightarrow{V_{roll}} \\ \text{\hspace{1em}(3-2)} \end{gathered}$$

将式（3-1）和式（3-2）展开式（3-3）和式（3-4）所示的分量形式，利用各分量之间的恒等关系，可以求出电机驱动轮线速度$\overrightarrow{V_1}$与底盘整体速度$\vec{v}$和$\vec{w}$之间的关系，如式（3-5）所示：
$$\left[\begin{array}{c}\overrightarrow{v_1}\end{array}\right]=\left[\begin{array}{c}\overrightarrow{v_{1x}}\\ \overrightarrow{v_{2x}}\end{array}\right]=\left[\begin{array}{c}\overrightarrow{v_x}\\ \overrightarrow{v_x}\end{array}\right]+\left[\begin{array}{c}-b\cdot \vec{w}\\ a\cdot \vec{w}\end{array}\right]\text{\hspace{1em}(3-3)}$$

$$\left[\begin{array}{c}\overrightarrow{v_1}\end{array}\right]=\left[\begin{array}{c}\overrightarrow{v_{1x}}\\ \overrightarrow{v_{2x}}\end{array}\right]=\left[\begin{array}{c}\overrightarrow{V_2}\\ 0\end{array}\right]+\left[\begin{array}{c}\overrightarrow{V_{roll}}\cdot cos45°\\ \overrightarrow{V_{roll}}\cdot sin45°\end{array}\right]\text{\hspace{1em}(3-4)}$$

$$\overrightarrow{V_1}=\overrightarrow{v_x}-\overrightarrow{v_y}-(a+b)\cdot \vec{w}\text{\hspace{1em}(3-5)}$$

同理，可以推导出$\overrightarrow{V_2}$、$\overrightarrow{V_3}$、$\overrightarrow{V_4}$与底盘整体速度$\vec{v}$和$\vec{w}$之间的关系，不过1轮、4轮和2轮、3轮中滚柱的朝向是不一样的，推导时会有一个符号的差异。
$$\overrightarrow{V_2}=\overrightarrow{v_x}+\overrightarrow{v_y}+(a+b)\cdot \vec{w}\text{\hspace{1em}(3-6)}$$

$$\overrightarrow{V_3}=\overrightarrow{v_x}+\overrightarrow{v_y}-(a+b)\cdot \vec{w}\text{\hspace{1em}(3-7)}$$

$$\overrightarrow{V_4}=\overrightarrow{v_x}-\overrightarrow{v_y}+(a+b)\cdot \vec{w}\text{\hspace{1em}(3-8)}$$

从（2-5）到（2-8）就已经是全向底盘的逆运动学公式了，对于这四个逆向运动学公式进行逆变换，就是全向底盘的前向运动学了，如式（2-9）所示。
$$\left[\begin{array}{c}\overrightarrow{v_x}\\ \overrightarrow{v_y}\\ \vec{w}\end{array}\right]=\frac{1}{4}\left[\begin{array}{cccc}1& 1& 1& 1\\ -1& 1& 1& -1\\ -\frac{1}{a+b}& \frac{1}{a+b}& -\frac{1}{a+b}& \frac{1}{a+b}\end{array}\right]\cdot \left[\begin{array}{c}\overrightarrow{V_1}\\ \overrightarrow{V_2}\\ \overrightarrow{V_3}\\ \overrightarrow{V_4}\end{array}\right]\text{\hspace{1em}(3-9)}$$
通过编码器反馈可以得到$\overrightarrow{V_1}$、$\overrightarrow{V_2}$、$\overrightarrow{V_3}$、$\overrightarrow{V_4}$的大小V₁、V₂、V₃、V₄，用编码器反馈求轮子速度的方法，参考（1-1）。

2.逆运动学

参考（2-5）到（2-8）。
$$\left[\begin{array}{c}\overrightarrow{V_1}\\ \overrightarrow{V_2}\\ \overrightarrow{V_3}\\ \overrightarrow{V_4}\end{array}\right]=\left[\begin{array}{ccc}1& -1& -(a+b)\\ 1& 1& (a+b)\\ 1& 1& -(a+b)\\ 1& -1& (a+b)\end{array}\right]\cdot \left[\begin{array}{c}\overrightarrow{v_x}\\ \overrightarrow{v_y}\\ \vec{w}\end{array}\right]\text{\hspace{1em}(3-9)}$$

3.轮式里程计

参考两轮差速模型的轮式里程计。

3. 精度问题

1.控制精度

控制过程一般由控制量和反馈量组成的闭环控制实现，那么控制量和反馈量的精度会直接影响整个控制精度。由于底盘中电机的控制量和反馈量一般为PWM信号与编码器信号，因此可以通过提高PWM信号和编码器信号的分辨率来提高整个底盘的控制精度。由于编码器分辨率越高，电机速度采样精度越高，闭环控制稳定后的偏差越小，因此可以选择分辨率更高的编码器来提高控制精度。PWM信号的分辨率通常由其量化等级决定，以0～100%占空比的PWM信号为例，如果PWM信号用16-bit量化，那么PWM的取值只能是1/216的倍数；如果PWM信号用32-bit量化，那么PWM的取值只能是1/232的倍数。显然采用32-bit量化的PWM信号的分辨率比采用16-bit量化的PWM信号的分辨率要高，因此可以选择量化等级更高的PWM信号来提高控制精度。

理论上电机的转速可以从0开始取值，但是电机会有一个截止速度，即当PWM小于某个阈值后，电机就不转了，处于截止状态。电机实际速度是从非0的截止速度开始的。在一些非常特殊的场合，底盘需要以非常小的速度逼近目标或者避开障碍时，如果该目标速度小于截止速度，那么底盘就会处于启动、停止交替的震荡状态，这个要特别注意。

2.里程计精度

在底盘中，里程计指轮式里程计。轮式里程计取值是相对值，只能用于局部定位。因此，其精度也只能用相对方式衡量。

在常规的评价方式中，比如让底盘行走10m的直线，产生的测量偏差作为直行误差，并让底盘自转360°，产生的测量偏差作为旋转误差。实验表明，底盘的旋转误差一般都会比直行误差大。所以，很多算法会采用IMU与轮式里程计融合的方式，利用IMU低旋转误差校准轮式里程计的高旋转误差，达到互补的效果。

另外一种是概率评价方式，对轮式里程计误差进行概率建模，用协方差来量化里程计误差，经过量化后的误差就可以用于各种算法的计算推导。这一部分也是SLAM里面状态估计问题，下面将会着重整理一下。

二、概率运动模型

在考虑实际机器人问题中存在的众多不确定性因素，比如传感器测量噪声、电机控制偏差、计算机软件计算精度近似，然后利用概率描述机器人中的不确定性，这样机器人中的不确定性就可以被计算和推演，这就是著名的概率机器人学。

概率机器人将模型与传感数据进行了无缝集成，同时克服了两者的局限性。这些思想不只是一个低级别的控制问题。他们涉及了机器人软件从底层到最高层的各个层面。

与传统的额机器人编程技术（如基于模型的运动规划技术或者被动的基于行为的方法）相比，概率方法在面对传感器的局限和模型的局限时鲁棒性更强。这使他们对复杂的真实世界的环境（不确定甚至更为显著）比以前的模式具有更好的伸缩性。事实上，对于目前较为困难的机器人估计问题，如定位问题或者大环境构建精确地图的问题，概率算法是目前已知唯一有效的解决办法。

与传统的基于模型的机器人技术相比，概率算法对机器人模型的精度要求较低，因此解除了程序员要给出精确模型这一不可逾越的负担。但是这种优势是要有代价的。概率算法最经常提到的两个局限性是计算的复杂性和近似的必要性。概率算法本质要比费概率算法效率低。这是因为如下事实：他们考虑的是整个概率密度而不是单一的推测。必要的近似源于这样一个事实，即大多数机器人世界是连续的。精确的后验分布往往很难计算，幸运的是，不确定性有时可以用一个紧凑的参数模型（如高斯模型）很好地近似。另一方面，这样的近似在使用中太过粗略，必须使用更复杂的表达。

上面只是对概率机器人一个简单介绍，下面将从机器人的底盘运动中具体讨论这个问题。

在工程中，在机器人底盘没有配备任何运动感知传感器的情况下，可以利用发送给底盘的控制命令（即前馈速度u_k=[v_x,v_y,w_z]^T）来预测底盘的运动情况。一般假设底盘在△t时间内以前馈速度u_k做匀速运动，那么底盘的运动就可以用u_k·△t来表示。但是底盘实际运动速度显然会与控制命令有很多出入，所以底盘基本上都配备了轮式里程计来获取实际运动反馈量，轮式里程计并不需要底盘匀速运动的假设条件就能有效预测底盘的运动情况。这两种情况就是所谓的速度运动模型和里程计运动模型。

1.速度运动模型

在理想情况下，前馈速度就是底盘的实际运动速度，底盘位姿在u_k作用下从x_k-1转移到x_k是不存在误差的，也就是状态转移过程是完全确定的。然而实际的运动速度与前馈速度是存在误差的，这也导致在u_k作用下从x_k-1转移到x_k是不确定的，需要用状态转移概率P(x_k|x_k-1，u_k)表示这种不确定性，即所谓的概率速度运动模型。下面从简单的理想情况开始讨论，然后在u_k中引入误差项，推导状态转移概率P(x_k|x_k-1，u_k)的形式。

1.不考虑速度的误差

机器人底盘以速度u_k在很短的时间△t内做匀速运动，位姿从x_k-1转移到x_k，运动速度模型如下图。在很小的时间内，可以用u_k·△t来近似表示状态转移量，状态转移方程如式（4-1）所示。
$$x_k=\left[\begin{array}{c}x{}^{\prime }\\ y{}^{\prime }\\ \theta {}^{\prime }\end{array}\right]=g(x_{k-1},u_k)=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]+\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ w_z\end{array}\right]△t\text{\hspace{1em}(4-1)}$$

式（4-1）成立需要满足两个条件：一是运动时间△t很小，二是实际的运动速度与前馈速度u_k之间不存在误差，也就是说状态转移过程是完全正确的。

2.考虑速度的误差

在实际情况中，我们需要考虑u_k存在的误差。假设u_k、x_k-1和x_k的误差都服从高斯分布，如式（4-2）、式（4-3）和式（4-4）所示。
$$u_k\sim N(\overline{u},\sum _{u_k})\text{\hspace{1em}(4-2)}$$

$$x_{k-1}\sim N(\overline{x_{k-1}},\sum _{x_{k-1}})\text{\hspace{1em}(4-3)}$$

$$x_k\sim N(\overline{x_k},\sum _{x_k})\text{\hspace{1em}(4-4)}$$

这样，u_k、x_k-1和x_k就都变成了多元随机变量，随机变量的均值就是其估计值，我们用协方差矩阵来描述其不确定性大小。在概率论中也学过方差表示两个随机变量的总体误差，如果需要求m个随机变量的总体误差，就要用协方差矩阵描述，u_k的误差主要由三个速度分量，v_x,v_y和w_z的误差决定，因此**u_k**的协方差矩阵如式(4-5)所示。
$$\sum _{x_k}=\left[\begin{array}{ccc}{\sigma }_{v_x}^2& 0& 0\\ 0& {\sigma }_{v_y}^2& 0\\ 0& 0& {\sigma }_{w_z}^2\end{array}\right]\text{\hspace{1em}(4-5)}$$
已知u_k和x_k-1的分布特性，利用式（4-1）所示的状态转移函数，就可以推导出x_k的分布特性，即状态转移概率P(x_k|x_k-1，u_k)。最简单的方法，现将状态转移函数g进行一阶泰勒级数展开来实现线性化，然后直接通过线性计算就能得到x_k的协方差矩阵，如下式所示。

$$\sum _{x_k}=\left[\begin{array}{cc}\frac{\partial g}{\partial x_{k-1}}& \frac{\partial g}{\partial u_k}\end{array}\right]\left[\begin{array}{cc}{\sum }_{x_{k-1}}& 0\\ 0& {\sum }_{u_k}\end{array}\right]{\left[\begin{array}{cc}\frac{\partial g}{\partial x_{k-1}}& \frac{\partial g}{\partial u_k}\end{array}\right]}^T\text{\hspace{1em}(4-6)}$$
最左面一项为函数g的雅克比矩阵。

上面的推导基于两大假设，一是假设了运动时间△t很小，这样式（4-1）的计算才成立；而是假设u_k、x_k-1和x_k的误差都服从高斯分布，这样x_k-1能直接用u_k、x_k-1推导出来。但是这两个假设性都太强了，有点违背实际情况。这个地方，有论文中有更好的方法，还没具体了解，先把论文附在这里吧。

THRUN S，BURGARD W，FOX D．Probabilistic Robotics：Intelligent Robotics and Autonomous Agents［M］．Cambridge：The MIT Press，2005.

2.里程计运动模型

上面的速度运动模型中有太强的假设，准确定很难保证，往往是在机器人底盘没有配备任何运动传感器时才迫不得已使用，而移动机器人通常都会配备轮式里程计用于反馈运动情况，因此里程计运动模型是主流模型，下面进行整理。

通过前后两个里程计反馈的差值，可以求出里程计增量值u_k，即所谓的机器人状态转移量，如式（4-7）所示。
$$u_k=odo{m}_k-odo{m}_{k-1}=\left[\begin{array}{c}△x\\ △y\\ △\theta \end{array}\right]\text{\hspace{1em}(4-7)}$$
与讨论速度运动模型一样，下面先从简单的理想情况开始讨论，然后在u_k中引入误差项，推导状态转移概率**P(x_k|x_k-1，u_k)**的形式。

1.不考虑里程计的误差

由于轮式里程计是依靠安装在底盘轮子上的编码盘来实时检测底盘的运动，所以里程计相邻帧间的增量值u_k不需要运动时间△t很小和匀速运动的假设，就能表示底盘状态转移。下图是里程计运动模型，可以看到机器人底盘的运动路线可以是任意曲线。

不考虑u_k的误差，且已知u_k和x_k-1时，能直接用式（4-8）所示的状态转移方程求出x_k。
$$x_k=\left[\begin{array}{c}x{}^{\prime }\\ y{}^{\prime }\\ \theta {}^{\prime }\end{array}\right]=g(x_{k-1},u_k)=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]+\left[\begin{array}{c}△v\\ △v\\ △w\end{array}\right]\text{\hspace{1em}(4-8)}$$

2.考虑里程计的误差

在实际情况中，我们需要考虑u_k中存在的误差，我们用最简单的方法，假设u_k、x_k-1和x_k的误差都服从高斯分布，如式（4-9）到式（4-11）。
$$u_k\sim N(\overline{u},\sum _{u_k})\text{\hspace{1em}(4-9)}$$

$$x_{k-1}\sim N(\overline{x_{k-1}},\sum _{x_{k-1}})\text{\hspace{1em}(4-10)}$$

$$x_k\sim N(\overline{x_k},\sum _{x_k})\text{\hspace{1em}(4-11)}$$

这样，u_k、x_k-1和x_k就都变成了多元随机变量，随机变量的均值就是其估计值，我们用协方差矩阵来描述其不确定性大小。在概率论中也学过方差表示两个随机变量的总体误差，如果需要求m个随机变量的总体误差，就要用协方差矩阵描述，u_k的误差主要由三个位姿偏移分量，△x、△y和△θ的误差决定，因此u_k的协方差矩阵如式(4-12)所示。
$$\sum _{x_k}=\left[\begin{array}{ccc}{\sigma }_{△x}^2& 0& 0\\ 0& {\sigma }_{△y}^2& 0\\ 0& 0& {\sigma }_{△\theta }^2\end{array}\right]\text{\hspace{1em}(4-12)}$$

式（4-12）机器人在二维平面运动时u_k的协方差矩阵。为了更加通用，用u_k表示机器人在三维空间运动时，对应的协方差矩阵如式（4-13）所示。虽然u_k表示三维空间运动，但是机器人底盘被限制在二维平面运动，所以z轴的偏移量、x轴旋转量和y轴旋转量均为0。
$$\sum _{x_k}=\left[\begin{array}{cccccc}{\sigma }_{△x}^2& 0& 0& 0& 0& 0\\ 0& {\sigma }_{△y}^2& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& {\sigma }_{△\theta }^2\end{array}\right]\text{\hspace{1em}(4-13)}$$
可以看出式（4-13）是个6×6的矩阵，ROS中里程计的而标准数据类型就是采用这用6×6的协方差矩阵来描述里程计的误差。

但是轮式里程计是存在累计误差的，即机器人底盘运动的越远，累计误差越大，因此可以用运动距离$$\sqrt{(△x{)}^2+(△y{)}^2}$$和旋转量$$|△\theta |$$来量化u_k的误差，如式（4-14）。
$${\sigma }_{△x}^2={\xi }_{△xy}+{\alpha }_1\sqrt{(△x{)}^2+(△y{)}^2}+{\alpha }_2|△\theta |\text{\hspace{1em}(4-14)}$$

$${\sigma }_{△y}^2={\xi }_{△xy}+{\alpha }_1\sqrt{(△x{)}^2+(△y{)}^2}+{\alpha }_2|△\theta |\text{\hspace{1em}(4-15)}$$

$${\sigma }_{△\theta }^2={\xi }_{△\theta }+{\alpha }_3\sqrt{(△x{)}^2+(△y{)}^2}+{\alpha }_4|△\theta |\text{\hspace{1em}(4-14)}$$

式中的 $${\xi }_{△xy}$$和$${\xi }_{△\theta }$$是人为引入的静态误差量，这两个量均为一个很小的常数，以避免后续协方差迭代计算陷入惰性。系数α₁~α₄描述运动距离$$\sqrt{(△x{)}^2+(△y{)}^2}$$和$$|△\theta |$$对u_k误差的影响，是机器人底盘固有的参数，可通过实验法测量得到。

通过上面的分析，可以知道里程计的误差既可以通过底盘系数α_1~ α₄描述，也可以用6×6的协方差矩阵来描述。比如ROS中的Gmapping算法使用的就是系数α₁-α₄，再如ROS中的AMCL算法使用的也是系数α₁~α₄。

给出机器人利用里程计运动模型推算位姿，其不确定性的演变过程如下图所示。图中轨迹上的黑点表示机器人可能出现的位姿，黑点周围的密度表示概率大小，黑点越集中表示出现的概率越大。在起始时，机器人位姿集中在一个点上，其概率是1，所以位姿是完全确定的。随着机器人逐渐移动，由于里程计存在的误差，因此直接利用里程计推导出的机器人位姿不确定性越来越大，即机器人可能出现的位姿点越来越分散，最后这些可能的位姿点将分散到整个空间，即机器人位姿将变得完全随机。为了不让机器人位姿发散，必须在里程计运动模型的基础上，引入传感器对环境的观测数据，SLAM中相机、雷达等传感器的作用，但是这些传感器也有误差，具体的方式，后面有机会还会整理。

通过上面的分析，可以知道里程计的误差既可以通过底盘系数α_1~ α₄描述，也可以用6×6的协方差矩阵来描述。比如ROS中的Gmapping算法使用的就是系数α₁-α₄，再如ROS中的AMCL算法使用的也是系数α₁~α₄。

给出机器人利用里程计运动模型推算位姿，其不确定性的演变过程如下图所示。图中轨迹上的黑点表示机器人可能出现的位姿，黑点周围的密度表示概率大小，黑点越集中表示出现的概率越大。在起始时，机器人位姿集中在一个点上，其概率是1，所以位姿是完全确定的。随着机器人逐渐移动，由于里程计存在的误差，因此直接利用里程计推导出的机器人位姿不确定性越来越大，即机器人可能出现的位姿点越来越分散，最后这些可能的位姿点将分散到整个空间，即机器人位姿将变得完全随机。为了不让机器人位姿发散，必须在里程计运动模型的基础上，引入传感器对环境的观测数据，SLAM中相机、雷达等传感器的作用，但是这些传感器也有误差，具体的方式，后面有机会还会整理。