万普插件库

jQuery插件大全与特效教程

2025年02月27日

协同精密定位 | 王铉彬:基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法

本文内容来源于《测绘学报》2022年第8期（审图号GS京（2022）0490号）


基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法
王铉彬, 李星星, 廖健驰, 冯绍权, 李圣雨, 周宇轩
武汉大学测绘学院，湖北 武汉 430079

基金项目：国家重点研发计划(2021YFB2501102)；国家自然科学基金(41974027；42142037)；中德科学中心-中德国际合作项目(M-0054)
摘要：基于单一传感器的同时定位与地图构建技术已经逐渐不能满足移动机器人、无人机及自动驾驶车辆等智能移动载体日益复杂的应用场景。为了进一步提升移动载体在复杂环境下的定位与建图性能，基于多传感器融合的SLAM技术成为目前研究的热点内容。本文提出了一种基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法(S-VIL SLAM)，该方法在视觉惯性系统中引入激光雷达原始观测，基于滑动窗口实现了IMU量测、视觉特征及激光点云特征的多源数据联合非线性优化。利用视觉与激光雷达的互补特性设计了视觉增强的激光雷达闭环优化算法，进一步提升了多源融合SLAM系统的全局定位与建图精度。为了验证本文算法的性能，利用自主搭建的集成多传感器的硬件采集平台在室外场景下进行了车载试验。试验结果表明，本文提出的紧耦合双目视觉/惯性/激光雷达里程计相比于紧耦合双目视觉惯性里程计和激光雷达里程计定位定姿性能显著提升，视觉增强的激光雷达闭环优化算法能够在大尺度场景下有效探测出轨迹中的闭环信息，并实现高精度的全局位姿图优化，经过闭环优化的点云地图具有良好的分辨率和全局一致性。
关键词：SLAM 图优化 视觉惯性里程计 激光雷达 多源融合
引文格式：王铉彬, 李星星, 廖健驰, 等. 基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法[J]. 测绘学报，2022，51(8)：1744-1756. DOI: 10.11947/j.AGCS.2022.20210503
WANG Xuanbin, LI Xingxing, LIAO Jianchi, et al. Tightly-coupled stereo visual-inertial-LiDAR SLAM based on graph optimization[J]. Acta Geodaetica et Cartographica Sinica, 2022, 51(8): 1744-1756. DOI: 10.11947/j.AGCS.2022.20210503
阅读全文：http://xb.chinasmp.com/article/2022/1001-1595/20220808.htm

引 言

