[发明专利]一种融合多传感器的环境地图构建方法

说明书

本发明涉及同步定位和地图构建的技术领域，公开了一种融合多传感器的环境地图构建方法，本发明通过构建状态估计问题，融合视觉‑惯性里程计VIO与激光‑惯性里程计LIO，当二者中的一者出现异常时，通过调整异常位姿在状态估计问题中的融合权重，减小异常位姿对系统的影响，从而提高了系统的鲁棒性，降低了系统失效的概率；通过对激光点云进行运动畸变去除，提高了系统在移动过程中的点云测量精度；通过在系统优化问题中引入几何特征约束，提高了地图的精度与一致性；通过回环检测消除了建图过程中的累计误差，从而构建得到高精环境地图。

技术领域

本发明涉及同步定位和地图构建的技术领域，具体是涉及一种融合多传感器的环境地图构建方法。

背景技术

目前，同步定位和地图构建(SLAM)技术是三维环境重建的主流技术。按主传感器类型进行分类，SLAM领域已涌现出两大类方案：以激光雷达为主传感器的激光SLAM方案，以及以相机为主传感器的视觉SLAM方案。由于激光雷达具备测距范围广、测距精度高的优点，激光SLAM通常用于道路、园区等开阔环境中；但是由于激光雷达稀疏的特性，激光SLAM在室内等狭小结构化环境中，容易发生退化现象，无法保证精度与稳定性。图像传感器由于采集到的信息丰富、稠密，因此视觉SLAM可以在狭小环境中可靠运行，然而其缺点是其对像素点的测距精度不高，故难以用于三维重建等对精度要求高的场景。

综上所述，如何在室内等狭小结构化场景中建立高精度的环境地图，是现有SLAM技术面临的挑战。

发明内容

针对上述问题，本发明提出了一种融合多传感器的环境地图构建方法，以解决上述背景技术中面临的问题。

为了实现上述目的，本发明提供一种融合多传感器的环境地图构建方法，包括以下步骤：

步骤S1、获取相机图像与IMU数据，通过视觉-惯性里程计VIO，获取当前环境中的第一位姿；

步骤S2、获取激光雷达点云与IMU数据，通过激光-惯性里程计LIO，获取当前环境中的第二位姿；

步骤S3、根据一定时间窗口内的所述第一位姿、第二位姿、IMU数据，构建状态估计问题，获取当前环境中的第三位姿；

步骤S4、根据所述第三位姿，确定并添加关键帧；对关键帧对应的激光点云进行平面、直线提取；对不同关键帧间的相似平面、直线进行融合；

步骤S5、进行回环检测；

步骤S6、构建优化问题，更新所述环境中的关键帧位姿；

步骤S7、根据关键帧集合、关键帧位姿、关键帧对应的相机图像数据、关键帧对应的激光点云数据、平面信息、直线信息，建立环境地图。

优选的，所述相机为单目相机、双目相机、RGBD相机中的任意一种，所述激光雷达为机械式激光雷达、固态激光雷达、混合固态激光雷达中的任意一种。

优选的，所述视觉-惯性里程计VIO采用紧耦合的方案，具体包含以下部分：

特征提取与跟踪：对图像进行直方图均匀化，提取Shi-Tomasi角点，利用KLT光流法跟踪相邻帧的特征点；采用基于基础矩阵模型的RANSAC算法进行异常点剔除；根据相机内参，对二维特征点进行去畸变，后投影至单位球面；

IMU预积分：加速度和角速度测量值的噪声被建模为高斯白噪声，加速度计偏置和角速度偏置被建模为随机游走噪声，并且认为其导数为高斯白噪声。那么IMU的测量方程如下：对于两帧序号为k,k+1的图像，时间区间为[t_k,t_k+1]，有预积分：

其中，