小豆包的学习之旅：里程计运动模型

合集 - SLAM(55)

1.个人资源索引2018-03-032.SLAM学习笔记（1）基本概念2015-08-063.SLAM学习笔记（2）SLAM算法2015-08-214.[ROS]2 尝试编译OrbSLAM2016-01-185.[ROS]1 小乌龟2016-01-186.SLAM最近的工作2015-11-057.SLAM学习笔记（3）相关概念2015-09-168.[SLAM]2D激光线特征提取2016-07-169.[SLAM] GMapping SLAM源码阅读（草稿）2016-07-0110.[QGLViewer]First Demo2016-06-2911.SLAM数据集2016-06-2312.[SLAM]2D激光扫描匹配方法2016-06-1513.[g2o]一个备忘2016-04-2114.[ROS] slam_gmapping2016-01-2415.[ROS]3 Linux编程练习2016-01-2016.Linux学习和ROS安装（1）2017-12-1917.小豆包的学习之旅：机器人定位2017-05-0218.小豆包的学习之旅：传感器观测模型2017-04-2119.Kinect2.0相机标定2017-04-10

20.小豆包的学习之旅：里程计运动模型2017-04-03

21.小豆包的学习之旅：入门篇2017-04-0222.Kinect2.0点云数据获取2017-03-3023.[硬件]Robot运动控制2017-03-2224.[硬件]Urg_viewer数据读取2017-03-1425.[硬件]三维点云数据获取2017-03-0426.Kinect2.0获取数据2017-02-1527.ndt histogram_direction2016-09-0828.rplidar & hector slam without odometry2016-08-2629.rplidar测试2016-08-1230.[原创]NDT方法在SLAM中的应用2016-08-0831.[SLAM]Karto SLAM算法学习（草稿）2016-07-2232.ROS学习备忘2018-07-1833.[ROS]激光驱动安装2018-07-1634.OrbSLAM2采集点云数据2018-03-2635.Cartographer源码阅读(8)：imu_tracker2018-03-2136.[硬件]点云数据采集22018-03-1937.Cartographer源码阅读(9)：图优化的前端——闭环检测2018-03-0838.Cartographer源码阅读(7)：轨迹推算和位姿推算的原理2018-03-0839.Ethzasl MSF源码阅读(3)：MSF_Core和PoseMeasurement2018-03-0740.Ethzasl MSF源码阅读(2)：百川汇海2018-03-0641.Ethzasl MSF源码阅读(1)：程序入口和主题订阅2018-03-0442.Cartographer源码阅读(6)：LocalTrajectoryBuilder和PoseExtrapolator2018-02-2543.Cartographer源码阅读(5)：PoseGraph位姿图2018-02-2444.Cartographer源码阅读(4)：Node和MapBuilder对象22018-01-1045.Cartographer源码阅读(3)：程序逻辑结构2017-12-2946.Cartographer源码阅读(2)：Node和MapBuilder对象2017-12-2847.Cartographer源码阅读(1)：程序入口2017-12-2848.实时Cartographer测试(1) - rplidar2017-12-2149.Cartographer安装2017-12-2050.ROS安装（2）2017-12-2051.[ROS]一些传感器数据读取融合问题的思考2017-05-0652.小豆包的学习之旅：占用概率栅格地图和cost-map2017-05-0253.小豆包的学习之旅：开发记录2017-04-2954.[概述]移动机器人自主探索2017-04-1855.MRPT编译2017-04-14

收起

星球大战里面有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

 

---

实例：