概率机器人：里程计运动模型

里程计运动模型

速度运动模型采用机器人的速度来计算位姿的后验概率。里程计运动模型（Odometry motion model）则以里程计测量为基础来计算机器人随时间的运动。

通常，通过编码器可以获得机器人的里程信息。相比而言，里程计虽仍存在误差，但比之速度信息更为精确。

模型建立

理想模型

里程计信息实际上是对传感器的读取测量值，而非控制量。为建立作为测量的里程计模型且保持状态空间较小，将里程计测量值认为控制信号。

在时刻$t$，机器人确切的位姿由随机变量$x_t$建立，里程计用于估计该位姿信息。由于里程计存在漂移、打滑情况，机器人里程计坐标系与世界坐标系间不存在固定的变换关系。

里程计模型使用由机器人内部里程计测量的相对运动信息。即在时间间隔$(t-1,t]$中，机器人位姿由$x_{t-1}$前进至$x_t$，里程计反馈了${\bar{x}}_{t-1}={\left[\begin{array}{ccc}\bar{X}& \bar{Y}& \bar{\theta }\end{array}\right]}^T$到${\bar{x}}_t={\left[\begin{array}{ccc}{\bar{X}}^{{}^{\prime }}& {\bar{Y}}^{{}^{\prime }}& {\bar{\theta }}^{{}^{\prime }}\end{array}\right]}^T$的相对前进。采用上标“$\stackrel{ˉ}{}$”表示为机器人坐标系下坐标，该坐标系与世界坐标系转换关系未知。

由此，机器人的控制量$u_t={\left[\begin{array}{cc}{\bar{x}}_{t-1}& {\bar{x}}_t\end{array}\right]}^T$，可将任意两相邻时刻机器人位姿的相对差可通过三个基本运动串联得到，如图所示。

• 初始旋转：${\delta }_{rot1}=atan2({\bar{Y}}^{{}^{\prime }}-\bar{Y},{\bar{X}}^{{}^{\prime }}-\bar{X})-\bar{\theta }$
• 平移：${\delta }_{trans}=\sqrt{(\bar{X}-{\bar{X}}^{{}^{\prime }}{)}^2+(\bar{Y}-{\bar{Y}}^{{}^{\prime }}{)}^2}$
• 二次旋转：${\delta }_{rot2}={\bar{\theta }}^{{}^{\prime }}-\bar{\theta }-{\delta }_{rot1}$

由此，机器人由初始位姿$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$沿着运动轨迹方向（下图中紫线所示）运动至当前时刻位姿$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$可拆分为机器人首先旋转角度${\delta }_{rot1}$再沿着此方向平移${\delta }_{trans}$，随后再次旋转${\delta }_{rot2}$抵达目标位姿，如图所示：

上图中，由三角函数化简可以得到如下表达式：
$$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ \theta \end{array}\right]+\left[\begin{array}{c}{\delta }_{trans}\cdot \cos (\theta +{\delta }_{rot1})\\ {\delta }_{trans}\cdot \sin (\theta +{\delta }_{rot1})\\ {\delta }_{rot1}+{\delta }_{rot2}\end{array}\right]$$

噪声模型

在真实环境下，机器人运动及里程计的测量存在噪声。定义里程计的测量值减去噪声得到的为实际里程值：
$$\begin{array}{rl}{\hat{\delta }}_{rot1}& ={\delta }_{rot1}-{\epsilon }_{{\alpha }_1{\delta }_{rot1}^2+{\alpha }_2{\delta }_{trans}^2}\\ {\hat{\delta }}_{trans}& ={\delta }_{trans}-{\epsilon }_{{\alpha }_3{\delta }_{trans}^2+{\alpha }_4{\delta }_{rot1}^2+{\alpha }_4{\delta }_{rot2}^2}\\ {\hat{\delta }}_{rot2}& ={\delta }_{rot2}-{\epsilon }_{{\alpha }_1{\delta }_{rot2}^2+{\alpha }_2{\delta }_{trans}^2}\end{array}$$
则，噪声下机器人的里程计运动模型如下所示：
$$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ Y^{{}^{\prime }}\\ {\theta }^{{}^{\prime }}\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ \theta \end{array}\right]+\left[\begin{array}{c}{\hat{\delta }}_{trans}\cdot \cos (\theta +{\hat{\delta }}_{rot1})\\ {\hat{\delta }}_{trans}\cdot \sin (\theta +{\hat{\delta }}_{rot1})\\ {\hat{\delta }}_{rot1}+{\hat{\delta }}_{rot2}\end{array}\right]$$

闭式算法

算法推导

