小豆包的学习之旅：里程计运动模型
星球大战里面有2个非常出色的机器人一个是3PO，一个是R2。我的机器人呢就叫小豆包啦，代号FR2，不许侵犯它的署名哦。
常见的机器人运动模型包括：速度计运动模型，里程计运动模型和惯导运动模型。概率运动模型是对机器人运动的一种概率描述。
里程计是比较常见的一种类型。严格来说，里程计是一种传感器测量信息而非控制信息。如果将里程计视为测量信息，贝叶斯滤波需要包含速度作为状态变量，将增加状态空间的维度。因此，为了减少状态空间，通常将里程计数据视为控制信号。
首先是定义：
小豆包在$t-1$时刻的位姿$x_{t-1}=(x,y,\theta)^T$，$t$时刻的位姿$x_{t}=(x',y',\theta')^T$运动控制信息$u_{t}$，概率运动模型为$P\{x_{t}|u_{t},x_{t-1}\}$。
其中，运动控制信息$u_{t}=\{\overline {x}_{t-1},\overline {x}_{t}\}$。$\overline {x}_{t-1}=( \overline x, \overline y, \overline \theta)^T$,$\overline {x}_{t}=( \overline x', \overline y', \overline \theta')^T$。
将小豆包在$(t-1,t]$时间间隔内的运动分解为三个阶段：旋转$\delta_{rot1}$，平移$\delta_{trans}$，旋转$\delta_{rot2}$.
转向和平移都存在不确定性，则存在三个阶段的误差，认为三个阶段的误差相互独立，采用高斯分布表达这种不确定性。
运用高斯分布表达噪声模型，则运动模型算法流程如下：
|
1: Algorithm motion_model_odometry $(x_{t}, u_{t}, x_{t-1})$: 2: $\delta_{rot1} = atan2(\overline y'- \overline y, \overline x’ - \overline x) - \overline \theta$ 3: $\delta_{trans} = (\overline x - \overline x')^2 + (\overline y - \overline y')^2$ 4: $\delta_{rot2} = \overline \theta' - \overline \theta- \delta_{rot1}$ //里程计读数，运动控制信息$u_{t}$ 5: $\delta_{rot1} = atan2(y' - y, x' - x) - \theta$ 6: $\delta_{trans} = (x - x')^2 + (y - y')^2$ 7: $\delta_{rot2} = \theta' - \theta - \delta_{rot1}$ 8: $p1 = prob(\delta_{rot1} - \widehat {\delta}_{rot1}, α1\widehat {\delta}_{rot1}^2 + α2\widehat {\delta}_{rot1}^2 )$ 9: $p2 = prob(\delta_{trans} - \widehat {\delta}_{trans}, α3\widehat {\delta}_{trans}^2 + α4\widehat {\delta}_{rot1}^2 + α4\widehat {\delta}_{rot2}^2 )$ 10: $p3 = prob(\delta_{rot2} - \widehat {\delta}_{rot2}, α1\widehat {\delta}_{rot2}^2 + α2\widehat {\delta}_{trans}^2 )$ 11: return $p1 · p2 · p3 $ |
采用采样方式表达运动模型，主要运用于粒子滤波算法中：
|
1: Algorithm Sample_motion_model_odometry $(u_{t}, x_{t-1})$: 2: $\delta_{rot1} = atan2(\overline y'- \overline y, \overline x’ - \overline x) - \overline \theta$ 3: $\delta_{trans} = (\overline x - \overline x')^2 + (\overline y - \overline y')^2$ 4: $\delta_{rot2} = \overline \theta' - \overline \theta- \delta_{rot1}$ //里程计读数，运动控制信息$u_{t}$ 5: $\widehat {\delta}_{rot1} =\delta_{rot1} - sample(α1\widehat {\delta}_{rot1}^2 + α2\widehat {\delta}_{rot1}^2)$ 6: $\widehat {\delta}_{trans}=\delta_{trans}- sample(α3\widehat {\delta}_{trans}^2 + α4\widehat {\delta}_{rot1}^2 + α4\widehat {\delta}_{rot2}^2)$ 7: $\widehat {\delta}_{rot2} =\delta_{rot2} - sample(α1\widehat {\delta}_{rot2}^2 + α2\widehat {\delta}_{trans}^2)$ //为运动控制的变化量增加误差项 8: $x'= x+\widehat {\delta}_{trans} cos(\theta+\widehat {\delta}_{rot1})$ 9: $y'= y+\widehat {\delta}_{trans} cos(\theta+\widehat {\delta}_{rot1})$ 10: $\theta'= \theta+\widehat {\delta}_{rot1}+\widehat {\delta}_{rot2}$ //加在$t-1$时刻的位姿上，计算出$t$时刻的位姿 11: return $x_{t}=(x',y',\theta')^T$ |
里程计数据通过轮编码器获取，随着时间的推移，误差会不断累积，而且角度误差累积的更厉害。IMU对角度的误差累积比距离累积要小。另外，机器人也可能存在机器人“绑架”之类的机械问题。这些都是运动模型的局限性，需要对运动观测数据进行优化校正，提高机器人的位姿精度。
机器人“绑架”指的是机器人在运动过程中发生了滑动,机器人传感器观测并不知道此情况的发生。
其他的运动模型：Probabilistic Motion Models
实例：
