CN115727851B - 一种煤矿井下掘进装备位姿检测系统及方法 - Google Patents

Abstract

本发明公开了一种煤矿井下掘进装备位姿检测系统及方法，涉及位姿检测技术领域。该系统包括：三激光指向仪模块、数据采集模块和位姿解算模块。其中，三激光指向仪模块位于待测掘进装备的后方巷道内，用于向待测掘进装备发射包括三条相互平行的激光线，且三条激光线呈三角形排列的组合激光；数据采集模块位于待测掘进装备的机身上，且与三激光指向仪模块相对，用于采集组合激光的特征图像；特征图像为三激光点图像或三激光点三激光线图像；位姿解算模块与数据采集模块连接，用于根据特征图像，计算待测掘进装备的位姿信息。本发明能够克服在高粉尘、低照度的工况环境下，巷道特征不明显甚至特征缺失的问题，提高位姿检测系统的稳定性和抗干扰能力。

Description

一种煤矿井下掘进装备位姿检测系统及方法

技术领域

本发明涉及位姿检测技术领域，特别是涉及一种煤矿井下掘进装备位姿检测系统及方法。

背景技术

掘进是煤矿生产的重要生产环节，煤矿巷道的快速掘进是煤矿保证矿井高产稳产的关键技术措施。随着综采技术的发展与少人化、无人化的需求，掘进装备位姿检测使巷道掘进成为了煤矿智能化应用、高效集约化生产的共性及关键性技术。

机器视觉位姿测量技术是利用空间几何投影约束条件建立位姿解算模型，从而实现对目标位姿的测量，其具有非接触测量、精度高、稳定性好等优点，已经在煤矿井下得到了初步应用。但是在煤矿井下巷道掘进中，高粉尘、低照度的工况环境下，巷道特征不明显甚至会出现特征缺失的情况，如果以巷道环境为特征，很难实现稳定的特征提取，也就很难实现稳定的特征匹配，采用常规方法难以实现定位。

发明内容

本发明的目的是提供一种煤矿井下掘进装备位姿检测系统及方法，以克服在高粉尘、低照度的工况环境下，巷道特征不明显甚至特征缺失的问题，提高位姿检测系统的稳定性和抗干扰能力。

为实现上述目的，本发明提供了如下方案：

一种煤矿井下掘进装备位姿检测系统，所述系统包括：

三激光指向仪模块，位于待测掘进装备的后方巷道内，用于向所述待测掘进装备发射组合激光；所述组合激光包括三条相互平行的激光线，且三条所述激光线呈三角形排列；

数据采集模块，位于所述待测掘进装备的机身上，且与所述三激光指向仪模块相对，用于采集所述组合激光的特征图像；所述特征图像为三激光点图像或三激光点三激光线图像；

位姿解算模块，与所述数据采集模块连接，用于根据所述特征图像，计算所述待测掘进装备的位姿信息；所述位姿信息包括：所述待测掘进装备的机身相对于所在巷道的俯仰角、偏航角、横滚角以及所述待测掘进装备的机身距离所在巷道中线的水平、竖直、前后偏移距离。

可选地，所述三激光指向仪模块包括：

激光指向仪支架，位于所述待测掘进装备的后方巷道内；

三个激光指向仪，分别位于所述激光指向仪支架上，用于向所述待测掘进装备发射组合激光；三个所述激光指向仪两两相互平行，且三个所述激光指向仪呈三角形分布。

可选地，所述数据采集模块包括：

相机防爆壳，固定于所述待测掘进装备的机身上；

工业相机，固定于所述相机防爆壳内，且镜头朝向所述三激光指向仪模块，用于采集所述组合激光的特征图像。

可选地，所述位姿解算模块包括：

图像特征判断单元，与所述数据采集模块连接，用于获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像；

激光点特征提取单元，与所述图像特征判断单元连接，用于当所述特征图像为三激光点图像时，提取所述特征图像中的三个激光点，并确定各所述激光点的位置坐标；

激光线特征提取单元，与所述图像特征判断单元连接，用于当所述特征图像为三激光点三激光线图像时，提取所述特征图像中的三条激光线，并确定各所述激光线的位置坐标和相应的激光点的位置坐标；所述激光点的个数为三个，且三个所述激光点分别对应三条所述激光线的起点；

