一种融合视觉与IMU的车载激光雷达建图与定位方法

林晨浩，彭育辉

(福州大学机械工程及自动化学院，福建 福州 350108)

0 引言

随着2020年2月《智能汽车创新发展战略》的发布，汽车无人驾驶技术的研究成为高校与工业界的热点.其中同时定位与建图 (simultaneous localization and mapping，SLAM)技术是无人驾驶技术中不可忽视的一环.当无人车在全球定位系统(global positioning system，GPS)失效后，SLAM技术可依靠自身搭载的传感器在没有先验信息的情况下，自主完成无人车的位姿估计和导航[1].目前主流的SLAM方法可根据传感器的类型分为基于相机的视觉SLAM和基于雷达的激光SLAM[2].近年来，由于融合惯性测量单元(inertial measurement unit，IMU)的视觉SLAM具有可估计绝对尺度，不受成像质量影响的优点，逐渐成为该领域的研究热点[3].

视觉SLAM又可进一步分为特征点法和光流法.特征点法通过特征点匹配来跟踪特征点，最后用重投影误差优化R，t.光流法则是基于灰度不变假设，把特征点法中的描述子和特征点匹配换成光流跟踪.在众多特征点法的SLAM方案中，ORB-SLAM(oriented FAST and rotated BRIEF)最具代表性，ORB特征具有视点不变性和光照不变性，关键帧的提取和冗余帧的剔除也为BA(bundle adjustment)优化的效率和精度提供了保证[4-5].考虑到纯视觉SLAM在旋转时比较容易丢帧，特别是纯旋转，对噪声敏感，不具备尺度不变性.因此，徐宽等[6]将IMU与视觉进行融合，将预积分后的IMU信息和视觉信息应用高斯牛顿法优化，再利用图优化的方法优化视觉的重投影误差和IMU的残差，从而获取更精确的位姿.目前的光流方案中，VINS-MONO(a robust and versatile monocular visual-inertial state estimator)在户外的性能较为出众，其通过IMU+单目相机的方案恢复出了物体的尺度，由于使用的光流追踪作为前端，相较于将描述子作为前端的ORB-SLAM具有更强的鲁棒性，在高速运动时不容易跟丢[7].

然而，纯视觉SLAM需要适度的光照条件和明显的图像特征，且在室外很难构建三维地图.激光SLAM可构建室外三维地图，但运动中易产生非匀速运动畸变，且在退化场景中定位不准确.为此，基于激光雷达采集的点云信息，本文提出一种融合多传感器的室外三维地图构建与定位方法.该方法首先计算视觉惯性里程计(visual-inertial odometry, VIO)并输出高频位姿，再通过高频位姿去除激光雷达运动畸变并计算激光里程计(laser-odometry, LO)，最终实现三维地图构建.

1 算法框架

图1 算法框架Fig.1 Algorithm framework

如图1所示，算法框架大致分为两个模块： 视觉惯性里程计模块，激光里程计与建图模块.视觉惯性里程计用KLT(kanade-lucas-tomasi tracking)光流追踪相邻两帧并将IMU预积分作为相邻两帧图像运动的预测值，同时在图像中提取词袋信息用于回环检测.初始化模块将视觉与IMU预积分进行松耦合求解出陀螺仪偏置、尺度因子、重力方向和相邻两帧间的速度.通过滑动窗口法优化基于视觉构造的残差项和基于IMU构造的残差项，输出VIO计算的高频绝对位姿.两模块间通过相机雷达联合标定得到外参矩阵将相机坐标系下的绝对位姿转换到雷达坐标系下.

激光里程计与建图模块将点云分类成不同类型的聚类点，方便后续特征提取，再融合高频VIO位姿将传统的雷达匀速运动模型改进为多阶段的匀加速模型.此时点云已融合相机与IMU的信息，将ICP(iterative closest point)帧间匹配后的点云用LM优化得到两帧点云间的位姿变换矩阵，并转换到初始点云坐标系下，最后融合基于词袋模型的回环检测构建三维地图.

2 建图与定位方法

2.1 相机、IMU数据预处理