同时定位与地图构建技术(simultaneous localization and mapping, SLAM)是智能化移动载体实现自我感知的一项关键技术，被广泛应用于移动机器人、无人机、自动驾驶等领域[1-4]。随着智能化移动载体搭载的传感器种类逐渐丰富，如何有效地融合不同类型的传感器，为移动载体提供更高精度的即时定位与建图是当下需要解决的重要问题，也是SLAM技术发展的需要。
视觉惯性SLAM技术充分利用了视觉传感器与惯性传感器的互补特性[5]，即在视觉友好的环境下视觉SLAM可以实现高精度的定位与建图，但在一些弱纹理、低光照、高动态的场景下表现欠佳，而惯性导航(inertial navigation system, INS)能够在短时间内实现高精度的航位推算，从而在视觉退化的情形下，融合视觉与惯性可以有效提升视觉SLAM的稳健性。此外，视觉SLAM也可以帮助INS修正传感器的累积误差。从视觉与惯性的融合方式来区分，可以分为松耦合[6]和紧耦合[7-8]，其中紧耦合方案能够充分发挥出两种传感器的优势，成为目前的主流方案。在已有的紧耦合视觉惯性SLAM算法中，从估计器的角度来区分，可以分为基于滤波的方法和基于优化的方法。基于滤波的方法具有效率高、易拓展等优势，其中具有代表性的算法是文献[9]提出的MSCKF算法，该方法对视觉路标点进行了边缘化处理，显著提高了算法效率，算法中同时维护了多个相机状态，通过多相机的共视关系构成多状态约束，有效抑制了里程计的误差累积。相比于基于滤波的方法，通常认为基于优化的方法在足够的计算资源下可以实现更高的估计精度[10]，主要原因是基于优化的方法在每次迭代优化时会重新线性化，从而减少了视觉量测模型中高度非线性带来的误差。文献[11]提出了基于关键帧的视觉惯性SLAM方案OKVIS，该方法采用了滑动窗口的策略，较好地平衡了算法的精度和效率。文献[12]开源了一套稳健的、高精度单目视觉惯性SLAM系统VINS-Mono，该算法能够实现系统快速初始化，并具备重定位和回环的能力。
相比于视觉SLAM，单次测距精度高、测距不受光照条件影响等特性使得基于激光雷达的SLAM方案具有更好的稳健性。但激光SLAM在应用过程中依然存在一些问题: ①基于匀速运动假设的点云畸变矫正模型不够精确；②非结构化场景中激光SLAM性能退化显著；③激光点云缺乏纹理信息导致激光SLAM闭环检测能力弱；④点云配准算法的精度与时间较大程度依赖初值等。为了提升激光SLAM算法的可靠性与稳定性，研究者将惯性传感器(inertial measurement unit, IMU)与激光雷达融合用于解决激光SLAM的性能退化问题。文献[13—14]利用IMU量测去除激光点云在运动过程中产生的畸变，减少了算法中基于匀速运动假设模型带来的误差。文献[15]提出了一种激光点云特征与IMU预积分量测联合优化的紧耦合激光雷达惯性系统，该算法有效提升了单一激光SLAM系统在高动态场景下的估计精度。文献[16]提出了一种基于因子图的紧耦合激光雷达惯性里程计框架，该方法提出了一种高效的基于局部滑动窗口的激光雷达帧到图匹配方法，同时支持闭环检测，进一步提升了SLAM系统的长时间导航性能。在闭环优化方面，文献[14, 16]均采用了基于里程计几何关系的闭环检测方法，当里程计累计误差较大时闭环检测性能将会退化。而视觉特征相比于激光点云具有丰富的纹理特性，可用于设计基于外观的回环检测算法从而不受里程计发散的影响。
为了进一步提升SLAM系统的定位建图性能，基于视觉、惯性及激光雷达的多源融合SLAM方法也得到了广泛研究[17]。文献[18]基于松耦合的方式融合了IMU、单目相机及激光雷达3种传感器，该方法利用视觉里程计的位姿估计结果为激光雷达帧到图的匹配提供初始位姿，有效提升了点云匹配效率与位姿估计精度。文献[19]采用了紧耦合的视觉惯性里程计为激光雷达帧到图匹配提供更精确的先验位姿信息，同时该算法具有回环检测能力，能够实现全局位姿图优化。但松耦合的方式没有充分利用原始观测信息，无法实现更高的状态估计精度。文献[20]基于MSCKF框架实现了一种紧耦合视觉/惯性/激光雷达里程计，该算法相比于视觉惯性里程计以及激光雷达里程计位姿估计性能大幅提升，但不具备闭环优化的能力。文献[21]基于因子图优化实现了一套基于视觉惯性系统(visual-inertial system, VIS)与激光雷达惯性系统(LiDAR- inertial system, LIS)的紧耦合视觉/惯性/激光雷达SLAM框架，该算法通过关联视觉特征与激光点云信息，有效提升了VIS的估计性能，并进一步提升了激光雷达帧到图匹配的效率和精度，但该方法同时维护了VIS与LIS两个独立的估计器，没有在估计器层面进行原始数据的联合优化。
综上，本文提出一种基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法(S-VIL SLAM)。该方法在原始观测值层面融合了IMU量测、视觉特征及激光点云特征，基于滑动窗口实现了多源数据的联合非线性优化。考虑到基于里程计的回环检测方法随着里程计误差累积，闭环检测性能将出现退化，本文提出视觉增强的激光雷达闭环优化算法，基于视觉特征进行基于外观的闭环检测，利用激光点云精配准实现精确的全局位姿图优化，进一步修正SLAM系统的累积误差。为了验证本文算法性能，基于自主搭建的多传感器硬件采集平台在室外环境中进行了车载试验，并对试验结果进行了分析。

1 本文方法

1.1 紧耦合双目视觉/惯性/激光雷达SLAM框架

