基于低成本设备的高精度三维点云地图构建方法

摘要

本发明针对自动驾驶场景，提供基于低成本设备的高精度三维点云地图构建方法，是在低成本三维点云地图构建平台上依次执行点云预处理、帧间配准、筛选关键帧、融合gps与imu数据、关键帧位姿优化构建五个步骤，生成三维点云地图。平台由激光点云采集单元、imu数据采集单元、gps数据采集单元和计算平台组成，其中激光点云采集单元采用16线或32线激光雷达；gps数据采集单元采用误差等级米的低成本gps，为系统提供低频的经纬度信息；imu数据采集单元为系统提供高频的加速度和角速度信息。本发明使用低成本的建图设备配合全新的建图算法，在满足同等精度的前提下，节省了三维点云地图的构建成本。

说明书

技术领域

本发明属于自动驾驶技术领域，涉及用于绘制地图的激光雷达系统及方法，具体是针对自动驾驶场景的基于低成本设备的高精度三维点云地图构建方法。

背景技术

在自动驾驶领域，车辆需要时刻获得自身的位置和姿态，并以此为基础对接下来的行为作出决策（加、减速，转向）。当前车辆主要通过两种方法获取当前位置。

第一种，通过三点定位法利用导航卫星的信号计算出车辆当前的经纬度，然后在包含经纬度信息的二维地图中匹配出当前的位置，同时，根据相邻位置信息计算出车身姿态。这种方法出现的时间早，经过多年的改进，可将定位精度提高到cm级。但是该方法对导航卫星的信号依赖性较强，在信号易被遮挡的高楼区，林荫道，地下车库，立体停车场，定位精度往往会大大下降，造成定位错误。

第二种，通过高频激光雷达扫描出车辆周围的三维点云，然后使用扫描得到的点云在预先制作好的三维点云地图中匹配出车辆的当前位置和朝向。这种方法使用三维空间中数量庞大的点进行匹配，匹配精度可以轻易达到cm级。同时，由于点云的三维特性，车身的姿态信息可以直接由当前扫描得到的点云与地图点云匹配得出，不需要利用相邻扫描进行运算得出，姿态精度高于方法一。

现有的高精度三维点云地图构建方法大多使用专用的车载或以人为载体的传感器平台采集激光点云（同时采集imu和高精度RTK数据），然后使用专业计算平台配合离线建图算法计算出点云地图。这种建图方法，大量使用了专业设备，通常包括带有基站的RTK和高线数激光雷达（64线、128线），专业的彩色图像采集系统和专业级计算平台。成本高昂，一次性投入大，设备维护难度较大是现有自动驾驶领域中构建高精度点云地图时面临的主要问题。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明针对自动驾驶场景，提供基于低成本设备的高精度三维点云地图构建方案。

技术方案：为解决上述技术问题，本发明提供的基于低成本设备的高精度三维点云地图构建方法，是在低成本三维点云地图构建平台上依次执行点云预处理、帧间配准、筛选关键帧、融合gps与imu数据、关键帧位姿优化构建五个步骤，生成三维点云地图；

所述低成本三维点云地图构建平台包括激光点云采集单元、imu数据采集单元、gps数据采集单元和计算平台；所述激光点云采集单元采用16线或32线激光雷达；所述gps数据采集单元采用误差等级米的低成本gps，为系统提供低频的经纬度信息；所述imu数据采集单元为系统提供高频的加速度和角速度信息。

具体地，所述激光点云采集单元使用32线激光雷达，以10Hz的频率采集三维点云数据，为系统提供精确的含有时间戳的点云数据；该单元是整个系统中最为重要的组成部分。

具体地，所述imu数据采集单元由程序完成上电后的零漂校正和温度补偿，该单元为系统提供的加速度和角速度信息的频率是50Hz。

具体地，所述gps数据采集单元为系统提供经纬度信息的频率是4Hz，低频的经纬度信息和高频的imu数据进行ekf融合得到含有时间戳的50Hz的激光雷达轨迹和姿态数据。