三点模型计算单元，与所述激光点特征提取单元连接，用于根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；

三点三线模型计算单元，与所述激光线特征提取单元连接，用于根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；

位姿解算单元，分别与所述三点模型计算单元和所述三点三线模型计算单元连接，用于根据所述待测掘进装备的位置坐标计算所述待测掘进装备的位姿信息。

可选地，所述系统还包括：

位姿可视化模块，与所述位姿解算模块连接，用于显示所述位姿信息。

本发明还提供一种煤矿井下掘进装备位姿检测方法，所述方法应用于上述系统，所述方法包括：

获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像；所述特征图像是由三激光指向仪模块发射的组合激光被数据采集模块采集得到的；

当所述特征图像为三激光点图像时，提取所述特征图像中的三个激光点，并确定各所述激光点的位置坐标；根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；

当所述特征图像为三激光点三激光线图像时，提取所述特征图像中的三条激光线，并确定各所述激光线的位置坐标和相应的激光点的位置坐标；所述激光点的个数为三个，且三个所述激光点分别对应三条所述激光线的起点；根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；

根据所述待测掘进装备的位置坐标计算所述待测掘进装备的位姿信息。

可选地，所述提取所述特征图像中的三个激光点，具体包括：

对所述特征图像进行畸变矫正、高斯滤波、形态学梯度转换、RGB通道分离和灰度转换，得到第一二值图像；

对所述第一二值图像进行闭运算，并确定所述第一二值图像中的各区域；

选择所述第一二值图像中满足第一设定条件的区域进行椭圆拟合，并去除杂光，得到多个椭圆光斑；所述第一设定条件为：区域边界点的个数在第一设定范围内；

判断所述椭圆光斑的个数是否等于三个；若是，则将三个所述椭圆光斑作为所述特征图像中的三个激光点；若否，则返回“获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像”的步骤。

可选地，所述提取所述特征图像中的三条激光线，具体包括：

对所述特征图像进行畸变矫正、高斯滤波和灰度转换，得到第二二值图像；

对所述第二二值图像进行闭运算，并确定所述第二二值图像中的各区域；

确定所述第二二值图像中满足第二设定条件的区域的最小外接矩形，并对满足第三设定条件的最小外接矩形进行矩形合并和轮廓点直线拟合，得到所述特征图像中的三条激光线；所述第二设定条件为：区域边缘点的个数在第二设定范围内；所述第三设定条件为：最小外接矩形的高宽比范围在第三设定范围内，且最小外接矩形的面积在第四设定范围内。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明通过设置三激光指向仪模块向待测掘进装备发射包括三条相互平行，且呈三角形排列的组合激光，并利用数据采集模块实时采集组合激光的特征图像，能够在高粉尘、低照度的工况环境下采集得到特征明显的特征图像，从而利用位姿解算模块根据所采集的三激光点图像或三激光点三激光线图像计算待测掘进装备的位姿信息，提高了位姿检测系统的稳定性和抗干扰能力。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的煤矿井下掘进装备位姿检测系统的结构图；

图2为三激光指向仪模块的模型示意图；

图3为特征图像为三激光点图像的示意图；

图4为特征图像为三激光点三激光线图像的示意图；

图5为本发明提供的煤矿井下掘进装备位姿检测方法的流程图；

图6为本发明提供的位姿检测流程图；

图7为门形三线结构透视原理图；

图8为三点三线模型中a、b、c三点的位置关系示意图；

图9为本发明提供的三点模型图像处理流程图；

图10为三激光点视觉位姿测量方法示意图；

图11为三点定位模型投影示意图。

符号说明：

1-激光指向仪支架，2-激光指向仪，3-工控机，4-防爆控制箱，5-工业相机，6-相机防爆壳。

具体实施方式

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

本发明的目的是提供一种煤矿井下掘进装备位姿检测系统及方法，以克服在高粉尘、低照度的工况环境下，巷道特征不明显甚至特征缺失的问题，提高位姿检测系统的稳定性和抗干扰能力。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1为本发明提供的煤矿井下掘进装备位姿检测系统的结构图，如图1所示，本发明提供的煤矿井下掘进装备位姿检测系统包括：三激光指向仪模块、数据采集模块和位姿解算模块。

