基于GNSS/INS与激光雷达的缓存池建图

叶珏磊,周志峰,王立端,庞正雅

(1.上海工程技术大学机械与汽车工程学院,上海 201620;2.上海司南卫星导航技术股份有限公司,上海 201801)

1 引 言

随着各类传感器技术的发展,人们对三维可视化技术的需求开始日益增长,如城市建图、三维建模、环境感知等。对于自动驾驶车辆自主稳定行驶时对外界环境模型的先验信息不足的问题,激光雷达是必不可缺的范围传感器解决方案[1-2]。激光雷达可以提供高频范围测量,无论测量的距离如何误差都相对恒定,其不易受天气影响且对光照不敏感,返回的点云包括三维环境信息[3]。在激光雷达的唯一运动为旋转激光束的情形下,点云的配准是简单的。但当激光雷达本体开始移动时,精确的点云配准建图就需要在激光雷达连续测距过程中了解激光雷达的姿态。

许多学者对这方面作了相关的研究,文献[4]基于单个二维激光雷达通过使用SMACH库对环境进行三维重建,使用ROS接口创建机器人环境的三维模型,无需先验信息或人工干预。文献[5]提出一种利用二维激光雷达在6自由度内移动时通过两种并行算法进行测距和作图的实时方法,通过激光雷达里程仪在高频下处理估计速度,而激光雷达在低频的速率下建图。文献[6]介绍了一种利用四台二维激光雷达采集三维数据完成基于车辆的街道测绘系统,在良好的GPS情况下预期精度可以达到30 mm。文献[7]、[8]利用里程计如轮速编码器和视觉里程表系统来配准激光雷达点云,但里程计集成了随时间漂移的小增量运动,因此必然会产生漂移。文献[9]通过结合视觉里程仪与三维激光雷达,利用视觉里程仪高频估计运动,激光雷达低频测程细化运动估计,并在扫描匹配时用线性运动模型对视觉里程计漂移引起的点云畸变进行校正。文献[10]、[11]通过使用由二维激光雷达和IMU组成的手持测绘设备,利用地理结构与局部点云簇的几何结构相匹配的方法进行点云配准。他们还使用多个二维激光雷达来绘制一个地下煤矿,该方法通过对分段数据进行批量优化处理,利用分段间的边界约束实现了轨迹的恢复,但该方法需要预先批处理,不适用于实时应用。

综合以上所述,本文提出一种基于GNSS/INS与激光雷达传感器的缓存池建图算法,该算法首先采用一种基于GNSSINS的偏移量线性差值法与四元数球面插值法并行算法将激光雷达点云转换到唯一坐标系下,纠正点云随时间的漂移。根据历史激光雷达数据帧生成的数据包,通过基于缓存池的建图方法对周围环境进行点云分割与拼接,对得到的子地图进行体素滤波,最终得到丰富环境信息的三维重建地图。本算法只使用到了激光雷达点云数据与GNSS/INS的偏移量与旋转量,程序将执行整个建图过程并输出三维建模结果。实验结果表明,该方法在大多数情况下都是可靠的,可以对未知的外界环境进行三维模型的重建。

2 三维建图方法

2.1 线性插值法与四元数插值法

激光雷达的数据采集频率为10 Hz,而GNSS/INS接收机输出数据的频率为100 Hz。为了将激光雷达的点云数据转换到对应的高斯投影坐标系下,必须将接收机与激光雷达的数据进行时间同步。但激光雷达与接收机的时间戳会有一个偏差,并不是完全一一对应的。例如:当激光雷达时间戳为51.2478 s时,GNSS/INS接收机数据的最近时间戳为51.2400 s与51.2500 s,两者毫秒位对应的数据没有匹配。对于十六线的激光雷达来说,探测距离有80～100 m,角度乘以目标物的距离就是目标物的幅长。即使激光雷达与接收机的真实姿态只相差略微,投射到的目标物幅长相对真实值会有较大的误差。随着时间推移,点云图对真实环境描述的精度会越来越低。因而本文采用线性插值法与四元数球面插值法并行算法对惯导时间帧之间的数据进行推算,提高激光雷达点云在高斯投影坐标系下的位置精确度,解决建图精度的问题。

对于旋转偏量,若采用欧拉角法需结算三个微分方程,但当载体俯仰角为90°时将出现奇点,具有局限性；三角函数法需要解算六个微分方程,计算量大,不适合实时解算。四元数法与另外两种相比较而言具有的优势:①只需求解四个微分方程；②四元数法可以避免在使用欧拉角法时出现的奇异值问题[12]；③与三角函数法相比,四元数法歪斜误差为零,刻度误差的推导也较简单。由于线性插值法对于四元数是不适用的。因此本文对旋转偏量采用四元数球面插值法,在两个四元数之间平滑插值。如图1所示,θ为单位向量P和Q之间的夹角,P和r之间的夹角为tθ,Q和r之间的夹角为(1-t)θ。 线性插值法与四元数球面插值法并行算法伪代码如表1所示。

