为了解决激光雷达不均匀运动畸变问题，将视觉惯性里程表与激光雷达里程表相结合，构建了三维地图同时定位和绘图(SLAM)方法。通过视觉估计和惯性测量单元IMU的预积分对预处理后的时间戳对齐数据进行初始化。通过约束滑动窗口优化和视觉里程计的高频位姿，将传统的雷达均匀运动模型改进为多级均匀加速模型。从而减少点云失真。同时，利用Levenberg ~ Marquard LM方法对激光里程表进行优化，提出了一种结合词袋模型的环路检测方法，最后构建了三维地图。基于实车试验数据，与LEGO￣LOAM(轻型和地面优化激光雷达测程和可变地形地图ping)方法的结果相比，平均误差和中位数误差分别提高了16%和23%。

０引言

随着2020年2月《智能汽车创新发展战略》的发布，汽车无人驾驶技术的研究已成为高校和产业界的热门话题。同时定位与地图绘制SLAM技术是无人驾驶技术的重要组成部分。当无人车在全球定位系统GPS中出现故障时，SLAM技术可以在没有先验信息的情况下，依靠自身的传感器独立完成无人车的姿态估计和导航[1]。目前主流的SLAM方法根据传感器类型可分为基于摄像机的视觉SLAM和基于雷达的激光SLAM[2]。近年来，由于惯性测量单元的集成(惯性测量单元IMU的视觉SLAM具有绝对尺度可估计且不受成像质量影响的优点，已逐渐成为该领域的研究热点[3]。

视觉SLAM可进一步分为特征点法和光流法。特征点法通过特征点匹配跟踪特征点，最后进行重投影光流规则基于恒灰度假设，将特征点法中描述子与特征点的匹配替换为光流跟踪。在特征点法的SLAM方案中，ORB￣SLAM(定向FAST和旋转BRIEF)最具代表性。ORB具有视点和光照的不变性，关键帧的提取和冗余帧的删除也保证了BA(bundle adjustment)优化的效率和准确性[4-5]。考虑到纯视觉SLAM在旋转过程中容易出现帧丢失，特别是纯旋转对噪声比较敏感，因此，徐宽等[6]将IMU与视觉融合，利用高斯-牛顿方法对预集成的IMU信息和视觉信息进行优化，再利用图优化方法对视觉重投影误差和IMU残差进行优化，从而获得更精确的位姿。VINS ~ MONO(一种鲁棒通用的单目视觉惯性状态估计器)在室外性能突出，它通过IMU+单目摄像机的方案来恢复目标的尺度。由于采用光流跟踪作为前端，ORB￣SLAM比使用描述子作为前端具有更强的鲁棒性，并且ORB￣SLAM在高速运动时不易失迹[7]。

但纯视觉SLAM需要光照条件适中，图像特征鲜明，在室外难以构建3D地图SLAM可以构建室外三维地图，但在运动过程中容易产生不均匀的运动畸变，在退化场景中定位不准确。因此，本文基于激光雷达采集的点云信息，提出了一种多传感器集成的室外三维地图构建与定位方法。该方法首先计算视觉惯性里程计VIO，并输出高频位姿。然后利用激光测距技术ꎬLO(ꎬ)通过高频位姿去除激光雷达的运动畸变，最后构建三维地图。

１算法框架

算法框架大致分为两个模块:视觉惯性里程表模块、激光里程表模块和映射模块。视觉惯性里程表采用KLT(kanade-lucas-tomasi tracking)光流跟踪相邻两帧，并使用IMU预积分作为相邻两帧运动的预测值。在初始化模块中，视觉和IMU预积分松散耦合，求解相邻帧之间的陀螺仪偏置、比例因子、重力方向和速度。采用滑动窗口法对基于视觉构造和IMU构造的残差项进行优化。输出VIO计算出的高频绝对位姿。通过两个模块之间的相机雷达联合标定得到外部参数矩阵，将相机坐标系中的绝对位姿转换为雷达坐标系。

激光里程表和映射模块将点云划分为不同类型的聚类点，方便后续特征提取，然后融合高频VIO位姿，将传统的雷达均匀运动模型改进为多级均匀加速度模型。此时，点云融合了相机和IMU的信息。通过ICP(it￣r - nearest point)匹配，利用LM优化两帧点云之间的位姿变换矩阵，并将其转换为初始点云坐标系。最后，结合基于词袋模型的循环检测，构建三维地图。

2绘图和定位方法

2.1摄像机和IMU数据预处理

由于FAST特征提取效率高，KLT光流跟踪不需要描述，因此选择两者进行特征提取和光流跟踪。设Ix、Iy分别表示像素亮度在x、y方向上的图像梯度，表示t方向上的时间梯度u、v，分别表示光流在x、y轴上的速度矢量。根据KLT光流原理，构造约束方程，利用最小二乘法得到u和v的方程:

在每张新图像中，利用KLT算法跟踪已有的特征点并检测新的特征点。为了保证特征点的均匀分布，将图像划分为18×10大小相同的子区域，每个子区域最多提取10个FAST角点，每张图像保持50 ~ 200个FAST角点。室外场景中相邻两帧之间的位移较大，且每个像素的亮度值可能会突然变化，这对特征点的跟踪有不好的影响。因此，有必要去除特征点的异常值，然后将其投影到单位球上。异常值消除采用RANSAC算法，并结合卡尔曼滤波，在室外动态场景中实现更鲁棒的光流跟踪。图2为未使用RANSAC算法和使用RANSAC算法的室外场景特征点跟踪。可以看出，RANSAC算法的使用减少了误跟踪的情况。

（a）未使用RANSAC算法特征点追踪

（b）使用RANSAC算法特征点追踪

图２ＲＡＮＳＡＣ算法对特征点追踪的影响

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

式中:b为IMU坐标系w为初始化时IMU所在坐标系的原点，即世界坐标系at和wt为IMU测量的加速度和角速度qb t k为时刻从IMU坐标系到世界坐标系的旋转Ω为四元数右乘法。整合帧k和帧k+1之间的所有IMU数据。得到k+1帧的位置(P)、速度(v)和旋转(Q)。这个PvQ被用作视觉估计的初始值，其中旋转是四元数形式。

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

式中: Ｒｉｐｉ为相机位姿的旋转和平移部分ｖｉ为相机在世界坐标系下的速度ａｂｉ、 ωｂｉ分别为ＩＭＵ的加速度偏置和角速度偏置.

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

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

其中X * k为最大估计后验值，r0为初始滑动窗口残差，rIij为IMU观测残差，提出的标定方法联合标定相机和激光雷达，确定相机和雷达的相对姿态。将优化后的VIO姿态转换为雷达坐标系，再输出到激光里程计模块。同时，利用BRIEF描述符构造的词包DBoW2计算当前帧与的相似度，进行循环检测。

２.３点云数据预处理
点云预处理部分改进ＬＥＧＯ＿ＬＯＡＭ方案, 并将点云分为地面点、有效聚类点和离群点, 分为两步:
１) 地面点云提取, 可将点云划分为０.５ｍ×０.５ｍ的栅格, 计算栅格内的最高点和最低点的高度差, 将高度差低于０.１５ｍ的栅格归类于地面点.
２) 有效聚类点提取, 在标记地面点后,某些小物体的扰动会对接下来的帧间配准环节造成影响. 故对点云进行欧式聚类ꎬ 将聚类点数少于３０或在竖直方向上占据的线束小于３的点进行滤除
２.４高频ＶＩＯ位姿去除点云畸变