其中，所述三激光指向仪模块，位于待测掘进装备的后方巷道内，用于向所述待测掘进装备发射组合激光；所述组合激光包括三条相互平行的激光线，且三条所述激光线呈三角形排列(如图1中的三条相互平行的直线所示)。所述数据采集模块，位于所述待测掘进装备的机身上，且与所述三激光指向仪模块相对，用于采集所述组合激光的特征图像；所述特征图像为三激光点图像或三激光点三激光线图像。所述位姿解算模块，与所述数据采集模块连接，用于根据所述特征图像，计算所述待测掘进装备的位姿信息；所述位姿信息包括：所述待测掘进装备的机身相对于所在巷道的俯仰角、偏航角、横滚角以及所述待测掘进装备的机身距离所在巷道中线的水平、竖直、前后偏移距离。

进一步地，所述系统还包括：位姿可视化模块；所述位姿可视化模块，与所述位姿解算模块连接，用于显示所述位姿信息。所述位姿可视化模块既可以安装于掘进装备机身，也可以安装于掘进装备机身以外，位姿可视化模块能够以各种形式(如数据、图形、波形图等形式)动态化实时显示掘进装备位姿信息，从而便于掘进装备的掘进、远程、智能控制与纠偏。

在本实施例中，所述三激光指向仪模块包括：激光指向仪支架1和三个激光指向仪2。其中，所述激光指向仪支架1，位于所述待测掘进装备的后方巷道内；三个所述激光指向仪2，分别位于所述激光指向仪支架1上，用于向所述待测掘进装备发射组合激光；三个所述激光指向仪2两两相互平行，且三个所述激光指向仪2呈三角形分布。图2为三激光指向仪模块的模型示意图。如图2所示，三个所述激光指向仪2分别位于所述激光指向仪支架1上的a点、b点和c点处，三个所述激光指向仪2两两之间的连线构成一个三角形。所述激光指向仪2优选为矿用防爆型激光指向仪，其成像特征作为数据采集模块的输入，用于为位姿解算提供数据来源。

所述数据采集模块包括：相机防爆壳6和工业相机5。其中，所述相机防爆壳6，固定于所述待测掘进装备的机身上；所述工业相机5，固定于所述相机防爆壳6内，且镜头朝向所述三激光指向仪模块，用于采集所述组合激光的特征图像。所述工业相机5优选为防爆工业相机。具体地，所述相机防爆壳6安装于掘进装备机身后部位置，所述工业相机5固定于相机防爆壳6内，所述工业相机的镜头朝向巷道后方，在掘进装备运行过程中，工业相机用于实时采集三激光指向仪图像信息。进一步地，所述相机防爆壳6通过螺栓固定在掘进机平台，相机口朝向后方，所述工业相机5安装在相机防爆壳6内部。在实际应用中，所述数据采集模块还包括通讯线。

所述位姿解算模块以工业计算机为计算核心，且优选为工控机3。所述工控机3安装于待测掘进装备的机身位置，可实时对所述数据采集模块中防爆工业相机采集的特征图像进行处理与位姿解算，从而为位姿可视化模块提供数据。为了保护所述工控机3不受外界环境(如巷道内落石)的伤害，所述系统还包括防爆控制箱4；所述防爆控制箱4固定于所述待测掘进装备的机身位置，所述工控机3安装于所述防爆控制箱4内。

进一步地，所述位姿解算模块包括：图像特征判断单元、激光点特征提取单元、激光线特征提取单元、三点模型计算单元、三点三线模型计算单元和位姿解算单元。

所述图像特征判断单元，与所述数据采集模块连接，用于获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像。此外，还可以根据巷道内的粉尘情况预测成像特征，当无粉尘或粉尘浓度较低时，特征图像为三激光点图像，当粉尘浓度较高时，特征图像为三激光点三激光线图像。

所述激光点特征提取单元，与所述图像特征判断单元连接，用于当所述特征图像为三激光点图像时，提取所述特征图像中的三个激光点，并确定各所述激光点的位置坐标。所述三点模型计算单元，与所述激光点特征提取单元连接，用于根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标。