图 1为本文提出的紧耦合双目视觉/惯性/激光雷达SLAM算法框架。整体框架主要由3部分构成，分别为：①模块Ⅰ多传感器数据预处理；②模块Ⅱ基于滑动窗口的多传感器数据联合非线性优化；③模块Ⅲ视觉增强的激光雷达闭环优化。模块Ⅰ主要是对不同传感器获取的原始数据进行预处理。对于输入的图像数据，SLAM前端首先进行视觉特征的提取与跟踪，然后根据相机的视差变化，进行关键帧的筛选。对于激光点云数据，首先提取原始激光点云中的几何特征(角点和面点)，然后利用IMU预报的位姿对载体运动过程中可能导致的点云畸变进行去除。对于IMU量测数据，本文对两个相邻关键帧之间的IMU数据进行预积分，构建新的预积分观测值用于后端优化。模块Ⅱ首先对不同传感器的观测数据构建残差项，包括视觉重投影残差、IMU预积分残差、激光点云匹配残差及边缘化残差。然后利用非线性最小二乘进行求解，本文使用了Ceres solver[22]优化库。为了兼顾精度和效率，本文后端优化采用了滑动窗口的方式，对于即将滑出窗口的状态进行边缘化操作，构建先验项参与后续优化。模块Ⅲ主要负责闭环优化，其中闭环检测部分采用了基于词袋模型的视觉回环检测方法，通过比较图像间的相似性，判断是否发生闭环。一旦检测到闭环，则进行时空一致性检核，进一步确保闭环的准确性。确认发生闭环后，使用迭代最近点算法(iterative closest point, ICP)对发生闭环的两帧激光点云进行精配准，得到精确的闭环约束信息，进行全局位姿图优化。
图 1 紧耦合双目视觉/惯性/激光雷达SLAM算法Fig. 1 The full pipeline of the proposed tightly-coupled stereo visual-inertial-LiDAR SLAM
图选项

1.2 多传感器数据预处理
本文采用文献[12]的方法对视觉和IMU数据进行预处理，得到用于后端优化的视觉特征点和IMU预积分观测值，这里不展开叙述。对于关键帧的筛选，本文主要基于以下准则。
(1) 基于连续跟踪的特征点判断当前帧与上一帧的平均视差，如果平均视差大于设定的阈值，则判断当前帧为关键帧，否则为非关键帧。计算平均视差的公式为
(1)
式中，N表示连续跟踪特征点的个数；k表示特征标识ID；duk=ujk－uik；dvk=vjk－vik；u和v表示像素坐标；i和j分别表示上一帧和当前帧序号。
(2) 如果当前帧连续跟踪的特征点数量低于设定的阈值，则设定当前帧为关键帧，这个准则主要是为了保证跟踪的质量，防止出现完全跟踪失败的情况。
对于获得的激光点云数据，首先进行特征提取[13]。对于每个激光点云计算曲率大小，然后根据曲率值排序提取几何特征(角点和面点)。机械式激光雷达将扫描一圈的点云数据作为单帧激光数据，当载体静止的时候，激光雷达扫描得到的每一帧数据都是相对于同一个激光雷达坐标系，而当载体在激光雷达扫描期间发生运动，则同一帧的点云数据会对应不同时刻的激光雷达坐标系，这会导致点云数据相对于实际环境中的真实物体产生位置偏差，这种位置偏差在点云匹配过程中会导致匹配错误，从而影响SLAM算法的整体精度。为了消除或者最大程度减小运动畸变的影响，本文利用IMU预报的高频位姿结果辅助激光雷达矫正点云畸变。如图 2所示，假设tk时刻采集到单帧激光数据Pk，Lpi=[Xi Yi Zi]为点集Pk内的一点，i∈[tk, tk+1]，则Lpi在tk+1时刻对应的坐标为
(2)
图 2 激光点云畸变矫正Fig. 2 Illustration of the LiDAR scan deskewing
图选项

