在一台机器人中不可避免地会具有很多种传感器（雷达，视觉，深度...）；在机器人的导航中对这些传感器的融合，并形成完整的导航策略是很关键的。对于一台室内/室外机器人来讲，传感器的融合主要分为两个方面：

融合定位，使用GPS/IMU/里程计/视觉/雷达等传感器的数据确定机器人的位置和姿态。

融合导航，在机器人的定位确定的前提下，使用雷达/声呐/图像/深度等数据对机器人进行融合避障。

今天我们就从不同传感器的融合方面来讲，使用ROS中的TF以及消息体系，实现传感器数据的初步融合，并生成局部代价地图（costmap）。

1. 传感器的数据类型以及依赖坐标

 机器人上常见的导航类传感器分为激光雷达，视觉传感器，深度传感器，超声传感器等。

1.1 激光雷达

 激光雷达是机器人中最为常用的传感器之一，在ROS系统中，激光雷达的数据一般以sensor_msgs/LaserScan格式的消息进行传递，LaserScan消息的内容如下：

其中Header代表了一帧雷达数据的基本的信息，包括它是第几帧（seq），它的发布时间（stamp），它是基于哪个坐标系发布的（frame_id）。而下面的参数就代表了雷达的数据与配置。我们可以用下面的图来阐述：

  雷达的信号输出为一串数据这串数据包含了ranges(扫描距离数据集)以及intensities（扫描强度数据集）。他们形成了一个以雷达为坐标系的极坐标数组。

1.2 深度传感器数据

深度传感器数据一般的输出形式一般比较多样化，不过比较常用的方式有两种：1）点云数据PointCloud/PointCloud2  2）自定义数据格式。

1.2.1 点云数据（PointCloud/PointCloud2）

点云数据一共有两类，其中PointCloud（简称pcl）是非压缩点云，可读性较好但是运行性能较差；PointCloud2是压缩点云，可读性一般但是运行效率高。

关于pcl可以参考下面的网站：https://pointclouds.org/

pcl的一帧数据结构如下图：

 图片里的Header跟雷达的一样，在此不再赘述，而points和channels则代表着点云中每一个点的数据。

  Points是一个数组，其中包含了成百上千个点，这些点按照其自身坐标值分布在整个空间，形成了紧贴障碍物的“云”，这也是点云叫法的由来。而点云的channel信息包含了云中每一个“点”的属性。下面的图就是一幅标准点云图像，点的位置就与Point对应，点的channel就对应了其颜色。

关于pcl2的转化及读取参考：https://blog.csdn.net/z824074989y/article/details/95639499

1.2.2 自定义数据格式

 其实深度数据也可以等效为立体的雷达扫描数据，传统的雷达执行的是二维扫描，而深度传感器扫描的是三维场景。例如一个320x240的深度传感器，它获取深度信息的方式如下：

    所以我们可以用多种形式对传感器的数据进行表征，例如最简单的sensor_msgs/LaserEcho

 我们就可以构建长度为320*240的Echo序列，通过对Echo进行解析，就可以获取到深度数据了。

1.3 反射类传感器（如超声波，红外等）

反射类传感器的类型有很多，但是基本原理很简答。反射类传感器输出的数据仅有一个数值。

 想要分析反射传感器，首先需要对传感器的位姿进行获取。获取位姿结合传感器的反射距离，就能够对障碍物进行单点分析，实际上在一个传感器中，我们获取到的是四个参数：

2. 传感器的数据融合及代价地图（Costmap）

Costmap是一类“附着于”机器人的地图，它时刻以机器人为坐标原点，以黑白栅格地图代表机器人周边的导航环境。机器人可以通过costmap获取对周边的地形状态的全方位感知。

costmap有两种，一种是二维costmap，其类型为：sensor_msgs/OccupancyGrid 另一种是三维costmap(pcl2类型)；一般在平面导航上较常用的是二维costmap，三维地图一般用于无人机。

2.1 传感器之间的TF坐标系变换

2.1.1基，坐标变换与极坐标变换

我们之前说到了激光雷达的数据变换，在雷达层面我们获取到了以雷达为原点的极坐标系。但是在机器人坐标系中，所有的坐标都依赖机器人的中心为基。就类似于下图所述的问题，在我们知道x,y,θ以及l的前提下，如果计算障碍相对机器人中心的距离就很重要。

    好在ROS中有内置的坐标系转换函数：tf.lookuptransfrom;它的使用方法如下：

 构建一个transformListener对象：listener=tf.TransformListener()

(trans,rot)=listener.lookupTransform(parent,child, rospy.Time(0)) 其中，parent，child分别为父坐标系以及子坐标系。第三个参数代表时间前推阈值，设置为0即可。

以上获得的trans是一个三变量矩阵，分别代表x，y，z坐标，而rot代表四元数的旋转角度。如果我们想将这个值转化为二维平面角度，则需要使用四元数向欧拉角的变换：

roll,pitch,yaw=tf.transformations.euler_from_quaternion(rot)其中的yaw即为z轴转角。

2.1.2 雷达回波点处理

    在1.1.2小节我们介绍了激光雷达回波点的描述，雷达的回波一般以距离数组（ranges）的形式回传，但是在实际使用中，我们期待的是每一个障碍点相对于机器人中心的x，y数值（平面直角坐标系）。那么此时我们就需要极坐标向直角坐标的变换。

  所以相对雷达坐标系，有：

  接下来，使用机器人中心坐标系与雷达坐标系之间的坐标变换，就可以将雷达回波点转化为相对于机器人中心坐标系的平面直角坐标。

2.2 构建空白costmap

平面costmap的类型一般为nav_msgs/OccupancyGrid,定义如下：

    这个消息的Header同样是标准的消息头，但是其中的frame_id是比较重要的，因为我们构建的costmap基于机器人传感器，所以它的坐标系也要以机器人为中心，即frame_id要配置为机器人的中心坐标系（一般为base_link）机器人地图的origin代表了地图左下角，也就是第0个像素的实际坐标。值得注意的是，在ROS中的地图像素扫描方向是从左下角向右上角扫描的。resolution表示地图上一个像素宽度代表实际的距离（通常为0.05，代表一个像素代表5cm*5cm），costmap的本质是栅格地图，其示意图如下：

    在机器人costmap的构建中，costmap需要按照一定频率（通常对应最低频传感器的发布频率）定频发布，保证其frame_id始终对应机器人，合理配置origin，保证机器人本体在地图中心即可。

2.3 生成栅格地图以及发布

    光构建一个空白地图是没有用的，我们需要基于各个传感器的实际数据在costmap上进行障碍栅格生成。这其中大致包含以下几个步骤：

扫描激光雷达的传感器数据，在激光雷达的回波点处生成障碍栅格（注意坐标和比例变换）获取深度传感器数据，映射为平面数据集，基于平面数据集生成障碍栅格获取各种单点传感器数据，基于坐标变换生成障碍栅格依照机器人半径以及提前配置好的相关文件，对栅格地图进行膨胀处理，膨胀处理依赖OpenCV，详见：https://docs.opencv.org/3.3.0/index.html

构建类型为OccupancyGrid型消息，并循环发布。