基于三激光束标靶的煤矿井下长距离视觉定位方法

杨文娟，张旭辉，张 超，万继成，杜昱阳，杨红强，石 硕

(1.西安科技大学 机械工程学院，陕西 西安 710054；2.陕西省矿山机电装备智能监测重点实验室，陕西 西安 710054)

煤矿巷道掘进装备的定向导航和定形截割、掘支运设备群位姿感知与协同控制，以及掘进设备群远程精准测控，均受制于煤矿井下巷道长距离定位的准确性和稳定性。国家发改委等部门联合印发《关于加快煤矿智能化发展的指导意见》，提出到2030年各类煤矿基本实现智能化。相比于综采工作面智能化技术迅速发展和应用，掘进工作面施工工艺复杂，智能化水平低，“掘进失衡”矛盾制约了生产效率。煤矿井下巷道掘进装备位姿自主测量和动态定位成为掘进装备智能化发展面临的首要难题。

针对掘进装备位姿测量问题，国内外众多研究机构在惯性导航、激光标靶、UWB、iGPS和全站仪等方面开展了卓有成效的研究。陶云飞等提出了基于iGPS的掘进机单站多点分时机身位姿测量方法、符世琛等提出基于超宽带定位的掘进机自主定位导向方法、刘超等提出了超宽带测距和TSOA原理的混合算法解算掘进机三维空间位姿。上述位姿测量方法存在诸如时间累积误差、测量精度不稳定等不足。近几年，智能视觉感知技术在煤矿井下生产视频监控、工作面异常状态识别方面得到初步应用。基于智能视觉技术实现井下动态目标的位姿非接触测量已经成为该领域的研究热点。

视觉定位技术采用光学成像原理和位姿解算模型求解被测目标的相对位置和姿态参数，具有结构简单、无累积误差、性价比高等优势。煤矿井下巷道的高粉尘水雾、低照度、杂光干扰大等特点，使得视觉测量应用面临巨大挑战。德国、澳大利亚等矿业发达国家，国内中国矿业大学吴淼、辽宁工程技术大学陈洪月、西安科技大学张旭辉等研究团队在掘进设备视觉定位技术进行了深入研究。奥地利莱奥本矿业大学GUGG C等研发全集成有源激光靶定位系统，利用射影变换进行平面到平面映射的方法。中国矿业大学童敏明团队采用十字激光器与激光标靶的图像识别测量方式构建位姿实时检测系统，吴淼团队提出基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法，到悬臂式掘进机在固定坐标系下的位姿状态，实现了掘进机机身的位姿检测。中国煤炭科工集团田原研究了基于惯性导航与四点式光靶组合的掘进机自动导航定位方法。上述方法为井下定位提供了新思路，但受接收器安装尺寸等因素限制，工作面底板起伏或者距离增大时存在脱靶可能性，系统稳定性尚待进一步研究。张旭辉教授团队开展了一系列井下定位方面的联合攻关，利用激光线标靶在煤矿井下低照度、高粉尘、复杂背景下的抗遮挡、远距离特征明显的独特优势，巷道平行布置2个激光指向仪，在掘进机机身安装防爆工业相机采集激光束图像，通过构建基于激光点线特征的两点三线(2P3L)煤矿井下动态目标单目视觉位姿测量模型，解算得到悬臂式掘进机机身的位置和姿态参数，结合截割臂位姿数据，实现了掘进机的精确定位、定向导航和定形截割控制。但是该方法在井下标定相对复杂，恶劣环境下的测量稳定性尚需进一步提高。

相比于点特征定位模型，基于直线特征的单目视觉定位方案具有良好抗遮挡能力，适合井下低照度、高粉尘环境下获取几何特征。由于激光指向仪发射出有一定宽度的线结构光，线状光束图像有一定的像素宽度，准确提取激光线图像的中心是实现高精度测量的关键。传统提取技术有Hough变换、Steger法、灰度重心法等。华北理工大学曾凯等提出融合自适应卷积模板滤波和大津法阈值分割的条纹中心线亚像素位置提取方法，李伟明等提出一种新的快速激光条纹中心提取算法，该方法利用阈值轮廓跟踪算法实现对激光条纹中心的快速提取，且具有较好的抗噪声或冗余点能力。武汉理工大学曾超等提出基于图像差分法的结构光光条图像分割算法，进而结合方向模板与灰度重心法实现光条中心的提取。上述研究主要针对背景简单、单束且光条均匀的激光条纹中心进行提取，需要对具有背景复杂、激光束不连续、非均匀等特点的煤矿巷道环境多激光束中心亚像素级特征提取方法进行研究。