式中，TtiW和Ttk+1W分别表示激光雷达在ti和tk+1时刻的姿态矩阵，通过对IMU预报位姿插值计算得到。对Pk内的所有点云做相同的操作，将其全部转换至tk+1时刻对应的激光雷达坐标系下，得到畸变矫正后的点云集Pk。
1.3 基于滑动窗口的紧耦合双目视觉/惯性/激光雷达里程计
1.3.1 系统状态定义
首先给出本文算法的状态定义
(3)
(4)
(5)
式中，X表示滑动窗口内待估计的状态量，包含了IMU状态向量xk、相机和IMU的外参数xcb以及视觉特征fi第一次被观测到的逆深度λi，i∈[0, m]；n和m分别表示滑窗内关键帧的个数和窗口维护的视觉特征点个数；xk表示第k个关键帧时刻对应的IMU状态，包含了载体的位置pbkw、速度vbkw、姿态qbkw及加速度计和陀螺仪的零偏参数ba、bω。pbkw、vbkw和qbkw的参考坐标系为世界坐标系w。需要指出的是，本文通过激光雷达与IMU的外参关系直接构建激光点云观测与IMU状态的观测方程，未在系统状态中单独增加激光雷达位姿状态量。
由于视觉和激光点云的量测模型都是高度非线性的，因此对3种不同类型的观测值进行联合优化需要用到非线性优化的手段。根据非线性优化理论[23]，首先构建不同类型观测值相对于待估状态的量测方程，其中表示含有噪声的观测值。然后构建残差项h(X)，进一步将不同类型观测的残差项加起来，就得到了整个优化问题的代价函数(cost function)。本文非线性优化问题的代价函数可以表达为[24]
(6)
式中，表示滑动窗口内部两个关键帧之间预积分测量残差；表示视觉重投影残差，f为连续跟踪的视觉特征(至少3次)，cj表示图像帧；表示激光点云匹配残差，i表示激光点云，s表示系统维护的局部地图。I、C、L分别表示滑窗内IMU预积分量测、视觉量测及激光点云量测的集合。{ rp, Hp}是边缘化操作后得到的先验信息，用作滑窗优化的先验约束。Pbk+1bk表示预积分观测协方差；Pfcj表示视觉观测噪声方差；Pis表示激光点云特征匹配方差，他们分别被用于在联合非线性优化过程中对不同观测的残差项分配权重。非线性优化过程中，如果由于特征误匹配等原因产生了异常观测值，将其加到图优化中，就会引入一条误差很大的边，给系统引导错误的优化方向。所以为了增加非线性优化的稳健性，本文使用Huber核函数[25]在优化过程中对于异常观测进行降权处理。对于Huber核函数参数的取值需要根据不同的应用场景和传感器设备进行调整，本文试验中使用的经验参数为1。
1.3.2 残差方程构建
(1) 视觉残差。以滑动窗口内被连续观测的特征点fl为例，介绍视觉残差项的构建。假设特征点fl分别被第i帧和第j帧图像观测到，并且第i帧为滑动窗口内首次观测到该点的关键帧。基于双目视觉观测，介绍本文算法构建视觉残差所使用的3种视觉重投影方程
(7)
(8)
(9)
式中，表示特征点fl在滑窗内第1次被观测到的像素坐标；ci, 1表示第i帧的左目图像；πc－1为用相机内参数表示的投影矩阵，用于将像素坐标投影到归一化坐标系下；(Rc1b，pc1b)和(Rc2b，pc2b)分别表示左相机和右相机相对于IMU的外参数；Plcj, 1和Plcj, 2分别表示特征fl的观测从第i帧左目图像投影到第j帧左目图像和第j帧右目图像的重投影结果；Plci, 2表示第i帧左目图像到右目图像的重投影结果。在得到以上视觉观测方程后，残差项可以通过计算值减去观测值得到。式(7)，对Plcj, 1进行归一化，则残差项可以表示为
(10)
式中，Plcj, 1取前两维。由于像素点被投影到归一化平面构建残差方程，则归一化平面下的视觉观测噪声方差可以通过像素噪声方差传播得到。即
(11)
式中，Dp表示像素噪声方差。
(2) IMU预积分残差。IMU预积分残差项定义为[12]
(12)
式中，为相邻两个关键帧之间的预积分观测值。预积分观测的协方差定义为
(13)
式中，Ft表示系统状态转移矩阵；Gt表示噪声驱动阵；Qt表示系统过程噪声协方差。初始协方差Pbkbk被置为0。
(3) 激光点云匹配残差。在本文算法中，维护了世界坐标系下的局部点云地图(Submap)，通过当前激光雷达帧与Submap的点云匹配构建残差项。首先将特征点从LiDAR坐标系投影到世界坐标系下，可以表示为
(14)
式中，Lpi表示LiDAR坐标系下的特征点i；(RLb, pLb)表示LiDAR坐标系与载体坐标系(IMU坐标系)之间的外参。(Rbw, pbw)表示当前载体相对于世界坐标系的位姿。对于角点，构建点到线的匹配残差，如图 3(a)所示，j、k为不在同一条激光扫面线上距离i最邻近的两点，分别表示为wpj、wpk，则i到直线jk的距离可以表示为
(15)
图 3 激光点云匹配残差Fig. 3 Illustration of the laser point cloud matching residuals
图选项

