[发明专利]一种基于多传感器融合及二维码校正的定位与建图方法

说明书

本发明公开了一种基于多传感器融合及二维码校正的定位与建图方法，属于计算机视觉领域，包括前端处理：利用RGB‑D相机与IMU惯性测量单元，基于紧耦合方法和EPnP方法完成视觉惯性融合，实现基于RGB‑D‑IMU视觉惯性模块测得视觉惯性模块估计位姿；使用扩展卡尔曼滤波方法进行位姿融合，构建位姿约束图；以及包括前端处理：利用L‑M优化算法进行优化，利用二维码扫描相机获取地面二维码内的位姿数据，对所得优化位姿进行校正，完成基于多传感器融合及二维码校正的定位与建图。本发明有效解决了单传感器定位与建图系统中建图精度不高、在退化环境中容易发生跟踪丢失的问题，有效实现了高精度、高鲁棒性的同步定位与建图。

技术领域

本发明属于计算机视觉领域，涉及一种基于多传感器融合及二维码校正的定位与建图方法。

背景技术

同步定位与地图构建(Simultaneous Localization And Mapping,简称SLAM)，是指把机器人放置于一个未知环境中，从一个未知位置开始移动，通过对机器人配备的各种传感器数据对环境信息进行采集和处理，生成对其自身位置姿态的定位和场景地图信息的系统。SLAM技术对于智能机器人自主行动及交互能力至关重要，它在自动驾驶、机器人、无人机、三维重建、AR/VR等领域有着广泛的应用。针对移动机器人在建图过程中的精度问题与鲁棒性问题，大量研究人员提出了各种基于滤波、基于图的激光SLAM方法与基于单目相机、双目相机及RGB-D相机的视觉SLAM方法，如激光SLAM中谷歌开源的Cartographer算法、视觉SLAM中开源的ORB-SLAM2算法等。这些方法能够在一定程度上满足定位与建图精度及鲁棒性需要，但仍存在许多缺陷，尤其是在退化环境中容易发生丢失的问题。

对于纯激光SLAM，其主要问题在于：①不擅于在动态环境中(如有大量人员活动于其测量环境)完成建图；②不擅长在非结构化环境(如又长又直的走廊环境)中建图，在此类环境中的回环检测极易出错，从而使得建图崩溃。由于重定位能力较差，激光SLAM在跟踪丢失后很难重新回到工作状态，从而导致所建地图严重变形。对于纯视觉SLAM，其主要问题在于：①在无纹理环境(如洁白墙壁)、透明墙环境中极易出现跟踪丢失。②在光照特别弱的环境中，由于相机提取的特征与实际相差较大，也会导致跟踪丢失。因此，现有纯激光或纯视觉的单传感器定位，存在精度不高、在退化环境中容易发生跟踪丢失等问题，无法实现高精度、高鲁棒性的同步定位与建图。

发明内容

为了克服上述现有技术的缺点，本发明的目的在于提供一种基于多传感器融合及二维码校正的定位与建图方法，有效解决了单传感器定位与建图系统中建图精度不高、在退化环境中容易发生跟踪丢失的问题，有效实现了高精度、高鲁棒性的同步定位与建图。

为了达到上述目的，本发明采用以下技术方案予以实现：

本发明公开了一种基于多传感器融合及二维码校正的定位与建图方法，采用激光雷达、RGB-D相机和IMU惯性测量单元，基于图优化方法，包括构建位姿与约束图过程的前端处理，以及优化位姿与约束图并形成2.5D地图过程的后端处理；

其中，前端处理包括下步骤：

RGB-D相机对所拍摄到图像数据利用ORB特征进行图像特征检测与提取，得到特征提取信息，计算所得特征提取信息的深度信息，根据所得深度信息估计相机位姿并生成词袋BoW；利用词袋BoW的视觉特征构造关键帧对应的字典，进行匹配并计算图像的相似性，完成回环检测；基于紧耦合的方法，利用RGB-D相机与IMU惯性测量单元构建运动方程与观测方程，并基于EPnP方法估计相邻帧的运动关系及位姿变换，得到相邻帧的相对运动关系，然后根据所得相邻帧的相对运动关系注册接收的点云，进行数据整合，完成视觉惯性融合，实现基于RGB-D-IMU视觉惯性模块测得视觉惯性模块估计位姿；使用扩展卡尔曼滤波方法，对激光扫描匹配得到的位姿与视觉惯性模块估计位姿进行位姿融合，构建位姿约束图；

其中，后端处理包括下步骤：