基于近红外双目立体视觉的悬臂式掘进机定位研究

王学， 周红旭， 张雷， 王华英

（1. 河北工程大学 数理科学与工程学院，河北 邯郸 056038；2. 河北省计算光学成像与光电检测技术创新中心，河北 邯郸 056038）

0 引言

自“十三五”以来，我国煤炭行业自主科研创新能力得到了大幅改善，但是创新推动能力依旧不足[1]。GB50213-2010《煤矿井巷工程质量验收规范》规定我国煤矿掘进断面的允许偏差为-50～+150 mm。但煤矿井下巷道掘进中悬臂式掘进机截割部位姿通常用固定在巷道顶板上的点光源来指示掘进方向，通过判断激光投影点在巷道掘进断面上的位置，确定下一步的掘进方向[2]。该方法存在掘进精度有限、测量工作量大、生产效率低、人身不安全等问题，很难保证巷道成形质量达到要求。为了实现掘进机的智能切割控制，亟需提高悬臂式掘进机位姿参数的自动实时检测[3]。

为实现巷道无人化掘进，学者提出了多种定位方法。文献[4]通过读取全站仪激光光斑距目标板中心的偏移量来推断当前隧道的偏差，实现隧道内掘进机的跟踪和引导，但全站仪无法实时自主测量且不能适应井下掘进状况。文献[5]采用惯性导航技术实现掘进机实时自主测量，然而系统存在时间偏差、成本高、初始对准时间较长等问题，加上掘进机抖动幅度大、巷道测量环境较差等缺陷，增加了累计误差。文献[6-7]开发了一种基于iGPS的掘进机测量系统，以基于随机误差传递方程来评估掘进机的空间角度和位移，该系统的测量介质为激光，掘进机作业时存在遮挡问题，同时激光发射装置结构复杂，易损坏，在复杂工况下，巷道掘进精度会受到很大影响。文献[8-9]采用超宽带技术实现掘进机位姿实时检测，但由于巷道环境复杂，金属材料设备较多，易导致超宽带信号发生漫反射，使得掘进机位姿测量误差偏大、失败等情况。文献[10]提出了一种基于激光束的悬臂式掘进机位姿测量系统。结合坐标系转换关系得到掘进机机身在巷道坐标系中的位姿信息，但测量距离的增加或相机视野被遮挡等问题易导致测量失效。

针对现有掘进机无法实时定位、定位不准确、视觉定位中相机视野被遮挡导致定位失败等问题，提出了一种基于近红外双目立体视觉的悬臂式掘进机定位方案。该方案以LED作为近红外标靶构建掘进机特征信息，通过图像处理、位姿解算实现掘进机机身与截割部的三维空间定位；为解决双目立体视觉位姿解算失效问题[11]，引入基于一维卷积神经网络（One Dimensional-Convolutional Neural Network，1D-CNN）[12-14]的井下磁场视觉辅助定位算法，通过构建神经网络模型训练磁场信号数据，预测掘进机截割部的位姿变化，为视觉定位掘进机位姿提供了一种在视觉测量失效情况下的辅助定位方案。

1 悬臂式掘进机定位系统

悬臂式掘进机机身及截割部位姿测量系统由悬臂式掘进机、机身近红外LED标靶、机臂近红外LED标靶、2台近红外相机组成的双目立体视觉相机、三轴数字磁场计、永磁体组成，如图1所示。机身近红外LED标靶根据作业的实际情况固定在机身四周边缘处，机臂近红外LED标靶安装在掘进机截割部护罩四周边缘处。将双目立体视觉相机安装在巷道顶端，获得足够的相机视野对掘进工作面进行图像采集。为防止在恶劣环境下图像采集失败、位姿测量失效等问题，在掘进机机身两侧各安装高精度三轴数字磁场计，并在机臂处安装永磁体，以磁场的强度分量和近红外双目立体视觉相机获取的位姿数据作为训练数据，构建1D-CNN模型，输出在视觉测量失效情况下掘进机位姿。

图1 悬臂式掘进机机身及截割部位姿测量系统结构Fig. 1 Structure of cantilever roadheader fuselage and cutting part pose measurement system

