CN116045965A - 一种融合多传感器的环境地图构建方法 - Google Patents

Abstract

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

Description

一种融合多传感器的环境地图构建方法

技术领域

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

背景技术

目前，同步定位和地图构建(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]，有预积分：

其中，

位姿优化：在一段滑动窗口中维持一定数量的***状态、IMU测量、特征点信息，通过光束平差法，最小化所有测量残差的先验与马氏范数之和，计算最大后验估计：

并采用鲁棒核函数

其中，***的完整状态定义为：

其中x^k为IMU在第k帧图像时的状态，n为图像关键帧数量，m为特征点数量，λ为特征点逆深度；

边缘化：为限制***的计算复杂度，当最新图像帧的上一帧是关键帧时，滑动窗口将边缘化最旧的帧与其相应的测量值，并将相应的测量值转化为先验。

优选的，所述激光-惯性里程计LIO具体包含以下部分：

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

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

点云帧间配准：在一段滑动窗口中维持一定数量的激光点云帧，将所述激光点云帧根据各自时刻位姿进行叠加，得到局部子地图；将当前最新时刻的激光点云与所述局部子地图进行配准，得到最新位姿。

优选的，所述状态估计问题的算法为卡尔曼滤波、粒子滤波、位姿图优化、因子图优化算法中的任意一种。

优选的，所述步骤S3还包括：

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

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

优选的，所述步骤S5具体包括以下步骤：

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

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

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

优选的，所述优化问题的算法为位姿图优化、因子图优化算法中的任意一种。

优选的，所述优化问题包括：

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

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

与现有技术相比，本发明的有益效果是：

本发明的一种融合多传感器的环境地图构建方法通过构建状态估计问题，融合视觉-惯性里程计VIO与激光-惯性里程计LIO，当二者中的一者出现异常时，通过调整异常位姿在状态估计问题中的融合权重，减小异常位姿对***的影响，从而提高了***在狭小易退化环境中的鲁棒性；通过对激光点云进行运动畸变去除，提高了***在移动过程中的点云测量精度；通过在***优化问题中引入几何特征约束，建立不变形且无重叠的地图，提升了地图构建的精度与一致性。

附图说明

图1是本发明的一种融合多传感器的环境地图构建方法的流程图；

图2是本发明的一种激光点云去畸变方法的示意图；

图3是本发明的一种状态估计方法的示意图；

图4是本发明的一种优化问题构建的示意图。

图中：201a第0帧状态，201b第n帧状态，202点云帧间配准约束，203视觉里程计约束，204IMU预积分约束，205IMU测量帧，210时间轴，220滑动时间窗口；310关键帧位姿节点，311里程计约束因子，320直线路标节点，321直线路标观测因子，330a平面路标节点，330b地平面路标节点，331平面路标观测因子，340回环约束因子。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意，本公开中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分，并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。本公开实施方式中的多个装置之间所交互的消息或者信息的名称仅用于说明性的目的，而并不是用于对这些消息或信息的范围进行限制。

本实施例提出的一种融合多传感器的环境地图构建方法，如图1所示，包括以下步骤：

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

具体的，所述IMU至少输出三轴加速度信息、三轴角速度信息；

可选的，所述为单目相机、双目相机、RGBD相机中的任意一种；

在本实施例中，所述所述视觉-惯性里程计VIO采用紧耦合的方案，具体包含以下部分：

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

IMU预积分：加速度和角速度测量值的噪声被建模为高斯白噪声，加速度计偏置和角速度偏置被建模为随机游走噪声，并且认为其导数为高斯白噪声。那么IMU的测量方程如下：

对于两帧序号为k,k+1的图像，时间区间为[t_k,t_k+1]，有预积分：

其中，

位姿优化：在一段滑动窗口中维持一定数量的***状态、IMU测量、特征点信息，通过光束平差法，最小化所有测量残差的先验与马氏范数之和，计算最大后验估计：

并采用鲁棒核函数

其中，***的完整状态定义为：

其中x_k为IMU在第k帧图像时的状态，n为图像关键帧数量，m为特征点数量，λ为特征点逆深度；

边缘化：为限制***的计算复杂度，当最新图像帧的上一帧是关键帧时，滑动窗口将边缘化最旧的帧与其相应的测量值，并将相应的测量值转化为先验。

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

在本实施例中，所述激光-惯性里程计LIO具体包含以下部分：

点云去畸变：根据所述第三位姿、IMU数据，预测当前激光点云帧中每个激光扫描时刻下，激光雷达位于环境中的位姿；并根据所述当前激光点云帧中每个激光扫描时刻下激光雷达位于环境中的位姿，对激光点云进行运动畸变去除。

具体的，如图2所示，假设当前对最新收到的激光雷达的第n个雷达扫描点进行畸变矫正，激光雷达数据帧开始时刻为t_lidar，激光雷达采样周期为Δt，则该扫描点的采样时刻为t_n＝t_lidar+(n-1)Δt。经查询得到该扫描点相邻两帧IMU数据的采样时刻与姿态分别为：t_m、q_m与t_m-1、q_m-1，则计算得到插值系数：

根据单位四元数形式下的球面线性插值公式，可得到线性插值后的姿态：

q_n＝Slerp(q_m,q_m-1,μ)；

已知该激光扫描点相对于激光雷达坐标系的空间坐标p_n＝[x_n,y_n,z_n]^T，并将四元数q₀、q_n分别转换至R₀、R_n，通过其次坐标变换，得到去畸变后的激光扫描点坐标：

