SLAM传感器篇:Velodyne激光雷达

VLP-16 & HDL-64E激光雷达

主要参数：

VLP-16基本特性

稍微介绍一下，扫描频率越高，短时间可以获取更多数据，在机器人运动速度快的时候可以获取连续性较好的数据，有利于SLAM的实时性;测距范围越广，意味着盲区越少;角度分辨率与两各相邻点云夹角有关，物体距离越远，角度分辨率越低，得到的相邻点云距离就越远，点云就会越稀疏。这里需要注意的一点，对于不同雷达，垂直角分辨率不一定是均匀分布，例如HDL-64E。

测试环境：
Ubuntu16.04+Kinetic

原理简介

velodyne VLP-16 coordinate system

激光雷达，是以发射激光束探测目标的位置等特征量的雷达系统。如图所示，描述了激光雷达解算三维坐标的原理。下面我们将从硬件开始，介绍一下主要的原理。

Firing Laser Sequence Timing

激光雷达首先会按照一定顺序发射激光线束，具体顺序参见表1中的laser_id，垂直方向上是跳跃发射，这样避免邻近的干扰，发射时序参见上图，发射与充电16束激光的周期为55.296us;然后，激光雷达内部马达会按照一定的转速（300-1200 r/min，以60为间隔，通常使用600r/min，对应频率10Hz，对应水平角分辨率0.2度）进行旋转，0.1s旋转1圈获取一帧数据。

表1：vertical angle，laser id，ring id of VLP-16

ring id	0	1	2	3	4	5	6	7	8	9	10	11	12	13	14	15
vertiacl angle	$-15^\circ$	$-13^\circ$	$-11^\circ$	$-9^\circ$	$-7^\circ$	$-5^\circ$	$-3^\circ$	$-1^\circ$	$1^\circ$	$3^\circ$	$5^\circ$	$7^\circ$	$9^\circ$	$11^\circ$	$13^\circ$	$15^\circ$
laser id	0	2	4	6	8	10	12	14	1	3	5	7	9	11	13	15

然后切换到软件层面，我们比较关心的一个问题是数据。那么，旋转1圈得到的数据是怎么定义的呢？激光雷达每次得到"一包数据（data packet，1284bytes）"，包含24个发射周期（384个点），具体的数据结构见下文.pcap描述。那么1圈数据是多少数据包呢？用时间计算一下，一圈大概是75.3包。ROS订阅的激光数据是通过velodyne_drive处理之后，收集组包得到的，感兴趣的同学可以看看源程序，里面还有一些数据结构以及处理方法。

参数配置

配置主机IP
点击主机网络设置，编辑以太网有线连接；
主机IP设置
将IP地址改为与激光雷达同一网段(192.168.1.x)，子网掩码为255.255.255.0，注意不要与激光雷达IP冲突，激光雷达的IP为192.168.1.201；
配置VLP-16
VLP-16参数配置

使用方法

参考Ros Wiki：http://wiki.ros.org/velodyne

安装驱动

sudo apt-get install ros-kinetic-velodyne

新建ROS工程

mkdir -p data_velodyne/src
cd data_velodyne/src
git clone https://github.com/ros-drivers/velodyne.git
rosdep install --from-paths src --ignore-src --rosdistro kinetic -y #安装工作空间src路径下所有功能包的依赖项
cd ..
catkin_make_isolated --install
source ~/data_velodyne/devel_isolated/setup.bash

启动采集程序

roscore
roslaunch velodyne_pointcloud VLP16_points.launch

记录数据包

rosbag record -o /data_velodyne/bags/data_velodyne /velodyne_points

可视化(rviz)
```
rosrun rviz rviz -f velodyne
```
在rviz中Add PointCloud2，输入topic /velodyne_points订阅话题信息；

3D点云可视化

最后，自己借助shell脚本封装程序，实现一键采集并录制数据，源码如下：

data_velodyne

数据格式

.pcap(原始数据格式)
激光雷达数据帧
pcap文件是常用的数据包存储格式，总体结构是文件头-数据包头1-数据包1-数据包头2-数据包2…，作为velodyne专用的数据格式，解析较为麻烦，一般在程序设计中都会采用其他格式。如上图所示，每一帧的数据长度固定为1248字节，包括前42字节的数据包标识、12组数据包、4字节时间戳和最后两字节雷达型号参数;12组数据包中前两字节为数据包的开始标识(0xFFEE)、接下去两字节为当前旋转角度值和连续32x(2字节的距离值+1字节的激光反射强度值)字节的距离信息，其中32x3 字节分别为雷达两次获取探测信息，每个数据包开头所携带的旋转角度是指当前数据包前16x3字节对应的角度，而后16x3字节对应的旋转角度激光雷达没有直接给出，需要通过计算前后两次旋转角度然后求取平均值获得。
.txt/.xyz/.pts/.csv(ASCII)
读写方便，但是数据读取较慢且占用大量空间，对海量点云数据的存储和处理较困难。
```
X Y Z R G B
...
```
.las
由美国摄影测量与遥感协会提出，目前通用的点云数据格式，开放的二进制数据格式，只能用存储空间相对较小，但读写需要专业软件。

.pcd
Point Cloud Library定义格式，包含文件头及点云数据。

# .PCD v.7 - Point Cloud Data file format
VERSION .7 #指定pcd文件版本
FIELDS x y z rgb #维度与字段
SIZE 4 4 4 4 #各字段字节大小
TYPE F F F F #字段类型
COUNT 1 1 1 1 #各维度包含元素数目
WIDTH 213 #点云数据集宽度
HEIGHT 1 #点云数据集高度
VIEWPOINT 0 0 0 1 0 0 0 #点云的获取视点
POINTS 213 #点云总数
DATA ascii　#数据类型
0.93773 0.33763 0 4.2108e+06 #点数据
0.90805 0.35641 0 4.2108e+06

ROS中的消息格式

sensor_msgs/LaserScan

# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]

sensor_msgs/PointCloud

#This message holds a collection of 3d points, plus optional additional information about each point.
#Each Point32 should be interpreted as a 3d point in the frame given in the header
Header header
geometry_msgs/Point32[] points  #Array of 3d points
ChannelFloat32[] channels       #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point

在结合ROS使用的时候，我们常常将数据记录到.bag文件中，若需要其他格式的点云，我们可以进行对应的转换，例如需要.pcd格式的点云文件，则运行以下命令即可：

rosrun pcl_ros bag_to_pcd data_velodyne.bag /velodyne_points pcds

这样，转换出来的pcd文件就会存到指定的目录下。

pcd格式点云

参考资料

[1]. Velodyne VLP-16

[2]. velodyne_driver

[3]. velodyne VLP-16线激光雷达驱动程序、相位锁、时钟同步测试总结

[4]. Velodyne VLP-16激光雷达数据格式解析

[5]. velodyne：激光雷达pcap文件格式及写入、数据解析

[6]. PointXYZIR