悬臂式掘进机机身及截割部位姿测量方案如图2所示。首先，根据双目立体视觉测量原理设计近红外双目立体视觉相机，并采用张正友标定原理对双目立体视觉相机进行标定，计算相机相关参数。在掘进过程中，相机与掘进机间的外部参数也需要标定，并将其作为下次掘进机工作的初始数据。随着掘进工作的不断进行，掘进机与相机间距离误差不断累计，在掘进机等待支护阶段需要对二者间的外部参数进行重新标定。其次，在掘进工作开始前利用迭代最近点（Iterative Closest Point，ICP）[15]算法对掘进机机身坐标系与相机坐标系进行外部参数解算。根据相机标定得到的参数，利用相机校正技术实现图像立体校正。对近红外图像进行图像增强、去噪，利用Canny边缘检测方法分割目标，并利用分散度筛选出真正的近红外LED标靶，得到标靶质心坐标，根据欧氏距离等方法对标靶质心进行定位、跟踪及匹配。再次，根据双目立体视觉测量原理计算巷道坐标系下的三维空间坐标，在掘进机机身与截割部建立坐标系，根据ICP算法求解巷道坐标系与机身坐标系、巷道坐标系与截割部坐标系间的位姿关系。最后，根据位姿解算模型求解掘进机截割部与机身间的位姿关系。

图2 悬臂式掘进机机身及截割部位姿测量方案Fig. 2 Pose measurement scheme of cantilever roadheader fuselage and cutting part

由于煤矿井下掘进机工作环境恶劣，巷道复杂多变，且双目立体视觉测量距离有限，导致掘进机在掘进过程中视觉位姿估计失效。针对该问题，引入基于1D-CNN的井下磁场视觉辅助定位算法。在1D-CNN的基础上搭建网络模型，该模型包括2个输入层，训练数据经过3层1D-CNN卷积，每层卷积都经过批标准化（Batch Normalization，BN）使其加快收敛，防止过拟合。数据经Selu激活函数处理后输入下一卷积层，不断改善训练参数。由于特征较少，所以未采用池化层，添加平展层将张量重新整形为矢量。数据经卷积后引入Selu激活函数连接输出层，得到悬臂式掘进机截割部位姿预测值。

悬臂式掘进机机身及截割部位姿空间坐标系如图3所示。以机身与机臂近红外LED标靶的中心为原点，建立机身坐标系OfXfYfZf和机臂坐标系OeXeYeZe；以截割部中心点为原点，建立截割部坐标系OhXhYhZh；以近红外相机1，2的光心为相机坐标系原点，设相机的光轴为Zc轴，垂直相机光轴向下为Yc轴，水平垂直相机光轴为Xc轴，建立相机坐标系Oc1Xc1Yc1Zc1和Oc2Xc2Yc2Zc2；将近红外相机1坐标系设置为世界坐标系（即巷道坐标系）OwXwYwZw。

图3 悬臂式掘进机机身及截割部位姿空间坐标系Fig. 3 Pose space coordinate systems of cantilever roadheader fuselage and cutting part

2 近红外标靶图像特征提取、定位与匹配

2.1 近红外LED标靶特征提取

受煤矿井下作业面高粉尘、高湿度、强振动和电磁等干扰[16]的影响，双目立体视觉相机采集到的图像质量较差且伴有噪声，因此采用滤波、平滑等预处理技术对立体校正后的图像进行处理，以提高图像对比度和亮度、改善图像质量。

将近红外LED标靶作为掘进机位姿估计的重要识别特征，通过最大类间方差法 OTSU 分割原理计算Canny边缘检测最佳阈值，对近红外图像进行处理。设近红外图像为二值函数I(x,y)，假设存在阈值f将图像分为前景和后景2类（图像的像素灰度值小于f的为前景，图像的像素灰度值大于f的为后景），前景和后景的均值分别为 ωmv1和 ωmv2，整个图像的均值为 ωmv， 图像被分为前景和后景的概率分别为p1和p2，则有

根据方差知识，可得类间方差为

将式（3）简化，可得

阈值的确定是控制边缘检测效果的关键，将通过OTSU得到的最佳阈值应用在Canny边缘检测双阈值检测步骤中，获取不同目标的图像边缘特征。之后对获取的边缘轮廓信息进行拟合，根据分散度进行特征选择，剔除非标靶的干扰目标。近红外LED标靶特征提取结果如图4所示。可看出近红外LED标靶的特征提取与识别效果较好。

图4 近红外LED标靶特征提取结果Fig. 4 Characteristic extraction results of infrared LED target

2.2 近红外LED标靶定位与匹配

悬臂式掘进机双目视觉位姿测量方法中利用重心进行特征点匹配，利用图像的矩计算近红外LED标靶轮廓重心坐标，以一阶矩和零阶矩计算重心，实现近红外双目图像中标靶质心的精确定位。匹配过程如下：