由于FAST特征提取效率高，KLT光流追踪不需要描述子，故选取此二者进行特征提取和光流追踪.设Ix、Iy表示图像中像素点亮度在x、y方向上的图像梯度，It表示在t方向上的时间梯度，u、v为光流沿x、y轴的速度矢量.根据KLT光流原理构建约束等式，应用最小二乘法可求得u、v，如下式.

(1)

在每张新的图像中，现有的特征点被KLT算法跟踪并检测新的特征点.为保证特征点的均匀分布，将图像划分为18×10 个大小完全相同的子区域，每个子区域最多提取10个FAST角点，保持每张图像存在50至200个FAST角点.室外场景相邻两帧图像之间位移较大，且各像素的亮度值可能发生突变，对特征点的跟踪造成不良影响.因此需要对特征点进行离群值剔除后再将其投影到单位球面上.离群值剔除使用RANSAC算法并融合卡尔曼滤波器，以实现在户外动态场景下较为鲁棒的光流追踪.图2展示了户外场景未使用RANSAC算法和使用RANSAC算法的的特征点跟踪，可以看到，使用RANSAC算法降低了误跟踪的情况.

图2 RANSAC算法对特征点追踪的影响Fig.2 The influence of RANSAC algorithm on feature point tracking

IMU响应快，不受成像质量影响，可估计绝对尺度的特性，对室外表面无结构的物体的视觉定位进行补充.如果在相机位姿估算时将IMU所有采样时刻所对应的全部位姿插入帧间进行优化，将会降低程序运行效率[8-9]，需进行IMU预积分处理，将高频率输出的加速度和角速度测量值转化为单个观测值，该测量值将在非线性迭代重新进行线性化，形成帧间状态量的约束因子[7].连续时刻的IMU预积分见下式.

(2)

2.2 滑窗优化

初始化模块恢复单目相机的尺度，需对视觉信息和IMU信息进行松耦合.首先，用SFM求解滑动窗内所有帧的位姿与所有路标点的三维位置，再将其与之前求得的IMU预积分值进行对齐，从而解出角速度偏置，重力方向、尺度因子和每一帧所对应的速度.随着系统的运行，状态变量的数目越来越多，使用滑动窗口法[10]优化窗口内的状态变量.定义在i时刻窗口中的优化向量xi如下式.

(3)

式中：Ri，pi为相机位姿的旋转和平移部分；vi为相机在世界坐标系下的速度；abi、ωbi分别为IMU的加速度偏置和角速度偏置.

设在k时刻参与优化滑窗中的所有帧的所有xi的集合为Xk，系统的所有观测量为Zk. 结合贝叶斯公式，用最大后验概率估计系统的状态量，如下式

将该最大后验问题转化为优化问题，定义优化目标函数见下式.

(5)

2.3 点云数据预处理

点云预处理部分改进LEGO_LOAM方案，并将点云分为地面点、有效聚类点和离群点，分为两步： 1) 地面点云提取，可将点云划分为0.5 m×0.5 m的栅格，计算栅格内的最高点和最低点的高度差，将高度差低于0.15 m的栅格归类于地面点.2) 有效聚类点提取，在标记地面点后，某些小物体的扰动会对接下来的帧间配准环节造成影响.故对点云进行欧式聚类，将聚类点数少于30或在竖直方向上占据的线束小于3的点进行滤除.

2.4 高频VIO位姿去除点云畸变

图3 时间戳对齐Fig.3 Timestamp alignment

由于机械式激光雷达在扫描过程中存在点云的非匀速运动畸变[13]，为提升点云配准的精确度，使用VIO输出的高频位姿去除点云畸变.首先，对齐两传感器系统的时间戳，如图3所示，定义tLq为雷达在第q次扫描时的时间戳，定义tV-Ik为VIO系统第k次位姿输出时的时间戳，则通过下式实现时间对齐戳：

tmin=min{|tLq-tV-Ik|，|tLq-tV-Ik+1|}

(6)

将tV-Ik，tV-Ik+1，tV-Ik+2，tV-Ik+3时刻计算的四个绝对位姿表示为Tk，Tk+1，Tk+2，Tk+3，将tLq至tLq+1时间内的位移分为二个匀加速阶段，改进为雷达运动模型为多阶段的匀加速模型，如下式.