获得的点线特征依赖视觉定位模型才能求解获得被测目标的位姿参数。PNP/PNL是出现最早、也是近年来研究最多的基于模型的定位方法。国防科技大学ZHANG Lilian等研究了曼哈顿世界下的基于统一相机模型的消失点估计和直线分类，所提出方法可用于未标定相机的焦距估计以及图像中3个正交线的方向估计。南京航空航天大学WANG Ping等提出从已知的参考点确定标定摄像机的位置和方向的PNP问题。中国科学院自动化研究所王波等从几何观点出发证明当摄像机光心与3个控制点之间的距离相对比较大时，至少存在2个解可以同时满足P3P问题的所有3个约束条件。沈阳理工大学秦丽娟等研究了应用线特征进行摄像机位姿测量的三线透视(P3L)问题，证明当摄像机光心不在过3条直线的2个交点且垂直于3条直线的平面时存在惟一解。新加坡A*STAR研究所XU Chi等研究了基于二维/三维直线对应关系的相机位姿估计的PnL问题，给出了PnL问题的完备性分析并提出基于子集的PnL问题的求解方法。

笔者团队前期以2个激光指向仪构建平行激光束作为目标标靶，用2根激光线和2个出射激光点构建了2P3L测量模型，利用2条平行直线与2个激光点构建的虚拟直线，建立了掘进机机身位姿解算模型，通过闭式解解算出掘进机相对巷道的全局位姿，井下工业试验验证了该定位模型在煤矿井下的适用性。但是，2P3L测量模型需要保证2条激光束平行，在煤矿井下安装时保证激光指向仪平行度难度较大，且系统外参标定复杂、难度大，需要进一步通过优化提高定位精度和稳定性。另外，在方案论证和系统测试时也发现：3条及以上激光束都可以构建视觉空间位姿测量模型，但是4线以上会提高现场安装难度，也容易造成特征线的空间交叉，多激光线特征难以提取，导致视觉定位算法失效。

因此，笔者在前期研究基础上，提出一种新型三激光束掘进机机身位姿单目视觉测量方法，以位姿检测要求和难度最大的悬臂式掘进机为测试对象，在开拓巷道上方安装3个矿用激光指向仪形成视觉测量合作标靶，采用机载防爆相机采集3条激光束，通过激光束点-线特征分割与提取，建立三点三线(3P3L)单目视觉定位模型，解算获得掘进机机身位姿，有效提高系统位姿测量精度和稳定性。

1 掘进设备位姿视觉测量系统

考虑直线特征在高粉尘水雾、低照度的煤矿井下环境中具有更强的抗遮挡能力，笔者提出基于三激光束的单目视觉测量方法，创新设计了由3个矿用激光指向仪构建的三激光束标靶，建立了基于点-线特征的三点三线(3P3L)单目视觉测量及定位数学模型，获得了掘进机机身的相对巷道安装点的位置和姿态参数。煤矿井下使用时借助全站仪进行外参标定，可获得大地坐标下的掘进机机身的位置和姿态绝对坐标数据。图1为悬臂式掘进机机身视觉测量系统测量原理示意。

图1 悬臂式掘进机机身视觉测量原理示意

该系统由三激光束标靶、矿用防爆相机和防爆计算机组成，其中计算机完成图像特征提取、定位模型解算、位姿显示等功能。三激光束标靶固定在安装架，挂载于巷道上方。选用的激光指向仪发射660 nm波长的红色激光，利用激光颜色分量约束可简化井下复杂光照条件下图像的处理，有效避免杂散光对图像分割与提取的干扰。

2 掘进机机身三点三线位姿解算模型

