基于双目视觉的悬臂式掘进机位姿测量技术研究

张 超，张旭辉,2,3，杜昱阳,2,3，杨文娟，张楷鑫

(1.西安科技大学 机械工程学院，陕西 西安 710054；2.陕西省矿山机电装备智能监测重点实验室，陕西 西安 710054;3.陕西省煤矿机电设备智能检测与控制科技创新团队，陕西 西安 710054)

0 引 言

随着煤矿综采工作面智能化推进，煤炭开采效率大幅提升，然而由于其复杂的工况环境和繁琐的工序导致巷道掘进自动化进展缓慢[1]。当前采掘工作队配比为1∶3.1，且掘进过程以人工操作为主，不能保证巷道成型质量。推动掘进的自动化、智能化和无人化研究是提高掘进速度，保证掘进安全的有效途径，掘进装备的位姿自主测量是实现自动化、智能化的前提。

当前巷道掘进作业中以地测部门按照巷道设计走向固定在掘进机后方巷道一定位置的点激光指向仪为掘进基准，根据前方煤壁上的激光光斑作掘进方向指示。这种指向方式要求掘进机司机经验丰富，但仍然会出现超挖欠挖，巷道成形不佳。这种落后的指向方式迫切地需要改变，为此许多专家学者做了大量的研究工作。文献[2]提出了基于全站仪的掘进机位姿测量方法，并在现场测试了有效性，但是难以实现自主测量。惯性导航技术应用在煤矿领域[3-4]，该方法可直接获得位姿信息，抗干扰强，但严重的累积误差导致需要其他信息定时修正。杜雨馨等[5]以十字激光器为特征源，利用视觉测量技术实时获取部分位姿信息，无法得到巷道轴线距离。文献[6]将双目相机与激光指向仪刚性连接拍摄机身上的特征达到求解位姿的目的，双目测量距离一般很近，导致距离较远时测量失效。基于iGPS定位技术[7]，基于空间交汇测量技术[8]、基于超宽带测量技术[9]等均能实现位姿测量，但其系统较为复杂，难以持续化运行。杨文娟等[10]将激光指向仪改进为两个平行激光指向仪束，利用单目视觉技术实现复杂背景下机身实时位姿获取，并在井下成功应用。

综上所述，悬臂式掘进机位姿测量可以分为：人工测量、半自动测量和自主测量[11]。结构简单、操作方便的自主测量方式是今后采掘装备的发展趋势。基于此，提出了基于双目立体视觉的悬臂式掘进机位姿自主测量方法，该方法采用红外LED特征替代巷道环境特征，构建双目视觉位姿测量结构，旨在为实现巷道掘进自动化、智能化和无人化研究提供借鉴。

1 双目视觉测量方案

1.1 双目视觉测量原理

理想情况下双目视觉空间点坐标测量原理如图1所示。双目相机左右视图的成像原点一致、两相机光轴平行及成像平面的极线行对齐[12]。以左相机坐标系为世界坐标系，在空间中有一点P，相对于双目相机的中心的世界坐标为(XW,YW,ZW)。

图1 双目视觉测量原理Fig.1 Principle of measurement in binocular vision

双目相机中左右相机光心分别是Ol、Or，焦距为f，基线距离为B。世界中点P在左右相机成像平面中的成像点为xl(xl,yl)、xr(xr,yr)。在理想状态下左右相机的光心一致，即

(1)

根据相似三角形原理可得：

(2)

(3)

联立式(2)和(3)从而可得：

(4)

为保证测量符合测量原理，开展测量的第一步工作应该是利用双目相机标定原理获得双目相机各自的内参数，根据双目立体校正技术对双目视觉进行校正[13]。

1.2 悬臂式掘进机双目视觉位姿测量方案

悬臂式掘进机双目视觉测量系统结构如图2所示。将掘进机简化为长方体模型，将双目相机固定在机身上镜头朝向机尾方向，便于拍摄后方的图像特征。

图2 悬臂式掘进机双目视觉测量系统结构Fig.2 Measurement structure of binocularvision for cantileverro adheader

在掘进机机身重心处建立机身坐标系Ob-XbYbZb，其中x方向指向掘进机左侧并垂直于前进方向，y方向垂直向上，z方向于前进方向一致；建立双目视觉测量坐标系Oc-XcYcZc，3个方向均与机身坐标系各方向平行但方向相反。