在真实环境下，通过里程计测量值减去实际值可以得到里程计误差：
$$\begin{array}{rl}{\delta }_{rot1,err}& ={\delta }_{rot1}-{\hat{\delta }}_{rot1}\\ {\delta }_{trans,err}& ={\delta }_{trans}-{\hat{\delta }}_{trans}\\ {\delta }_{rot2,err}& ={\delta }_{rot2}-{\hat{\delta }}_{rot2}\end{array}$$
由此，得到各项噪声的概率密度计算如下：
$$\begin{array}{rl}{p}_1& ={\epsilon }_{{\alpha }_1{\delta }_{rot1}^2+{\alpha }_2{\delta }_{trans}^2}({\delta }_{rot1,err})\\ p_2& ={\epsilon }_{{\alpha }_3{\delta }_{trans}^2+{\alpha }_4{\delta }_{rot1}^2+{\alpha }_4{\delta }_{rot2}^2}({\delta }_{trans,err})\\ p_3& ={\epsilon }_{{\alpha }_1{\delta }_{rot2}^2+{\alpha }_2{\delta }_{trans}^2}({\delta }_{rot2,err})\end{array}$$
假设各项噪声相互独立，则由$x_{t-1}$状态得到$x_t$状态的概率密度：
$$p(x_t|u_t,x_{t-1})=p_1\cdot p_2\cdot p_3$$

算法设计

系统输入： 初始位姿$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$、控制量$u_t={\left[\begin{array}{cc}{\bar{x}}_{t-1}& {\bar{x}}_t\end{array}\right]}^T$、估计后继姿态$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$

系统输出： 后继姿态为$x_t$的概率$p(x_t|u_t,x_{t-1})$

假设条件： 控制算法以固定时间间隔$\Delta t$执行。

用参数${\alpha }_1$ ~ ${\alpha }_4$表示机器人里程计运动噪声系数：

• 里程计平移噪声
  • 参数${\alpha }_1$：平移运动中平移分量的噪声
  • 参数${\alpha }_2$：平移运动中旋转分量的噪声
• 里程计旋转噪声
  • 参数${\alpha }_3$：旋转运动中平移分量的噪声
  • 参数${\alpha }_4$：旋转运动中旋转分量的噪声

算法实现逻辑如下：
$$\begin{array}{rl}& Algorithmmotion\_model\_odometry(x_t,u_t,x_{t-1}):\\ 1:& {\delta }_{rot1}=atan2({\bar{Y}}^{{}^{\prime }}-\bar{Y},{\bar{X}}^{{}^{\prime }}-\bar{X})-\bar{\theta }\\ 2:& {\delta }_{trans}=\sqrt{(\bar{X}-{\bar{X}}^{{}^{\prime }}{)}^2+(\bar{Y}-{\bar{Y}}^{{}^{\prime }}{)}^2}\\ 3:& {\delta }_{rot2}={\bar{\theta }}^{{}^{\prime }}-\bar{\theta }-{\delta }_{rot1}\\ & \\ 4:& {\hat{\delta }}_{rot1}=atan2(Y^{{}^{\prime }}-Y,X^{{}^{\prime }}-X)-\theta \\ 5:& {\hat{\delta }}_{trans}=\sqrt{(X-X^{{}^{\prime }}{)}^2+(Y-Y^{{}^{\prime }}{)}^2}\\ 6:& {\hat{\delta }}_{rot2}={\theta }^{{}^{\prime }}-\theta -{\hat{\delta }}_{rot1}\\ & \\ 7:& p_1=prob({\delta }_{rot1}-{\hat{\delta }}_{rot1},{\alpha }_1{\hat{\delta }}_{rot1}^2+{\alpha }_2{\hat{\delta }}_{trans}^2)\\ 8:& p_2=prob({\delta }_{trans}-{\hat{\delta }}_{trans},{\alpha }_3{\hat{\delta }}_{trans}^2+{\alpha }_4{\hat{\delta }}_{rot1}^2+{\alpha }_4{\hat{\delta }}_{rot2}^2)\\ 9:& p_3=prob({\delta }_{rot2}-{\hat{\delta }}_{rot2},{\alpha }_1{\hat{\delta }}_{rot2}^2+{\alpha }_2{\hat{\delta }}_{trans}^2)\\ & \\ 10:& return{p}_1\cdot p_2\cdot p_3\end{array}$$
其中，1~3行从里程计测量值得到相对运动参数值：${\left[\begin{array}{ccc}{\delta }_{rot1}& {\delta }_{trans}& {\delta }_{rot2}\end{array}\right]}^T$

4~6行由机器人初始位姿$x_{t-1}$及预计位姿$x_t$计算得到实际相对运动参数：${\left[\begin{array}{ccc}{\hat{\delta }}_{rot1}& {\hat{\delta }}_{trans}& {\hat{\delta }}_{rot2}\end{array}\right]}^T$

7~9行计算各自噪声引起的概率密度$p_1、p_2、p_3$，由于各噪声相互独立，最后返回累乘值$p_1\cdot p_2\cdot p_3$。

函数$prob(x,b^2)$用于实现x上均值为0，方差为$b^2$的概率分布，可用正态分布或三角形分布实现，同速度运动模型，不在赘述。

应注意，所有角度差值应保持在区间$[-\pi ,\pi ]$内，即函数$prob(x,b^2)$中参数$x$属于该区间。

算法效果

