来源：福州大学学报( 自然科学版) 作者：林晨浩，彭育辉 转载自智驾产业汇 侵删
摘要: 针对激光雷达非匀速运动畸变问题，提出一种融合视觉惯性里程计和激光雷达里程计，进行三维地图构 建与定位( simultaneous localization and mapping，SLAM) 方法． 经预处理和时间戳对齐后的数据，应用视觉估计 和惯性测量单元( inertial measurement unit，IMU) 预积分对视觉进行初始化，通过约束的滑窗优化和视觉里程计 的高频位姿，将传统雷达匀速运动模型改进为多阶段匀加速模型，从而降低点云畸变． 同时，利用列文伯格-马 夸尔特( Levenberg-Marquardt，LM) 方法优化激光里程计，提出一种融合词袋模型的回环检测方法，最终实现三 维地图构建． 基于实车试验数据，通过与 LEGO-LOAM( lightweight and ground-optimized lidar odometry and mapping on variable terrain) 方法的结果对比，本文方法在平均误差和误差中位数上分别提升了 16%和 23%．
引言
随着 2020 年 2 月《智能汽车创新发展战略》的发布，汽车无人驾驶技术的研究成为高校与工业界的热 点． 其中同时定位与建图 ( simultaneous localization and mapping，SLAM) 技术是无人驾驶技术中不可忽视的 一环． 当无人车在全球定位系统( global positioning system，GPS) 失效后，SLAM 技术可依靠自身搭载的传 感器在没有先验信息的情况下，自主完成无人车的位姿估计和导航． 目前主流的 SLAM 方法可根据传感 器的类型分为基于相机的视觉 SLAM 和基于雷达的激光 SLAM． 近年来，由于融合惯性测量单元( inertial measurement unit，IMU) 的视觉 SLAM 具有可估计绝对尺度，不受成像质量影响的优点，逐渐成为该领域 的研究热点。
视觉 SLAM 又可进一步分为特征点法和光流法． 特征点法通过特征点匹配来跟踪特征点，最后用重投 影误差优化 Ｒ，t． 光流法则是基于灰度不变假设，把特征点法中的描述子和特征点匹配换成光流跟踪． 在 众多特征点法的 SLAM 方案中，OＲB-SLAM( oriented FAST and rotated BＲIEF) 最具代表性，OＲB 特征具有 视点不变性和光照不变性，关键帧的提取和冗余帧的剔除也为 BA( bundle adjustment) 优化的效率和精度 提供了保证． 考虑到纯视觉 SLAM 在旋转时比较容易丢帧，特别是纯旋转，对噪声敏感，不具备尺度 不变性． 因此，徐宽等将 IMU 与视觉进行融合，将预积分后的 IMU 信息和视觉信息应用高斯牛顿法优 化，再利用图优化的方法优化视觉的重投影误差和 IMU 的残差，从而获取更精确的位姿． 目前的光流方案 中，VINS-MONO( a robust and versatile monocular visual-inertial state estimator) 在户外的性能较为出众，其通 过 IMU+单目相机的方案恢复出了物体的尺度，由于使用的光流追踪作为前端，相较于将描述子作为前端 的 OＲB-SLAM 具有更强的鲁棒性，在高速运动时不容易跟丢。进激光雷达交流群，请加VX:zhijia0124.
然而，纯视觉 SLAM 需要适度的光照条件和明显的图像特征，且在室外很难构建三维地图． 激光 SLAM 可构建室外三维地图，但运动中易产生非匀速运动畸变，且在退化场景中定位不准确． 为此，基于 激光雷达采集的点云信息，本文提出一种融合多传感器的室外三维地图构建与定位方法． 该方法首先计算 视觉惯性里程计( visual-inertial odometry，VIO) 并输出高频位姿，再通过高频位姿去除激光雷达运动畸变 并计算激光里程计( laser-odometry，LO) ，最终实现三维地图构建．
1 算法框架
如图 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，如下式．
在每张新的图像中，现有的特征点被 KLT 算法跟踪并检测新的特征点． 为保证特征点的均匀分布，将图 像划分为 18×10 个大小完全相同的子区域，每个子区域最多提取 10 个 FAST 角点，保持每张图像存在 50 至 200 个 FAST 角点． 室外场景相邻两帧图像之间位移较大，且各像素的亮度值可能发生突变，对特征点的跟踪造成不良影响． 因此需要对特征点进行离群值剔除后再将其投影到单位球面上． 离群值剔除使用 ＲANSAC 算 法并融合卡尔曼滤波器，以实现在户外动态场景下较为鲁棒的光流追踪． 图 2 展示了户外场景未使用 ＲANSAC 算法和使用 ＲANSAC 算法的的特征点跟踪，可以看到，使用 ＲANSAC 算法降低了误跟踪的情况．进激光雷达交流群，请加VX:zhijia0124.
IMU 响应快，不受成像质量影响，可估计绝对尺度的特性，对室外表面无结构的物体的视觉定位进行 补充． 如果在相机位姿估算时将 IMU 所有采样时刻所对应的全部位姿插入帧间进行优化，将会降低程序 运行效率，需进行 IMU 预积分处理，将高频率输出的加速度和角速度测量值转化为单个观测值，该测 量值将在非线性迭代重新进行线性化，形成帧间状态量的约束因子． 连续时刻的 IMU 预积分见下式．
式中: b 为 IMU 坐标系; w 为原点初始化时 IMU 所在的坐标系即世界坐标系; at 和 wt 是 IMU 所测得的加 速度和角速度; qbk t 为 t 时刻从 IMU 坐标系到世界坐标系的旋转; Ω 为四元数右乘． 将第 k 帧到第 k+1 帧之 间的所有 IMU 数据进行积分． 即可得到第 k+1 帧的位置( P) 、速度( v) 和旋转( Q) ，此 PvQ 作为视觉估计 的初始值，此处的旋转为四元数形式．
2．2 滑窗优化
初始化模块恢复单目相机的尺度，需对视觉信息和 IMU 信息进行松耦合． 首先，用 SFM 求解滑动窗 内所有帧的位姿与所有路标点的三维位置，再将其与之前求得的 IMU 预积分值进行对齐，从而解出角速 度偏置，重力方向、尺度因子和每一帧所对应的速度． 随着系统的运行，状态变量的数目越来越多，使用 滑动窗口法优化窗口内的状态变量． 定义在 i 时刻窗口中的优化向量 xi 如下式
式中: Ｒi，pi 为相机位姿的旋转和平移部分; vi 为相机在世界坐标系下的速度; abi、ωbi 分别为 IMU 的加速 度偏置和角速度偏置．
设在 k 时刻参与优化滑窗中的所有帧的所有 xi 的集合为 Xk，系统的所有观测量为 Zk ． 结合贝叶斯公 式，用最大后验概率估计系统的状态量，如下式
将该最大后验问题转化为优化问题，定义优化目标函数见下式．
式中: X* k 为最大估计后验值; r0 为滑窗初值残差; rIij为 IMU 观测残差; rCil为相机观测残差． 结合 Unni-krishnan 等和 Kassir 等提出的标定方法对相机与激光雷达进行联合标定，确定相机和雷达的相对位 姿，将优化过后的 VIO 位姿转换到雷达坐标系下再输出给激光里程计模块，同时用 BＲIEF 描述子构建的 词袋 DBoW2 与当前帧进行相似度计算，进行回环检测．
2．3 点云数据预处理
点云预处理部分改进 LEGO_LOAM 方案，并将点云分为地面点、有效聚类点和离群点，分为两步: 1) 地面点云提取，可将点云划分为 0．5 m×0．5 m 的栅格，计算栅格内的最高点和最低点的高度差，将高度 差低于 0．15 m 的栅格归类于地面点． 2) 有效聚类点提取，在标记地面点后，某些小物体的扰动会对接下 来的帧间配准环节造成影响． 故对点云进行欧式聚类，将聚类点数少于 30 或在竖直方向上占据的线束小 于 3 的点进行滤除．
2．4 高频 VIO 位姿去除点云畸变
由于机械式激光雷达在扫描过程中存在点云的 非匀速运动畸变，为提升点云配准的精确度，使 用 VIO 输出的高频位姿去除点云畸变．
首先，对齐 两传感器系统的时间戳，如图 3 所示，定义 tLq为雷 达在第 q 次扫描时的时间戳，定义 tV－Ik为 VIO 系统 第 k 次位姿输出时的时间戳，则通过下式实现时间 对齐戳:
将tV－Ik ，tV－Ik+1 ，tV－Ik+2 ，tV－Ik+3 时刻计算的四个绝对位姿表示为 Tk，Tk+1，Tk+2，Tk+3，将 tLq 至 tLq+1 时间内的 位移分为二个匀加速阶段，改进为雷达运动模型为多阶段的匀加速模型，如下式．
通过两阶段的位移和速度，插值计算点云的速度、位移和欧拉角，消除雷达非匀速运动产生的畸变．
2．5 点云特征点的提取与匹配
点云特征点主要包含两类: 平面特征点和边缘 特征点．
如图 4 所示，由于断点曲率较大，平行点曲 率较小，会分别被误当做边缘点和平面点提取． 因此，在进行特征提取前，必须对断面上的断点和 与激光线方向相平行的平行点进行去除处理． 定义 点云粗糙度 ck，i，Sk，i为在 k 时刻距点云 pk，i最近的 前后五个点的集合，通过对 ck，i的大小来对边缘点 和平面点进行阈值分割，如下式:
帧间匹配环节的根本在于求解两帧之间位姿变化的最小二乘问题，将匹配点之间的距离范数和作为 误差函数 f( Pk－1，Pk ) ，如下式:
式中: p( k，i) 为第 k 帧中的第 i 个点云; T L ( k－1，k) 为 k 帧与 k－1 帧间的变换矩阵． 令 k－1，k 两帧间对点云进行约束的方程集合为 D( k－1，k) ，第 k 帧的边缘点和平面点的集合为 Sk，得到误差方程式
通过非线性迭代优化法 LM 法使 D( k－1，k) 趋近于 0，即可求得 T L ( k－1，k) ，如下式:
回环模块采用点云近邻搜索算法，以激光雷达当前位姿为搜索点，查找半径范围为 5 m 内的若干个位 姿，为提高搜索精度，同步 DBoW2 检测结果，增加回环检测时间上的约束．
3 实际场景验证
本文数据集源自于福州大学旗山校园区域，实验车辆为自主研制的无人驾驶方程式赛车测试平台，如图 5 所示． 激光雷达为 Pandar 40P，摄像头为 MYNT-1000 D( 内置 IMU) ，工控机为 Nuvo-7160 GC，操作系统为 ubuntu16．04． 进激光雷达交流群，请加VX:zhijia0124。实验路线位于福州大学旗山校区北区，分别对应大场景、方型、Z 型、P 型地图，如图 6 所示．
将本文方法标记为 A 法，LEGO-LOAM 法标记为 B 法． 表 1 为 A 法和 B 法的对比，两者均为真值，即 GPS 数据进行对比． 表 1 中，Max 为最大误差; Mean 为均平均误差; Median 为误差中位数; Min 为最小误 差; ＲMSE 为均方根误差; SSE 为误差平方和; STD 为标准差
表1 中，由序 1 至 4 的对照组可以看出，在各类型路线的各项误差上，A 法比 B 法具有更小的误差． 在大场景地图下，最大误差减小了 26%，平均误差减小了 16%，误差中位数减小 23%，最小误差减小 88%，均方根误差减小 20%，误差平方和减小 36%，标准差减小 30%． 图 7( a) 、7( b) 为大场景地图下 A 法 与真值的轨迹对比和误差分析，图 7( c) 、7( d) 为大场景地图下 B 法与真值的轨迹对比和误差分析，A 法相较于 B 法，在大场景建图下各方面误差都得到有效的缩小．
图 8( a) 为两种方法建图的轨迹与真值轨迹的对比，图 8( b) 、8( c) 、8( d) 为局部放大图，图 8 中虚线为 GPS 真值，蓝线为 A 法所建轨迹，可见，A 法所建的车辆轨迹比 B 法更贴近于 GPS 所输出的真值． 绿线为 B 法所建轨迹． 图 9 是本文方法所建的福州大学旗山校区北区的三维地图，可见各部分回环情况良好．
4 结语
室外激光雷达三维建图存在点云非匀速运动畸变，在传统激光 SLAM 的方法上融合了视觉惯性里程 计，将雷达匀速运动模型改进为多阶段的匀加速运动模型，并在回环检测模块引入词袋模型． 通过本文方 法和 LEGO-LOAM 方法在建图绝对位姿误差上进行对比，本文方法在平均误差和误差中位数上分别提升 了 16%和 23%，建图精度有较大提升． 由此可见，多阶段匀加速的雷达运动模型在长时间建图下能有效减 小里程计累计误差． 提出的双重回环检测在回环时刻精确度上相较传统方法具有更强的时间约束，能满足 户外三维建图的需求．