悬臂式掘进机双目视觉位姿测量方案如图3所示。首先采用张正友标定原理对双目相机进行内参数标定，获得两相机内参数和两相机之间的变换矩阵。利用双目相机采集双目图像获得双目图像序列，根据标定得到的参数利用双目校正技术实现双目图像的立体校正，随后利用区域生长的方法实现图像特征分割并采用高斯拟合的方式实现特征的快速精确提取。将前后2帧双目图像进行环形匹配实现双目图像的时域特征匹配，得到获得4组匹配好的特征点组，即2组相邻时刻双目图像特征点。根据前述三角测量原理，结合相机内参数计算得到2组3D空间坐标，最后利用运动估计方法求解机身的运动参数。

图3 悬臂式掘进机双目视觉位姿测量方案Fig.3 Measure mentpro gramme of binocular visual for cantilever roadheader

2 双目图像特征选取

2.1 红外LED图像特征

在视觉测量技术中，常用的图像特征有点特征、线特征、图形特征和边缘特征。在煤矿井下巷道掘进中，低照度、高粉尘和机电设备众多且集中等造成图像特征不明显或特征缺失，因此，通过人为构建图像特征的方法可以有效地保证稳定的特征。在研究截割头位姿视觉测量方法[14-15]时采用了一种红外LED灯作为图像特征，有效地避免了杂光的干扰，增加了特征的稳定性，在此基础上，人为构建红外LED标靶作为图像特征。双目相机拍摄的红外LED双目图像如图4所示。

图4 红外LED双目图像Fig.4 Binocu larimages based on infrared LED

2.2 红外LED图像井下应用

在山西某矿综掘工作面，基于红外LED特征的截割头位姿视觉测量方法在井下成功应用。如图5所示，是现场拍摄的井下红外LED标靶图像和在高粉尘、低照度、大水雾中拍摄的标靶图像。

由图5a近距离拍摄的红外LED标靶图像可以看出，在巷道掘进过程中高粉尘、大水雾和低照度等工况十分恶劣；图5b是未掘进时工业相机拍摄的红外LED标靶图像，可以看出特征非常清晰；图5c中红外LED标靶受到了粉尘水雾的影响，但是光斑特征依然清晰。在煤矿井下应用时，红外LED标靶安装在截割臂上，距离截割头和前煤壁较近，在此种情况下图像特征依然可以精确提取说明这种方案是可行的。

图5 红外LED标靶在煤矿井下的应用Fig.5 Application of infrared LED targetin underground coal mine

红外LED标靶放置在掘进机后方一定位置处，受到的粉尘和水雾影响较小，因此采用红外LED作为视觉测量中的图像特征。

3 特征提取与匹配

3.1 特征提取

图像特征提取即准确地提取红外LED光斑中心。光斑中心精确定位分为光斑区域分割和中心点定位2个步骤。

3.1.1 光斑区域分割

采用红外SE3470灯组成的红外LED标靶作为图像特征，在基于红外LED特征的双目图像中，每个光斑区域具有很强的连通性。红外LED灯的图像特征成像如图6所示，特征点成像近似椭圆，且其灰度值从光斑中心向边缘减小。

图6 红外LED光源成像与放大Fig.6 Imaging and amplifi cation of infrared LED light source

根据这种特性，采用区域生长的方法对双目图像进行光斑区域分割。区域生长算法的思想是将具有相似性的像素点合并到一起[16]。利用区域生长的算法实现红外LED特征点光斑区域分割的关键在于确定生长种子点、区域生长和停止生长的条件。在双目图像中光斑区域较多，采用人机交互的方式确定种子点不能满足需求，因此提出一种自动确定种子生长点的区域分割方法。具体实现步骤如下：

步骤①：创建一个全黑的空白图像J，即灰度值J(x,y)=0。

步骤②：以原图像I(x,y)>T&J(x,y)=0为种子的自动判定条件，确定多种子点起始生长点坐标,其中T为阈值，(x,y)为像素坐标。

步骤③：判断种子点(x0,y0)周围8邻域像素(x,y)与种子像素灰度值之差的绝对值小于某个阈值，如果满足条件，将(x,y)与(x0,y0)合并为统一区域，并将其灰度值写入图像J，并压入堆栈。