式中，分子的几何含义是如图 3(a)所示虚线平行四边形的面积，分母表示jk两点的距离，所以由平行四边形面积公式就可以得到角点i到直线jk的距离。对于面点，构建点到面的匹配残差。如图 3(b)所示, 点到面的距离可以表示为
(16)
式中，wpj、wpk、wpm为与wpi最邻近的不共线3点。dplanar分子表示的是四面体的体积，分母表示的是平面jmk的面积，所以点到面的距离可以通过四面体体积公式求得。激光点云的匹配残差包含两部分rcorner=dcorner和rplanar=dplanar。在后端优化过程中，如果对激光点云距离残差赋予不合适的权重将会导致整个系统的优化结果失衡，本文在多传感器融合SLAM系统中根据经验值对激光特征构建的点到线和点到面的残差统一赋予单位权重。
1.3.3 滑动窗口模型
本文滑动窗口模型如图 4所示，窗口内X0到X10表示载体的待估状态。对于视觉、惯性及激光雷达3种不同观测，本文采用“高频+低频”的量测更新方式，其中视觉量测进行高频更新(10 Hz)，可以用于维护一定精度的连续状态估计，激光雷达量测采用低频更新(2 Hz)，利用帧到图的匹配实现更高的估计精度，抑制里程计的发散。IMU数据除了用于构建关键帧之间的预积分观测值外，也用来进行全局状态传递，为激光点云帧到图匹配提供较好的初值。本文算法维护了世界坐标系下的全局地图，主要由激光点云特征组成，滑窗内的子图在每次进行帧到图匹配前由全局地图临时生成，它可以看作是以载体为中心的随着载体运动的3D体素，大小可以根据应用场景的范围进行调整。
图 4 滑动窗口模型Fig. 4 The sliding window model
图选项

本文滑动窗口大小设置为10[26]，当窗口满时，需要对窗口内的状态节点进行滑动操作。如果最新节点为关键帧，则窗口滑出最老节点，否则，窗口滑出次新节点。为了维护全局地图，当被滑出的节点包含激光点云时，则将激光点云投影到世界坐标系下，并补充到全局地图中。最老节点的状态经过多次迭代优化已经达到了局部最优并在滑出窗口后被固定，用窗口内最老节点的位姿将点云投影到世界坐标系下是可靠的。在滑窗过程中如果直接丢弃掉最老节点相关的状态和观测量会导致滑窗中原有的状态节点间约束丢失，从而降低估计器的精度，在本文基于滑动窗口的优化算法中，对滑窗中的最老节点进行了边缘化处理[26]，主要目的是在减少算法计算复杂度的同时保留该节点对窗口内其他节点的约束关系，将这种约束关系以先验项的形式参与后续滑窗的优化。
1.4 视觉增强的激光雷达闭环优化
随着载体运动距离的增加，里程计不可避免地会发生漂移，而载体在运动过程中，轨迹中经常会包含闭环，有效的闭环信息可以帮助SLAM系统建立当前时刻和先前时刻的数据关联，进而修正里程计的累积误差，提高SLAM系统估计器在长时间下的正确性和稳定性。本文算法通过里程计输出的关键帧信息构建全局位姿图，利用轨迹中的闭环信息和关键帧之间的相对约束关系对整体轨迹进行优化，用于得到更优的定位和建图效果。本文设计了一种视觉增强的激光雷达闭环优化算法，即充分考虑视觉信息和激光点云的互补特性，利用视觉图像丰富的纹理信息进行高效的闭环检测，当检测出闭环信息后，对当前关键帧与闭环帧进行基于ICP的点云精配准，得到精确的相对位姿关系，然后用于位姿图优化，具体流程如图 1所示。
本文使用经典的DBoW算法[27]来实现闭环检测。对于输入的每个关键帧图像提取至少500个新的角点并计算BRIEF[28]描述子，然后利用已经训练好的字典查询得到当前关键帧的向量描述，对于每幅图像，只将其向量描述存入数据库中用于闭环检索。对于当前关键帧，DBoW算法会对比其和数据库中关键帧的相似性，如果检测到闭环帧，则返回闭环帧的序列号，称其为候选帧，但为了得到准确的闭环帧，还需要进一步对闭环的两帧做验证。通常载体回到先前经过的地方会发生时间上连续的闭环，相比于单次检测到闭环，连续时间检测到的闭环被认为是更正确的闭环情况，除了上述时间一致性的检测，还需要进行空间一致性检测，本文利用发生闭环的两帧相匹配的视觉特征，通过PnP[29]算法得到它们之间的位姿关系，并和里程计给出的相对位姿关系进行比较，进一步确定是否发生闭环。图 5为两组大尺度场景下的视觉闭环检测图像相似度评分序列图。当后端每次收到新的关键帧，首先会在已有的数据库内检索，然后返回评分最高的3幅图像ID和对应的相似度评分，得分越高表明两幅图像的相似度越高。图 5中出现明显峰值的图像序列与实际轨迹中发生闭环的区域一致。在设置闭环阈值的时候往往追求更高的准确率，防止错误的闭环约束对系统造成严重的影响。在实际应用过程中会先对应用场景进行测试分析，然后设置较为合理的阈值。
图 5 图像相似度评分Fig. 5 Image similarity score
图选项

