Ros机器人坐标系总结学习笔记。
坐标轴与坐标系
对于大多数用户来说坐标转换是一个“黑箱”，不了解这个部分并不影响用户愉快的使用模拟器，但理解ROS以及Autolabor Simulation的坐标转换机制能帮助用户更好的控制他们的机器人，也使他们能够更快的查找出编码中可能出现的问题。
在ROS中，坐标轴规定可以参考 ROS-REP 103 ，坐标系可以参考 ROS-REP 105，为了方便大家的理解，这里结合模拟器做简单的讲解
坐标轴的定义
三维坐标轴朝向定义
常规用来表示三维位置关系时，使用的是右手坐标系，何为右手坐标系呢？我们来看下图：
|  |
右手坐标系定义[1] 把右手放在原点的位置，使大姆指，食指和中指互成直角，把 大姆指 指向 Z轴 的正方向，食指指向X轴的正方向时向时，中指所指的方向就是 Y轴 的正方向
通常相对于我们的身体而言
- X -> 朝前
- Y -> 朝左
- Z -> 朝上
举例: 坐标点 (3,4,5) 表示距原点 前3个单位 ，并向左4个单位以及向上5个单位 的位置。
三维坐标轴旋转定义
有了三维坐标轴的定义，那么对于旋转的定义也就比较容易了，对于一个三维空间里面的旋转，可以分解成绕着坐标轴的旋转。旋转的方向使用右手法则定义：
|  |
绕坐标轴旋转定义 用右手握住坐标轴，大拇指 的方向朝着坐标轴朝向的正方向，四指环绕的方向定义沿着这个坐标轴旋转的正方向
一般来说
- 绕 Z轴 旋转，称之为 航向角，使用yaw表示;
- 绕 X轴 旋转，称之为 横滚角，使用roll表示;
- 绕 Y轴 旋转，称之为 俯仰角，使用pitch表示;
举例:
我们通常用来表述小车运动的二维平面指的是 X-Y平面，也就是X轴和Y轴张成的平面，在这个平面中，用来描述 小车转弯的角就是绕Z轴的旋转，也就是经常说的
航向角。Z轴朝上，所以按照右手法则可以知道小车向 左转为正，右转为负。
坐标系定义
空间中的同一位置，在不同坐标系下，其坐标值也不相同。
|  |
在上图中，有两个坐标系，分别为坐标系A和坐标系B，其中 橘红色坐标轴表示X轴(朝前)，蓝色坐标轴表示Y轴(朝左)。对于坐标系A中，粉色的圆点的坐标为(3, -3)，对于坐标系B中，粉色的圆点的坐标为(5, 1)
ROS中常见的坐标系
在ROS中，定义了许多坐标系，这里介绍几个常见的坐标系，为了比较直观的解释这几个坐标系，我们使用一个小例子来说明。
|  |
在刚开始的时候，按照之前坐标轴的定义，将小车的朝向方向作为X轴，正左方标识为Y轴，并将小车所在的位置定义为原点，如图(1-1)。
|  |
如图(1-2)，在原点插一面小旗子，并控制小车行驶一段距离，这个时候我们能得到三个位置信息
- 使用测量工具测量小车相对旗子的位置，在X轴正方向距原点3个单位，在Y轴正方向距原点2个单位
- 小车安装里程计，记录自己前进3个单位，并向左平移了2个单位
- 小车使用激光雷达数据与已有地图进行匹配，并结合里程计数据，将数据融合得到小车的位置在X轴正方向3个单位，在Y轴正方向2个单位
在上面的例子中，三个坐标值都相同。但真实情况下，三个坐标值由于测量误差或者其他原因导致坐标值并不相同，然而这三个坐标都用来表示小车中心在空间中的位置，这就引出了不同坐标系
- 对于第一种测量，称之为真实坐标系，这是一个理想的坐标系，即我们拥有一种绝对准确的测量方式获得小车相对于地图原点的坐标，但这种坐标系在真实情况下是不存在的。
-
对于第二种测量，称之为里程计坐标系，在这个坐标系中得到的测量值通常是基于轮速里程计，视觉里程计或者惯性单元得到的。在这个坐标系中，新坐标值通常是根据前一个时刻坐标得到的，一般使用odom来表示。
- 优点: 坐标值是连续的并且以平稳的方式演变，没有离散的跳跃。
- 缺点: 测量产生的误差会累计。
-
适合: 短时程相对定位
-
对于第三种测量，称之为地图坐标系，在这个坐标系中得到坐标值通常是通过传感器的数据重新计算或测量得到的，一般使用map来表示。
-
优点:由于每次得到的坐标点都是重新测量计算得到的，累计误差影响较小
-
缺点: 坐标数据会有跳变。
-
适合: 长时程绝对定位
-
假设对于上面描述的三种测量是以下结果
- 真实坐标系：坐标值为(3, 2)
- 里程计坐标系：坐标值(3, 1)
- 地图坐标系：坐标值(3.5, 2.5)
|  |
那么坐标系表示如图(1-3)
模拟器使用的坐标系
模拟器除了沿袭上面讲述的两个坐标系(odom, map)之外，同时还引入了真实坐标系(real_map)。
由于是模拟器，所以可以精确的知道小车的真实位置，在autolabor_simulation_base模块中，会发布两种坐标系的坐标
- 小车在odom坐标系下的坐标：发布的坐标是模拟轮速里程计产生的，我们可以在autolabor_simulation_base模块中设置轮速里程计噪音，用于模拟真实情况。但由于存在噪音，所以在odom坐标系下的小车坐标并不等于其真实位置。
- 小车在real_map坐标系下的坐标：发布的坐标是根据上层控制速度结合小车底层限制(小车的最大速度，加速度等信息)产生的，在这个坐标系下会排除轮速里程计产生的噪声，输出小车的真实坐标。
对于map坐标系下的坐标，并不由模拟器产生，而是通过上层的定位算法得到的，这部分在后面的讲解中可以看到。
为什么引入真实坐标系
我们往往希望找到一种量化的方法去评价算法的优劣，这样对于调整算法或者某些参数有很大的帮助。在使用真实机器人去测试，需要借助外界测量工具去测量，并且需要想一些巧妙的办法让这些测量合理并且准确。但使用模拟器就能够比较方便的解决这个问题。
模拟器能对传感器数据添加噪声，从而来模拟真实情况。将有噪音的数据带入算法中，如果得到的数据很接近“真实数据”，那么我们就能认为这个算法比较好。
比如我们使用AMCL算法对小车进行定位，其中里程计和激光雷达数据都带有噪音，这个时候我们可以得到在map坐标系下的坐标，同时我们能得到小车在real_map坐标系下的坐标，我们就可以使用这两个坐标的距离来判断定位的准确性。
在学习slam的开始，遇到这几个map，odom，base_link，base_laser坐标系。
map:地图坐标系，一般设该坐标系为固定坐标系（fixed frame），一般与机器人所在的世界坐标系一致。
世界坐标系是系统的绝对坐标系，在没有建立用户坐标系之前画面上所有点的坐标都是以该坐标系的原点来确定各自的位置的。
base_link坐标系原点一般为机器人的旋转中心，对于每个一个硬件平台，在基座上都会有一个不同的位置，提供一个明显的参照点，base_footprint坐标系原点为base_link原点在地面的投影，有些许区别（z值不同）
odom：里程计坐标系，这里要区分开odom topic，这是两个概念，一个是坐标系，一个是根据编码器（或者视觉等）计算的里程计。但是两者也有关系，odom topic 转化得位姿矩阵是odom–>base_link的tf关系。这时可有会有疑问，odom和map坐标系是不是重合的？（这也是我写这个博客解决的主要问题）可以很肯定的告诉你，机器人运动开始是重合的。但是，随着时间的推移是不重合的，而出现的偏差就是里程计的累积误差。那map–>odom的tf怎么得到?就是在一些校正传感器合作校正的package比如amcl会给出一个位置估计（localization），这可以得到map–>base_link的tf，所以估计位置和里程计位置的偏差也就是odom与map的坐标系偏差。所以，如果你的odom计算没有错误，那么map–>odom的tf就是0.
base_laser:激光雷达的坐标系，与激光雷达的安装点有关，其与base_link的tf为固定的。
举例分析
本文以 gmapping 为例，其中 map、odom、base_link、laser 均来自 gmapping 中的坐标系。这里的 gmapping 是指使用laser_scan_matcher包，在仅使用激
光雷达、无需里程计的情况下跑 gmapping。
下图是执行rosrun rqt_tf_tree rqt_tf_tree的结果：
- map：可以理解为世界坐标系
- odom：机器人以为的世界坐标系
- base_link：机器人本体坐标系
- laser：激光雷达坐标系
对于 map –> odom 举个例子
- 首先，我们制定机器人路径时，使用的必然是绝对坐标系。要完成这件事，机器人需要先知道自己在哪。它没有GPS，所以只能倒推——通过里程计。这也是为什么“没有偏移的话 odam 应该与 map 重合”，因为 odam 本来就是用来倒推 map 的。
- 假设机器人终点定在了（8，5），初始时刻，map 的（8，5）和 odom 的（8，5）是重合的。
- odom 坐标系视角看运动过程：假设里程计告诉它，自原点启动起，它在 X 方向移动 2 ，Y 方向移动 5 ，运动描述为（+2，+5）。于是它就认为自己在 odom 坐标系下的（2，5）。反馈给 base_link，则 base_link 坐标系下，机器人需要运动（8，5） - （2，5） = （+6，+0）才能到达目标点（8，5）。而此时如果机器人按（+6，+0）运动完后，确实能到达odom下的终点（8，5），并开始以完成任务为由而沾沾自喜，殊不知，它并没有运动到真正的终点终点：map 坐标系下的（8，5）。
- map 坐标系看运动过程：然而实际上，因为里程计累计误差，它其实第一次运动到了（3，4）。但在 odom 中它在（2，5）。它理应运动（8，5）-（3，4） = （+5，+1）到达目标点，但它会运动（+6，+0），因为 odam 反馈给 base_link 后，目标在 base_link 坐标系下的（6，0）。其实到达的是 map 坐标系下的（9，4），并不能达到运动到 map 坐标系下目标点（8，5）的目的。机器人只能自以为是地认为自己运动到了odom 坐标系下的（8，5）就算万事大吉了。
- 而这时，校正传感器又告诉它了，“我觉得你的里程计刚才 X 漏算了 0.999，Y 多算了 1.001”。 于是它把自己所在位置修正为了（2 + 0.999，5 - 1.001）=（2.999,3.999），接下来该移动（8，5）-（2.999,3.999）=（+5.001，+1.001）。就能将偏差校正回来，到达 map 坐标系下的终点（8，5）了。