所述激光线特征提取单元，与所述图像特征判断单元连接，用于当所述特征图像为三激光点三激光线图像时，提取所述特征图像中的三条激光线，并确定各所述激光线的位置坐标和相应的激光点的位置坐标；所述激光点的个数为三个，且三个所述激光点分别对应三条所述激光线的起点。所述三点三线模型计算单元，与所述激光线特征提取单元连接，用于根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标。

所述位姿解算单元，分别与所述三点模型计算单元和所述三点三线模型计算单元连接，用于根据所述待测掘进装备的位置坐标计算所述待测掘进装备的位姿信息。

在具体应用中，如图1所示，所述激光指向仪支架1安装在掘进面的巷道顶端，并与煤巷顶部锚索采用三个螺母固定，安装可靠、拆卸方便。所述激光指向仪支架2用来安装激光指向仪2。所述激光指向仪2的安装位置呈倒三角形，且所述激光指向仪2的前端面保持在同一平面内，该平面与地面保持垂直，所述激光指向仪2安装完成后互相平行，从而保证激光点发射之初形成的三角形边长与激光照射一段距离后在空中某一平面上形成的三角形边长之间的误差不超过5厘米。

将三个矿用防爆型激光指向仪及其支架安装于掘进装备后方煤巷顶部，通过固定在掘进装备机身上的防爆工业相机实时采集三个矿用防爆型激光指向仪的成像信息；当粉尘较少时，三个矿用防爆型激光指向仪成像特征为三个点，即特征图像为三激光点图像，如图3所示，故利用三个激光点进行位姿解算；当粉尘浓度较大时，三个矿用防爆型激光指向仪成像特征为由三个激光点发射而形成的三条线，即特征图像为三激光点三激光线图像，如图4所示，故利用三个点三个线进行位姿解算；最终得到掘进装备的掘进过程中的俯仰角、偏航角、横滚角以及机身相对于巷道中线的水平、竖直、前后偏移距离。

本发明还提供一种煤矿井下掘进装备位姿检测方法，所述方法应用于上述系统，图5为本发明提供的煤矿井下掘进装备位姿检测方法的流程图，图6为本发明提供的位姿检测流程图。如图5及图6所示，所述方法包括：

步骤S1：获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像；所述特征图像是由三激光指向仪模块发射的组合激光被数据采集模块采集得到的。

步骤S2：当所述特征图像为三激光点图像时，提取所述特征图像中的三个激光点，并确定各所述激光点的位置坐标；根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标。

步骤S3：当所述特征图像为三激光点三激光线图像时，提取所述特征图像中的三条激光线，并确定各所述激光线的位置坐标和相应的激光点的位置坐标；所述激光点的个数为三个，且三个所述激光点分别对应三条所述激光线的起点；根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标。

步骤S4：根据所述待测掘进装备的位置坐标计算所述待测掘进装备的位姿信息。其中，由于激光指向仪相对巷道具有三维坐标信息，激光指向仪相对工业相机具有三维坐标信息，工业相机相对掘进装备机身具有三维坐标信息。因此，可根据待测掘进装备的位置坐标计算得出掘进装备机身相对于巷道的俯仰角、偏航角、横滚角以及距离巷道中线的水平、竖直、前后偏移距离。

本发明的具体实施步骤如下：

步骤一：将掘进机(即待测掘进装备)设置在掘进巷道中，在掘进机机身后部安装三激光指向仪，所述三激光指向仪安装在巷道顶部，距离掘进机后部安装的工业相机不远处；掘进机机身后部安装防爆相机外壳，内部放置工业相机。

步骤二：利用防爆工业相机采集图像信息，进行相机标定，确定标定参数；所述标定参数包括所述相机(即工业相机)的内参数，相机的所述内参数包括相机的焦距、主像素点坐标及图像畸变系数。

步骤三：在掘进过程中，通过掘进机机身上安装所述位姿解算模块内部的工业计算机处理工业相机实时采集的图像信息(即特征图像)，利用三个激光点模型或三个激光点和三个激光线模型实时解算掘进装备位姿。

步骤四：在掘进过程中，所述位姿解算模块中通过巷道坐标系下特征点的坐标、相机坐标系下特征点的坐标和机身坐标系下的相机坐标，将机身信息转换到巷道坐标系下求得掘进装备位姿。具体地，当所述特征图像为三激光点图像时，所述特征点为三个所述激光点；当所述特征图像为三激光点三激光线图像时，所述特征点为两个激光点和一个透视原理所得点。

