写在前面
上一节我们实现了基本的2D-SLAM导航包的使用。 本节我们将在2D-SLAM的基础上，通过手动遥控实现控制云台俯仰角来生成三维点云（如下图，视频还没上传可以在我朋友圈看）。 
看完本节，你将掌握：
- 动态发布三维空间下的TF坐标信息
- 激光雷达扫描点的三维空间变换
- 空间点云构成以及点云发布
- ROS消息控制二维云台的运动
注意：本节涉及的矩阵和空间变换相关的内容较多，不了解代数和矩阵的老铁们可以先简单学习一下那一部分。
单线雷达如何生成三维点云
从2D到3D的变换原理
我们使用一个2D单线雷达能够获取到一系列的平面反射点，通过极坐标向平面直角坐标的转换，能够绘制出一系列的“平面点集”（如下图） 
但是很明显，这个二维数据是没法投射到三维的。 所以我们使用了一个云台，现在假设我们将云台进行一定角度的倾斜，那么以雷达为平面，数据依然是二维的，但是在空间层面，雷达的回波点数据便有了空间特征。可以看下面的图，在舵机控制雷达角度上仰时，雷达的回波点便具有了z轴信息。通过精确控制舵机角度，就可以实现对空间纵向（z轴）立体信息的生成。 
在空间变换中，我们通常使用三个参量来描述一次三轴空间变换，它们分别为：
- 绕X轴旋转的变量：标记为roll
- 绕Y轴旋转的变量：标记为pitch
- 绕Z轴旋转的变量：标记为yaw
机器人头部在空间旋转的示意图如下（图片来自网络）。值得注意的是，在我们的机器人中，机器人的前进方向是X轴，机器人从由向左的坐标轴为y轴，而竖直方向为z轴。 
从激光平面2D到云台中心的矩阵变换
我们来看一下机器人的头部舵机： 
参考下图，我们可以知道，以机器人激光雷达平面，我们可以获取到基于激光雷达平面的第n个激光回波点的二维坐标： 当激光雷达进行仰视或者俯视（为了简化公式我们暂时只考虑云台的俯仰）的时候，就相当于对二维平面进行了三维的空间变换。我们假设由旋转点到激光雷达中心点的侧面投影距离为l（如下图），旋转角度为δ，那么原有平面上的点在空间上的三维坐标变为： 
既然如此，我们就可以构造出以激光雷达为原点，到以机器人旋转点为原点的空间变换矩阵： 
而下面这个矩阵，就是基于雷达平面2D直角坐标系向三位空间坐标系变换的线性变换矩阵 
从云台中心到/map坐标系中心的变换
我们现在构造了以云台为中心的坐标系变换体系，也就是说，经过了此次变换，激光雷达的回波点已经全部变换为以机器人云台中心（base_link）为原点的三维空间坐标。 然而，要想完整的测绘出三位空间，机器人必须要在空间中进行移动。而生成的三维激光反射点，最后也都要映射到空间原点上。 
与之前的变换类似，我们现在假设机器人的基座相对于空间原点的坐标为（x,y,w），其中x,y分别为机器人的横纵坐标，w为机器人的旋转角度；那么从以机器人为原点的坐标系变换到空间原点坐标系的变换矩阵为： 
我们可以发现，我们在矩阵中增加了一个维度，这是为了将多次变换矩阵进行合并，以在后续减少计算量。 可以看出，在我们把雷达的回波数据映射到三维地图空间的流程中，一共进行了两次空间变换： 第一次：雷达平面到机器人云台三维坐标系（laser—>base_link） 第二次：云台三维坐标系到地图三维坐标系（base_link—>map） 最后的结果就是，我们将雷达的三维数据，映射到了以/map为原点的三维坐标系上。
雷达回波点云的映射的Python程序实现
获取激光雷达扫描信息，并做基础变换
雷达信息的输出是一个以/laser坐标系为原点的LaserScan类型数据。 
我们在上一章节中已经介绍了机器人位置的获取。当机器人雷达平视的时候，我们可以利用gmapping生成地图，并对机器人在空间的坐标进行定位。 当我们要开始绘制3D地图时，我们首先将机器人停在某个位置，然后开始控制云台扫描z轴。
直接获取激光雷达到/map的坐标系变换
我们在前面介绍到，使用下面两个方式能够将laser坐标系映射为/map坐标系： 第一次：雷达平面到机器人云台三维坐标系（laser—>base_link） 第二次：云台三维坐标系到地图三维坐标系（base_link—>map） 我们也可以用第二种方法，调用TF Listener 直接获取/laser到/map的坐标系变换： 
本节总结
因为内容有点多，所以我们本小节拆分成两个部分；下一小节我们继续~
联系作者
