3D激光雷达的空间建模

赵贵平

（中煤晋中能源化工有限责任公司 山西省太原市 030006）

1 多线激光雷达工作原理

随着智能化的时代到来，机器人等一些智能仪器与设备已经融入了我们的生活与工作当中，在一些复杂或危险的场所，我们开始寻求一种无人化的工作模式；而扫地机器人、服务机器人等在生活中的应用，也给人们带来了便利。与此同时，怎样做到精确的定位仪器设备是我们现在需要解决的难题。在已有的可实行定位方案中，通过激光雷达来构建地图同时基于构建的地图进行定位的方法逐渐成为了主流。激光雷达Lidar（Light Detection And Ranging）是将现代光学技术应用到遥感技术当中的产物，由于激光的本质是电磁波，具备高亮度性、高方向性以及高相干性等特点，所以激光雷达相较于传统的雷达技术，在角分辨率、距离分辨率、速度分辨率与测速范围等方面都具有一定的优势，并且还能抓取多种目标的图像数据以及不易受外界的干扰。激光雷达的信息可以通过观测的振幅、相位与偏振来搭建获取，因此激光雷达不仅能在测量距离的时候做到精确，还能在测量速度与跟踪目标时减少误差[1]。

通过激光雷达测量距离的方法可以大致的分为TOF测距法和三角测距法。

TOF测距方法简单的说就是通过速度和时间的乘积来求得距离，激光雷达通过激光发射器发射一个激光脉冲，与此同时，计时器记录下此时的时间为初始时间，当激光脉冲照射到目标物后进行反射由激光雷达的接收器接收，计时器记录当前的时间为接收时间，接收时间与初始时间的差值则是激光脉冲的“飞行时间”，由于光的速度是已知的，所以可以求出雷达与目标之间的距离，表达式如下：

其中D为测站点两点间距离，c为光在大气中传播的速度，t为光往返一次所需的时间。

三角法测量的原理是通过激光雷达与线性CCD元件来完成的，当激光雷达的激光发射器发出一个激光脉冲后，碰到目标物体通过反射后被线性CCD所接收，而激光发射器和激光探测器之间存在间隔，通过光学原理，可以在CCD上获取不同距离的物体信息，最后通过三角公式数学方法来计算被测目标的距离。

从测量距离、采样率、精度、转速等方面对两种方法进行了比较，如表1所示，发现TOF雷达从性能上来说总体优于三角雷达。

2 基于LOAM算法的空间建模方法

SLAM（Simultaneous Iocalization and Mapping）主要功能是在构建环境地图的同时，还能通过不间断的估计自身的位姿信息来完成定位。当前激光SLAM框架主要内容为前端扫描匹配和后端优化。前端扫描匹配是激光SLAM整个流程当中的重点，其主要作用是在已知前一帧位姿时，利用相邻帧之间的关系来估计当前帧的位姿；后端优化是当长时间增量式扫描匹配后优化里程计及地图信息，其中前端扫描使用最多的算法为ICP算法与NDT算法，后端优化主要基于粒子滤波与图优化两种方法。其中激光雷达二维建模常用的算法有Hector算法、Gmapping算法以及Cartographer算法，三维建模常用LOAM算法[2]。

本文选择LOAM算法进行三维空间建模，目标是使用一个三维空间中运动的两轴单线激光雷达来构建实时激光里程计并建图，LOAM算法的创新点在于可以不通过高精度的测距和惯性测量方法就可以得到一个低漂移和低复杂度的数据信息[3]。该算法的核心是将定位和建图两个版块分割开来解决，通过执行高频率的里程计信息和低精度的运动估计来进行空间定位，与此同时，通过比定位低一个数量级的频率执行匹配和注册点云信息来实现环境地图的构建。将这两个算法结合就获得了高精度、实时性的激光里程计。

2.1 算法流程框图

由流程图1可知，整个工作主要分为四部分来完成。第一步是要获得当前激光雷达坐标下的点云数据，接着把k次扫描所获得的全部点云组成一帧数据pk。然后将得到的数据pk在建图和定位两个算法中进行处理。激光里程计的作用是获取连续两帧点云的运动信息，估计自身的位姿信息用于去除pk中的运动畸变。这个节点进行的频率为10Hz，该节点使用地图去匹配和注册没有畸变的点云数据并以1Hz的频率输出。最后由坐标转换节点接收前面两个节点输出的信息并将其融合处理以10Hz的活动频率输出。

图1：LOAM算法流程

表1：TOF测距法与三角测距法的比较

图2：三视角下的室内实时点云图

2.2 环境特征点提取

