ROS机器人里程计模型

1常用坐标系系统模型

在移动机器人技术研究中，最为常用的坐标系统是笛卡尔坐标系统。而在SLAM算法研究中，有如下几个比较常见的笛卡尔坐标系统，它们分别为机器人坐标系XR YR OR 、传感器坐标系XS YS OS 、世界坐标系XW YW OW 。顾名思义，世界坐标系是描述机器人全局信息的坐标系；机器人坐标系是描述机器人自身信息的坐标系；传感器坐标系是描述传感器信息的坐标系。他们之间的关系如下图所示：

从图中我们可以知道，首先世界坐标系是固定不变的，机器人坐标系和传感器坐标系是在世界坐标系下描述的。这里我们看到的是俯视图，机器人坐标系和传感器坐标系原点重合但是存在一定的角度，不同的机器人坐标系关系是不同的。当我们使用传感器数据时，这些坐标系间的关系就是我们变换矩阵的参数，因为传感器的数据必定是要变换到机器人坐标系或者世界坐标系中使用的。

2移动机器人位姿模型

移动机器人的位姿模型就是机器人在世界坐标系下的状态。常用随机变量Xt =(xt ,yt ,θt )来描述t时刻的机器人在世界坐标系下的状态，简称位姿。其中(xt ,yt )表示的在t时刻机器人在世界坐标系下的位置，θt 表示机器人的方向。默认世界坐标系的X正半轴为正方向，逆时针旋转为旋转正方向，初始时刻机器人坐标系和世界坐标系重合。某时刻t机器人的位姿描述如下图所示：

3移动机器人里程计模型

简单的说，移动机器人的里程计就是机器人每时每刻在世界坐标系下位姿状态。常用的激光SLAM和导航算法通常都需要移动机器人的里程计作为控制输入。这也是我们对本章理论内容讲解的必要性。

不同底盘的里程计模型有所不同，本文以两轮差分轮式机器人为例介绍里程计模型以及如何计算里程计。在介绍里程计模型之前，要先介绍差分轮式机器人的运动学模型，这样可以先了解一下机器人的物理特性。首先明确差分模型的机器人始终做的是以R为半径的圆弧运动。如下图所示，机器人的线速度V、角速度ω，左右轮速用VL和VR表示，用D表示轮间距，D=2d，右轮到旋转中心的距离为L。

ROS端给机器人底盘（STM32端）发送的是机器人要达到的线速度V和角速度ω，而我们底盘控制板需要的是左右轮速VL和VR来进行速度控制。所以，我们通过高中知识可以得到他们之间的关系：

上面的公式是由角速度和线速度的关系得来，如果不知道这个关系，那就自行百度吧，我相信你一定知道。从这些公式我们是不是发现了什么，机器人的轮间距影响着我们向左右轮分发速度以及合成角速度，所以这是个我们需要注意的参数，不同的机器人结构，该参数就不同。

有了上面坐标系系统模型、位姿模型的基础，里程计就非常简单了。里程计的计算是指以机器人上电时刻为世界坐标系的起点（机器人的航向角是世界坐标系X正方向）开始累积计算任意时刻机器人在世界坐标系下的位姿。通常计算里程计方法是速度积分推算：通过左右电机的编码器测得机器人的左右轮的速度VL和VR，在一个短的时刻△t内，认为机器人是匀速运动，并且根据上一时刻机器人的航向角计算得出机器人在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理，关于航向角θ采用的IMU的yaw值。然后根据以上描述即可得到机器人的里程计==。具体计算如下图所示：

至此，里程计模型原理及计算就结束了。

2D激光雷观测模型

我们做移动机器人平台少不了使用2D激光雷达，所以了解激光雷达的观测模型也是有必要的。
激光雷达通常由精准控制的旋转电机、红外激光发射器、红外接收视觉系统和主控组成。

激光雷达的测距原理分为两种，一种是基于三角测距，另一个是基于TOF（飞行时间）。基于三角测距的激光雷达表现出的特点，价格便宜、中近距离测距较准确、远距离精度差。基于TOF的激光雷达表现出来的特点，价格昂贵、测距精度高、测距范围广、扫描频率高。

本文采用市面上比较便宜的基于三角测距的低成本激光雷达RPLIDAR A1。该激光雷达在每次测距过程中，发射器发射红外激光信号，视觉采集系统接收激光反射信号。在经过主控实时解算后，将激光雷达几何中心到被测物体的距离值以及当时的角度值，通过主控的通信接口发出。RPLIDAR A1 工作原理如图所示。

激光雷达在整个SLAM和导航中起着不可替代的作用。第一，通过激光雷达观测数据与地图进行匹配，估计出机器人的位姿；第二，当机器人估计出较准确的位姿时，通过激光雷达的观测数据建立环境地图；第三，在机器人导航过程中，检测地图中的未知障碍物。本文主要使用的观测传感器是2D激光雷达RPLIDAR A1如下图所示：

RPLIDAR A1是一款360度激光雷达，角度分辨率≤1，输出的每一个激光点的数据都是使用极坐标的方式描述，一个距离值以及一个对应的角度值。

其实关于差分轮式机器人搭建的数学知识就这些。当然这里只是说的搭建，而不是以后的算法。

好好理解这些内容吧，下一节，我们就开始ROS小车的启动文件编写了，我们要通过C++代码，完成一个机器人启动功能包。在ROS中启动该功能包，我们就可以通过ROS的键盘功能包或者ROS app控制我们的机器人了。

同时，我们也会启动传感器获取导航和建图需要的深度信息，发布里程计odom，发布tf维护base_link到odom的tf变换。

感谢知乎作者小白学移动机器人，原文连接
https://zhuanlan.zhihu.com/p/218319127