在平面空间XOY下，设置机器人具有相同的初始姿态$x_{t-1}$和控制量$u_t$，则在不同里程计噪声参数下，模型具有不同效果。

当机器人具有中等误差（参数${\alpha }_1$ ~ ${\alpha }_4$），则如图所示：

当机器人具有较大的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较小的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

当机器人具有较小的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较大的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

当两次测量间时间较短时，里程计运动模型同速度运动模型的闭式算法效果差不多。也即，一个机器人如果置信度经常更新（0.1s更新一次），则运动模型间的不同差异不大。

采样算法

算法推导

采用粒子滤波用于采样，与闭式算法不同。该算法通过给定的初始姿态$x_{t-1}$和控制量$u_t={\left[\begin{array}{cc}{\bar{x}}_{t-1}& {\bar{x}}_t\end{array}\right]}^T$，估计出一个当前位姿$x_t$

算法设计

系统输入： 初始位姿$x_{t-1}={\left[\begin{array}{ccc}X& Y& \theta \end{array}\right]}^T$、控制量$u_t={\left[\begin{array}{cc}{\bar{x}}_{t-1}& {\bar{x}}_t\end{array}\right]}^T$

系统输出： 估计后继姿态$x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T$

假设条件： 控制算法以固定时间间隔$\Delta t$执行。

算法实现逻辑如下：
$$\begin{array}{rl}& Algorithmsample\_motion\_model\_odometry(x_t,u_t,x_{t-1}):\\ 1:& {\delta }_{rot1}=atan2({\bar{Y}}^{{}^{\prime }}-\bar{Y},{\bar{X}}^{{}^{\prime }}-\bar{X})-\bar{\theta }\\ 2:& {\delta }_{trans}=\sqrt{(\bar{X}-{\bar{X}}^{{}^{\prime }}{)}^2+(\bar{Y}-{\bar{Y}}^{{}^{\prime }}{)}^2}\\ 3:& {\delta }_{rot2}={\bar{\theta }}^{{}^{\prime }}-\bar{\theta }-{\delta }_{rot1}\\ & \\ 4:& {\hat{\delta }}_{rot1}={\delta }_{rot1}-sample({\alpha }_1{\hat{\delta }}_{rot1}^2+{\alpha }_2{\hat{\delta }}_{trans}^2)\\ 5:& {\hat{\delta }}_{trans}={\delta }_{trans}-sample({\alpha }_3{\hat{\delta }}_{trans}^2+{\alpha }_4{\hat{\delta }}_{rot1}^2+{\alpha }_4{\hat{\delta }}_{rot2}^2)\\ 6:& {\hat{\delta }}_{rot2}={\delta }_{rot2}-sample({\alpha }_1{\hat{\delta }}_{rot2}^2+{\alpha }_2{\hat{\delta }}_{trans}^2)\\ & \\ 7:& X^{{}^{\prime }}=X+{\hat{\delta }}_{trans}\cos (\theta +{\hat{\delta }}_{rot1})\\ 8:& Y^{{}^{\prime }}=Y+{\hat{\delta }}_{trans}\sin (\theta +{\hat{\delta }}_{rot1})\\ 9:& {\theta }^{{}^{\prime }}=\theta +{\hat{\delta }}_{rot1}+{\hat{\delta }}_{rot2}\\ & \\ 10:& return{x}_t=x_t={\left[\begin{array}{ccc}{X}^{{}^{\prime }}& Y^{{}^{\prime }}& {\theta }^{{}^{\prime }}\end{array}\right]}^T\end{array}$$
其中，1~3行从里程计测量值得到相对运动参数值：${\left[\begin{array}{ccc}{\delta }_{rot1}& {\delta }_{trans}& {\delta }_{rot2}\end{array}\right]}^T$

4~6行由机器人初始位姿$x_{t-1}$及采样样本计算得到实际相对运动参数值：${\left[\begin{array}{ccc}{\hat{\delta }}_{rot1}& {\hat{\delta }}_{trans}& {\hat{\delta }}_{rot2}\end{array}\right]}^T$

7~9行根据实际相对运动参数值，预测当前时刻机器人位姿$x_t$

式中，函数$sample(b^2)$用于实现均值为0，方差为$b^2$的随机样本生成，同速度运动模型中采样算法，不在赘述。

算法效果

在平面空间XOY下，设置机器人具有相同的初始姿态$x_{t-1}$和控制量$u_t$，且控制量与初始位姿同闭式计算中相同，则在不同不同里程计噪声参数下，具有不同效果。（采样500次）

当机器人具有中等误差（参数${\alpha }_1$ ~ ${\alpha }_4$），则如图所示：

当机器人具有较大的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较小的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

当机器人具有较小的平移误差（参数${\alpha }_1、{\alpha }_2$），但具有较大的角度误差（参数${\alpha }_3、{\alpha }_4$），则如图所示：

当机器人不断进行移动时，不确定性将增加，样本将遍布越来越大的空间，如下图所示机器人沿着实现不断移动：