点击蓝字 关注我们
本文引用信息
刘辉, 张雪波, 李如意, 等. 双目视觉辅助的激光惯导SLAM算法[J]. 控制与决策, 2024, 39(6): 1787-1800.
2024年第6期封面文章推荐
双目视觉辅助的激光惯导SLAM算法
刘辉，张雪波，李如意，苑晶
✦✦
研究背景
同步定位与地图构建(simultaneous localization and mapping，SLAM)是机器人领域的热点研究方向之一，其要求机器人通过自身携带的传感器对自身状态/位姿进行估计并同时构建环境地图。随着机器人应用范围的不断扩大，从地下隧道到外太空，从灾难现场到深海探测，机器人常常需要具备在缺乏明显特征的环境中进行精确的自我定位和环境地图构建的能力。在结构特征缺失的场景下进行定位与建图是一项极具挑战性的任务，此类环境通常缺乏明显的特征点或者几何结构，这使得一般的定位和建图方法难以应用。因此，无结构或低结构环境中的准确定位和高精地图构建研究具有重要意义。
激光雷达作为常用的距离传感器，具有直接测量深度、不易受光照影响等优势，因此，激光SLAM/里程计是近年来的重要研究方向之一，并且发展出诸多有效的解决方案。相比于纯激光方案，激光雷达与惯性测量单元(IMU)融合可以发挥更为优良的性能。
在一般情况下，激光-IMU里程计/SLAM解决方案能够获得精度较高的位姿与地图。然而对于一些特殊的具有挑战性的场景，例如开阔区域或长直隧道中，往往由于几何结构缺乏或结构极其相似，激光雷达作为环境感知传感器无法获取足够的几何特征约束，此类算法会面临定位精度显著降低，甚至运行失败的问题。与激光雷达相比，视觉传感器对环境中的纹理特征敏感，在环境几何结构特征缺乏或结构相似环境下视觉里程计/SLAM可以辅助激光雷达里程计进行自身状态估计。
✦✦
研究内容
基于此，本文提出一种双目视觉辅助的激光惯导定位和建图算法，通过融合相机-IMU-激光雷达传感器数据实现结构特征退化场景与普通场景下的位姿估计与地图构建功能。具体而言，采用双目视觉惯导里程计为激光扫描匹配模块提供先验位姿约束，完成当前激光数据帧的配准。考虑到视觉里程计可能失效的情况，本文通过维护激光-IMU融合模块为激光扫描匹配部分提供候补的相对位姿预测， 以达到更佳的配准效果。此外，所提出算法在闭环检测模块中结合激光结构信息与视觉图像信息同时检测闭环，并采用6自由度位姿图优化策略降低位姿估计的漂移误差。图1为本文所提出方法的整体框图，主要由视觉惯导里程计、点云预处理、扫描匹配、激光惯导融合和闭环优化模块组成，其中视觉惯导里程计独立运行，分别为激光惯导里程计与闭环优化模块提供先验位姿和闭环信息。
在获得传感器收集的环境信息后，SLAM算法需要依据传感器数据估计机器人在环境中的位置和姿态，并利用传感器数据创建环境的地图表示。其中扫描匹配与数据关联是最为关键的一环，即将当前的感知数据与地图中的已有信息进行匹配，以更新机器人的位置估计和地图。为提升位姿估计的精度与激光惯导里程计算法的鲁棒性，本文在扫描匹配中额外引入视觉先验位姿约束，实现双目视觉辅助的激光惯导里程计算法。此外，本文算法基于双目匹配获得三维特征点，所估计位姿为真实尺度，大大简化了单目视觉/单目视觉惯导等算法的初始化流程及对运动充分性的要求，适用于车载运动。
激光雷达在采集数据时随着载体运动，会导致采集的点云数据中包含运动畸变。本文利用IMU的高频运动观测对激光雷达点云的畸变进行校正，以减小环境感知误差，保证数据的一致性，提升定位精度。
除使用激光数据构建的结构特征约束外，本文的扫描匹配模块融合视觉惯导里程计模块提供的先验位姿增强约束信息，提高算法在结构特征缺乏环境下的鲁棒性。该模块包含当前帧的位姿初值预测、激光特征关联和优化方程构建等3个部分。首先，激光当前帧位姿初值预测值包括视觉惯导里程计提供的视觉-IMU位姿和激光惯导融合模块输出的IMU积分位姿两个来源，优先使用视觉惯导里程计提供的位姿作为位姿估计的初值以融合更多的信息；其次，本文基于最近邻匹配规则，建立当前激光帧特征点集与历史局部地图之间的数据关联，其中局部地图由历史关键帧集合组成；最后，本文通过同时最小化激光几何特征约束项与视觉先验位姿约束项的误差和来估计当前激光帧位姿。当环境中的几何特征缺乏时，如载体运行于开阔的环境中，激光雷达的有效测量仅来自于地面部分，平行于地面的平移和载体航向的不确定度较高，因而本文通过添加视觉先验位姿约束项实现与视觉信息融合以获得额外约束，进而保证算法在几何特征缺乏环境下的性能。
为了将激光雷达位姿与IMU数据进行融合，保证点云数据和IMU数据的一致性，需要确定里程计参考系在惯性参考系中的初始姿态。为此，本文在数据融合前进行参考系对齐，采用互补滤波算法提供的初值与因子图优化精确求解的估计策略，依据IMU原始测量值快速完成姿态估计。此外，为降低计算开销，本文采用逐渐增加数据的策略执行对齐过程，即每获得一个激光帧位姿便执行一次优化，直到位姿收敛。准确而高效的位姿对准可以避免误差累积，保证状态估计的准确性和鲁棒性。
里程计模块在长时间运行之后需要加入闭环约束来消除历史轨迹的漂移误差。本文提出激光ICP匹配与视觉闭环检测结合的混合闭环检测策略，通过6自由度位姿图融合里程计约束与闭环约束。本文额外引入基于视觉图像的场景识别方法来辅助检测闭环信息。若产生有效的激光闭环信息，则采用6自由度位姿图进行全局优化，以消除历史轨迹的漂移误差。
✦✦
实验验证
(a) MVSEC公开数据集序列
为验证所提出算法的有效性，如图2所示，本文使用公开数据集和自制数据集分别针对激光扫描匹配、激光-惯导参考系对齐以及混合闭环检测等3部分内容进行测试验证，且与开源算法进行对比以验证本文算法的性能。通过不同配置下激光扫描匹配测试，本文方法展现出卓越的全局轨迹精度，且对光照剧烈变化鲁棒。在几何结构稀疏场景下对多种SLAM算法运行对比实验的结果表明，所提出的算法在环境中几何特征缺乏时通过结合视觉信息可以保证充足的环境特征约束。本文提出的参考系对齐流程的有效性在里程计参考系与惯性参考系对准实验中得到了充分验证。本文的闭环检测策略可以快速准确地识别闭环并校正误差。所提出算法VLIO-Loop与A-LOAM、LIO-SAM、LIOM、R2LIVE和LVI-SAM等算法进行对比，如表1所示，所提出算法的绝对轨迹误差小，可以准确检测有效的闭环约束，显著降低轨迹误差，对场景、起点与运动鲁棒。
✦✦
结论
本文提出的双目视觉信息辅助的激光惯导SLAM算法通过视觉位姿为激光扫描匹配部分提供先验位姿约束，与激光几何结构特征所构建的约束共同用于位姿估计，引入激光雷达与IMU 数据融合模块进行相对位姿预测，基于互补滤波算法与因子图优化策略快速对准激光里程计参考系和惯性参考系，保证激光-IMU数据正确融合并基于因子图策略约束IMU偏置。此外，本文提出了视觉激光混合闭环检测策略，利用激光雷达观测视角范围大以及视觉检测机制不受距离限制的优势同时检测闭环，并采用6自由度位姿图优化方法显著降低了轨迹误差，完成了全局连续地图的构建。
主要作者介绍
刘辉，南开大学机器人与信息自动化研究所，工程师，硕士，从事移动机器人定位与建图、多传感器融合SLAM 等研究。
张雪波，南开大学机器人与信息自动化研究所，教授，博士生导师，从事移动机器人学、视觉伺服、运动规划、无人车自主导航、强化学习与智能博弈、多摄像机网络等研究。
相关文章推荐
郑重声明：根据国家版权局相关规定，纸媒、网站、微博、微信公众号转载、摘编本微信信息，需包含本微信名称、二维码等相关信息，并在文首注明《控制与决策》原创。个人请按本微信原文转发、分享。
－联系我们－
《控制与决策》
网站：http://kzyjc.alljournals.cn
电话：024-83687766
邮箱：kzyjc@mail.neu.edu.cn
《Journal of Control and Decision》
网站：
https://www.tandfonline.com/toc/tjcd20/current
邮箱：tengrong@ise.neu.edu.cn
点击“阅读原文”查看全文信息