（1） 采集目标轮廓信息。计算左相机（近红外相机1）图像目标第j个边缘轮廓点像素坐标Y方向坐标均值为计算右相机（近红外相机2）图像目标第j个边缘轮廓点像素坐标，X，Y方向坐标均值为

（2） 计算X方向和Y方向的差值。将右相机图像边缘轮廓点像素坐标X方向平均值与左相机图像边缘轮廓点像素坐标X方向的平均值相减，得X方向上的差值，同理计算Y方向上的差值。

（3） 计算右相机图像质心平移后的坐标点。将右相机图像质心坐标与差值相减，即右相机图像标靶质心平移至左相机图像处，得到右相机图像各质心平移到左相机图像后的坐标点

（4） 计算欧氏距离。以左相机图像第1个特征点 (xl1,yl1)为 匹配点，分别计算右相机图像各质心平移后的特征点之间的欧氏距离D。

提取左相机图像第1个特征点 (xl1,yl1)与距右相机图像标靶质心平移后的特征点欧氏距离最小的最邻点与次邻近点。提取这2个关键点，用最近邻点欧氏距离Dm除以次邻近点的欧氏距离Dm+1，得到比率λ。

当λ＜0.8时，左相机图像第1个特征点与最近邻点为1对匹配点，否则重新匹配。依此类推，对左相机图像中所有特征点进行匹配。

3 悬臂式掘进机位姿估计

3.1 掘进机位姿估计数学模型

将巷道坐标系与机身坐标系下的3D坐标作为输入，通过刚性变换（旋转变换和平移变换）使其转换到同一坐标系下。设巷道坐标系下目标点集为S(X,Y,Z)， 机身坐标系下参考点集为J(X′,Y′,Z′)，将机身坐标系下参考点集通过刚性变换转换到巷道坐标系下目标点集，其转换关系为

式中M为映射矩阵。

三维点云围绕X,Y,Z轴旋转，可得如下转换关系。

式中：α为偏航角； β为俯仰角；γ为翻滚角；tX,tY,tZ分别为围绕X,Y,Z轴旋转的偏移量。

3.2 掘进机位姿估计算法

ICP算法用于寻找目标点集S和参考点集J之间的刚性变换，使2个匹配数据在某种度量标准下满足最优匹配。假设目标点集S的坐标为{Si|Si∈Z3,i=1,2,···,N}，N为目标点集中目标点，参考点集J的坐标为，第i个参考点Ji经过k次迭代后的坐标为。计算目标点集S与参考点集J经过k次迭代后对应点之间的变换矩阵并更新原始变换，直到目标点集与参考点集间的距离小于给定阈值τ。ICP算法步骤如下：

（1） 计算参考点集J经过k次迭代后第i个对应点与目标点集S经过k次迭代后第i个对应点的对应关系，使得最小。

（2） 计算经过k次迭代后旋转矩阵Rk和平移矩阵Tk，满足关系最小。

（3） 运用经过k次迭代后旋转矩阵Rk与平移矩阵Tk变换目标点集S的位置，即

（4） 将经过k+1次迭代后的均方误差dk+1作为目标函数。

（5） 当dk+1≥τ 时 ，则返回步骤（1），直到dk+1＜τ或迭代次数k大于预设的最大迭代次数，算法结束。

当算法结束后有效收敛到局部最小值，则最后1次迭代得到的目标点集即为最终的目标点集。

3.3 掘进机位姿估计实现

搭建模拟测量掘进机位姿实验平台，分别在机身与机臂安装近红外LED标靶。掘进机机身近红外LED标靶在巷道坐标系及机身坐标系下的点集分别为A和A′。掘进机机臂近红外LED标靶在巷道坐标系及机身坐标系下的点集分别为B和B′。

通过ICP算法使得不同坐标系下的空间点数据合并到同一坐标系中，即得出巷道坐标系与掘进机机身坐标系的变换矩阵分别为旋转矩阵，平移矩阵。根据位姿解算模型可得巷道坐标系与掘进机机臂坐标系的变换矩阵H2，分别为旋转矩阵，平移矩阵。

由悬臂式掘进机的构造可知掘进机机臂通过回转结构和液压系统与机身进行连接，实现掘进机机臂的上下摆动与左右回转。通过位姿解算模型可得掘进机机身与机臂在巷道坐标系下的位姿信息，从而计算出掘进机机身与机臂间的位姿关系。

