CN115855045A - 一种应用于矿井巷道的多模态融合建图与定位方法 - Google Patents

Abstract

本发明涉及矿井巷道建图与定位技术领域，解决了弱特征下且GNSS无法作用的矿井下定位与建图困难的技术问题，尤其涉及一种应用于矿井巷道的多模态融合建图与定位方法，包括以下步骤：获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征；根据当前帧的图像信息采用KLT算法和KnnMatch算法分别跟踪当前滑动窗口关键帧的点特征和线特征得到新的视觉点线特征信息；获取激光雷达当前激光帧的点云信息，计算点云信息中每个点的曲率，并根据每个点的曲率划分出边特征和面特征。本发明达到了能够做到高精度实时定位和建图，并且时间上和功耗上均衡的目的。

Description

一种应用于矿井巷道的多模态融合建图与定位方法

技术领域

本发明涉及矿井巷道建图与定位技术领域，尤其涉及一种应用于矿井巷道的多模态融合建图与定位方法。

背景技术

为了保障矿井下轨道机车的安全运输从而减少矿工作业人员伤亡，并进一步实现建设智能化矿山的终极目标，自动驾驶机车正逐渐被应运到井下轨道运输。但是由于井下光线照射条件较弱能见度低，巷道狭窄且轨道线路铺设复杂，操作环境较为恶劣，具有非结构化特征环境，一般的单模态SLAM系统难以独自处理这一恶劣未知环境，当GNSS技术又无法应用在矿井巷道区域时，就很难做到高精度定位与建图，由于环境感知性差，矿井机车在煤矿井下的自主定位系统就无法实现高精准定位，因此降低了煤矿开采的安全性。

发明内容

针对现有技术的不足，本发明提供了一种应用于矿井巷道的多模态融合建图与定位方法，解决了弱特征下且GNSS无法作用的矿井下定位与建图困难的技术问题，达到了能够做到高精度实时定位和建图，并且时间上和功耗上均衡的目的。

为解决上述技术问题，本发明提供了如下技术方案：一种应用于矿井巷道的多模态融合建图与定位方法，包括以下步骤：

S1、获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征；

S2、根据当前帧的图像信息采用KLT算法和KnnMatch算法分别跟踪当前滑动窗口关键帧的点特征和线特征得到新的视觉点线特征信息；

S3、获取激光雷达当前激光帧的点云信息，计算点云信息中每个点的曲率，并根据每个点的曲率划分出边特征和面特征；

S4、获取激光雷达下一激光帧的点云信息，并采用NDT和ICP的两阶段扫描匹配算法对下一激光帧的点云信息与当前激光帧的点云信息匹配，得到激光边面特征信息；

S5、依据IMU提供的先验尺度信息不断更新当前状态的位姿；

S6、在更新后的位姿下，将最新的视觉点线特征信息与激光边面特征信息建立深度关联；

S7、将视觉因子、激光雷达因子和IMU预积分因子不断添加到联合因子图中做非线性优化以及实现三维地图建模。

进一步地，在步骤S1中，获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征，具体过程包括以下步骤：

S11、采用ORB算法提取图像信息中的点特征；

S12、采用LSD算法提取图像信息中的线特征。

进一步地，在步骤S11中，采用ORB算法提取图像信息中的点特征，具体过程包括以下步骤：

S111、订阅相机当前帧的图像信息，任意选取图像信息中的一个像素点P，并判断该像素点是否为所需的特征点；

S112、以像素点P为圆心，设定半径得到离散化的Bresenhan圆；

S113、以Bresenhan圆上像素点的像素值与圆心做差，如果有连续N个点像素值都大于I_P+s或小于I_p-s，则把这个圆点作为一个候选特征点；

S114、对图像信息中的每一个像素点都进行步骤S112-步骤S113得到若干候选特征点；

S115、对若干个候选特征点添加尺度不变性和旋转不变性的描述得到提升特征点；

S116、以BRIEF描述子添加属性选取N个点对；

