煤炭在未来较长时间内仍将是我国的主体能源，是保障国家经济发展的基础性能源^[1-2]。相比综采工作面，掘进工作面智能化水平低，导致“采掘失衡”问题严峻^[3-4]。由于井下掘进工作面空间狭小、设备种类繁杂，设备之间交替作业易发生干涉、碰撞事故，造成工作面设备间协同作业困难，因此解决掘锚设备的跟踪定位与碰撞预警难题，对实现掘进工作面设备智能化具有重要意义^[5-6]。

针对掘进工作面设备位姿测量问题，国内外研究学者进行了广泛研究。常见的掘进设备定位方法主要有惯性导航定位方法^[7-9]、超宽带(ultra-broadband，UWB)定位方法^[10]、iGPS定位方法^[11]和视觉定位方法^[12-13]。惯性导航定位方法利用惯性导航元件实时测量并输出设备的角速度和线加速度。超宽带(UWB)定位方法利用带有超宽带天线的基站发射极高频率的无线电波信号，机身定位点接收到基站发射的无线电波经过解算信号飞行时间，得到设备定位点相对于基站的三维坐标，从而确定掘进机位姿。iGPS定位方法利用安装在巷道的激光发射站和安装在设备机身上的激光接收器，通过测量激光接收器在巷道坐标系三维坐标，最终计算出设备的姿态角。上述方法面向单一设备定位，难以获取不同复杂装备间的相对位姿测量，而视觉定位方法利用光学成像原理和位姿解算模型求解被测目标的相对位置和姿态参数，在处理空间移动目标的相对位姿关系方面具有较大的优势。

近年来，随着数字孪生(Digital Twin, DT)技术和虚拟现实(VR)技术在煤矿智能化建设过程中的广泛关注^[14-15]，国内多所高校和煤炭行业研究单位将其引入到采掘工作面设备群远程智能控制决策系统，葛世荣等^[16]提出数字孪生智采工作面系统的概念、架构及构建方法，并分析了智采工作面系统关键技术。吴淼等^[17]提出数字孪生理论指导下的综掘巷道并行施工技术流程与工艺体系，通过远程智能调控系统实现掘进机的自主纠偏、障碍物感知及自动截割。王学文等^[18]研究了VR技术驱动的综采生产数字孪生系统、虚实交互技术以及VR/AR技术在智能化综采工作面的应用。杨健健等^[19]基于平行控制基础理论及ACP方法框架，提出智能掘进复杂系统的掘−支−锚智能平行控制理论与关键技术。通过构建掘进工作面数字孪生体，将井下人员、设备、环境相关信息呈现到数字空间，通过数字掘进与物理掘进智能协同与虚实交互，对于解决掘进工作面设备碰撞检测提供了有效路径。另外，针对设备碰撞检测问题，众多学者进行了深入研究，常见的碰撞检测方法主要包括图像空间法^[20-22]、空间分解法^[23-24]和层次包围盒法^[25-27]。图像空间法可以处理具有复杂形状和不规则的物体，能够通过图像处理技术实现高效的碰撞检测，但易受设备数量和复杂背景影响。空间分解法将场景空间划分为较小的子空间，减少了被检测的对象数量，从而提高碰撞检测的效率。层次包围盒法^[28]通过合理构建层次包围盒树，可以快速、准确地检测出碰撞物体，在狭小、复杂的地下环境中也能保证碰撞检测的效率。通过上述分析，借助数字孪生技术和虚拟现实技术，融合传感器数据和包围盒碰撞检测方法，有望解决掘进工作面复杂设备间的碰撞检测难题。

笔者团队以“DT+VR”的虚实共智远程决策为目标，在数字孪生驱动的采掘设备智能化决策控制^[29]、碰撞检测^[30]、虚拟示教记忆截割^[31]进行了相关研究，构建了掘进工作面数字孪生体，形成数字孪生驱动掘进装备远程智能控制技术体系。借助团队前期基于数字孪生驱动的虚拟边界碰撞检测、异常预警和虚拟示教记忆截割等研究基础，研究掘锚设备虚拟碰撞检测，对实现工作面设备远程控制及辅助干预具有重要价值。因此，针对掘进工作面掘锚设备相对位姿测量和异常碰撞检测难题，笔者在团队前期研究基础上，提出了一种基于数字孪生驱动的掘锚设备跟踪定位与碰撞检测方法，该方法分别在悬臂式掘进机和锚杆钻机机身安装具有ID标识的红外标靶，通过机载防爆相机分别采集按照在掘进机和锚杆钻机的标靶图像并进行特征提取和光斑映射，通过掘锚设备相对位姿解算模型得到两者相对位姿及对应的标靶ID标识，并将其作为孪生数据驱动虚拟空间掘锚设备数字孪生体模型状态实时变化，根据碰撞检测模型对掘锚设备进行碰撞检测，对掘进工作面掘锚设备远程智能协同控制具有重要意义。

1   掘锚设备跟踪定位与碰撞检测系统总体方案

掘锚设备跟踪定位与碰撞检测系统总体方案如图1所示，由红外LED标靶1、红外LED标靶2、防爆相机1、防爆相机2及计算机组成。该系统包括物理空间和虚拟空间两个部分，其中物理空间指实际掘进工作面中的掘锚设备、工作环境和红外标靶，旨在将获得的掘锚设备状态数据用于数字孪生系统，从而驱动虚拟空间中掘锚设备状态的实时变化；而虚拟空间是物理空间的实时、精准数字化映射，利用掘锚设备相关位姿数据对虚拟空间的各设备位姿进行实时修正，同时借助虚拟空间中悬臂式掘进机、锚杆钻机的力学碰撞模型实现虚拟碰撞检测，为掘−支设备交互施工控制提供技术支撑。