步骤五：在掘进过程中，所述位姿可视化模块用于实时显示位姿解算模块的掘进装备位姿信息，便于掘进装备远程、智能控制与纠偏。

下面分别对上述各步骤分别进行详细论述。

步骤S3中，所述提取所述特征图像中的三条激光线，具体包括：

步骤S3.1：对所述特征图像进行畸变矫正、高斯滤波和灰度转换，得到第二二值图像。

步骤S3.2：对所述第二二值图像进行闭运算，并确定所述第二二值图像中的各区域。

步骤S3.3：确定所述第二二值图像中满足第二设定条件的区域的最小外接矩形，并对满足第三设定条件的最小外接矩形进行矩形合并和轮廓点直线拟合，得到所述特征图像中的三条激光线；所述第二设定条件为：区域边缘点的个数在第二设定范围内；所述第三设定条件为：最小外接矩形的高宽比范围在第三设定范围内，且最小外接矩形的面积在第四设定范围内。

作为一种具体的实施方式，当所述特征图像为三激光点三激光线图像时，所采用的图像处理方法如下：

(1)利用上述的焦距、主像素点坐标及图像畸变系数对原始图像进行畸变矫正，并输出矫正后的图像。

(2)对于矫正后的图像进行高斯滤波，消除高斯噪声。

(3)对高斯滤波后的图像进行灰度转换，输出二值化后的图像(即第二二值图像)。其中，二值化阈值为经验值。

(4)检测二值图像中的所有轮廓，所述检测方法为：对二值化的图像进行闭运算，再求取各区域的边缘，对于区域边缘点数量在(a，b)区间的区域找出最小外接矩形，根据最小外接矩形的高宽比范围(c，d)和最小外接矩形的面积范围[e，f]判断，满足条件的直线轮廓。所述高宽比范围(c，d)和面积范围[e，f]为经验值。记录每一个轮廓点的坐标；第一次筛选满足要求的区域轮廓，解决线上比较多的小区域；第二次进行矩形合并，将区域外接矩形角度相似的进行合并；最后一次筛选满足线性比例的聚合后的大区域，为直线拟合提供依据。

(5)将轮廓点通过最小二乘拟合处理成为一条直线。以上述方法依次拟合其他两条直线。分别获取三条直线的斜率和截距，将其代入到三点三线模型中，根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标。

所述根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标，具体包括：根据各所述激光线和各所述激光点的位置坐标，确定至少一个门形三线结构；所述门形三线结构由任意两个所述激光点的连线与相应的两条激光线构成，三个所述激光点的连线与三条所述激光线共同构成三个门形三线结构；基于门形三线结构透视原理，对所述门形三线结构进行求解，得到所述待测掘进装备的位置坐标。

此外，当粉尘浓度较大，成像特征为三点三线(即特征图像为三激光点三激光线图像)，此时如果有物体遮挡招致有一个点和一条线被遮挡时，基于门形三线结构透视原理，利用两点两线也可以进行位姿解算。

在具体应用中，首先以两点两线的掘进机位姿解算模型为基础，对激光点及其产生的线进行提取，将图像处理得到的直线的斜率和截距代入模型中，得到掘进装备的位置坐标，然后通过坐标转换，进而得到机身在巷道中的位姿。

图7为门形三线结构透视原理图，如图7所示，根据门形结构的三线透视特点可知，当两平行直线与相机图像平面不平行时，相机坐标系与激光测量坐标系之间位姿解算存在唯一解。由于两点两线所得的精度不高，有待提高，因此本发明提出了一种三点三线的视觉测量模型，具体方法为添加一个新的激光，即由原来的两个激光束变为三个激光束，激光特征抽象出的由原本的两个光斑和两条线，变为三个光斑和三条线。

本发明所提出的三点三线模型基于门形结构的三线透视几何结构模型，不同的是该门形三线结构模型在本发明中计算了三次，分别为ac，ab，bc三种不同的点线组合方式，a、b、c三点的位置关系参见图8。

x_g＝t₁*x_ac+t₂*x_ab+t₃*x_bc (1)

y_g＝t₁*y_ac+t₂*y_ab+t₃*y_bc (2)

z_g＝t₁*z_ac+t₂*z_ab+t₃*z_bc (3)