在视觉/惯性/激光雷达里程计算法中，由于惯性量测可以提供重力向量的方向，所以在世界坐标系下，可以确定载体相对于水平面的绝对变化，即俯仰角和横滚角是完全可观的，在整个状态估计期间能够实现较好的估计，而航向角和X、Y、Z 4个自由度是相对估计，累积误差也主要集中在这4个方向。所以本文在全局位姿图优化过程中对里程计得到的俯仰角和横滚角进行固定，只对航向角、X、Y、Z 4个自由度进行优化。
图 6所示的位姿图中，每一个关键帧都代表一个节点，不同的节点之间通过边连接。位姿图中共包含两种类型的边，一种边称为序列边，表示不同关键帧之间的约束关系，由里程计估计的位姿得到，其相对关系可以表达为
(17)
图 6 全局位姿图优化Fig. 6 Diagram of the global pose graph optimization
图选项

另一种边是闭环边，闭环边的约束由发生闭环的两帧激光点云通过ICP精配准得到，可表示为。其中c和l分别表示当前帧和闭环帧。
四自由度的位姿图优化残差项可以表达为“计算值减去观测值”
(18)
最终位姿图优化的目标函数可以表达为
(19)
通过最小化序列边和闭环边的残差，实现对参数的最优估计。本文对于闭环边使用了稳健核函数，防止错误的闭环边对优化造成破坏。对于序列边的残差项没有使用核函数，因为观测值来自里程计本身，已经经过了粗差检核，不会存在明显异常结果。

2
车载试验与结果分析

2.1 试验描述

本文算法基于武汉大学测绘学院GREAT软件[30]实现，为了验证本文算法性能，利用自主搭建的集成多种传感器的硬件采集平台，在室外环境下进行了车载试验。多传感器集成硬件采集平台如图 7所示，平台搭载了多频多系统GNSS接收机、测量型GNSS天线、MEMS-IMU、战术级惯导、双目相机及十六线激光雷达。设备的具体参数见表 1。
图 7 多传感器集成硬件采集平台Fig. 7 Multi-sensor integrated data acquisition platform
图选项
表 1 传感器设备型号与信息
Tab. 1 Sensor models and information