借助3条非平行激光束的直线特征及3个出射激光点特征，构建了基于三点三线(3P3L)的掘进机机身位姿视觉测量模型。图2为三点三线(3P3L)透视投影位姿测量模型。

图2 三点三线(3P3L)透视投影位姿测量模型

(1)

(2)

(3)

其中，

(4)

(5)

(6)

(7)

其中，

(8)

(9)

其中，

由式(8)，(9)可得到如下的掘进机机身三点三线位姿视觉测量模型的矩阵约束方程：

=

(10)

其中，

=[…,]

假设= ()为矩阵的广义逆矩阵。将式(10)中的替换为=，得到

(11)

考虑到视觉测量系统不可避免的存在误差，根据式(11)，可得到掘进机机身三点三线位姿视觉测量模型的最小二乘法损失函数如下：

(12)

(13)

根据获得的旋转矩阵的参数，，，，结合式(14)可以获得摄像机坐标系到激光标靶坐标系的相对旋转矩阵：

(14)

根据式(15)可获得摄像机坐标系到三激光标靶坐标系的平移矩阵:

=

(15)

建立的最小化重投影误差目标函数为

(16)

(,,,)=(+)

(17)

式中，为第个激光点的像素坐标；为第割激光点的空间三维坐标；为相机内参数矩阵；，分别为利用GB求解器获得的第个旋转矩阵和平移矩阵；为将激光束上任意一点的空间三维坐标转化为图像投影像素点坐标的函数；为将投影像素点坐标转化为齐次坐标的系数。

最后结合全站仪标定获得的三激光束标靶坐标系与巷道坐标系的相对位姿转换关系，获得掘进机机身在巷道坐标系下的位置和姿态角信息。

笔者利用3条激光线以及3个激光点构建的虚拟直线，通过摄像机光心与图像投影直线所构成投影平面的法向量和空间直线的垂直约束，构建了掘进机机身3P3L定位模型，建立测量模型的最小二乘法损失函数，最后结合最小化重投影误差实现掘进机机身位姿的非迭代全局最优解估计。相比于2P3L模型，该模型无需激光束平行等位置约束，标定时只需借助全站仪获得激光束上的6个点坐标，解决了井下现场安装和标定难题，减少了误差产生环节，可有效提高掘进机机身的定位精度及稳定性。

3 激光束标靶图像分割与特征提取

3.1 三激光束标靶图像特性

三激光束标靶是由3个矿用激光指向仪形成的3条激光束所构成，激光指向仪采用波长约660 nm的红色激光。图3(a)为三激光束标靶图像，理想情况下每个激光束分布均匀且截面灰度呈高斯分布，如图3(b)所示，灰度最大值处为激光束中心。但是由于煤矿井下掘进工作面的粉尘浓度分布并不均匀且受气流扩散、颗粒散射等影响，大多情况激光束分布非均匀且截面灰度呈近似高斯分布，如图3(c)所示。

图3 三激光束图像及其截面灰度分布特性

通过上述分析可知，三激光束标靶的二维图像的特征分割与提取可以转化为图像行截面的一维灰度信号来进行处理，通过提取具有一定宽度阈值的3个波峰特征，可以稳定、有效的获取激光束标靶图像的直线特征。但是，对于因粉尘分布不均影响导致的非均匀分布的激光束，需要考虑实际灰度的分布差异(图4)，因此，利用上述激光束峰值特性获取初始粗略中心直线提取的基础上，还需要进一步对激光束中心直线进行优化。

图4 实际采集的激光束图像灰度差异

3.2 三激光束标靶图像分割与特征提取

适用于煤矿井下三激光束标靶图像处理，包括激光束区域分割与特征提取、激光光斑的区域分割与特征提取、三激光束点-线特征定位等3个部分。该三激光束标靶图像的区域分割、特征提取与定位流程如图5所示。

图5 三激光束标靶图像的区域分割、特征提取与定位流程

图6为实验中的图像特征分割、提取与定位结果。图6(a)为采集的原始图像。

图6 三激光束标靶图像特征提取与定位结果

..三激光束标靶图像ROI分割