由于机械式激光雷达在扫描过程中存在点云的非匀速运动畸变[１３] ,为提升点云配准的精确度, 使用ＶＩＯ输出的高频位姿去除点云畸变. 首先,对齐两传感器系统的时间戳,如图３所示,定义ｔＬｑ为雷达在第ｑ次扫描时的时间戳ꎬ 定义ｔＶ－Ｉｋ为ＶＩＯ系统第ｋ次位姿输出时的时间戳ꎬ 则通过下式实现时间对齐戳:

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

２.５点云特征点的提取与匹配

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

３实际场景验证

图5 实验路线

将本文方法标记为Ａ法ＬＥＧＯ￣ＬＯＡＭ法标记为Ｂ法. 表１为Ａ法和Ｂ法的对比两者均为真值即ＧＰＳ数据进行对比. 表１中Ｍａｘ为最大误差Ｍｅａｎ为均平均误差Ｍｅｄｉａｎ为误差中位数Ｍｉｎ为最小误差ＲＭＳＥ为均方根误差ＳＳＥ为误差平方和ＳＴＤ为标准差.

表１实车验证结果对比

表１中由序１至４的对照组可以看出，在各类型路线的各项误差上，Ａ法比Ｂ法具有更小的误差.在大场景地图下，最大误差减小了２６％，平均误差减小了１６％，误差中位数减小２３％最小误差减小８８％均方根误差减小２０％误差平方和减小３６％标准差减小３０％. 图７(ａ)、７(ｂ)为大场景地图下Ａ法与真值的轨迹对比和误差分析图７(ｃ)、７(ｄ)为大场景地图下Ｂ法与真值的轨迹对比和误差分析Ａ法相较于Ｂ法在大场景建图下各方面误差都得到有效的缩小.