图  1  掘锚设备跟踪定位与碰撞检测系统总体方案

Figure  1.  Overall scheme of tracking, positioning and collision detection system for excavating and anchoring equipment

下载: 全尺寸图片 幻灯片

掘进工作面施工中，悬臂式掘进机完成截割作业后退至左侧帮停机后，锚杆钻机从右侧帮行至煤壁处开始支护的过程中，利用防爆相机1采集红外标靶1图像，滤除杂光并进行图像畸变矫正，通过特征提取得到光斑中心并进行光斑映射，建立掘锚设备相对位姿解算模型，并实时驱动虚拟空间中的悬臂式掘进机模型作业，结合三维实体碰撞检测方法实现悬臂式掘进机与锚杆钻机的碰撞检测。同理，当锚杆钻机完成支护后退回到悬臂式掘进机后方时，利用防爆相机2采集红外标靶2图像并完成位姿解算，基于位姿数据驱动掘锚设备模型运动，最终实现掘锚设备交替作业的碰撞检测。

2   红外标靶特征提取与ID映射

2.1   图像预处理

在煤矿井下掘进工作面的特殊环境下，低照度、粉尘和复杂背景干扰等因素极大地增加了标靶检测和跟踪的难度。掘进工作面产生的多种杂散光源包括矿工头灯、矿用照明灯以及激光指向仪等设备进一步加剧了图像的噪声和干扰。为了解决这些问题，本文选用波长为850 nm的SE3470型红外LED作为特征点，利用窄带滤光片滤除掘进工作面杂散光源的干扰。在实验室环境下模拟煤矿井下掘进工作面的典型光照条件和杂散光源，采用矿用防爆相机采集并识别标靶上的红外LED特征点，图像滤光前后的对比结果如图2所示。

图  2  图像滤光前后对比结果

Figure  2.  Comparison results before and after image filtering

下载: 全尺寸图片 幻灯片

可以看出，采用滤镜消除了可见光光源、激光指向仪等干扰，有效简化了煤矿井下掘进工作面复杂光照下的图像处理过程。同时，考虑到相机镜头引起的图像畸变问题，需对相机镜头进行标定。采用张正友标定法获得相机内参及畸变参数，对采集的图像进行畸变矫正，提高图像处理的准确性和可靠性。图像矫正前后对比如图3所示。

图  3  图像畸变矫正对比结果

Figure  3.  Comparative results of image distortion correction

下载: 全尺寸图片 幻灯片

2.2   光斑中心提取与标靶ID映射

掘锚设备的位姿精确测量是实现掘锚设备交替作业碰撞检测系统的关键。掘锚设备的位姿测量是以红外标靶上的红外LED光斑作为图像特征信息，通过阈值分割将原始图像转换为二值图像，利用Hough轮廓检测与质心法对二值图像进行轮廓检测并计算其面积和圆度，进一步设置最小面积和最小圆度阈值滤除小光斑点，筛选面积大于阈值并且圆度大于圆度阈值的光斑为有效光斑，通过对图像的像素值进行加权求和计算光斑的几何矩，提取的特征点光斑中心结果如图4所示。在此基础上，利用工业相机(MV-EMV510M)进行不同场景、不同角度以及部分遮挡条件下的红外LED标靶图像数据采集，对具有不同二进制编码ID标识的标靶图像进行人工标注，并将其作为标签文件，以建立红外LED标靶与ID号之间的映射关系，从而完成红外标靶数据集构建。同时，基于Pytorch深度学习框架，采用YOLOv8目标检测算法对红外标靶数据集进行训练并获得红外标靶ID映射识别模型，得到的标靶ID映射识别结果如图5所示。进而根据提取的光斑中心和标靶ID映射识别结果进行后续掘锚设备的跟踪和定位。

图  4  光斑中心提取结果

Figure  4.  Results of spot center extraction

下载: 全尺寸图片 幻灯片

图  5  标靶ID映射识别结果

Figure  5.  Target ID obtained by mapping and identification

下载: 全尺寸图片 幻灯片

3   基于改进稀疏光流的掘锚设备跟踪定位方法

3.1   Shi-Tomasi角点检测

角点检测用于识别图像中的关键点，角点即具有明显灰度变化的像素点。Shi-Tomasi角点检测算法^[32]是对Harris角点检测算法的改进，其核心思想是通过移动窗口在图像上进行局部特征计算，确定窗口内灰度值的变化情况。设图像在点(x,y)处的灰度值为A(x,y)，以该点为中心建立窗口H，将图像窗口平移($\nabla $x,$\nabla $y)后产生的灰度变化$E(\nabla x,\nabla y)$表示为：

其中，h(x,y)为窗口函数，A(x,y)和A(x+$\nabla $x,y+$\nabla $y)为窗口内像素点移动前后的灰度值。

将式(1)中A(x+$\nabla $x,y+$\nabla $y)泰勒展开得：

将式(2)代入式(1)化简后近似可得：

令${\boldsymbol{B}} = \left[ \begin{gathered} \;\;A_x^2\quad \quad {A_x}{A_y} \\ {A_x}{A_y}\quad \;\;\;\;A_y^2\quad \\ \end{gathered} \right]$，则式(3)可表示为：