1＝t₁+t₂+t₃ (4)

式中：x_g，y_g，z_g为组合定位后的坐标(即待测掘进装备的三维点坐标)，t为三种组合方式中某一种的加权值，其中，t₁为ac组合的加权值，t₂为ab三种组合的加权值，t₃为bc组合的加权值；x_ac，y_ac，z_ac为ac两点计算出的掘进装备位置坐标，x_ab，y_ab，z_ab为ab两点计算出的掘进装备位置坐标，x_bc，y_bc，z_bc为bc两点计算出的掘进装备位置坐标。基于门形三线结构计算其坐标的方式在此不做赘述。

步骤S2中，所述提取所述特征图像中的三个激光点，具体包括：

步骤S2.1：对所述特征图像进行畸变矫正、高斯滤波、形态学梯度转换、RGB通道分离和灰度转换，得到第一二值图像。

步骤S2.2：对所述第一二值图像进行闭运算，并确定所述第一二值图像中的各区域。

步骤S2.3：选择所述第一二值图像中满足第一设定条件的区域进行椭圆拟合，并去除杂光，得到多个椭圆光斑；所述第一设定条件为：区域边界点的个数在第一设定范围内。

步骤S2.4：判断所述椭圆光斑的个数是否等于三个；若是，则将三个所述椭圆光斑作为所述特征图像中的三个激光点；若否，则返回“获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像”的步骤。

作为一种具体的实施方式，当所述特征图像为三激光点图像时，所采用的图像处理方法参见图9，包括：

(1)利用标定得到的工业相机的内参数(即焦距、主像素点坐标及图像畸变系数)对图像进行畸变矫正，并输出矫正图像。

(2)对于矫正图像进行高斯滤波，滤波后再进行形态学梯度转换。

(3)对转换完成的图像进行R、G、B通道分离，分离出R通道图像，再求取图像最大灰度，根据最大灰度进行二值化阈值范围确定。

(4)经二值化后的区域边界可能含有未封闭的区域，需进行闭运算，然后再求取各区域的边界，若区域边界的个数大于3，则再判断是否满足边界点的个数大于a，且小于b，满足则求取区域的最小矩形，再根据最小矩形的高宽比，删除一些杂光。其中，边界点个数范围(a，b)为经验值。

(5)利用椭圆拟合对剩余区域进行拟合，并获取光斑中心坐标，根据椭圆光斑之间的距离范围[g，h]，去除部分杂光，再与上一时刻的图像进行对比，再去除部分杂光。

(6)判断点个数，若点的个数不等于3个，则重新采集图像，否则代入到掘进机三点定位模型中，根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标。

所述根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标，具体包括：根据各所述激光点的位置坐标确定位于所述待测掘进装备的机身上的工业相机的旋转矩阵及各所述激光点在相机坐标系中的方向测量向量；根据所述旋转矩阵及所述方向测量向量确定所述待测掘进装备的位置坐标。

图10为三激光点视觉位姿测量方法示意图。如图10所示，在具体应用中，三点定位模型给定激光光斑特征f_i＝1,2,3在参考坐标系中的位置^GP_i，以及特征点在相机坐标系中的方向测量向量^Cb_i，目标是估计相机的旋转矩阵和位置^Gp_C。其中{C}表示相机坐标系，其位置为p_C，方位为{G}表示参考坐标系。

根据几何关系，得出

式(5)中：^Gp_i表示三激光点在参考坐标系下的坐标，^Gp_C为相机的位置，^Cb_i为特征点在相机坐标系中的方向测量向量。d_i表示参考坐标系中相机到特征点的欧式距离，两两相减，并投影到对应面的法向量。

相机坐标具体计算如下：

图11为三点定位模型投影示意图。如图11所示，对于3个已知空间点P₁、P₂和P₃，其构成的三角形3个边长长度为A、B和C。在相机成像平面的成像点分别为P_c1、P_c2和P_c3，相机光轴中心点O与空间点P₁、P₂和P₃构成的单位向量为k₁、k₂和k₃。

将k₂和k₃之间的夹角设为α，将k₁和k₃之间的夹角设为β，将k₁和k₂之间的夹角设为γ，则有：

记O点与空间点P₁、P₂和P₃之间的距离分别为m₁、m₂和m₃。根据三角形余弦定理，可得式(8)。