表选项
数据采集平台采用硬件同步的方式将不同传感器的时间统一到GPS时间系统下。该设备利用GNSS/INS组合导航板卡[31]发出的高频PPS脉冲信号(10~20 Hz)来触发相机和激光雷达工作，保证多源数据的采集时间同步。在完成多传感器时间同步以后，需要进一步确定不同传感器之间的空间关系，对于相机和IMU之间的外参，本文使用ETH ASL实验室开源的Kalibr工具箱[32]对IMU和相机进行联合标定。对于LiDAR和相机之间外参数，本文使用Autoware工具箱[33]进行标定，LiDAR和IMU的空间关系可以通过相机和IMU的外参进一步转换得到。本文使用战术级惯导和多频多系统GNSS RTK紧耦合双向平滑结果作为参考真值，使用MEMS-IMU数据进行融合算法验证。
本文在武汉大学校园内进行了两组车载测试，试验轨迹如图 8所示。试验过程中，车辆进行了静止、加减速、转弯等丰富的机动，其运动速度、姿态及高程变化如图 9所示。测试1全长2090 m，轨迹中包含狭长林荫道，行驶道路两旁有高大的树木，测试路段存在明显的高程起伏变化。测试2整段轨迹全长约2760 m，在车辆行驶期间，有光线明暗的变化，部分区域还存在过度曝光，测试2场景相比于测试1更具挑战性，包含了多个连续上下坡的路段，高程起伏变化最大超过10 m。
图 8 车载试验轨迹Fig. 8 The test trajectories of vehicle-borne experiment
图选项

图 9 车辆运动状态时间序列Fig. 9 Vehicle motion during the vehicle-borne experiment
图选项

2.2 试验结果与分析
利用本文S-VIL SLAM算法对上述复杂场景中采集的多传感器数据进行离线解算，并与紧耦合双目视觉惯性里程计(S-VIO)[34]、激光雷达里程计(LOAM)[13]及开源紧耦合激光雷达/视觉/惯性SLAM算法LVI-SAM[21]作了对比。本文主要从绝对轨迹误差(absolute trajectory error, ATE)、相对位姿误差(relative pose error, RPE)及建图一致性3个方面进行分析和评估。
本文算法结果与参考真值有着不同的参考坐标系，在进行绝对轨迹误差分析之前，需要利用一段轨迹数据对估计轨迹和真值轨迹进行对齐。本文采用文献[35]的方法，求得两段轨迹的转换关系(旋转和平移)，并将该转换参数用于估计结果的整段轨迹，将其转到真值轨迹坐标系下，然后求解估计轨迹相对于真值轨迹的偏差。图 10为不同方法的平面轨迹对比图。其中红色五角星代表轨迹的起点，三角形代表轨迹的终点，“w/o loop”表示里程计结果，“w/loop”表示施加闭环优化的结果。可以发现在两组试验中，LOAM与S-VIO的轨迹与Ground Truth都发生明显的分离；而S-VIL(w/o loop)与LVI-SAM(w/o loop)轨迹在大部分区域都能够与Ground Truth吻合得较好，随着行驶距离的增加，在轨迹后半段逐渐出现航向偏差；而经过全局闭环优化的S-VIL(w/loop)与LVI-SAM(w/loop)轨迹几乎与Ground Truth重合。
图 10 绝对轨迹对比Fig. 10 Absolute trajectory comparison
图选项

进一步对上述结果进行量化分析，统计结果见表 2。本文方法S-VIL(w/o loop)估计结果相比于S-VIO三维位置精度提升66.2%~78.5%，姿态精度提升16.2%~48.9%；相比于LOAM三维位置精度提升64.7%~65.2%，姿态精度提升26.9%~36.7%。此外，对比S-VIO与LOAM方法在X、Y、Z 3个方向上的估计结果，可以看出两组测试中，激光雷达里程计相比于视觉惯性里程计平面估计精度较高，但高程估计较差，而本文基于原始观测值的融合方法则实现了多源信息的优势互补，在X、Y、Z 3个方向的估计性能都实现了提升。在第1组试验中，S-VIL(w/o loop) 与LVI-SAM(w/o loop)实现了相当的平面估计精度和航向角估计精度，但本文方法的高程估计性能优于LVI-SAM。第2组试验中，随着试验场景更加挑战，尤其是存在明显的高程起伏变化，LVI-SAM里程计出现了较为明显的退化，Z方向误差RMS为25.50 m，而本文方法仅为2.71 m。


表 2 绝对轨迹误差RMS统计表
Tab. 2 RMS of absolute trajectory error

表选项