其中，B为2×2的矩阵，求矩阵B的特征值，设2个特征值分别为α和β，若α≤β，${\alpha _{{\mathrm{max}}}}$<α，(${\alpha _{{\mathrm{max}}}}$为图像角点较小特征值里的最大值)，则该角点为强角点。Shi-tomasi角点检测算法引入自适应权重，不易受图像旋转、光照条件、视角变化和噪声等因素的影响。

3.2   稀疏光流原理

LK(Lucas-Kanade)光流法利用像素之间的局部空间连续性假设，通过分析图像局部区域内像素灰度值的差异来推断像素点的运动方向和速度信息，从而得到运动矢量的估计值。LK光流法的成立有3个前提条件：

1)图像光照变化不大。图像序列的前后相邻帧中，目标对象的像素点的外观保持不变，即目标像素点在前后帧具有恒定的亮度，表达式为：

2)目标在帧间的位移呈微小波动。图像的运动相对于时间的变化比较缓慢，即基本的光流约束方程为：

式(6)中，令$u = {\mathrm{d}}x/{\mathrm{d}}t$，$v = {\mathrm{d}}{{y}}/{\mathrm{d}}t$。

3)局部小领域内像素点运动参数相同。在同一场景中，同一表面上的点往往具有相似的运动特征，并且它们在图像表面上的成像位置也会相邻。LK光流法中选择了一个目标区域作为临近点集，针对每个点建立了一个方程。通过求解这些方程，可以得到中心点的运动参数。其矩阵形式为：

式(7)可以进一步简写为：

解最小二乘即可得到式(5)：

由此可以得到图像上每一个特征点的速度。LK光流法适用于两帧图像间目标物体的小尺度低速运动，在实际应用中，物体的运动往往是复杂的，变化尺度也比较大，因此需要一种更加鲁棒的算法来解决这一问题。金字塔LK光流法是一种通过多层次的图像处理实现由粗到细的光流跟踪方法。该方法基于金字塔结构，在最高层对两幅图像进行LK光流计算，然后利用上一层估计的结果作为初始点，不断重复这一过程，直到达到金字塔的最底层。满足在不同尺度下分别考虑物体的运动情况，从而实现对更快目标的跟踪，提高了算法的鲁棒性和准确性。

掘锚设备交替作业中，由于掘锚设备和防爆摄像机之间存在相对运动，会产生大量非运动目标的光流子。这些非运动目标的光流子指的是由于背景干扰和噪声引起的图像中的虚假运动信息。导致算法检测结果的精度显著降低。另外由于巷道中随机不可控因素引起的标靶局部遮挡，会造成标靶识别不准确导致检测失效。因此，将红外标靶光斑作为关键特征点，使用掩膜限制标靶特征点的检测区域，在掩膜中的区域使用Shi-Tomasi算法检测内部角点，当标靶因不可控因素导致局部遮挡时，也能快速精准地检测出目标标靶并对其进行跟踪，具有较高的鲁棒性和精度，避免直接使用SIFT、SURF、ORB等特征检测方法可能带来的误匹配、精度低等问题，从而实现标靶的精确检测和跟踪，为后续掘锚设备位姿测量奠定基础。改进稀疏光流算法对比如图6所示。

图  6  改进稀疏光流算法前后跟踪结果对比

Figure  6.  Comparison results of the improved and original sparse optical flow algorithm

下载: 全尺寸图片 幻灯片

3.3   掘锚设备相对位姿解算模型

建立如图7所示的掘锚设备相对位姿解算模型，提取红外LED标靶内部特征点光斑，利用红外标靶的二进制编码ID映射识别模型，得到标靶ID信息，同时选取分布于红外LED标靶共面的4个顶点的特征点光斑解算掘锚设备相对位姿，利用基于PNP的位姿解算方法对红外LED特征点的空间三维坐标进行计算。其中，标靶1坐标系${O_{{\mathrm{b}}1}}({X_{{\mathrm{b}}1}},{Y_{{\mathrm{b}}1}},{Z_{{\mathrm{b}}1}})$，标靶2坐标系${O_{{\mathrm{b}}2}}({X_{{\mathrm{b}}2}},{Y_{{\mathrm{b}}2}},{Z_{{\mathrm{b}}2}})$，防爆相机1坐标系$ {O_{{\mathrm{c}}1}}({X_{{\mathrm{c}}1}},{Y_{{\mathrm{c}}1}},{Z_{{\mathrm{c}}1}}) $，防爆相机2坐标系$ {O_{{\mathrm{c}}2}}({X_{{\mathrm{c}}2}},{Y_{{\mathrm{c}}2}},{Z_{{\mathrm{c}}2}}) $，旋转矩阵${\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{c}}$和${\boldsymbol{R}}_{\mathrm{b}}^{{\mathrm{c}}'}$，平移矩阵${\boldsymbol{T}}_{\mathrm{b}}^{\mathrm{c}}$和${\boldsymbol{T}}_{\mathrm{b}}^{{\mathrm{c}}'}$，$ {P}_{1}、{P}_{2}、{P}_{3}、{P}_{4} $和$ {P}'_{1}、{P}'_{2}、{P}'_{3}、{P}'_{4} $为红外LED特征点，形状为200 mm×200 mm正方形，标靶特征点之间空间位置关系已知，其在图像坐标系下形成的像点为$ {n}_{1}、{n}_{2}、{n}_{3}、{n}_{4} $和$ {n}'_{1}、{n}'_{2}、{n}'_{3}、{n}'_{4} $。

图  7  掘锚设备相对位姿解算模型

Figure  7.  Relative pose solution model for excavating and anchoring equipment