步骤④：从堆栈中取出一个像素，把它当做新的种子点(x0,y0)返回到步骤②，直到堆栈为空，此时一个光斑区域分割结束。

步骤⑤：重复步骤①—④直到图像中每个点都有归属，生长结束。如图7所示是利用区域生长算法实现的双目左图像区域分割结果。

图7 左图像光斑区域分割Fig.7 Region-dependent segment ation of spotsin leftimage

3.1.2 光斑中心定位

通过区域生长算法实现了特征点光斑区域分割，获得了各个区域的所有像素坐标。一个光斑图像的灰度分布如图8所示，近似于二维高斯分布[17]，其模型是：

图8 光斑图像的灰度分布情况Fig.8 Grayscale distribution of spot image

(5)

式中：I0为红外LED的总能量；(x,y)为高斯分布函数区域内一点坐标；(x0,y0)为光斑的成像中心坐标；σpsf为高斯函数的均方差。

采用粗提取的方法提取光斑的中心点(x′,y′)，记粗提取中心点左边的像素点坐标为(xl,y′)，中心点右侧的像素坐标(xr,y′)，中心点上方的像素点坐标为(x′,yup)，中心点下方的像素点坐标为(x′,ydown)。由公式(6)可知：

(6)

对于(xl,y′)、(x′,y′)、(xr,y′)三点，可以得出以下方程组：

(7)

由此可计算出光斑中心点坐标为：

(8)

由此可知，光斑中心点(x0,y0)可以快速计算得出。光斑中心点定位结果如图9所示。

图9 光斑中心定位结果Fig.9 Location of spot center

3.2 特征点匹配

悬臂式掘进机双目视觉位姿测量方法中的特征点匹配是时域特征点匹配，即同一帧左右图像的特征点匹配和相邻两帧之间的特征点匹配,本文采用环形匹配策略实现特征点精确匹配。环形匹配策略结构如图10所示。

图10 特征点环形匹配策略Fig.10 Ring-matching strategy for feature points

沿时间轴方向选择t1、t2时刻2帧图像，截取前后两帧双目图像，t1时刻左右图像分别是Pl1和Pr1，t2时刻左右图像分别是Pl2和Pr2。根据前文所述方法实现双目图像中光斑中心的精确定位。环形匹配具体的实现过程如下：

步骤①：建立匹配坐标系。在每幅图像中，取所有特征点像素坐标中x和y方向坐标值的最大和最小值，计算各自的平均值组成新的坐标A(xav,yav)，以此为原点，x和y方向不变建立匹配坐标系。并求取每个特征点与坐标A(xav,yav)的特征向量。

以Pl1图像中为例，假设特征点Pi坐标为(xi,yi)，其中(i=1,2,3,…,n)。计算xi中的最大值xmax和最小值xmin，并计算两者的平均值，记为xav；同理计算出yi中的最大值ymax和最小值ymin，并计算两者的平均值，记为yav。组成新的坐标点A(xav,yav)。

(9)

(10)

其平均值为

(11)

同理：

(12)

(13)

其平均值为

(14)

(15)

(16)

(17)

步骤③:匹配成功后，继续匹配t1时刻右图像中对应的特征点。t2时刻右图像中的匹配成功的特征点按步骤②找到t1时刻右图像中的匹配点，若不满足，返回t2时刻左图像选择新的特征点进行匹配。

步骤④:匹配成功后，继续匹配t1时刻左图像中对应的特征点。t1时刻右图像中的匹配成功的特征点按步骤②找到t1时刻左图像中的匹配点，若不满足，返回匹配t2时刻左图像选择新的特征点进行匹配。

步骤⑤:匹配成功后，继续匹配t2时刻左图像中对应的特征点。将t1时刻左图像中的匹配得到的特征点对应的特征向量按照步骤②对t2时刻左图像进行匹配，若匹配得到的特征点与起始匹配点一致，匹配成功；否则匹配失败。

步骤⑥:选择t2时刻左图像新的特征点按步骤②—⑤循环执行，直至t2时刻左图像中所有特征点已执行匹配过程。

环形匹配结果如图11所示。

图11 特征点环形匹配结果Fig.11 Result soffeature point sbasedon ring matching