以巷道坐标系为中间量，根据巷道坐标系与机身坐标系、巷道坐标系与机臂坐标系的关系，计算掘进机机身坐标系与机臂坐标系的变换矩阵H3，

由此可解算出机身坐标系与机臂坐标系间的变换关系，即旋转矩阵R3与 平移矩阵T3。 根据 α，β，γ解算出绕机臂坐标系Xe，Ye，Ze轴的旋转角度α′′，β′′，γ′′。

综上，可计算出掘进机在巷道坐标系下掘进机机身与机臂的空间位姿及机臂相对于机身的空间位姿。

因截割部与掘进机机臂的运动相同，则掘进机机身坐标系与截割部坐标系的变换矩阵H4=H3，截割部平移矩阵为T5=T3+T4，T4为机臂坐标系与截割部坐标系的平移矩阵，， 其中，a为y方向上轴心距，b为掘进机截割部中心点距离机臂中心的初始位置，设置初始位置为0，d为掘进机截割部液压关节处的伸缩量，d=0。

4 双目视觉位姿辅助估计

4.1 数据采集与处理

对处于不同强度范围的数据进行归一化处理，用于网络训练。

式中：为第n个输入变量归一化值；Cmin,Cmax分别为输入变量的最小，最大实测值；Cn为第n个输入变量的实测值。

4.2 1D-CNN截割部位姿估计网络模型

3层卷积层的一维卷积核大小均为3，卷积核的数量依次为16，32，64，模型使用Adam优化方法训练参数，学习率为0.003。采用Selu激活函数对输入信息进行非线性变换，之后输入下一卷积层，不断改善训练参数，批大小为64，迭代次数为1 000。数据经卷积后引入Selu激活函数连接输出层，经过不断改进优化，最后得到1D-CNN截割部位姿估计网络模型。

4.3 视觉测量辅助方法

双目视觉测量失效下磁场辅助定位时相机前移如图5所示。随着掘进机不断前进，双目立体视觉相机与掘进机距离不断增加，当掘进距离较远时，近红外LED标靶采集出现缺失，此时位姿解算模型位姿解算失败。为保证掘进机位姿解算的实时连续性，磁场辅助定位算法开始工作，掘进机两侧磁场计采集的磁场信号经过数据处理，调用已经训练好的1D-CNN位姿估计模型（视觉位姿解算数据实时更新，磁场数据训练模型需不断进行更新，以符合掘进机当时的巷道磁场环境），输出此时掘进机截割部位姿信息。双目立体视觉相机从地点A移动到适宜工作的地点B，双目立体视觉相机开始正常工作。以此类推，保证掘进机截割部位姿解算保持连续性，其中网络模型需实时训练，即边掘进边训练，以防止井下磁场异常变化导致模型失效。

图5 双目视觉测量失效下磁场辅助定位时相机前移Fig. 5 Camera moves forward in magnetic field assisted positioning under the failure of binocular vision measurement

5 实验结果与分析

5.1 双目立体视觉位姿解算

为验证近红外双目立体视觉的悬臂式掘进机截割部及机身位姿测量方案的可行性，从深度信息和掘进机位姿2个方面进行验证分析。

本实验采用2部Mako G-223B NIR近红外视觉相机搭建双目立体视觉相机测量系统，由8组LED灯组成近红外LED标靶，采用690 nm工业激光测距模块对近红外LED标靶进行深度信息测量，采用角度传感器对掘进机截割部相对机身坐标系的位姿变换信息进行测量。

在实验过程中模拟掘进机截割部做切割煤层运动，将高精度传感器安装在机臂中间位置，对截割部的空间位置进行检测，双目立体视觉相机测量系统对截割部模拟切割煤层过程进行实时数据采集，获取数据后传输至计算机，计算机对图像进行分析处理，计算出截割部与机身的三维空间信息及截割部与机身的位姿信息，最后根据截割部与机身的转换关系解算截割部空间位姿。

在巷道坐标系下掘进机机身与截割部空间坐标变化如图6所示。可看出在掘进机机身静止、截割部做截割煤层运动时，机身空间坐标未出现明显波动，截割部空间坐标发生变化，符合实际截割部运动状态，说明基于近红外双目立体视觉空间位置定位方法可行。

图6 掘进机机身与截割部空间坐标变化Fig. 6 Space coordinate change of roadheader fuselage and cutting part