使用图1中的两轴激光雷达获得的点云数据提取特征点信息，由于两个雷达的构造会产生非均匀分布的点云数据。在一次扫描数据中点的分辨率为0.25度，并且这些点分布在一个扫描平面上。然而固定激光雷达的轴的转速为180度每秒。而激光的频率为40Hz，因此这个轴上点的分辨率为4.5度。考虑到点云数据的非均匀性，采取从一次扫描中获取特征点。

特征点选取边缘点和平面点，方法是得到我们想要求曲率点i周围连续几个点集合S用于求曲率。S中的点一半在i的一侧从而避免雷达顺时针和逆时针的影响。求取点的曲率的公式如下：

通过上式求出每个点的曲率c。当求得的曲率大于设定阈值时则归类为边缘点，当求得的曲率小于设定曲率阈值时则归类为平面点。在选择点时应避免选择离已选点距离过近的点和与激光光束相平行的平面上的点，同时我们也希望避免可能遮挡点。

2.3 寻找特征点的对应关系

里程计算法估计获取一帧点云时间内的运动，使用tK表示第K次扫描开始的时间。在一次扫描结束时，这一次扫描获得点云pK会有一个获取时间tK+1。将投影后的点云记为，在下一次扫描时会使用做匹配求激光雷达的运动。将两帧点云的特征点相匹配，匹配的边角线和pK+1的边角点，匹配的平面和pK+1的平面点，从而寻找到对应关系。

在第K+1扫描开始计算，pK+1中起初没有点云数据，当开始不断扫描后，pK+1中的点云不断累计。每次迭代的时候，都会将pK+1中的边缘点εK+1和平面点HK+1投影到扫描起始点的坐标系下，用重新表示。将P¯k中曲率大的点和曲率小的点保存在KD树中，用于寻找对应点。

点到线段的距离公式如下：

图3：点云地图

接下来求平面点到对应平面的距离，距离公式如下：

2.4 运动估计

LOAM算法是在默认激光雷达是匀速运动的前提下工作的。因此当获取了一帧数据的终止点相对于起始点的转换矩阵的时候，就可以按照获得点数据时相对于起始点的时间对这一帧数据中的任意点进行插值。获得这一帧中每一个点的位姿。公式如下：

接着使用一个旋转矩阵R和一个平移量T表示当前帧数据中的点和前一时刻帧数据中点的对应关系。

用罗德里杰斯公式将旋转矩阵R展开为：

对旋转矩阵进行求导，有了点到线和面的距离，并获得用于优化的误差函数：

f中的每一行代表一个特征点，接下就要求解雅克比矩阵，最后使用LM的方法进行优化：

3 实验验证

3.1 硬件介绍

本实验采用的仪器是VLP-16激光雷达，该激光雷达是Velodyne公司研发的体量最小的三维激光雷达，保留了电机转速可调节的功能，通过5Hz-20Hz的频率实时的传输环境测量值数据。VLP-16的测量距离最远可达100多米，830克的轻重量使得使用和安装时都非常的方便，适合安装在小型无人机和小型移动机器人上进行实验和数据测量。测量数据信息以点云的形式存储，并且提供横向360°，纵向±15°，每个点精度在2mm以内的信息，每秒可以输出30多万个点云数据。

VLP-16型号一排16个IR激光器与IR检测器配对，具有在垂直方向上16线的激光束，在采集三维数据时，每一步的旋转（旋转频率可设定，频率不同旋转的步进角度不同）可在空间上采集16个点的三维数据，以测量到物体的距离。在垂直方向上的视角范围-15°到+15°。该设备牢固的安装在紧凑的耐候外壳内。激光/检测器对阵列在其固定外壳内快速旋转以扫描周围的环境，每秒发射大约18000次激光，实时提供丰富的3D点数据集。先进的数字信号处理和波形分析可提供高精度的远程感测以及校准后的反射率数据，可轻松检测出路标，车牌和车道标志等后向反射器。将16对激光/检测器组合成一个VLP-16传感器，并以18.08kHz的频率进行脉冲处理，每秒可进行多达30w个数据点的测量。

3.2 实验结果

本文选用中国矿业大学（北京）沙河校区学8-413实验室点云（由VLP-16激光雷达采集的模拟实验室点云）进行算法验证。实验中，上位机选择工控机，选用Ubuntu16.04系统和PCL点云库来进行点云的采集与环境的构建。激光雷达采用UDP协议传输数据（UDP称为用户数据报协议，提供了一种无需建立连接的封装的IP数据报发送给应用程序），由于网络传输的UDP协议通过解析获得的数据包是16进制数据，不能直观的得到极坐标形式或者xyz空间坐标系，所以需要在ROS系统中利用PCL转换成PCD文件来进行下一步的算法处理。实验得到的实时点云和最后的点云地图如图2，图3所示。