下载: 全尺寸图片 幻灯片

基于特征点在图像上的位置和预先设定的先验知识，通过光斑特征提取获得特征点的像素坐标$({u_1},{v_1})$和$({u_2},{v_2})$，进一步转换为图像坐标$({X_1},{Y_1})$和$({X_2},{Y_2})$，利用相机针孔投影模型和空间几何投影约束条件，对特征点在防爆相机坐标系下的空间三维坐标$({X_{{\mathrm{c}}i}},{Y_{{\mathrm{c}}i}},{Z_{{\mathrm{c}}i}})$进行求解，从而实现位姿解算。标靶坐标系到防爆相机坐标系的转化属于刚体变换，根据Chasles定理，在标靶1坐标系和防爆相机1坐标系下，假设标靶1的红外LED特征点坐标分别为${P_{{\mathrm{b}}i}}({X_{{\mathrm{b}}i}},{Y_{{\mathrm{b}}i}},{Z_{{\mathrm{b}}i}})$和${P_{{\mathrm{c}}i}}({X_{{\mathrm{c}}i}},{Y_{{\mathrm{c}}i}},{Z_{{\mathrm{c}}i}})$，因此，建立一个防爆相机1坐标系与标靶1坐标系间的转换关系，可以表示为：

其中，旋转矩阵${\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{c}}$和平移矩阵${\boldsymbol{T}}_{\mathrm{b}}^{\mathrm{c}}$可以通过对偶四元数表示，对偶四元数由2个基本四元数组成。

其中，r和s分别称为对偶四元数的实部和对数部分，对偶运算符$\xi $遵循${\xi ^2} = 0,\xi \ne 0$，并满足以下2个约束条件：

利用对偶四元数将旋转矩阵${\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{c}}$和平移矩阵${\boldsymbol{T}}_{\mathrm{b}}^{\mathrm{c}}$分别表示为：

另外，由于存在误差，假设红外LED特征点在摄像机坐标系下对应的实测值为$ {\bar P_{{\text{c}}i}} $，建立掘锚设备位姿解算模型，其中，N表示掘锚设备机身所安装标靶的红外LED特征点个数。

根据四元数的旋转矩阵用对偶四元数表示，得到目标方程：

其中：

通过求解目标方程，获得对偶四元数的实部r和对偶部分s，代入到式(9)，计算出防爆相机坐标系与标靶坐标系间的旋转矩阵${\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{c}}$和平移向量${\boldsymbol{T}}_{\mathrm{b}}^{\mathrm{c}}$。结合防爆相机和红外LED标靶在掘锚设备机身的位置关系，得到悬臂式掘进机和锚杆钻机间的相对位姿。同理，在标靶2坐标系和防爆相机2坐标系下，建立防爆相机2坐标系与标靶2坐标系间的转换关系，利用对偶四元数表示旋转矩阵${\boldsymbol{R}}_{\mathrm{b}}^{{\mathrm{c}}'}$和平移矩阵${\boldsymbol{T}}_{\mathrm{b}}^{{\mathrm{c}}'}$，通过求解目标方程，获得对偶四元数的实部${{\boldsymbol{r}}{'}}$和对偶部分${{\boldsymbol{s}}{'}}$，最终得到锚杆钻机和悬臂式掘进机间的相对位姿。

4   掘锚设备间的碰撞检测

4.1   数字孪生体构建

掘进工作面虚拟空间建立包括掘进工作面虚拟场景与掘锚设备虚拟模型。建立流程如图8所示。

图  8  掘进工作面数字孪生体构建流程

Figure  8.  Construction flowchart of digital twin of exacavating face

下载: 全尺寸图片 幻灯片

首先在Soildworks中对掘进工作面虚拟模型以及掘锚设备进行建模与装配，将导出.STEP格式文件导入到3Ds Max中对模型进行二次处理，完成虚拟模型单位调整、比例关系调整、渲染等；处理后的虚拟模型以.FBX文件格式导入Unity3D，并对掘锚设备机身运动部件的从属关系进行整合，最后添加C#脚本分别对掘锚设备虚拟模型进行功能开发、交互开发，使虚拟模型与物理实体的运动效果保持一致。

4.2   孪生数据传输

孪生数据承载着掘进工作面物理空间中掘锚设备的运动信息，是监测掘锚设备状态、发现异常的依据。在低延迟的前提下，实现掘锚设备虚拟模型与物理实体的运动状态同步的核心在于孪生数据在虚拟空间与物理空间之间的实时传输以及交互。为了保证数据的可靠性和完整性，采用TCP协议的Socket通信，在Python和Unity3D之间建立可靠连接，通信流程如图9所示，

图  9  Socket通信流程

Figure  9.  Socket communication flowchart

下载: 全尺寸图片 幻灯片

Python服务器端和Unity3D客户端分别创建一个Socket对象，设置绑定的地址和端口，Listen()监听Unity3D客户端的请求，当Python服务器端监听到Unity3D客户端的连接请求后，双方建立通信，通过Unity3D客户端数据请求，Python服务器端数据回应的机制来传输数据，同时实时更新数据，将掘锚设备的运行数据传输到Unity3D中，数据传输结束后，调用close关闭连接。

4.3   模型驱动与碰撞检测

在掘进工作面虚拟空间中，由于物理空间坐标系与虚拟空间坐标系不同，在数据处理中将物理空间的笛卡尔右手坐标系转换为虚拟空间的笛卡尔左手坐标系并建立全局坐标系完成坐标统一后，通过C#脚本实时驱动虚拟模型并结合OBB碰撞检测算法实现碰撞检测，建立流程如图10所示。