图7 两种方法所建的轨迹与真值轨迹的误差对比

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

参考文献:

[1] HENING S IPPOLITO C A KRISHNAKUMAR K S et al. 3D LiDAR SLAM integration with GPS/INS for UAVs in urban areas GPS ~ degraded environments[C] / / AIAA Information Systems ~ AIAA Infotech@Aerospace. Grapevine: AIAA 2017: 0448.

[2] SHIN Y S PARK Y S KIM A. Direct visual SLAM using sparse depth for camera￣lidar system[C] / / International Conference on Robotics and Automation (ICRA). Brisbane: IEEE 2018:1-8.

[3] NUTZI G WEISS S SCARAMUZZA D et al. Fusion of IMU and vision for absolute scale estimation in monocular SLAM[J].Journal of Intelligent & Robotic Systems201161(1/2/3/4): 287-299.

[4] MUR￣ARTAL R MONTIEL J M M TARDOS J D ORB￣SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics 2015 31(5): 1147-1163.

[5]高翔，张涛，刘毅等。视觉SLAM十四讲:从理论到实践[M]。北京:电子工业出版社2017。(中文)

[6]徐凯。基于IMU信息融合的双目视觉SLAM研究[D]。哈尔滨:哈尔滨工业大学2018。(中文)

[7] QIN T LI P SHEN S. Vins￣mono: a robust and versatile monocular visual￣inertial state estimator[J]. IEEE Transactions on Robotics 2018 34(4): 1004-1020.

[8] INDELMAN V WILLIAMS S KAESS M et al. Factor graph based incremental smoothing in inertial navigation systems[C] / /15th International Conference on Information Fusion. Singapore: IEEEꎬ 2012:2154-2161.

[9]程传奇，郝向阳，李建生等。基于非线性优化的单目视觉/惯性组合导航算法[J]。惯性技术学报，2017,25(5):643-649。

[10] HINZMANN T SCHNEIDER T DYMCZYK M et al. Monocular visual￣inertial SLAM for fixed￣wing UAVs using sliding window based nonlinear optimization[J]. Lecture Notes in Computer Science 2016 10072: 569-581.

[11] UNNIKRISHNAN R HEBERT M. Fast extrinsic calibration of a laser rangefinder to a camera: CMU￣RI￣ TR-05-09 [R].Pittsburgh: Robotics Institute Carnegie Mellon University 2005.

[12] KASSIR A PEYNOT T. Reliable automatic camera￣laser calibration[C] / / Proceedings of the 2010 Australasian Conference on Robotics & Automation. Australasia: ARAA 2010: 1-10.

[13] ZHANG Jꎬ SINGH S. LOAM: lidar odometry and mapping in real￣time[C] / / Robotics: Science and Systems. Berkeley: CA 2014 2:9.

[14] SHAN Tꎬ ENGLOT B. Lego￣loam: lightweight and ground￣optimized lidar odometry and mapping on variable terrain[C] / /

IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEEꎬ 2018: 4758-4765.