S117、定义操作S；

S118、输入选取的点对进行S操作，并将得到的结果进行组合，最后输出点特征。

进一步地，在步骤S12中，采用LSD算法提取图像信息中的线特征，具体过程包括以下步骤：

S121、订阅相机当前帧的图像信息，对图像信息中的图像以比例为0.8进行一次高斯降采样以缩小图像；

S122、计算像素点在图像中(x,y)位置的灰度值，并得到该像素点在X轴、Y轴的梯度幅值G(x,y)和梯度方向LLA；

S123、依据梯度幅值G(x,y)的大小进行梯度排序，设定阈值k，

像素点的梯度幅值G(x,y)若小于阈值k则不会被处理，并由若干像素点构成线段支持域区域和梯度方向；

像素点的梯度幅值G(x,y)若大于阈值k则为孤立像素点，并进入步骤S124；

S124、计算孤立像素点的梯度方向与线段支持域的梯度方向容忍值是否小于阈值

小于阈值

则纳入线段支持域并更新；

S125、将更新后的线段支持域进行矩形化得到矩形区域；

S126、通过像素点密度值f对生成的每个矩形区域进行判断，最后作为线特征输出。

进一步地，在步骤S2中，具体过程包括以下步骤：

S21、判断当前帧图像中点特征的数量；

若当前帧图像的点特征数量少于10个，直接设为跟踪失败；

若跟踪点特征的数量满大于10个，则直接进行LK光流跟踪；

S22、判断线特征是否为跟踪成功；

S23、最后基于RANSAC的几何约束保留最佳的点和线。

进一步地，在步骤S3中，具体过程包括以下步骤：

S31、从点云信息中提取某一个点的前后各若干个点并计算其相应的曲率；

S32、将每一个点的曲率根据阈值

判断是否为边特征；

若曲率小于阈值

则为边特征；

若曲率大于阈值

则为面特征；

S33、遍历点云信息中的所有点，根据边特征和面特征分别建立边特征集合B_i和面特征M_i集合。

进一步地，在步骤S5中，具体过程包括以下步骤：

S51、利用IMU测量的原始陀螺仪和加速度计信息，在两个连续帧之间进行预积分得到IMU测量值即为先验尺度信息；

S52、使用IMU测量值进行位姿的估计和不断更新。

进一步地，在步骤S6中，具体过程包括以下步骤：

S61、将视觉特征点和点云点投影到以相机为球心的单位球上，用极坐标形式表示点的位置，进行下采样；

S62、采用kd-tree算法搜索每个视觉特征点附近的三个临近点云点，以这三个点云点确定的平面与相机光心的距离作为最终的深度值。

借由上述技术方案，本发明提供了一种应用于矿井巷道的多模态融合建图与定位方法，至少具备以下有益效果：

1、本发明融合了两种相对独立且精准度较高的定位方法，实现了对矿井巷道下进行高鲁棒性建图与定位，同时考虑了精度和计算效率，并通过提高图像帧之间的匹配精度，大大提高了定位精度。

2、本发明达到了能够做到高精度实时定位和建图，并且时间上和功耗上均衡的目的，在环境感知性差的前提下也能够实现高精准定位，从而提高了矿井开采的安全性。

附图说明

此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明多模态融合建图与定位方法的流程图；

图2为本发明中提取点特征所涉及的视觉SLAM模块的原理框图；

图3为本发明中提取线特征所涉及的激光SLAM模块的原理框图；

图4为本发明中非线性优化所涉及的融合SLAM模块的原理框图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。借此对本申请如何应用技术手段来解决技术问题并达成技术功效的实现过程能充分理解并据以实施。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

请参照图1-图4，示出了本实施例的一种具体实施方式，本实施例融合了两种相对独立且精准度较高的定位方法，实现了对矿井巷道下进行高鲁棒性建图与定位，同时考虑了精度和计算效率。

请参照图1，本实施例提出了一种应用于矿井巷道的多模态融合建图与定位方法，该方法包括以下步骤：