图  10  虚拟模型驱动与碰撞检测流程

Figure  10.  Flowchart of virtual model driving and collision detection

下载: 全尺寸图片 幻灯片

实时数据驱动虚拟模型分为数据层、逻辑层和表现层，数据层是实现数据驱动虚拟模型的基础，由数据采集系统、服务器组成；数据采集系统负责获取掘锚设备的实时运行数据，并将数据通过服务器传输给Unity3D；服务器在接收到来自逻辑层的数据请求后开始不断进行数据的实时更新。逻辑层是指在Unity3D中使用C#脚本开发模型驱动的部分，这些脚本会对需要的实时数据进行采集、传输和处理，并将其作为参数传递给脚本函数，通过调用函数实现物体的运动。编写完成的驱动脚本会绑定到相应的悬臂式掘进机虚拟模型和锚杆钻机虚拟模型上，从而实现虚拟模型的脚本驱动。数据层的服务器收到逻辑层的请求后，会对掘锚设备的位姿进行检测，并将所需的位姿数据反馈给服务器，实现数据的读取。脚本在接收到服务器数据后，根据脚本接收到的物理空间中掘锚设备的位姿信息对虚拟空间中掘锚设备虚拟模型的位姿信息进行修改，从而驱动悬臂式掘进机虚拟模型和锚杆钻机虚拟模型运动，完成对掘锚设备交替作业的映射。表现层是基于Unity3D客户端平台的数字孪生技术应用，通过数字孪生体实时显示掘锚设备的运行状态。

表现层的虚拟模型状态由逻辑层的C#脚本控制，这些脚本以组件的形式绑定在悬臂式掘进机和锚杆钻机的虚拟模型上，以面向对象的方式实现虚拟模型信息的实时更新，从而实现对实际掘锚设备交替作业的三维可视化。OBB碰撞检测算法一种领先的方向包围盒，用于确定不同形状的物体之间是否存在碰撞。其主要优点在于可以根据被检测对象的形状特点灵活地进行包围。当几何体旋转时，只需要对OBB的基底轴进行相应的旋转即可实现快速的碰撞检测。因此，OBB碰撞检测算法被广泛应用于刚体间碰撞检测，该算法的关键在于找到使包围盒在特定方向上体积达到最小值的最佳方向。本研究中为悬臂式掘进机和锚杆钻机的虚拟模型分别添加盒型碰撞器(Box Collider)，采用OBB碰撞检测算法对悬臂式掘进机和锚杆钻机进行碰撞检测。

图11为悬臂式掘进机和锚杆钻机在巷道中的作业流程图。图11a—图11d为悬臂式掘进机完成截割作业退后到左侧帮停机后，锚杆钻机从右侧帮行至煤壁处开始支护的过程；图11e—图11h为锚杆钻机完成支护作业原路退回到悬臂式掘进机后方过程，当悬臂式掘进机和锚杆钻机的距离足够接近时，均可发生碰撞。

图  11  悬臂式掘进机和锚杆钻机作业过程

Figure  11.  Operation process of boom-type roadheader and anchor drilling rigs

下载: 全尺寸图片 幻灯片

由图11可知，在煤矿井下掘进工作面，悬臂式掘进机和锚杆钻机的碰撞发生在近距离交会过程，本文采用OBB碰撞检测算法对悬臂式掘进机和锚杆钻机之间的碰撞进行检测。OBB碰撞检测原理如图12所示，其中，A表示悬臂式掘进机包围盒，B表示锚杆钻机包围盒，对于A和B包围盒，如果它们之间没有发生重叠，那么一定存在一个平面能够将两个设备分离开来。A和B间的直线表示一个可以将其分离的平面d。将垂直于平面的线称为分离轴L，通过计算A和B包围盒在L上的投影$ {{M}_{\mathrm{A}}} $和$ {M_{\mathrm{B}}} $，判断2条粗实线$ {M_{\mathrm{A}}} $和$ {M_{\mathrm{B}}} $是否发生重叠。对于A和B包围盒，分离轴L是指悬臂式掘进机和锚杆钻机包围盒各边的法线，如图12中箭头所示的轴向。一旦找到一个分离轴L，通过检查掘锚设备在该轴上的投影是否相交来确定A和B包围盒是否相交，从而判断悬臂式掘进机和锚杆钻机在物理空间中是否发生碰撞。

图  12  OBB碰撞检测原理

Figure  12.  Principles of OBB collision detection

下载: 全尺寸图片 幻灯片

由图12可知，由于悬臂式掘进机和锚杆钻机的包围盒是矩形，$ {p_1} $与$ {p_3} $，$ {p}_{2} $与$ {p_4} $，$ {q_1} $与$ {q_3} $，$ {q_2} $与$ {q_4} $两两平行无需检测，在碰撞检测中只需要考虑它们互相垂直的两条边所在的轴，即4个潜在分离轴。如果4个轴上的投影判断都发生重叠，那么可以确定A和B包围盒之间发生了碰撞。

5   实验验证

5.1   实验平台搭建

数字孪生驱动的煤矿井下掘锚设备跟踪定位与碰撞检测实验平台由软件平台和硬件平台两部分组成。基于Unity3D软件开发了掘锚设备碰撞检测实验平台，其可视化界面如图13所示。

图  13  掘锚设备碰撞检测实验平台可视化界面

Figure  13.  Visual interface of collision detection experiment platform for excavating and anchoring equipment

下载: 全尺寸图片 幻灯片