(7)

通过两阶段的位移和速度，插值计算点云的速度、位移和欧拉角，消除雷达非匀速运动产生的畸变.

2.5 点云特征点的提取与匹配

图4 雷达扫描中的平行点与断点Fig.4 Paralel points and breakpoints in lidar scanning

点云特征点主要包含两类： 平面特征点和边缘特征点.如图4所示，由于断点曲率较大，平行点曲率较小，会分别被误当做边缘点和平面点提取[14].因此，在进行特征提取前，必须对断面上的断点和与激光线方向相平行的平行点进行去除处理.定义点云粗糙度ck, i，Sk, i为在k时刻距点云pk, i最近的前后五个点的集合，通过对ck, i的大小来对边缘点和平面点进行阈值分割，如下式:

(8)

帧间匹配环节的根本在于求解两帧之间位姿变化的最小二乘问题，将匹配点之间的距离范数和作为误差函数f(Pk-1,Pk)，如下式:

(9)

(10)

(11)

(12)

回环模块采用点云近邻搜索算法，以激光雷达当前位姿为搜索点，查找半径范围为5 m内的若干个位姿，为提高搜索精度，同步DBoW2检测结果，增加回环检测时间上的约束.

3 实际场景验证

本文数据集源自于福州大学旗山校园区域，实验车辆为自主研制的无人驾驶方程式赛车测试平台，如图5所示.激光雷达为Pandar 40P，摄像头为MYNT-1000 D(内置IMU)，工控机为Nuvo-7160 GC，操作系统为ubuntu16.04.实验路线位于福州大学旗山校区北区，分别对应大场景、方型、Z型、P型地图，如图6所示.

图5 实验路线Fig.5 Experimental route

图6 实验平台Fig.6 Experimental facilities

将本文方法标记为A法，LEGO-LOAM法标记为B法.表1为A法和B法的对比，两者均为真值，即GPS数据进行对比.表1中，Max为最大误差；Mean为均平均误差；Median为误差中位数；Min为最小误差；RMSE为均方根误差；SSE为误差平方和；STD为标准差.

表1 实车验证结果对比

表1中，由序1至4的对照组可以看出，在各类型路线的各项误差上，A法比B法具有更小的误差.在大场景地图下，最大误差减小了26%，平均误差减小了16%，误差中位数减小23%，最小误差减小88%，均方根误差减小20%，误差平方和减小36%，标准差减小30%.图7(a)、7(b)为大场景地图下A法与真值的轨迹对比和误差分析，图7(c)、7(d)为大场景地图下B法与真值的轨迹对比和误差分析，A法相较于B法，在大场景建图下各方面误差都得到有效的缩小.

图7 两种方法所建的轨迹与真值轨迹的误差对比Fig.7 Error comparison between the trajectory built by the two methods and the true data trajectory

图8(a)为两种方法建图的轨迹与真值轨迹的对比，图8(b)、8(c)、8(d)为局部放大图，图8中虚线为GPS真值，蓝线为A法所建轨迹，可见，A法所建的车辆轨迹比B法更贴近于GPS所输出的真值.绿线为B法所建轨迹.图9是本文方法所建的福州大学旗山校区北区的三维地图，可见各部分回环情况良好.

图8 轨迹局部放大图Fig.8 Partial enlarged drawing of route

图9 福州大学旗山校区北区三维地图Fig.9 Three dimensional map of north district of Qishan campus of Fuzhou University

4 结语

室外激光雷达三维建图存在点云非匀速运动畸变，在传统激光SLAM的方法上融合了视觉惯性里程计，将雷达匀速运动模型改进为多阶段的匀加速运动模型，并在回环检测模块引入词袋模型.通过本文方法和LEGO-LOAM方法在建图绝对位姿误差上进行对比，本文方法在平均误差和误差中位数上分别提升了16%和23%，建图精度有较大提升.由此可见，多阶段匀加速的雷达运动模型在长时间建图下能有效减小里程计累计误差.提出的双重回环检测在回环时刻精确度上相较传统方法具有更强的时间约束, 能满足户外三维建图的需求.