S1、获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征；

在步骤S1中，获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征，具体过程包括以下步骤：

S11、采用ORB算法提取图像信息中的点特征；

请参照图2，在步骤S11中，采用ORB算法提取图像信息中的点特征，具体过程包括以下步骤：

S111、订阅相机当前帧的图像信息，任意选取图像信息中的一个像素点P，并判断该像素点是否为所需的特征点；

S112、以像素点P为圆心，设定半径得到离散化的Bresenhan圆；

具体的，选择的像素点为P，其像素值为I_P，以像素点P为圆心，半径为3，能得到离散化的Bresenhan圆，这个圆的边界上有16个像素点，分别记为p₁,p₂……p₁₆。设定一个阈值s，例如：阈值s为像素值I_P的20％。

S113、以Bresenhan圆上像素点的像素值与圆心做差，如果有连续N个点像素值都大于I_P+s或小于I_p-s，则把这个圆点作为一个候选特征点，N的取值为12；

S114、对图像信息中的每一个像素点都进行步骤S112-步骤S113得到若干候选特征点；

S115、对若干个候选特征点添加尺度不变性和旋转不变性的描述得到提升特征点；

具体的，尺度不变性由构建图像金字塔，并在金字塔的每一层上检测特征点和记录在图像金字塔的第几层来实现，当进行图像特征匹配时，必须规定同一尺度下进行匹配操作。旋转不变性由灰度质心法实现，使用灰度质心法来规定方向。

首先在Bresenhan圆内提取特征点后，采用：

定义图像块A的矩m_pq为：

上式中，x,y表示像素点坐标，I_(x,y)表示此像素点坐标的灰度值。

依据图像块A的矩m_pq定义图像块A的质心C。

上式中，m₁₀表示p＝1，q＝0时的矩、m₀₀表示p＝0，q＝0时的矩、m₀₁表示p＝0，q＝1时的矩。

通过图像块A的几何中心O和它的质心C连接在一起得到方向向量

根据方向向量

可以定义特征点的方向θ为：

S116、以BRIEF描述子添加属性选取N个点对；

具体的，在得到提升特征点后，以BRIEF描述子添加属性，核心步骤是以关键点的像素点P为圆心，以d为半径做圆O，在圆O内选取N个点对，N一般取值为4，定义为P_i(a，b)；(i＝1,2…，N)，其中，P_i(a，b)表示第i个点对对应的坐标(a，b)。

S117、定义操作S；

定义操作S采用下述公式进行，具体为：

上式中，I_a表示像素点在横坐标轴上值大小，I_b表示像素点在竖坐标轴上值大小。

S118、输入选取的点对进行S操作，并将得到的结果进行组合，最后输出点特征。

S12、采用LSD算法提取图像信息中的线特征。

请参照图3，在步骤S12中，采用LSD算法提取图像信息中的线特征，具体过程包括以下步骤：

S121、订阅相机当前帧的图像信息，对图像信息中的图像以比例为0.8进行一次高斯降采样以缩小图像；