点云帧间配准：在一段滑动窗口中维持一定数量的激光点云帧，将所述激光点云帧根据各自时刻位姿进行叠加，得到局部子地图；将当前最新时刻的激光点云与所述局部子地图进行配准；

在本实施例中，所述点云帧间配准方法为NDT(Normal DistributionTransformation)。

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

可选的，所述状态估计问题的算法为卡尔曼滤波、粒子滤波、位姿图优化、因子图优化算法中的任意一种；

在本实施例中，所述状态估计问题通过因子图表示，并采用固定滞后平滑器(Fixed-lag smoother)的方式对所述因子图进行优化。

具体的，参考图3，所述因子图包括：第0帧状态201a，第n帧状态201b，点云帧间配准约束202，视觉里程计约束203，IMU预积分约束204，IMU测量帧205，时间轴210，滑动时间窗口220。时间轴210的箭头方向由历史时刻指向最新时刻，滑动时间窗口220表示固定滞后平滑器的平滑时间范围，因子图的优化范围包括所述滑动时间窗口范围内第0帧至第n帧的所有变量与因子。

具体的，所述点云帧间配准约束202由帧间配准结果得到。

具体的，所述视觉里程计约束203由视觉-惯性里程计VIO在第i帧与第i+1帧激光点云扫描时刻的状态估计得到。

具体的，IMU预积分约束204由第i帧与第i+1帧激光点云扫描时刻间产生的IMU测量帧205的集合进行预积分得到。

在本实施例中，所述状态估计问题还包括：判断所述第一位姿与第二位姿是否存在异常，若所述第一位姿或第二位姿存在异常，通过调整异常位姿在状态估计问题中的融合权重，减小异常位姿对状态估计结果，即所述第三位姿的影响；

具体的，所述第一位姿的异常判断方法是：若VIO***中跟踪的有效特征点数量少于一定阈值或VIO***中IMU预积分项的残差超过一定阈值，则第一位姿出现异常。

具体的，所述第二位姿的异常判断方法是：

所述点云配准算法是一个非线性优化问题，可以通过线性化后的迭代优化进行求解，其一般表达式为：min_T‖AT-b‖²。计算

矩阵中的最小特征值，若该最小特征值小于一定阈值，则第二位姿出现异常。

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

在本实施例中，所述关键帧的添加依据是：1.当前帧与最新关键帧间的运动距离超过一定阈值；2.当前帧与最新关键帧间的角度变化超过一定阈值；二者满足一个条件，即将当前帧确定并添加为最新的关键帧。

在本实施例中，所述平面提取的方法为RANSAC。具体的，从原始点云中随机选取3个点，拟合空间平面；查找邻近点，若某邻近点距离平面距离小于一定阈值，则加入该平面并更新平面参数；若最终所得平面包含的点数量大于一定阈值，则认为平面被正确拟合；重复上述操作，直至原始点云中所有平面都被提取。

具体的，所述平面的表示公式为：

在本实施例中所述直线提取的方法为：根据局部曲率大小提取点云中的所有角点；将所有三维角点投影至水平面上，得到二维点集；对所述二维点集进行欧氏聚类，得到若干聚类点集；对所述聚类点集对应的三维点集进行RANSAC直线拟合，从而提取出所有与水平面近似垂直的直线特征。

具体的，所述直线的表示公式为：

在本实施例中，平面融合的判断条件为：

且

在本实施例中，直线融合的判断条件为：

且

具体的，当平面或直线发生融合后，新的平面或直线参数将由融合前的平面或直线的点云集合进行计算得到。

步骤S5、进行回环检测；

在本实施例中，回环检测包括以下步骤：

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

具体的，回环检测采用DBoW2算法提供的方案，对每一帧图像关键帧提取BRIEF描述子，并保存。

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

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

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

可选的，所述优化问题的算法为位姿图优化、因子图优化算法中的任意一种。

在本实施例中，采用因子图优化的方案，并采用iSAM2算法对因子图进行增量式更新，以实现较高的计算效率。

在本实施例中，参考图4，因子图包含：关键帧位姿节点310，里程计约束因子311，直线路标节点320，直线观测因子321，平面路标节点330a，地平面路标节点330b，平面路标观测因子331，回环约束因子340。

具体的，所述关键帧位姿节点310、直线路标节点320、平面路标节点330a、地平面路标节点330b为因子图中的待优化变量；所述里程计约束因子311、直线路标观测因子321、平面路标观测因子331、回环约束因子340为变量间的约束。

具体的，所述直线路标观测因子321的误差计算公式为：

具体的，所述平面路标观测因子331的误差计算公式为：

具体的，因子图优化的求解方法为：用φ_i(X_i)表示一个因子的后验概率密度，假设所有因子近似为零均值高斯分布，则该因子对应的节点可以表示为：

其中h_i(·)为该因子对应的观测方程，z_i为观测量，Σ_i为测量的协方差。***的优化问题可以表示为各个因子的乘积，再通过取负对数，可以将最大后验估计问题转化为最小二乘问题：

最后，通过非线性优化的方法，如Levenberg-Marquart法，进行求解。

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

在本实施例中，所述建立的环境地图，包括但不限于：三维点云地图、视觉特征点地图、彩色点云地图、二维平面地图、三维矢量地图等。

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。

Claims (8)

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所述的一种融合多传感器的环境地图构建方法，其特征在于，所述优化问题包括：

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

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

Images