为了简化计算，做如下等量代换

式(9)中，p表示m₁和m₃之间的比例关系，q表示m₂和m₃之间的比例关系。

可得式(10)和式(11)

式(11)中，a_j表示多项式的系数，j表示多项式的阶数。

其中，

由以上可求出p、q、m₁、m₂和m₃的值。各空间点在相机中的位置坐标为P_ci＝m_ik_i，i＝1,2,3。2D点的图像坐标已知，3个余弦角已知，3D点的坐标已知，只有x和y未知。由式(11)可知a_j已知。因此我们可以求得p和q的值。四次方程组理论上有四组解。设四个位姿解为(x_m，y_m)，m＝1,2,3,4，求取最小二范数。

如式(12)，最小值为相机(即掘进机机身)相对于标靶坐标系的位姿。

通过三激光点的掘进机机身定位，再根据激光指向仪坐标系与巷道坐标系之间的已知关系，从而得到机体坐标系与巷道坐标系之间的关系，便可求得掘进机在巷道坐标系下位姿。

本发明为了解决掘进或者检修过程中粉尘较大时无特征的问题，提供了一种基于三激光指向仪的煤矿井下掘进装备位姿检测系统及方法，与现有技术相比，本发明具有以下优势：

(1)系统完整稳定，可以初步应用于实际作业中。

(2)使用三组两点两线进行融合解算，使计算结果更加稳定准确。

(3)本发明融合三点与三点三线计算方法，实现自适应环境切换。所提供的三点模型计算方法适用煤矿井下粉尘浓度较小时，三点三线模型计算方法适用煤矿井下粉尘浓度较大时，具有较好的适用性。

(4)将基于激光指向仪的机器视觉应用于煤矿井下掘进装备位姿检测，具有成本低、操作灵活、抗干扰性强、测量精度高、系统稳定性好等特点，具备一定的市场推广价值。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。

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

Claims (7)

1.一种煤矿井下掘进装备位姿检测系统，其特征在于，所述系统包括：

三激光指向仪模块，位于待测掘进装备的后方巷道内，用于向所述待测掘进装备发射组合激光；所述组合激光包括三条相互平行的激光线，且三条所述激光线呈三角形排列；

数据采集模块，位于所述待测掘进装备的机身上，且与所述三激光指向仪模块相对，用于采集所述组合激光的特征图像；所述特征图像为三激光点图像或三激光点三激光线图像；

位姿解算模块，与所述数据采集模块连接，用于根据所述特征图像，计算所述待测掘进装备的位姿信息；所述位姿信息包括：所述待测掘进装备的机身相对于所在巷道的俯仰角、偏航角、横滚角以及所述待测掘进装备的机身距离所在巷道中线的水平、竖直、前后偏移距离；

所述位姿解算模块包括：图像特征判断单元，与所述数据采集模块连接，用于获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像；激光点特征提取单元，与所述图像特征判断单元连接，用于当所述特征图像为三激光点图像时，提取所述特征图像中的三个激光点，并确定各所述激光点的位置坐标；激光线特征提取单元，与所述图像特征判断单元连接，用于当所述特征图像为三激光点三激光线图像时，提取所述特征图像中的三条激光线，并确定各所述激光线的位置坐标和相应的激光点的位置坐标；所述激光点的个数为三个，且三个所述激光点分别对应三条所述激光线的起点；三点模型计算单元，与所述激光点特征提取单元连接，用于根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；三点三线模型计算单元，与所述激光线特征提取单元连接，用于根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；位姿解算单元，分别与所述三点模型计算单元和所述三点三线模型计算单元连接，用于根据所述待测掘进装备的位置坐标计算所述待测掘进装备的位姿信息；

所述根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标，具体包括：根据各所述激光线和各所述激光点的位置坐标，确定至少一个门形三线结构；所述门形三线结构由任意两个所述激光点的连线与相应的两条激光线构成，三个所述激光点的连线与三条所述激光线共同构成三个门形三线结构；基于门形三线结构透视原理，对所述门形三线结构进行求解，得到所述待测掘进装备的位置坐标；其中，三个门形三线结构分别采用ac、ab和bc三种点线组合方式，a、b和c分别为三个激光点；对三个门形三线结构分别进行求解，得到对应的掘进装备位置坐标并进行组合定位，得到组合定位后的坐标作为所述待测掘进装备的位置坐标；组合定位的具体公式为：