根据红色激光束的HSV颜色空间分量，，的对应范围，可以区分出复杂背景下的激光束与杂散光，其中的范围设为0～10，的范围设为40～250，的范围设为40～250，如图6(b)所示，可以滤除矿灯及其他杂光，并允许红色较亮的激光束像素通过。

通过颜色空间约束获取的红色激光束像素点聚类，可以有效地获得杂光背景下的激光束区域。假设三激光束标靶图像中的第个像素点表示为(,)，则激光束的像素点集合可定义为

={,,…,},=[,,(,)]

(18)

0<(,)<10,40<(,)<250

(19)

40<(,)<250

(20)

..激光光斑区域分割与光斑中心定位

利用欧氏距离约束进一步对分割得到的光斑区域进行筛选，分别通过计算不同光斑区域中心沿轴和轴的像素距离来确定激光光斑。激光光斑区域分割结果如图6(d)所示。光斑定位算法有加权质心法、曲面拟合法等，这里激光光斑中心定位采用高斯拟合算法。

..三激光束区域聚类分割

霍夫变换直线检测方法没有考虑激光束线宽，对于线宽变化的、不连续的激光束中心线检测是不准确的，Steger激光条纹检测精度高，但算法时间复杂度高。根据前面对三激光束标靶图像的特性分析，激光标靶二维图像的特征分割与提取可以通过激光束图像行截面的一维灰度信号来进行处理，通过峰值法提取具有一定宽度阈值的3个波峰特征，进而通过霍夫变换建立的累加器对初始分割聚类结果进行杂点滤波处理。

首先利用峰值法求取每行图像的峰值，形成初始左侧激光束像素点簇、初始中间激光束像素点簇和初始右侧激光束像素点簇(图7)，获取三激光束初始粗略中心线上像素点聚类信息，进而利用和构建离散参数空间，建立累加器={,}，其中,为横坐标轴与垂直于激光线矢量的夹角;为坐标系原点到激光线的距离，通过Hough变换将获得的激光束初始中心线上的像素点聚类由图像空间域转换到-参数空间。如图8所示，激光束所在直线对应于参数空间的投票数量最多的点，因此，根据投票数量确定的局部极大值以及所对应的直线-参数，可以对所提取的激光束初始中心线像素点聚类中的杂点进行滤除，获得激光束初始中心线像素点聚类集合。

图7 三激光束初始中心线像素点聚类的参数空间表示

图8 三激光束标靶图像聚类及杂点滤除过程

采用峰值法提取的激光束初始中心点信息有效克服了Hough变换无法有效提取具有一定宽度的激光束直线的问题，同时降低了Hough变换的计算量；而Hough变换采用可以有效滤除峰值法提取的初始中心线像素点聚类的杂点干扰。

..三激光束亚像素中心直线提取

煤矿井下掘进工作面所采集的激光束图像因粉尘分布不均影响导致激光束呈非均匀分布，在上述获取初始粗略中心直线提取的基础上，还需要进一步对激光束中心直线进行优化处理。

(21)

式中，(，)为二维高斯函数；(，)为粗提取的激光束中心线处的像素点灰度;(，)为对应图像方向的高斯函数的二阶偏导模板;(，)为对应图像方向的高斯函数的二阶偏导模板；(，)为对应图像方向的高斯函数的二阶偏导模板。

因此，每一处初始激光束中心线的像素点可以获得一个Hessian矩阵，Hessian矩阵的最大特征值所对应的特征向量为该像素点处的法线方向=(,)，提取的像素点处法线方向如图9所示。

图9 三激光束初始粗略中心线处的法线方向提取结果

假设

(22)

其中，

在激光束粗略中心线像素点的法线方向利用泰勒展开可以得到激光束亚像素中心线位置，激光束中心的亚像素坐标可以表示为

=(,)，=,=

(23)

(24)

其中，

式中，为图像沿方向的一阶偏导数；为图像沿方向的一阶偏导数；为图像沿方向的二阶偏导数；为图像沿方向的二阶偏导数；为图像沿方向的二阶偏导数。