该软件平台由掘进工作面环境、悬臂式掘进机、锚杆钻机组成。实际应用中是采集井下掘进工作面的悬臂式掘进机、锚杆钻机位姿，作为虚拟空间设备碰撞检测的驱动数据。掘锚设备视觉位姿测量系统实验平台如图14所示，由掘锚设备跟踪定位视觉测量系统获取物理空间悬臂式掘进机和锚杆钻机位姿数据。系统在虚拟空间显示掘锚设备动态跟踪过程，可实时动态呈现悬臂式掘进机和锚杆钻机的实时移动位置变化，并通过虚拟碰撞模型实现掘锚设备交替作业过程中设备间的碰撞预警。掘锚设备跟踪定位视觉测量系统由悬臂式掘进机物理样机、履带机器人(代替锚杆钻机)、红外LED标靶1、红外LED标靶2、MV-EMV510M防爆相机1、MV-EMV510M防爆相机2、防爆计算机、全站仪、捷联惯导组成。防爆相机均增加滤镜，防爆相机1固定在掘进机机身前方，防爆相机2固定在履带机器人上，红外LED标靶1通过螺丝固定在悬臂式掘进机机身上，红外LED标靶2固定在履带机器人上，利用全站仪测量掘锚设备的机身位置，同时使用惯导测量掘锚设备的机身姿态，验证本文算法的可靠性和精确性。

图  14  掘锚设备视觉位姿测量系统实验平台

Figure  14.  Experimental platform of visual pose measurement system for excavating and anchoring equipment

下载: 全尺寸图片 幻灯片

5.2   掘锚设备跟踪定位实验

如图15所示，为了便于实验分析和描述，在巷道起始位置建立巷道坐标系$ {O_{\mathrm{H}}}{X_{\mathrm{H}}}{Y_{\mathrm{H}}}{Z_{\mathrm{H}}} $，原点$ {O_{\mathrm{H}}} $为巷道起始点，$ {X_{\mathrm{H}}} $轴垂直于巷道掘进方向向右，$ {Y_{\mathrm{H}}} $轴沿巷道掘进方向向前，$ {Z_{\mathrm{H}}} $轴垂直于巷道底板向上；分别在悬臂式掘进机和履带式机器人中心位置建立机身坐标系$ {O_{01}}{X_{01}}{Y_{01}}{Z_{01}} $和$ {O_{02}}{X_{02}}{Y_{02}}{Z_{02}} $，原点$ {O_{01}} $和$ {O_{02}} $为掘进机和履带式机器人的机身中心点，$ {X_{01}} $轴和$ {X_{02}} $轴垂直于机身纵轴线向左，$ {Y_{01}} $轴和$ {Y_{02}} $轴沿机身纵轴线向后，$ {Z_{01}} $轴和$ {Z_{02}} $轴向上。巷道及相关实验设备的几何参数见表1。

图  15  实验坐标系构建

Figure  15.  Construction of experimental coordinate system

下载: 全尺寸图片 幻灯片

表  1  巷道及相关实验设备几何参数

Table  1.  Geometrical parameters of roadway and relevant experiment equipments

设备	参数格式	几何参数
巷道	宽×高/(mm×mm)	3 000×2 000
悬臂式掘进机	长×宽×高/(mm×mm×mm)	2 250×700×685
履带式机器人	长×宽×高/(mm×mm×mm)	600×585×530

下载: 导出CSV 

| 显示表格

为了验证本文提出的掘锚设备跟踪定位方法的稳定性和精确性，利用矿灯、烟雾机模拟煤矿井下复杂工况的干扰因素，基于5.1节所述的实验平台模拟掘锚装备交汇过程，完成实验验证。实验分成两组进行，第一组模拟悬臂式掘进机完成截割作业退后到左侧帮停机后，锚杆钻机从右侧帮行至煤壁处开始支护的过程，实验中悬臂式掘进机相对于巷道的坐标为(−2 000 mm，5 000 mm，100 mm)，控制履带机器人向前运动，通过防爆相机1采集后方带有红外LED标靶1的履带机器人的不同位置图像，设置采集时间间隔为100 ms，通过滤镜滤除杂光，进行畸变矫正、光斑中心提取、稀疏光流法跟踪以及掘锚设备位姿解算，得到防爆相机1在标靶1坐标系中的位姿，根据防爆相机1与掘进机机身坐标系之间的已知关系，及标靶1与锚杆钻机机身坐标系之间的已知关系，实现悬臂式掘进机和锚杆钻机间相对位姿的实时定位。第二组为锚杆钻机完成支护作业原路退回到悬臂式掘进机后方的过程，控制履带机器人向后运动，通过防爆相机2每间隔100 ms采集后方带有红外LED标靶2的悬臂式掘进机的不同位置图像，通过图像处理、光流跟踪及掘锚设备位姿解算，得到防爆相机2在标靶2坐标系中的位姿，通过防爆相机2与履带机器人机身坐标系的已知关系，及标靶2与掘进机机身坐标系之间的已知关系，从而实现锚杆钻机和悬臂式掘进机间相对位姿的实时定位。使用惯导与全站仪获得悬臂式掘进机和锚杆钻机的真实位姿，并与视觉测量结果进行对比，对掘锚设备视觉系统的测量精度进行验证。由于实验中掘进机与巷道之间的相对位姿未发生改变，因此锚杆钻机与掘进机之间的相对位姿以掘进机机身坐标系为基准进行表示。部分视觉位姿测量数据见表2，掘锚设备视觉相对位姿的测量误差见表3和图16所示。