S122、计算像素点在图像中(x,y)位置的灰度值，并得到该像素点在X轴、Y轴的梯度幅值G(x,y(和梯度方向LLA；

根据灰度值，那么该像素点在X轴、Y轴的像素梯度分别为：

上式中，x,y表示像素点在图像中的坐标。

同时，根据像素梯度可以得到梯度幅值G(x,y)和梯度方向LLA：

S123、依据梯度幅值G(x,y)的大小进行梯度排序，设定阈值k，

像素点的梯度幅值G(x,y)若小于阈值k则不会被处理，并由若干像素点构成线段支持域区域和梯度方向；

像素点的梯度幅值G(x,y)若大于阈值k则为孤立像素点，并进入步骤S124；

S124、计算孤立像素点的梯度方向与线段支持域的梯度方向容忍值是否小于阈值

小于阈值

则纳入线段支持域并更新；

其中阈值

定义为：

上式中，LLA为梯度方向，j为常数。

S125、将更新后的线段支持域进行矩形化得到矩形区域；

对区域内进行矩形近似计算，计算结果如下：

上式中，l_x，l_y为矩形的中心点坐标，G(j)为像素点j的梯度幅值。

线段支持域矩形化后矩形区域的主方向为：

其中：

S126、通过像素点密度值f对生成的每个矩形区域进行判断，最后作为线特征输出。

具体的，满足像素梯度幅值的像素点数量越多，该矩形越有可能是一条“线段”。

在面对矿井巷道等狭窄区域，我们发现有很多的信息可以使用，单模态的SLAM通过简单的提取特征点信息和利用稀疏点云信息达不到高精度的建图与定位，利用矿井下的无人矿车轨道线信息提取线特征，来提高图像帧之间的匹配精度，大大提高了定位精度。

S2、根据当前帧的图像信息采用KLT算法和KnnMatch算法分别跟踪当前滑动窗口关键帧的点特征和线特征得到新的视觉点线特征信息；

在步骤S2中，具体过程包括以下步骤：

S21、判断当前帧图像中点特征的数量；

若当前帧图像的点特征数量少于10个，直接设为跟踪失败；

若跟踪点特征的数量满大于10个，则直接进行LK光流跟踪；

具体的，一但检测到点特征，使用KLT算法中的LK光流法来跟踪抓取点特征，判断当前帧图像中点特征的数量，若当前帧图像中的点特征少于10个，直接设为跟踪失败；若跟踪点特征的数量满足预设阈值，则直接进行LK光流跟踪。

S22、判断线特征是否为跟踪成功；

具体的，一但检测到线特征，使用KnnMatch算法跟踪抓取直线特征，根据前后帧的线端点的移动距离判断是否为跟踪成功，由跟踪算法计算出的汉明距离需要大于30，通过相对于图像帧X轴的斜率来跟踪识别新线为水平线还是其他角度线。

S23、最后基于RANSAC的几何约束保留最佳的点和线。

S3、获取激光雷达当前激光帧的点云信息，计算点云信息中每个点的曲率，并根据每个点的曲率划分出边特征和面特征；

在步骤S3中，具体过程包括以下步骤：

S31、从点云信息中提取某一个点的前后各若干个点并计算其相应的曲率；

S32、将每一个点的曲率根据阈值

判断是否为边特征；

若曲率小于阈值

则为边特征；

若曲率大于阈值

则为面特征；

具体的，某一个点的前后各若干个点以参数5为例，取一个点的前五个点以及后五个点，共计十个点并计算每个点的曲率，每个点相应的曲率的为距离差值平方，之后定义阈值

(默认0.1)，大于阈值则为边特征，反之为面特征。

S33、遍历点云信息中的所有点，根据边特征和面特征分别建立边特征集合B_i和面特征M_i集合；

建立边特征集合B_i和面特征M_i集合，则体素地图T_i为：

其中：

S4、获取激光雷达下一激光帧的点云信息，并采用NDT和ICP的两阶段扫描匹配算法对下一激光帧的点云信息与当前激光帧的点云信息匹配，得到激光边面特征信息；

具体的，当获得新的雷达帧

和M_i，也就是下一激光帧的点云信息，使用NDT和ICP的两阶段扫描匹配算法进行匹配，首先输入相邻帧点云P_i、Q_j，使用预处理之后的点云数据利用NDT算法进行点云匹配，对位姿进行粗估计，得到一个好的初始估计和粗略的点云配准矩阵H，然后在良好的点云位姿状态下使用ICP算法利用点云配准矩阵H进行点云精确匹配。

相比较于基于地面的大规模点云信息，采用简单快速的点云匹配方法就能够达到效果，但是在矿井下能利用到的点云信息较少，采用提出的二阶段点云匹配方法能更好的匹配点云，大大提高建图与定位效果。

S5、依据IMU提供的先验尺度信息不断更新当前状态的位姿；

在步骤S5中，具体过程包括以下步骤：

S51、利用IMU测量的原始陀螺仪和加速度计信息，在两个连续帧之间进行预积分得到IMU测量值即为先验尺度信息；

IMU测量的原始陀螺仪和加速度计信息即为，IMU的加速度和角速度定义为：

式中，

和

是IMU在时刻t和B坐标系下的测量值。b_t是IMU偏置和n_t是白噪声，

是从坐标系W到B的旋转矩阵，g是重力向量。

S52、使用IMU测量值进行位姿的估计和不断更新。

由此，可以获得时刻i和时刻j之间的预积分测量值：

其中，Δv_ij表示i和j时刻的速度差，Δp_ij表示i和j时刻的位置差，ΔR_ij表示i和j时刻的姿态差。

S6、在更新后的位姿下，将最新的视觉点线特征信息与激光边面特征信息建立深度关联；

在步骤S6中，具体过程包括以下步骤：

S61、将视觉特征点和点云点投影到以相机为球心的单位球上，用极坐标形式表示点的位置，进行下采样；

S62、采用kd-tree算法搜索每个视觉特征点附近的三个临近点云点，以这三个点云点确定的平面与相机光心的距离作为最终的深度值。

其中，最新的视觉点线特征信息包括视觉特征点，为最新的视觉点线特征信息中的一个特征，同样的，激光边面特征信息包括点云点，为激光边面特征信息中的一个点。

需要说明的是，在步骤S6中，以最终的深度值建立最新的视觉点线特征信息与激光边面特征信息之间的深度关联，通过建立深度关联后的当前两帧图像中，从其中的新的视觉点线特征信息中得到视觉因子；从步骤S4的扫描匹配所得到的激光边面特征信息中得到出激光雷达因子，其中将当前的激光雷达关键帧匹配到一个全局特征图；而IMU预积分得到的尺度信息构成IMU预积分因子，此过程的视觉因子、激光雷达因子和IMU预积分因子皆为通过现有技术的本领域常规手段即可获得，因此不再详细赘述。

S7、将视觉因子、激光雷达因子和IMU预积分因子不断添加到联合因子图中做非线性优化以及实现三维地图建模。

请参照图4，具体的，将视觉因子、激光雷达因子和IMU预积分因子实时添加到联合因子图中，构建残差得到雅可比矩阵，使用高斯牛顿法求解状态量；基于ROS1底层通信机制，在RVIZ上实现三维地图建模。

基于每个详细模块都可集成到工控机中，调配相机的图像帧信息、激光雷达的点云信息和IMU的加速度与角速度信息，结合所提出的鲁棒性强的算法，能适应矿井巷道的复杂环境建图和定位。

以上实施方式对本发明进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种应用于矿井巷道的多模态融合建图与定位方法，其特征在于，包括以下步骤：

S1、获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征；

S2、根据当前帧的图像信息采用KLT算法和KnnMatch算法分别跟踪当前滑动窗口关键帧的点特征和线特征得到新的视觉点线特征信息；

S3、获取激光雷达当前激光帧的点云信息，计算点云信息中每个点的曲率，并根据每个点的曲率划分出边特征和面特征；

S4、获取激光雷达下一激光帧的点云信息，并采用NDT和ICP的两阶段扫描匹配算法对下一激光帧的点云信息与当前激光帧的点云信息匹配，得到激光边面特征信息；

S5、依据IMU提供的先验尺度信息不断更新当前状态的位姿；

S6、在更新后的位姿下，将最新的视觉点线特征信息与激光边面特征信息建立深度关联；

S7、将视觉因子、激光雷达因子和IMU预积分因子不断添加到联合因子图中做非线性优化以及实现三维地图建模。

2.根据权利要求1所述的多模态融合建图与定位方法，其特征在于：在步骤S1中，获取相机当前帧的图像信息，并从当前帧的图像信息中提取点特征和线特征，具体过程包括以下步骤：

S11、采用ORB算法提取图像信息中的点特征；

S12、采用LSD算法提取图像信息中的线特征。

3.根据权利要求2所述的多模态融合建图与定位方法，其特征在于：在步骤S11中，采用ORB算法提取图像信息中的点特征，具体过程包括以下步骤：

S111、订阅相机当前帧的图像信息，任意选取图像信息中的一个像素点P，并判断该像素点是否为所需的特征点；

S112、以像素点P为圆心，设定半径得到离散化的Bresenhan圆；

S113、以Bresenhan圆上像素点的像素值与圆心做差，如果有连续N个点像素值都大于I_P+s或小于I_p-s，则把这个圆点作为一个候选特征点；

S114、对图像信息中的每一个像素点都进行步骤S112-步骤S113得到若干候选特征点；

S115、对若干个候选特征点添加尺度不变性和旋转不变性的描述得到提升特征点；

S116、以BRIEF描述子添加属性选取N个点对；

S117、定义操作S；

S118、输入选取的点对进行S操作，并将得到的结果进行组合，最后输出点特征。

4.根据权利要求2所述的多模态融合建图与定位方法，其特征在于：在步骤S12中，采用LSD算法提取图像信息中的线特征，具体过程包括以下步骤：

S121、订阅相机当前帧的图像信息，对图像信息中的图像以比例为0.8进行一次高斯降采样以缩小图像；

S122、计算像素点在图像中(x,y)位置的灰度值，并得到该像素点在X轴、Y轴的梯度幅值G(x,y)和梯度方向LLA；

S123、依据梯度幅值G(x,y)的大小进行梯度排序，设定阈值k，

像素点的梯度幅值G(x,y)若小于阈值k则不会被处理，并由若干像素点构成线段支持域区域和梯度方向；

像素点的梯度幅值G(x,y)若大于阈值k则为孤立像素点，并进入步骤S124；

S124、计算孤立像素点的梯度方向与线段支持域的梯度方向容忍值是否小于阈值

小于阈值

则纳入线段支持域并更新；

S125、将更新后的线段支持域进行矩形化得到矩形区域；

S126、通过像素点密度值f对生成的每个矩形区域进行判断，最后作为线特征输出。

5.根据权利要求1所述的多模态融合建图与定位方法，其特征在于：在步骤S2中，具体过程包括以下步骤：

S21、判断当前帧图像中点特征的数量；

若当前帧图像的点特征数量少于10个，直接设为跟踪失败；

若跟踪点特征的数量满大于10个，则直接进行LK光流跟踪；

S22、判断线特征是否为跟踪成功；

S23、最后基于RANSAC的几何约束保留最佳的点和线。

6.根据权利要求1所述的多模态融合建图与定位方法，其特征在于：在步骤S3中，具体过程包括以下步骤：

S31、从点云信息中提取某一个点的前后各若干个点并计算其相应的曲率；

S32、将每一个点的曲率根据阈值

判断是否为边特征；

若曲率小于阈值

则为边特征；

若曲率大于阈值

则为面特征；

S33、遍历点云信息中的所有点，根据边特征和面特征分别建立边特征集合B_i和面特征M_i集合。

7.根据权利要求1所述的多模态融合建图与定位方法，其特征在于：在步骤S5中，具体过程包括以下步骤：

S51、利用IMU测量的原始陀螺仪和加速度计信息，在两个连续帧之间进行预积分得到IMU测量值即为先验尺度信息；

S52、使用IMU测量值进行位姿的估计和不断更新。

8.根据权利要求1所述的多模态融合建图与定位方法，其特征在于：在步骤S6中，具体过程包括以下步骤：

S61、将视觉特征点和点云点投影到以相机为球心的单位球上，用极坐标形式表示点的位置，进行下采样；

S62、采用kd-tree算法搜索每个视觉特征点附近的三个临近点云点，以这三个点云点确定的平面与相机光心的距离作为最终的深度值。

Images