在巷道坐标系下掘进机机身与截割部空间深度及误差见表1。可看出掘进机机身在Zw轴方向的测量误差在±11 mm以内，相对误差在0.4%以内。截割部在Zw轴方向的测量误差在±50 mm以内，相对误差在1%以内。该误差在井下煤矿施工标准误差允许范围内，说明该系统满足掘进过程中悬臂式掘进机机身及截割部空间位置的精确测量需求。

表1 掘进机机身与截割部空间深度及误差Table 1 Space depth and error of roadheader fuselage and cutting part

巷道坐标系下的机身与截割部位姿变化如图7所示。可看出在掘进机机身静止、截割部做截割煤层运动时，机身空间位姿角未出现明显波动，截割部空间位姿角发生变化，其位姿角变化符合实际截割部运动状态，说明基于近红外双目立体视觉空间姿态定位方法可行。

图7 巷道坐标系下的机身与截割部位姿变化Fig. 7 The relation between the cutting part and pose of the fuselage in the tunnel coordinate system

通过位姿解算模型解算出机身与巷道坐标系、截割部与巷道坐标系的位姿变化及截割部与机身的位姿变化关系。实验过程中机身固定，掘进机模型通过不断改变截割部的动作模拟掘进机切割煤层，其位姿测量误差如图8所示。可看出掘进机机身与截割部间的相对位姿误差在 ± 2.5°以内。通过计算可得俯仰角的均方根误差为0.930 1°，偏航角的均方根误差为0.922 0°（由于截割部是旋转的，不考虑翻滚角的变化）。此误差在巷道作业允许范围之内，说明采用基于近红外双目立体视觉的悬臂式掘进机位姿测量方案进行掘进机姿态解算具有较高的有效性和可靠性。

图8 掘进机机身与截割部相对位姿误差Fig. 8 The relative pose error of roadheader fuselage and cutting part

5.2 视觉位姿解算失效下的磁场位姿解算

为验证基于1D-CNN的掘进机截割部磁场辅助定位方法的有效性，在实验室内搭建了模拟掘进机实验平台进行实验。为模拟井下复杂磁场环境，在掘进机附近随机添加干扰磁源，将采集到的2 228组磁场强度信号作为数据集，选取80%数据作为训练集，剩下20%数据作为测试集。基于1D-CNN的掘进机截割部磁场辅助定位方法对截割部俯仰角、偏航角、翻滚角预测值与实测值对比如图9-图11所示。可看出掘进机截割部俯仰角、偏航角、翻滚角预测值与实测值基本吻合。

图9 掘进机截割部俯仰角预测结果Fig. 9 Prediction results of pitch angle of roadheader cutting part

图11 掘进机截割部翻滚角预测结果Fig. 11 Prediction results of rolling angle of roadheader cutting part

图10 掘进机截割部偏航角预测结果Fig. 10 Prediction results of yaw angle of roadheader cutting part

为了更加精准评价基于1D-CNN的掘进机截割部磁场辅助定位方法的有效性，采用决定系数G2作为评价指标（G2越接近1，说明预测结果的准确率越高）。基于1D-CNN的掘进机截割部磁场辅助定位方法预测俯仰角G2为0.992 4，偏航角G2为0.995 9，翻滚角G2为0.917 4，说明该方法能够很好地满足在视觉定位失效下的掘进机定位需求。

将基于近红外双目立体视觉的悬臂式掘进机位姿测量方案与基于1D-CNN的磁场辅助定位方法相结合，解决了井下复杂环境导致掘进机截割部位姿实时定位问题，最终实现基于近红外双目立体视觉的悬臂式掘进机定位测量研究。

6 结论

（1） 从深度信息和掘进机位姿2个方面对掘进机测量进行验证分析。结果表明：机身测量误差在±11 mm以内，相对误差在0.4%以内。截割部测量误差在±50 mm以内，相对误差在1%以内；掘进机机身与截割部间的相对位姿误差在±2.5°以内，俯仰角的均方根误差为0.930 1°，偏航角的均方误差为0.922 0°。

（2） 掘进机截割部俯仰角、偏航角、翻滚角预测值与实测值基本吻合。基于1D-CNN的掘进机截割部磁场辅助定位方法预测俯仰角G2为0.992 4，偏航角G2为0.995 9，翻滚角G2为0.917 4，说明该方法能够很好地满足在视觉定位失效下的掘进机定位需求。

（3） 为进一步提高掘进机定位的可行性，未来应改进视觉测量的解算结果，加入滤波平滑等技术使其在实际应用中稳定输出，不受干扰；在磁场辅助定位方向，应将磁源与机身融合，使其模块化，从而保证在实际生产中稳定高效运行。