具体地，所述计算平台使用 Linux操作系统的PC平台，地图生成软件基于PCL点云处理库，图优化工具库和ROS。

建图算法的具体步骤如下：

1>点云预处理：首先标记出当前点云帧中的地面点和非地面点，然后在非地面点中标记出角点和平面点，剔除点云数据中除了地面点，角点和平面点之外的所有点。

2>帧间配准：使用经过预处理的当前点云帧和经过预处理的上一帧点云进行点云配准，计算出自上一帧点云数据到这一帧点云数据，激光雷达的位移矩阵和姿态旋转矩阵，以系统收到的第一帧点云数据为起点利用相邻帧位移矩阵和旋转矩阵，累加相邻点云帧得到连续的点云地图。

3>筛选关键帧：以系统收到的第一帧点云数据作为第一个关键帧，随着帧间配准的进行，统计当前帧到上一关键帧的欧式距离，当欧式距离大于设定的阈值时，则将当前帧标记为关键帧。

4>融合gps与imu数据：首先利用imu的加速度和角速度对时间积分计算出带有时间戳的路径点，使用ekf方法融合gps路径点和imu积分出的路径点，得到融合后的路径点数据集合。

5>关键帧位姿优化：根据路径点的时间戳在步骤4得到的融合后路径点数据集合中寻找时间与当前点云关键帧最接近的两个路径点，在匀速直线运动的假设下，利用线性插值计算出关键帧基于路径点的假设位姿，以这个假设位姿为收敛的目标，优化上一个关键帧直到当前关键帧之间的每一帧点云的位移矩阵和旋转矩阵。

使用时，本发明使用低成本的卫星导航信号接收设备（误差等级：米），IMU（惯性测量单元）和低成本激光雷达（16线、32线）配合一种新颖的算法构成一套离线建图方案。算法首先对点云进行预处理修正，然后利用预处理后的点云拼接出局部地图，之后利用导航信号和IMU融合出的地理轨迹修正局部地图中的关键帧，最终以关键帧为约束优化得到符合地理轨迹的三维点云地图。

有益效果：现有的高精度激光点云地图构建方法使用高精度的RTK轨迹建立约束，然后利用这个约束来优化建图过程中扫描到的连续点云数据从而拼接出一张精确、完整的激光点云地图。由于激光雷达本身的误差，扫描到的点云数据与真实场景的三维结构不是完全重合（平面有凹凸，直线发生扭曲）。利用RTK轨迹建立约束的点云优化方法对此类误差的修正较差，因此现有的建图方法既依赖于高精度RTK设备提供正确的轨迹约束，也需要高精度的激光雷达提供低误差的环境三维扫描数据，这使得整套系统的成本高昂。

本发明使用低成本的建图设备配合一种新颖的建图算法，可以大大节省三维点云地图建图成本，经实施例验证相同的点云定位算法，静态条件下，使用本专利提出的建图方法生成的点云地图，定位精度与现有高精度点云地图构建方法生成的点云地图定位精度相同。城市动态条件下（小于 50 KM/H），在二者生成的点云地图上进行定位，定位精度相同。

除以上所述的本发明解决的技术问题、构成技术方案的技术特征以及由这些技术方案的技术特征所带来的优点外。为使本发明目的、技术方案和有益效果更加清楚，下面将结合本发明实施例中的附图，对本发明所能解决的其他技术问题、技术方案中包含的其他技术特征以及这些技术特征带来的优点做更为清楚、完整的描述。

附图说明

图1是本发明实施例中的低成本三维点云地图构建平台的硬件架构图；

图2是低成本三维点云地图构建平台所执行的建图算法流程图。

具体实施方式

实施例：

如图1所示，本实施例针对自动驾驶场景地图构建所提供的低成本的激光点云硬件组合方案，使用低成本的卫星导航信号接收设备（误差等级：米），IMU（惯性测量单元）和低成本激光雷达（16线、32线）。该硬件系统由激光点云采集单元，imu数据采集单元，gps数据采集单元，计算平台四部分构成。