采用该方法可以快速获取亚像素级的三激光束中心聚类，不受掘进机机身在巷道移动过程中距离变化导致的激光束标靶成像大小改变的影响，具有较高的提取精度和良好的稳定性；对于因遮挡或粉尘分布不均影响导致的激光束区域边界不连续而具有容错性和鲁棒性。

..激光束中心线的参数求解

结合得到的激光束聚类结果，分别用带约束条件的最小二乘拟合方法得到激光束的线性方程。假设左侧、右侧、中间激光束的像素聚类簇内每个像素点到拟合直线的距离方程为

(25)

(26)

(27)

式中，，，为左激光束的中心线参数；，和为右激光束的中心线参数；,,分别为3个激光束像素点聚类簇内的第个、第个和第个像素点。

三激光束的中心线所在直线的拟合线性方程可分别用以下带约束条件的最小二乘法拟合：

(28)

+=1

(29)

定义如式(30)所示的直线拟合的误差目标函数，当直线拟合误差小于最大允许误差时得到最优解。

(30)

4 掘进机机身位姿视觉测量精度分析

视觉测量主要误差影响因素包括通过外参标定获得激光束点-线特征的三维空间先验信息、通过图像处理获取的激光光斑中心的像素坐标，以及激光束中心线直线方程等。为分析多因素影响下掘进机机身位姿测量精度，本节采用Mento Carlo方法对多因素影响下的掘进机机身视觉测量精度和稳定性进行数值仿真测试。

利用上述给定的平移矩阵与旋转矩阵，借助透视投影模型将标靶坐标系下的激光束特征点-线转换到相机坐标系，得到激光束标靶点-线特征在二维图像平面内的理想像素坐标，进而利用Mento Carlo仿真对位姿测量精度进行评估。

激光光斑中心定位误差、激光束中心线斜率提取误差以及外参数标定误差等多因素对掘进机机身位置和姿态角测量精度的影响如图10，11所示。可以看出，位置和姿态误差都随着测量距离的增大而增大，其中，轴、轴2个方向具有较高的位置测量精度，轴方向的位置测量误差较大，绕轴、轴的俯仰角和航向角具有较高的角度测量精度，绕轴的翻滚角误差略大于绕轴、轴的角度误差。相机与激光标靶间距离在100 m内时，轴、轴、轴方向的位置测量误差分别在4.994，5.608，127.113 mm以内，俯仰角、偏航角、翻滚角的最大角度误差分别为0.048°，0.093°，0.282°，能够满足煤矿掘进工作面巷道施工允许的最大测量误差。

图10 不同距离时掘进机机身位置和姿态角的视觉测量误差

5 掘进机视觉测量系统性能测试

搭建悬臂式掘进机机身位姿视觉测量平台进行系统性能测试，如图12所示。测试系统由悬臂式掘进机(5∶1缩比)、矿用激光指向仪、三激光标靶固定装置、工业相机(MV_EM510C)、数字全站仪(SOKKIA IR1)、烟雾发生器、工业计算机等组成，长距离测试中利用履带式移动机器人模拟掘进机在巷道中的运动，用烟雾发生器模拟煤矿粉尘环境。采用矿用相机采集三激光标靶图像，利用全站仪对悬臂式掘进机机身视觉测量系统进行评估。

图11 不同距离下的不同截面的位置和姿态角的误差分布

5.1 测试系统的环境适应性实验

实验室模拟掘进工作面的低照度、高粉尘、杂光等煤矿井下复杂背景环境，搭建的测试系统环境适应性验证平台如图12所示。

图12 悬臂式掘进机机身位姿视觉测量系统实验平台

实验中在不同位置和姿态的3处固定位置各采集100幅三激光束标靶图像，并利用本文提出的激光束分割、提取与定位算法进行重复性测试，采集的激光束图像及其处理结果如图13所示，其中图13分别表示3处相对固定位置的特征提取结果，从左到右表示不同粉尘浓度的激光标靶图像，每个位置上面一行为原始图像，对应的下面一行为激光束特征提取结果。激光束中心线定位的均方根误差评估对比结果见表1。

表1 不同位置处激光束中心线定位的均方根误差评估

图13 三激光束标靶点-线特征提取与定位结果