图1 四元数球面插值法

插值的一般公式为:r=a(t)P+b(t)Q,在公式两端分别点乘P与Q向量可得式(1):

(1)

利用式(1)求解出未知量a(t)与b(t),得式(2):

(2)

于是求得四元数球面线性插值公式为:

(3)

表1 线性插值法与四元数球面插值法并行算法伪代码

2.2 基于缓存池模型的三维建图算法

激光雷达点云数据量庞大,每秒钟都会有十万个点的数据上传。若不及时处理这些数据会造成系统的卡顿与崩溃。基于以上问题本文采用缓存池分块保存地图。如图2所示,在内存中设立n块缓存池。右边的缓存池为较老且信息丰富的缓存池。当小车在向前行驶过程中再也扫描不到缓存池1中的点云时。将缓存池1从内存中释放掉,保存为子地图存放在硬盘中,并将新建的缓存池new推入内存中,对缓存池new进行点云的建图。

图2 基于缓存池模型的三维建图

这就保证了内存在任意时刻都只有n块缓存池,不会发生内存的不够或者崩溃现象。因为地图内的点云信息是在高斯投影坐标系下的,因此扫描到的点云都是具有唯一性的。当小车行驶一圈再次来到相同的地点时,程序会把实时扫描到的相同点云信息代入地图检测是否这一块已经有先验子地图。如果发现存在对应的子地图,算法将子地图提出再次放入缓存池中,将扫到的实时点云添加到子地图上的相应位置区内。这样,无论在场景内跑多少圈,都可以在先验地图的基础上对地图进行优化。如图3所示为基于缓存池模型的三维建图算法流程。

图3 基于缓存池模型的三维建图算法流程图

对得到的三维子地图进行Voxel体素网格方法下采样,减少点云的数量。在每个体素(即设置的三维立方体大小)内,用单个体素中所有点云的重心来近似显示体素中其他点,这样该体素内所有点就用一个重心点最终表示,既压缩了点云数据量的大小,同时保持了激光雷达点云的形状特征。将所有子地图体素滤波后得到过滤后的点云子地图,拼接成最后的三维地图。

3 实验设计与分析

为了验证本文提出的算法的有效性,我们搭建一个实验平台和一个测试区域来记录和测试数据,如图4所示。平台由以下几项组成:一台GNSSINS接收机M600mini,由上海司南卫星导航技术股份有限公司生产,规格参数见表2；一台16线激光雷达,由深圳速腾聚创公司所生产,规格见表3,实验中将激光雷达架高以防止激光雷达扫到太多车顶的数据失去对环境信息的获取。实验中,激光雷达数据处理算法运行在一台2.5 GHz四核6GB内存的笔记本电脑上。在基于Linux下的机器人操作系统(ROS)上,编写C++程序完成算法的实现。本次实验场景设置在路面上动态物体较少的环境下,避免非静态物体对静态环境的干扰。

图4 实验平台及实验设备

表2 司南M600-mini接收机参数 Tab.2 The parameters of Sino M600-mini

司南M600-mini接收机惯导部分司南M600-mini接收机GNSS部分加速度计陀螺仪HV测量量程±4 g±250°/sRTK精度10 mm+1 ppm20 mm+1 ppm稳定性 35°/h40 mg静态差分精度2.5 mm+1 ppm5.0 mm+1 ppm

表3 速腾聚创16线激光雷达参数

本次实验设计在城市道路环境下行人车辆较少的情景下进行,在内存中设置了24块缓存池以完成高精度建图的要求。图5为实验车运动时激光雷达采集的原始数据,这些原始点云都是单帧互无关联的点云而且没有丰富的外界环境信息,显示不同的颜色为物体对激光雷达射束的不同的反射强度。图6为采用基于缓存池模型的三维建图算法后得到的三维地图。可以看出经过本文算法处理后的三维地图对地面、树木,路面交通牌等标志的还原度较好,且地图中的点云都带有高斯投影坐标系的坐标属性,能够为下一步载体通过高精度地图定位提供先验信息。

图5 原始点云数据

图6 三维重建后的道路环境地图

4 结束语

多线程激光雷达在自主机器人、智能驾驶领域的重要性正在与日俱增,是自动驾驶汽车感知的主要传感器。为了提升智能导航载体对外界未知环境的感知能力。本文提出一种基于GNSS/INS与激光雷达传感器的缓存池建图算法,该算法首先采用一种基于GNSSINS的偏移量线性差值法与四元数球面插值法并行算法将激光雷达点云转换到唯一坐标系下,纠正点云随时间的漂移；通过基于缓存池的建图方法对周围环境进行点云分割与拼接,点云经过体素滤波后得到丰富环境信息的三维重建地图。实验结果表明,使用该方法成功对汽车载体的周围环境进行扫描匹配,可以实现对未知的外界环境进行三维的地图重建。下一步的研究将着重于提升算法的实时性与基于三维地图的应用。