本文提出的闭环优化算法在大尺度试验场景下有效探测出了轨迹中的闭环信息，并利用轨迹闭环信息实现了全局位姿图优化。对比最终位姿图优化结果，本文算法在测试1中，X、Y、Z 3个方向的误差RMS分别为0.49、1.59、0.69 m，航向误差RMS为0.23°；测试2中X、Y、Z 3个方向的误差RMS分别为1.12、1.13、0.56 m，航向误差RMS为0.7°。与LVI-SAM(w/loop)相比，两种方法实现了较为相当的平面估计精度和航向估计精度，但本文方法高程估计精度更高，在两组测试中均实现了分米级精度，而LVI-SAM算法的高程估计精度为2~3 m。
本文采用文献[36]的方法对SLAM算法的相对位姿精度进行评估。相对位姿误差可以表达为
(20)
式中，Qi表示参考真值的位姿；Pi表示估计结果的位姿；Δ表示轨迹中固定的距离间隔。相对位姿误差统计结果如图 11、图 12所示。统计结果表明，S-VIL(w/o loop)相比于S-VIO和LOAM方法相对位姿估计性能显著提升，其中航向估计提升最为明显。随着统计结果的间隔距离增加，本文方法的相对误差发散更加缓慢。与LVI-SAM(w/o loop)相比，本文算法在两组试验中表现出更加稳定的相对位姿估计性能，相对平移估计也更优。两组试验中经过闭环优化的结果均实现了最高的相对位姿估计精度。
图 11 测试1相对位姿误差Fig. 11 The relative pose errors in test 1
图选项

图 12 测试2相对位姿误差Fig. 12 The relative pose errors in test 2
图选项

通过上文的对比分析，本文算法在两组试验中均实现了最高的绝对位姿估计精度，本文利用全局位姿图优化结果S-VIL(w/ loop)将LiDAR特征点云投影到世界坐标系下用于定性分析本文SLAM算法建图效果，用视觉/惯性/激光雷达里程计S-VIL(w/o loop)投影得到的点云地图作为对照。图 13为第1组试验的全局地图，车辆行驶过程中3次经过白色方框所在路段，道路中间绿化带能够帮助验证前后点云地图的一致性，可以看出闭环优化后的点云地图绿化带无明显的重影，分辨率更高，而通过里程计结果投影得到的点云地图有明显的错位现象。图 14为第2组试验的全局地图，通过放大闭环区域的点云地图可以看出，图 14(b)里程计投影得到的全局地图出现了明显的位移偏差，而通过位姿图优化结果得到的点云地图具有更好的前后一致性，全局地图的分辨率也更高。在测试1与测试2试验场景下，本文视觉增强的激光雷达闭环优化算法可以有效探测出了轨迹中存在的闭环，并基于激光点云的ICP精配准实现了高精度的位姿图优化结果，进一步提升了SLAM算法的整体位姿估计结果，构建了具有良好全局一致性的点云地图。
图 13 测试1全局点云地图Fig. 13 The global point cloud map in test 1
图选项

图 14 测试2全局点云地图Fig. 14 The global point cloud map in test 2
图选项

3 结论

本文提出了一种基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法，通过室外大尺度场景下的车载试验验证了本文算法的正确性和有效性。试验结果表明：本文紧耦合视觉/惯性/激光雷达里程计相比于紧耦合视觉惯性里程计和激光雷达里程计位姿估计精度均实现了显著提升，视觉增强的激光雷达闭环优化算法可以有效探测出大尺度场景下的轨迹闭环信息，在实现全局高精度位姿估计的同时能够提供具有良好全局一致性和高分辨率的点云地图。本文算法能够为移动载体提供高精度的局部导航定位，帮助移动载体在室外GNSS信号不佳或者失锁场景下获得连续高精度导航定位能力。未来工作中，将针对多源融合SLAM算法中存在的传感器退化问题展开研究，进一步提升多源融合SLAM算法在复杂环境下的稳健性。


致谢

本文的数值计算得到了武汉大学超级计算中心的计算支持和帮助。

作者简介
第一作者简介：王铉彬(1996—)，男，博士生，研究方向为多传感器融合导航。E-mail：wangxuanbin@whu.edu.cn
通信作者：李星星, E-mail：xxli@sgg.whu.edu.cn


初审：张艳玲
复审：宋启凡
终审：金 君

资讯



标签：js closest 

作者:wanpu88 , 分类:插件库 , 浏览:28 , 评论:0

控制面板
您好，欢迎到访网站！
  查看权限
网站分类
最新留言