表  2  掘锚设备视觉相对位姿测量结果

Table  2.  Results of relative pose of excavating and anchoring equipment based on visual measurement system

组别	标靶ID	X/mm	Y/mm	Z/mm	偏航角/(°)	横滚角/(°)	俯仰角/(°)
第一组/测量值	10	76.26	1 008.72	208.26	−4.87	−0.03	0.69
		185.44	2 016.20	210.44	4.91	−0.10	0.37
		309.57	3 024.65	215.32	5.43	−0.35	0.35
		363.02	4 022.53	211.54	−5.45	−0.05	0.33
		363.85	5 046.22	306.98	5.29	−0.26	−0.10
第一组/真实值		90.27	1 022.46	222.66	−5.06	−0.28	0.49
		200.94	2 026.76	221.54	5.05	−0.31	0.50
		319.35	3 033.33	224.99	5.55	−0.13	0.52
		353.79	4 036.99	226.67	−5.26	−0.31	0.59
		381.15	5 028.24	289.98	5.02	−0.51	0.13
第二组/测量值	1	104.27	−1 031.60	231.95	−4.85	−0.05	0.71
		215.83	−2 002.52	208.06	4.90	−0.10	0.36
		305.97	−3 013.28	210.05	5.42	−0.35	0.36
		340.53	−4 042.71	240.28	−5.47	−0.05	0.32
		363.85	−5 049.58	306.21	5.30	−0.26	−0.12
第二组/真实值		91.77	−1 021.65	222.90	−5.06	−0.29	0.49
		202.43	−2 015.46	224.65	5.06	−0.30	0.53
		319.12	−3 027.23	223.88	5.53	−0.16	0.55
		352.46	−4 028.43	228.53	−5.32	−0.33	0.58
		381.77	−5 032.37	291.46	5.04	−0.52	0.16

下载: 导出CSV 

| 显示表格

表  3  掘锚设备视觉相对位姿测量结果误差分析

Table  3.  Error analysis of visual relative pose measurement results of excavating and anchoring equipment

组别	X误差/mm	Y误差/mm	Z误差/mm	偏航角误差/(°)	横滚角误差/(°)	俯仰角误差/(°)
第一组	−14.01	−13.74	−14.40	0.19	0.25	0.20
	−15.50	−10.56	−11.10	−0.14	0.21	−0.13
	−9.78	−8.68	−9.67	−0.12	−0.22	−0.17
	9.23	−14.46	−15.13	−0.19	0.26	−0.26
	−17.30	17.98	17.00	0.27	0.25	−0.23
第二组	12.50	−9.95	9.05	0.21	0.24	0.22
	13.40	12.94	−16.59	−0.16	0.20	−0.17
	−13.15	13.95	−13.83	−0.11	−0.19	−0.19
	−11.93	−14.28	11.75	−0.15	0.28	−0.26
	−17.92	−17.21	14.75	0.26	0.26	−0.28

下载: 导出CSV 

| 显示表格

图  16  掘锚设备位姿测量误差

Figure  16.  Pose measurement errors of digging and anchoring equipment

下载: 全尺寸图片 幻灯片

根据表3以及图16的相对位姿测量测量结果可知，掘锚设备位姿跟踪定位实验的误差值保持在一定范围内，实验的位置误差不超过20 mm，角度误差不超过0.30°。实验结果表明，利用所构建的掘锚设备位姿解算模型可以获取比较精确、稳定的相对位姿信息。

5.3   掘锚设备动态碰撞检测实验

为验证构建的掘锚设备碰撞预警检测算法，以实验室现有的掘锚设备视觉位姿测量系统实验平台为依托，搭建如图17所示掘锚设备虚拟碰撞检测实验平台。在实验室搭建测试平台时，在虚拟空间对设备相对尺寸等参数作了相应的修改，以保持物理空间和虚拟空间的一致性。并在虚拟空间中建立与掘锚设备视觉位姿测量系统实验相对应的巷道坐标系。实验中，掘锚设备动态碰撞检测实验分两组实验对应掘锚设备视觉位姿测量系统实验在掘锚设备碰撞检测虚拟空间平台中进行仿真和碰撞检测，每组测量距离分别为1 000、2 000、3 000、4 000、5 000 mm。为了模拟悬臂式掘进机和锚杆钻机交替协同作业的实际运动，

图  17  掘锚设备碰撞虚实对比

Figure  17.  Comparison of virtual and physical space in the collision process of excavating and anchoring equipment

下载: 全尺寸图片 幻灯片

第一组实验设置悬臂式掘进机在履带机器人前；第二组实验设置履带机器人在悬臂式掘进机前。在局域网环境下通过Socket通信，将掘锚设备定位系统采集的位姿数据传输到掘锚设备虚拟碰撞检测平台，对虚拟设备的位置和姿态进行实时驱动，实现“虚实同步”。结合构建的OBB碰撞检测模型，在虚拟实验平台设定掘锚设备包围盒相对于原物体的膨胀系数为1.2，进而验证掘锚设备碰撞检测系统的准确性和可行性。

图17为掘锚设备碰撞虚实对比，左图为物理样机，右图为虚拟样机。图18为掘锚设备运动过程虚实数据对比结果。

图  18  掘锚设备运动虚实数据对比

Figure  18.  Comparison of virtual and real data on the movement of excavating and anchoring equipment

下载: 全尺寸图片 幻灯片

由图18分析可得，掘锚设备在运动过程中，虚拟位置与真实位置一致，其具体坐标及误差见表4。

表  4  掘锚设备运动虚实位置坐标对比