4 悬臂式掘进机运动估计

利用环形匹配结果，结合双目视觉测量原理就可以计算出相机坐标系的2组3D点空间坐标。那么基于双目视觉的悬臂式掘进机运动估计问题就变成了已知2组匹配好的3D点坐标求解相邻时刻的运动变换参数[18]。

假设2组3D点表示为

P=p1,…,pn,P′=p1′,…,pn′

(18)

求解变换矩阵R,D满足：

∀i,pi=Rpi′+D

(19)

得到的R和D就是相机运动的变换矩阵。采用线性代数方法求解(SVD)该问题。

定义第i对点的误差项为

ei=pi-Rpi′+D

(20)

则构建其最小二乘问题，求解R和D使得误差平方和达到极小：

(21)

首先定义两组点的质心：

(22)

将公式(21)中的最小二乘函数可化简为

(23)

交叉项中pi-p-R(pi′-p)在求和之后为0，因此目标函数可以优化为

(24)

1)计算2组点的质心位置p，p′，然后计算每个点的去质心坐标：

qi=pi-p;qi′=pi′-p′

(25)

2)根据以下优化问题计算旋转矩阵：

(26)

3)根据第(2)步计算结果R，求得D:

展开R的误差项：

(27)

显然第1项与R无关，RTR=I，第2项也与R无关，所以目标函数简化为：

(28)

定义矩阵：

(29)

所以W是一个3×3的矩阵，对其进行SVD分解得到：

W=U∑VT

(30)

当W满秩时，R为

R=UVT

(31)

解得R后，代入公式(25)即可求出D。式中R是一个3×3的矩阵，表示两帧图像之间姿态的旋转矩阵，D是一个平移向量，表示2帧图像之间的位移变化量。

将采集的第一帧图像时的载体位姿作为初始姿态，记为P0，包括旋转矩阵R0和平移矩阵T0，则

(33)

通常初始位姿P0由煤矿地测科利用全站仪测量得到。设Pk为第k个位姿，Rk,Tk,k=0,1,2,…,k表示每1帧的旋转矩阵和平移矩阵。则具体每个时刻的位姿计算如下：

(34)

由相邻两帧即可得到位移量和姿态角的变化量，将一小段离散化的位移按照时间顺序进行迭代计算，便可得到悬臂式掘进机连续的运动轨迹，如图12所示。

图12 悬臂式掘进机运动轨迹推算Fig.12 Calculation of trajectory of cantileverroad header

5 双目视觉位姿测量策略

由于双目视觉测量原理自身的不足导致测量距离有限，考虑基于双目立体视觉的悬臂式掘进机位姿测量方法在实际使用中存在标靶移动问题，提出了一种双标靶交叉移动的悬臂式掘进机双目视觉位姿测量使用策略。如图13所示，将双目相机固定在悬臂式掘进机上，通过采集布置在后方的双红外LED标靶A和B的图像实现机身位姿检测的目的。

图13 双标靶双目测量初始状态Fig.13 Initialstate of doubletar getbino cularmea surement

在实际工作中，随着掘进效率的提升，掘进机前进的速度越来越快，双目的测量距离很难保证长距离或超长距离测量，此时可以通过交叉移动的方式，既保证测量的精度，也能保证测量的连续性。如图14所示，在标靶和相机之间的距离超过双目视觉测量距离而不能满足测量精度时，保证B标靶保持不动正常工作，同时切断A的供电使其前移至双目位姿测量的最小测量距离处，打开电源使其正常工作。移动期间，双目视觉位姿检测连续工作没有中断，这样就保证了测量的连续性。

图14 标靶A断电前移Fig.14 Target A forward after power off

将A移动至前方后，再按照同样的方法移动B至前侧，如图15所示。如此循环交替移动即可实现连续测量，弥补了其测量距离不足的缺陷。

图15 标靶B断电前移Fig.15 Target B for ward after power off

6 试验结果与分析

为有效地验证基于双目视觉的悬臂式掘进机位姿测量性能，论文分别从位置和姿态测量2个方面进行验证。

1)双目视觉位置测量精度试验。