x_g＝t₁*x_ac+t₂*x_ab+t₃*x_bc；

y_g＝t₁*y_ac+t₂*y_ab+t₃*y_bc；

z_g＝t₁*z_ac+t₂*z_ab+t₃*z_bc；

1＝t₁+t₂+t₃；

式中：x_g、y_g和z_g为组合定位后的坐标，t₁为ac组合的加权值，t₂为ab组合的加权值，t₃为bc组合的加权值；x_ac、y_ac和z_ac为ac组合计算出的掘进装备位置坐标，x_ab、y_ab和z_ab为ab组合计算出的掘进装备位置坐标，x_bc、y_bc和z_bc为bc组合计算出的掘进装备位置坐标。

2.根据权利要求1所述的煤矿井下掘进装备位姿检测系统，其特征在于，所述三激光指向仪模块包括：

激光指向仪支架，位于所述待测掘进装备的后方巷道内；

三个激光指向仪，分别位于所述激光指向仪支架上，用于向所述待测掘进装备发射组合激光；三个所述激光指向仪两两相互平行，且三个所述激光指向仪呈三角形分布。

3.根据权利要求1所述的煤矿井下掘进装备位姿检测系统，其特征在于，所述数据采集模块包括：

相机防爆壳，固定于所述待测掘进装备的机身上；

工业相机，固定于所述相机防爆壳内，且镜头朝向所述三激光指向仪模块，用于采集所述组合激光的特征图像。

4.根据权利要求1所述的煤矿井下掘进装备位姿检测系统，其特征在于，所述系统还包括：

位姿可视化模块，与所述位姿解算模块连接，用于显示所述位姿信息。

5.一种煤矿井下掘进装备位姿检测方法，其特征在于，所述方法应用于如权利要求1所述的系统，所述方法包括：

获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像；所述特征图像是由三激光指向仪模块发射的组合激光被数据采集模块采集得到的；

当所述特征图像为三激光点图像时，提取所述特征图像中的三个激光点，并确定各所述激光点的位置坐标；根据各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；

当所述特征图像为三激光点三激光线图像时，提取所述特征图像中的三条激光线，并确定各所述激光线的位置坐标和相应的激光点的位置坐标；所述激光点的个数为三个，且三个所述激光点分别对应三条所述激光线的起点；根据各所述激光线和各所述激光点的位置坐标，计算所述待测掘进装备的位置坐标；

根据所述待测掘进装备的位置坐标计算所述待测掘进装备的位姿信息。

6.根据权利要求5所述的煤矿井下掘进装备位姿检测方法，其特征在于，所述提取所述特征图像中的三个激光点，具体包括：

对所述特征图像进行畸变矫正、高斯滤波、形态学梯度转换、RGB通道分离和灰度转换，得到第一二值图像；

对所述第一二值图像进行闭运算，并确定所述第一二值图像中的各区域；

选择所述第一二值图像中满足第一设定条件的区域进行椭圆拟合，并去除杂光，得到多个椭圆光斑；所述第一设定条件为：区域边界点的个数在第一设定范围内；

判断所述椭圆光斑的个数是否等于三个；若是，则将三个所述椭圆光斑作为所述特征图像中的三个激光点；若否，则返回“获取组合激光的特征图像，并判断所述特征图像为三激光点图像还是三激光点三激光线图像”的步骤。

7.根据权利要求5所述的煤矿井下掘进装备位姿检测方法，其特征在于，所述提取所述特征图像中的三条激光线，具体包括：

对所述特征图像进行畸变矫正、高斯滤波和灰度转换，得到第二二值图像；

对所述第二二值图像进行闭运算，并确定所述第二二值图像中的各区域；

确定所述第二二值图像中满足第二设定条件的区域的最小外接矩形，并对满足第三设定条件的最小外接矩形进行矩形合并和轮廓点直线拟合，得到所述特征图像中的三条激光线；所述第二设定条件为：区域边缘点的个数在第二设定范围内；所述第三设定条件为：最小外接矩形的高宽比范围在第三设定范围内，且最小外接矩形的面积在第四设定范围内。