Cartographer——Lidar数据初始化

1.Tim561激光雷达

  雷达都是扫描一圈360°，Tim561固定是270°范围的点， 扫描频率15Hz，则相邻点云的时间步长为time_step=1/（15*360*3）。

如上图左，箭头对应右图的90°方向，激光有效扫描范围（-45°，225°），角度步进0.3345719°（参考西克的官方手册），扫描一帧得到连续的808个点的距离值，即雷达传过来的数据。

 

2.NAV310雷达

  雷达有效扫描范围可以自行根据频率设置，与Tim561的区别在于：

1.NAV310可以设置扫描范围为360°，和更快的频率

2.驱动：Tim561返回的distance信息，16位字符串转无符号int类型后。是以毫米为单位，除以1000即为以米为单位的distance，而NAV310，高八位以米为单位，低八位需除以256再和高八位的整数相加，最后得到以米为单位的distance。

注NAV310的最终distance计算方式：D=distance/256+(distance%256)/256 。（参考西克的官方手册）

 

3.数据初始化

  下面以Tim561雷达为例，介绍一帧雷达数据转成点云的过程。

将一帧激光雷达数据存在数组里，如Range[808]，实际使用时，机器人坐标系的x轴在雷达的90°方向，我们以x轴为正方向，首先要得到的是每个点云的distance及它相对机器人坐标系x轴的角度。由于雷达内的转轴是正时针旋转，那么第一个点云的角度为-135°，后面每个点云的角度=前一个点云的角度+角度步进值。这步操作可以和第1点介绍的distance解码同时进行，那么在雷达驱动部分，就可以得到每个点云的distance及相对x轴角度的二维数组。

  接下来将二维数组转成点云，每个点云包含在世界坐标系下的x,y,z坐标及点的时间信息。关键代码如下：

  方法就是：1.当前点的distance*(1,0,0)，即距离乘以x轴的单位向量，设得到坐标G(distance,0,0)

                    2.将当前点的角度换算成旋转矩阵R，坐标G左乘旋转矩阵R,得到的就是当前点云相对雷达的坐标

                    3.计算当前点的时间：一帧点云生成，这帧点云的时间为当前的系统时间（orUTC时间）设为T，那么每个点的时间就是往后递推。如，设当前为第i个点，那么它的时间为t=T-i*time_step。为什么要有每个点的点云时间？这个会在后续点云畸变滤除中讲到，因为雷达帧率太低，在这个time_step下会滤掉很多正常的点（可以先思考下，点云同步会讲（会涉及多雷达融合）），我们设置time_step的值非常小，以至于可以认为一帧雷达的所有点都是同时生成的，那么这就直接干掉了点云的运动畸变去除的功能（多传感器融合会讲），但是在AGV小车上运行正常，应该还是车速不快的缘故（才3m/s左右）。

 

  通过以上3步，得到了一组点云，但这些点云是相对于雷达坐标系的点云，最后一步：设机器人坐标系到雷达坐标系的旋转矩阵pose(含平移t和旋转R)，遍历每个点云，用点云坐标左乘pose。

  这个时候的点云数据ranges（x,y,z,time）,就是最终喂给算法的数据结构，但是它是和当前帧点云时间，初始位姿合成一个数据结构TimedPointCloudData喂给了算法，即：

，其中 。

 

4.数据流