Table  4.  Comparison of virtual and real position coordinates of excavating and anchoring equipment movement

组别	真实位置			虚拟位置			X方向误差	Y方向误差
	X/mm	Y/mm		X/mm	Y/mm		X/mm	Y/mm
第一组	100.78	1 011.43		100.13	1 011.43		−0.65	0
	223.56	2 002.52		222.68	2 002.52		−0.88	0
	306.57	3 012.00		306.25	3 012.00		−0.32	0
	337.98	4 026.44		337.36	4 025.74		−0.62	−0.07
	363.85	5 021.43		363.24	5 022.33		−0.61	0.09
第二组	103.27	1 013.45		102.27	1 013.45		−1.00	0
	214.93	2 014.67		214.37	2 014.67		−0.56	0
	305.97	3 026.55		305.97	3 027.15		−0.32	0.06
	339.53	4 008.33		338.46	4 009.03		−1.07	0.07
	363.26	5 022.66		362.12	5 021.56		−1.14	−1.10

下载: 导出CSV 

| 显示表格

由表4中掘锚设备运动虚实位置坐标对比数据，可以观察到在掘锚设备作业流程中，位置坐标中X轴方向最大误差不超过1.14 mm；Y轴方向最大误差不超过1.10 mm，实验结果表明基于方向包围盒的掘锚设备碰撞预警检测算法可以实时输出掘锚设备的运动信息，满足井下掘进工作面的施工要求。

6   结 论

(1) 针对煤矿井下粉尘大、水雾多、杂光干扰、设备种类繁杂等导致的掘锚设备间碰撞问题，提出了数字孪生驱动的掘锚设备跟踪定位与碰撞预警系统，通过Unity3D软件搭建虚拟空间平台，采用基于Socket通信的数据传输与交互方法，实现了虚拟空间数字孪生模型和掘锚设备跟踪定位实验平台之间实时映射与虚实交互。

(2) 针对煤矿井下掘进工作面复杂背景干扰、局部遮挡等对视觉定位稳定性和精度影响问题，以多点红外LED标靶作为信息源，提出了一种基于二进制编码的红外标靶ID映射识别方法，通过改进稀疏光流法对红外LED标靶进行实时跟踪，建立基于PNP的掘锚设备位姿解算模型，采用基于对偶四元数方法完成位姿解算，实现了掘锚设备交替作业过程的精确跟踪定位，结合任意多边形OBB碰撞检测算法实现了掘进工作面掘锚设备的虚拟碰撞检测。

(3) 实验结果表明：掘锚设备跟踪定位实验的位置误差不超过20 mm，角度误差不超过0.30°；虚实位置坐标对比中X轴方向最大误差不超过1.14 mm；Y轴方向最大误差不超过1.10 mm，虚拟预警检测与实际碰撞一致。本系统的同步运动和碰撞检测功能实时有效，为数字孪生技术在煤矿井下掘进工作面掘锚设备跟踪定位与碰撞检测提供了新的思路。

符号注释：

$ {A_{{{n}} \times {\text{2}}}} $为n幅图像分别在x、y方向上的光流方程式；${A_x}$、${A_y}$为角点灰度值的偏导数；b为对应点在两帧图像上亮度差分；$ {b_{{{n}} \times {\text{1}}}} $为n个对应点在两帧图像上亮度差分；D_t为帧之间的时间差； $ F({\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{c}}{\boldsymbol{T}}_{\mathrm{b}}^{\mathrm{c}}) $为建立的掘锚设备位姿解算模型；$ F(r,s) $为对偶四元数的目标方程；λ、j、k为三维空间的单位分量；I为整幅图像；I(t)为第t帧的图像亮度值；$ {I_{{x}}} $为图像坐标系中x方向的差分；$ {I_{{y}}} $为图像坐标系中y方向的差分；$ {I_{{t}}} $为对应点在两帧图像上的亮度差分；t为时间；n为整幅目标图像特征点领域内的所有像素点总数；N为掘锚设备机身所安装标靶的红外LED特征点个数；$O(\nabla {{x}^{2}},\nabla {{y}^{2}}) $为泰勒展开的误差项；${P_{{\mathrm{b}}i}}$为光斑在标靶坐标系的坐标；${P_{{\mathrm{c}}i}}$光斑在相机坐标系的坐标；$ {\bar P_{{\text{c}}i}} $为红外LED特征点在摄像机坐标系下对应的实测值；$ {\boldsymbol{Q}}(r) $、$ {\boldsymbol{W}}(r) $为两个定义的四元数矩阵函数；${\boldsymbol{R}}_{\mathrm{b}}^{\mathrm{c}}$为旋转矩阵；r为对偶四元数$\hat q$的实部；s为对偶四元数$\hat q$的对数部分；${\boldsymbol{T}}_{\mathrm{b}}^{\mathrm{c}}$为平移矩阵；u为目标像素点在x方向上的速度；${V_{2{{ \times }}1}}$为n个目标像素点分别在x、y方向上的速度；v为目标像素点在y方向上的速度；$\partial {I_x}/\partial x$为像素点在x轴上的偏导；$\partial {I_y}/\partial y$为像素点在y上的偏导； $\partial {I_t}/\partial t$为像素点在时间轴上的偏导； $\xi $为对偶运算符；$ {r}_{0}、{r}_{x}、{r}_{y}、{r}_{{\textit{z}}} $和$ {s}_{0}、{s}_{x}、{s}_{y}、{s}_{{\textit{z}}} $分别为四元数r和s的4个元素。