结果表明，该方法能够在低照度、杂散光干扰的复杂背景下对激光标靶进行有效的分割、特征提取与定位，相比于Hough，Steger等算法，本文方法获得的激光束中心线均方差较小，对激光束中心直线检测的准确性更高，如图14所示。

图14 掘进机机身位置和姿态角测量误差对比

5.2 机身位姿视觉测量系统性能实验

为了对不同距离视觉测量系统性能进行测试与验证，搭建的测量系统实验平台如图15所示。实验中控制移动机器人在楼道移动，模拟掘进机在巷道的移动，使用矿用相机对三激光束标靶图像进行采集，对每一固定距离处采集50幅激光束图像，解算获得掘进机机身位置和姿态角信息，从而对视觉测量系统性能进行评估。实验过程中，以全站仪测量值为真实值，进而利用前面提出的外参标定方法，在构建的全站仪统一坐标系下，对掘进机机身位姿视觉测量系统性能进行对比评估。

图15 基于激光束标靶的掘进机机身位姿视觉测量系统实验平台

对于6处不同位置采集到的激光束图像及其处理结果如图16所示。结果表明，本文算法在60 m测试范围内稳定性良好，具有较好的鲁棒性。

图16 不同距离下的三激光束标靶点-线特征提取与定位结果

履带式机器人在楼道移动时机身位姿测量数据与真实值(全站仪数据)对比结果见表2，可见位置和姿态角误差随着测量距离的增大而增大，在测试距离范围内，利用本文提出的位姿估计方法的所测位置和姿态角结果接近于真实值，具有较好的定位精度。表3给出了移动机器人在模拟巷道6处不同距离时的位置和姿态角误差评估结果。

表2 相机与激光标靶之间不同距离时的位置和姿态角对比结果

表3结果显示，相比于轴方向，轴、轴2个方向具有较高的位置测量精度，绕轴、轴、Z轴的俯仰角和航向角都具有较高的角度测量精度。相机与激光标靶间距离在50 m范围内时，沿轴、轴、轴的平均测量误差分别为8.02，27.62，95.32 mm。俯仰角、偏航角、翻滚角的平均测量误差分别为0.26°，0.27°，0.32°。根据煤矿井下巷道施工规程允许的最大位姿估计误差，在测试距离范围内所提出的基于三激光标靶的视觉定位方法能够满足悬臂式掘进机机身位姿测量的要求。

表3 相机与激光标靶之间不同距离时的位置和姿态角误差评估

6 结 论

(1)井下巷道定位用三激光束视觉测量合作标靶可应对煤矿高粉尘、水雾严重、易遮挡等因素影响。通过采用颜色空间约束与激光束截面灰度峰值聚类方式实现了激光束标靶图像的分割；采用基于霍夫变换建立的累加器与欧氏距离约束对初始图像粗略中心线的像素点聚类，可以有效滤除激光束图像杂点；利用Hessian矩阵对初始粗略中心线的像素点处的法线方向进行求解，结合泰勒展开实现了三激光束中心线的亚像素级特征提取和定位。

(2)基于三点三线(3P3L)的掘进机机身位姿视觉测量模型解决了非平行激光束远距离定位难题。针对测量模型构建的最小二乘法损失函数，分别对旋转矩阵参数进行微分处理，获得四阶方程系统，利用GB求解器对所构建的方程系统进行求解，得到了旋转矩阵的四元数参数，最后结合最小化重投影误差实现非迭代全局最优解估计，获得了掘进机机身位姿的最优解，提高了视觉测量精度。

(3)对多因素影响下的掘进机机身位姿视觉测量系统精度与稳定性进行了测试。数值仿真结果表明，在100 m测量距离范围内，轴、轴、轴方向的位置测量误差分别在4.994，5.608，127.113 mm以内，俯仰角、偏航角、翻滚角的最大角度误差分别为0.048°，0.093°，0.282°。同时搭建实验台进行定位性能测试，实验测试结果与理论仿真分析符合，结果表明：在50 m测量距离范围内，姿态角测量精度在0.4° 以内，轴位置测量误差小于10.28 mm，轴位置测量平均误差小于31.21 mm，轴的位置测量误差小于100.49 mm。