1>激光点云采集单元使用32线激光雷达，以10Hz的频率采集三维点云数据。这个单元为系统提供精确的含有时间戳的点云数据，是整个系统中最为重要的组成部分。

2>imu数据采集单元使用千元级低成本imu，由程序完成上电后的零漂校正和温度补偿。这个单元为系统提供高频（50Hz）的加速度和角速度信息，通过ekf融合gps数据得到含有时间戳的激光雷达运动轨迹和姿态。

3>gps数据采集单元使用千元级低成本gps，误差等级米，数据输出频率4Hz。这个单元为系统提供低频的经纬度信息，通过和高频的imu数据进行ekf融合可以得到含有时间戳的高频（50Hz）的激光雷达轨迹和姿态数据。

4>计算平台使用一台dell笔记本（i7-8750h处理器，16G内存），操作系统为ubuntu16.04，地图生成软件基于PCL点云处理库，图优化工具库和ROS。

如图2所示，配合上述低成本的激光点云地图制图硬件方案，与激光点云地图生成算法组成一套离线建图方案。算法首先对点云进行预处理修正，然后利用预处理后的点云拼接出局部地图，之后利用导航信号和IMU融合出的地理轨迹修正局部地图中的关键帧，最终以关键帧为约束优化得到符合地理轨迹的三维点云地图。

构建三维点云的软件流程主要包括五个部分:点云预处理，帧间配准，筛选关键帧，gps和imu数据融合，关键帧位姿优化。其中：

1>点云预处理：首先标记出当前点云帧中的地面点和非地面点，然后在非地面点中标记出角点和平面点，剔除点云数据中除了地面点，角点和平面点之外的所有点。

2>帧间配准：使用经过预处理的当前点云帧和经过预处理的上一帧点云进行点云配准，计算出自上一帧点云数据到这一帧点云数据，激光雷达的位移矩阵和姿态旋转矩阵，以系统收到的第一帧点云数据为起点利用相邻帧位移矩阵和旋转矩阵，累加相邻点云帧得到连续的点云地图。

3>筛选关键帧：以系统收到的第一帧点云数据作为第一个关键帧，随着帧间配准的进行，统计当前帧到上一关键帧的欧式距离，当欧式距离大于设定的阈值时，则将当前帧标记为关键帧。

4>gps和imu数据融合：首先利用imu的加速度和角速度对时间积分计算出带有时间戳的路径点，使用ekf方法融合gps路径点和imu积分出的路径点，得到融合后的路径点数据集合。

5>关键帧位姿优化：根据路径点的时间戳在4>得到的融合后路径点数据集合中寻找时间与当前点云关键帧最接近的两个路径点，在匀速直线运动的假设下，利用线性插值计算出关键帧基于路径点的假设位姿，以这个假设位姿为收敛的目标，优化上一个关键帧直到当前关键帧之间的每一帧的位移矩阵和旋转矩阵。

流程的伪代码如下：

BEGIN

IF 点云数据到达 THEN

a.进行点云预处理

b.帧间配准

c.根据帧间配准的结果，将当前帧拼入点云地图

IF 当前帧与上一个关键帧的欧式距离达到阈值 THEN

将当前点云加入关键帧序列

IF 找到时间戳离当前关键帧最近的前后两个路径点 THEN

利用前后两个路径点得到假设位姿矩阵，优化上一

个关键帧直到当前关键帧之间的每一帧的位移矩阵和

旋转矩阵

END IF

END IF

END IF

END

实验证明，相同的点云定位算法，静态条件下，使用本专利提出的建图方法生成的点云地图，定位精度与现有高精度点云地图构建方法生成的点云地图定位精度相同。城市动态条件下（小于 50 KM/H），在二者生成的点云地图上进行定位，定位精度相同。方案使用的平台成本对比如下：

本发明针对自动驾驶场景，为低成本建图提供了一种全新的思路与方法，具体实现该技术方案的方法和途径很多，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，对附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。