采用MYNTEYE的D1000-IR-120/Color双目相机和SE347-003880 nm红外LED灯组成的被测标靶构成双目视觉位姿测量系统。将加装了红外滤镜的双目相机固定在精度为0.1 mm的高精度移动平台，并将红外LED光源组成的被测标靶固定于机身后方一定距离处。如图16所示是搭建的双目视觉位置测量动态试验平台。

图16 双目视觉位置测量试验验证平台Fig.16 Experimental veri fication platform for binocular visual position measurement

通过调整相机横向位移，每次移动5 cm并利用双目图像采集软件采集固定于机身后方2 m处的红外LED标靶图像。以初始位置(0,0,0)和初始姿态(0°,0°,0°)为起始位姿，得到连续的位姿测量结果。如图17所示是测量距离为2 m时，双目视觉测量方法在X方向的测量结果。

图17 测量距离为2 m时X方向实际位移Fig.17 Actual displacement in X direction when measuring distance is 2 m

从上图中可以得到，双目视觉在X方向的位移变换情况与高精度的位移变化情况完全吻合，但是由于存在测量误差的情况，出现了小幅的偏差。如图18所示是与双目相机保持同步运动时高精度移动平台位移数据。

图18 X方向位移误差Fig.18 Displace menterror of X directional

如图18所示是X方向位移测量误差，可以看出误差在±30 mm以内。如图19和图20是在移动过程中双目视觉位姿测量得到的Y和Z方向的位移变化情况。

图19 Y方向的位移变化Fig.19 Displace ment changein Y direction

其中Y方向的测量误差在(-20 mm，0 mm)之间，Z方向测量误差在±10 mm以内。如图20—图22所示，分别是通过双目视觉位姿测量方法计算得到的横滚角、航向角和俯仰角姿态值。

图20 Z方向的位移变化Fig.20 Displace ment changein Z direction

图21 横滚角变化情况Fig.21 Variationo frol langle

图22 航向角变化情况Fig.22 Variation of rawangle

其中，横滚角测量误差在±0.2°以内，航向角在(-0.5°，0.3°)之内变化，俯仰角在(-0.72°，0°)之间变化。从总体上来说，姿态的变化相对稳定且精度高。

图23 俯仰角变化情况Fig.23 Variation of pitchangle

2)双目视觉测量姿态测量试验。

试验器材包括MYEYEN120双目相机，红外LED标靶，精度为0.01°的高精度三轴转台。为便于研究，将双目相机固定在高精度三轴转台,如图24所示。

图24 双目视觉位姿测量实验验证平台Fig.24 Experimental verification plat form for binocular visual position measurement

通过旋转双目相机并利用双目图像采集软件采集固定于机身后方2 m处的红外LED标靶图像。如图25所示是高精度三轴转台回转角数据变化情况，图26是通过双目视觉计算得到的航向角数据，可以看出姿态变化情况基本一致。

图25 双目视觉航向角与高精度三轴转台对比Fig.25 Comparison of positive and high precision triaxial turbine with dual visual camera

图26 双目视觉测量航向角误差值Fig.26 Error of heading anglemea sured by binocular vision

绘制得到的误差结果如图26所示，可以得到航向角的误差在±1°以内。如图27和图28分别是俯仰角和横滚角姿态测量情况，可以看出其在(-1.2°，1°)以内变化。

图27 航向角误差值Fig.27 Errorin heading angle

图28 双目视觉测量横滚角Fig.28 Roll angle of Binocular visionmea surement

双目视觉位姿检测的横滚角在0.1°以内变化，但是由于安装过程相机不是绝对水平的原因，横滚角出现了小幅的降低。

7 结 论

1)煤矿井下高粉尘、低照度的实际工况环境，造成了严重的图像特征缺失，因此采用稳定的红外LED光源组成特征标靶作为视觉测量的目标。

2)提出了一种基于双目立体视觉的悬臂式掘进机位姿测量方法，采用高斯拟合法计算红外LED多光斑中心特征提出，同时采用环形匹配策略实现双目视觉的前后时域特征匹配并验证了匹配的正确性，根据双目视觉基本测量原理计算出光斑中心的空间三维坐标，最后利用3D-3D运动估计计算出空间位姿。

3)搭建试验验证平台评估了其测量性能。理论与实验表明：该测量方法在X方向即横向位移的测量误差在±30 mm，航向角的测量误差在±1°以内，满足煤矿井下巷道掘进的要求。