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

权利要求书

1.一种融合多传感器的环境地图构建方法，其特征在于，包括以下步骤：

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

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

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

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

步骤S5、进行回环检测；

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

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

2.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述相机为单目相机、双目相机、RGBD相机中的任意一种，所述激光雷达为机械式激光雷达、固态激光雷达、混合固态激光雷达中的任意一种。

3.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述激光-惯性里程计LIO包含点云运动畸变去除算法：

根据所述第三位姿、IMU数据，预测当前激光点云帧中每个激光扫描时刻下，激光雷达位于环境中的位姿；

根据所述当前激光点云帧中每个激光扫描时刻下激光雷达位于环境中的位姿，对激光点云进行运动畸变去除。

4.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述状态估计问题的算法为卡尔曼滤波、粒子滤波、位姿图优化、因子图优化算法中的任意一种。

5.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述步骤S3还包括：

判断所述第一位姿与第二位姿是否存在异常；

若所述第一位姿或第二位姿存在异常，通过调整异常位姿在状态估计问题中的融合权重，减小异常位姿对所述第三位姿的影响。

6.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述步骤S5具体包括以下步骤：

步骤S51、采用基于视觉图像的回环检测筛选可能的候选关键帧匹配对；

步骤S52、通过视觉特征匹配与投影匹配，判断是否构成回环；

步骤S53、提取构成回环的关键帧的激光点云，进行配准，得到所述关键帧间的相对位姿。

7.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述优化问题的算法为位姿图优化、因子图优化算法中的任意一种。

8.根据权利要求1所述的一种融合多传感器的环境地图构建方法，其特征在于，所述优化问题包括：

待优化变量，所述待优化变量包括各关键帧的位姿、各关键帧包含的平面特征参数和各关键帧包含的直线特征参数；

变量间约束，所述变量间约束包括各关键帧间的里程计约束、各关键帧间平面观测约束和各关键帧间直线观测约束。