0.   引言

掘进机导航定位系统是掘进机智能化的关键组成部分^[1-3]。目前掘进机导航定位技术主要有单一测量导航技术和组合导航技术2类。在单一测量导航技术研究方面：张国喜^[4]采用惯导设备直接测量掘进机位姿；田原^[5]采用基于相机+光学靶标的视觉测量方法对掘进机机身进行定位测量；刘超等^[6]基于超宽带测距和TSOA（Time Sum of Arrival，波达时间和）定位原理对掘进机机身姿态进行测量；石勇^[7]提出了基于3D激光雷达的动态测距、测向方法，实时检测掘进机相对巷道中线的位置偏移和角度偏移；薛光辉等^[8]提出了基于激光靶向跟踪的掘进机位姿测量方法；杨文娟等^[9-10]提出了一种基于激光束特征的悬臂式掘进机机身测量系统。单一测量导航技术仅能提供单一的位置或姿态测量功能，在能见度低、空间狭窄的环境下适用性较差。组合导航技术以惯导技术为核心，同时辅助其他测量手段。在组合导航技术研究方面：刘豪^[11]将捷联惯导与里程计组合，构建了掘进机自主导航定位系统，但掘进机履带打滑造成里程计空转，进而引入较大的定位误差；张旭辉等^[12]采用全站仪与捷联惯导进行联合定位，通过卡尔曼滤波融合全站仪与捷联惯导测量数据，实现掘进机精确定位，但井下大范围移站时全站仪操作过于复杂，难以广泛推广；崔玉明^[13]提出了掘进机视觉/惯性融合定位策略，通过视觉测量系统自动识别并测量环境特征量，但井下光照不足、粉尘浓度大等因素对视觉测量造成影响；为进一步提升视觉测量的抗干扰能力，Yang Wenjuan等^[14]、雷孟宇等^[15]、黄东等^[16]提出了直接在掘进机机身上设置光学靶标，利用视觉测量系统在掘进机后端对靶标进行测量。光学靶标自身可发光、光束可透粉尘等特性有效克服了光照不足、粉尘浓度大等环境因素的影响，因此惯导+视觉测量+光学靶标组合的掘进机导航定位成为目前广泛应用的技术方案^[17]。

通常情况下，光学靶标由固定在同一平面上、排列参数已知的一系列靶标点组成，最常见的形式是由4个呈矩形分布的靶标点组成。光学靶标与惯导组成的掘进机位姿测量装置固定在掘进机尾部，并且朝向视觉测量设备。视觉测量设备采集到光学靶标的图像后，获得图像中靶标点对应的光斑像素坐标，采用N点位姿透视求解（Perspective-N-Point，PNP）算法计算得到光学靶标中心位置坐标，并将该坐标信息发送至惯导系统进行组合导航解算，得到掘进机的位置和姿态信息。但由于掘进工作面经常会出现其他设备（如电缆、锚杆钻臂、风筒等）遮挡靶标点的情况，此时相机采集的光斑图像与靶标点未被遮挡时采集的光斑图像不符，进而造成PNP算法无法正常解算，导致掘进机定位中断。

本文以光学靶标、视觉测量系统、惯导系统构成的掘进机组合式导航定位系统为研究对象，提出了一种光学靶标遮挡条件下掘进机定位解算方法。掘进机组合式导航定位系统获取无遮挡情况下的光学靶标图像并将图像中的光斑进行标记，当下一帧图像中部分靶标点被遮挡后，通过帧间图像匹配，得到被遮挡靶标点的图像信息，利用惯导提供的靶标姿态信息，建立投影后的靶标点与光斑的对应关系，进而求解出被遮挡靶标点对应的光斑像素坐标，从而解决靶标被遮挡情况下掘进机定位中断的问题。

1.   掘进机组合式导航定位原理

掘进机组合式导航定位系统采用惯导+视觉测量+光学靶标的定位方案，由悬挂在掘进机后端巷道顶板上的激光导引装置和安装在掘进机上的位姿测量装置组成，如图1所示。激光导引装置包含测量相机、指向激光器、姿态测量模块和调整机构。通过预先标定，指向激光器的激光束与相机坐标系O_cX_cY_cZ_c的Y_c轴重合。位姿测量装置包含光学靶标、惯导系统和数据解算模块。光学靶标由4个呈矩形分布的靶标点组成，光学靶标坐标系O_tX_tY_tZ_t与惯导坐标系O_iX_iY_iZ_i相平行。

图  1  掘进机组合式导航定位系统

Figure  1.  Combined navigation and positioning system for roadheader

下载: 全尺寸图片 幻灯片

激光导引装置的相机坐标系O_cX_cY_cZ_c的Y_c轴与巷道掘进方向一致，且Y_c轴位于巷道中轴面上，X_c轴与巷道宽度方向一致，Z_c轴与巷道高度方向一致。位姿测量装置中的光学靶标朝向激光导引装置，且光学靶标坐标系O_tX_tY_tZ_t、惯导坐标系O_iX_iY_iZ_i与掘进机机身坐标系O_rX_rY_rZ_r完全平行。因此惯导系统提供的航向角、横滚角和俯仰角代表光学靶标和掘进机机身的航向角、横滚角和俯仰角，通过与地测部门提供的巷道航向角作差，可得掘进机机身相对于巷道坐标系的航向角、俯仰角和横滚角。

激光导引装置的测量相机检测光学靶标上光斑坐标，采用PNP算法对光学靶标进行坐标定位，即确定光学靶标坐标系原点O_t（光学靶标中心点）在相机坐标系中的坐标。由于相机坐标系与巷道坐标系平行，所以该定位坐标也反映了光学靶标中心点在巷道中的坐标。激光导引装置解算得到的初步定位结果通过无线方式传输至位姿测量装置中的惯导系统，由于光学靶标坐标系、惯导坐标系与掘进机机身坐标系完全平行，所以惯导系统内部可直接采用初步定位结果进行组合导航解算，并通过坐标位置推算出掘进机机身坐标系原点O_r在巷道内的准确坐标及掘进机机身的航向角、俯仰角和横滚角。

2.   光学靶标遮挡条件下掘进机定位解算方法

掘进机组合式导航定位系统的光学靶标与惯导系统都集成在位姿测量装置中，因此当光学靶标中部分靶标点被遮挡时，可根据已知的靶标几何尺寸结合惯导系统提供的靶标姿态信息，对被遮挡靶标点对应的光斑像素坐标进行推算，进而采用PNP算法求得光学靶标中心位置的空间坐标，实现掘进机定位解算。

2.1   光斑标记

相机采集无遮挡条件下光学靶标图像，通过灰度阈值处理并识别4个光斑的位置，按顺时针方向将4个光斑依次标记为$ A， B， C， D $，如图2所示。使用高斯拟合方法^[18-20]获得$ A， B， C， D $ 4个光斑对应的像素坐标，分别为（$ {\mu }_{{A}}， {v}_{{A}} $），（$ {\mu }_{{B}}， {v}_{{B}} $），（$ {\mu }_{{C}}， {v}_{{C}} $），（$ {\mu }_{{D}}， {v}_{{D}} $）。

图  2  光斑标记与辅助矩形区域框构建

Figure  2.  Labeling of light spot and construction of auxiliary rectangular region box

下载: 全尺寸图片 幻灯片

将4个光斑构造成矩形G，然后将G按一定比例扩大，构造辅助矩形区域框。将辅助矩形区域框4个顶点按顺时针方向设为O₁，O₂，O₃，O₄，其对应像素坐标分别为（$\mu_{O_ 1}， v_{O_ 1} $），（$\mu_{O_ 2}， v_{O_ 2} $），（$\mu_{O_ 3}， v_{O_ 3} $），（$\mu_{O_ 4}， v_{O_ 4} $）。

2.2   光斑匹配

由于掘进机移动速率较低，4个靶标点在下一帧图像中的光斑像素位置不会发生剧烈变化。在获取下一帧图像后，可以参考上一帧图像中构造的辅助矩形区域框。相较于其他光斑，A点对应的靶标点在下一帧图像中的光斑$ {A}' $应与辅助矩形区域框顶点O₁的欧氏距离最小，如图3所示。将下一帧图像中检测出来的4个光斑依次标记为$ {A}'， {B}'， {C}'， {D}' $。如果光学靶标的部分靶标点被遮挡，则可以根据辅助矩形区域框顶点的关系确定缺失的光斑标记。

图  3  帧间光斑匹配

Figure  3.  Light spot matching between the frames

下载: 全尺寸图片 幻灯片

2.3   无遮挡情况下靶标点成像投影关系

已知光学靶标上4个靶标点T_A，T_B，T_C，T_D构成长为2a、宽为2b的矩形靶面，如图4所示。在光学靶标坐标系O_tX_tY_tZ_t中，4个靶标点的坐标分别为T_A（−a，0，b），T_B（a，0，b），T_C（a，0，−b），T_D（−a，0，−b）。光学靶标坐标系O_tX_tY_tZ_t与惯导坐标系O_iX_iY_iZ_i完全平行，利用惯导坐标系O_iX_iY_iZ_i与相机坐标系O_cX_cY_cZ_c的夹角，可将由T_A，T_B，T_C，T_D构成的光学靶标坐标系O_tX_tY_tZ_t通过姿态矩阵变换生成与相机坐标系O_cX_cY_cZ_c平行的由$ {T}_{{{{A}}}'} $，$ {T}_{{{{B}}}'} $，$ {T}_{{{{C}}}'} $，$ {T}_{{{{D}}}'} $构成的投影靶面坐标系O_{t_P}X_{t_P}Y_{t_P}Z_{t_P}，其中$ {T}_{{{{A}}}'} $，$ {T}_{{{{B}}}'} $，$ {T}_{{{{C}}}'} $，$ {T}_{{{{D}}}'} $分别为4个靶标点T_A，T_B，T_C，T_D在投影靶面坐标系下的对应投影点。

图  4  实际靶标点根据惯导姿态信息的投影变换

Figure  4.  Projection transformation of the actual target point according to the attitude information provided by inertial navigation

下载: 全尺寸图片 幻灯片

光学靶标坐标系O_tX_tY_tZ_t下的靶标点T_A，T_B，T_C，T_D在投影靶面坐标系O_{t_P}X_{t_P}Y_{t_P}Z_{t_P}中的投影点$ {T}_{{{{A}}}'} $，$ {T}_{{{{B}}}'} $，$ {T}_{{{{C}}}'} $，$ {T}_{{{{D}}}'} $可表示为

式中$ {{\boldsymbol{R}}}_{{\mathrm{b}}}^{{\mathrm{t}}} $为实际靶面坐标系到投影靶面坐标系的转换矩阵。

式中γ，α，β分别为惯导系统提供的航向角、俯仰角、横滚角。

光学靶标在经过姿态变换后，投影到与相机成像平面（相机坐标系X_cO_cZ_c平面）平行的投影靶面坐标系上的投影点坐标，用于后续光斑像素坐标的推算。

2.4   被遮挡靶标点对应的光斑像素坐标推算

下面对检测到3个光斑和2个光斑的情况分别进行缺失光斑像素坐标推算。

2.4.1   1个靶标点被遮挡时光斑像素坐标推算

如果光学靶标中的1个靶标点被遮挡，则在下一帧图像中的辅助矩形区域框内将找到3个光斑。对检测到的3个光斑进行标记，标记结果为A，B，C或A，B，D或A，C，D或B，C，D 4种情况。

以光斑标记结果A，B，C为例（被遮挡点为D点），由式（1）可得4个靶标点的投影点$ {T}_{{{{A}}}'} $，$ {T}_{{{{B}}}'} $，$ {T}_{{{{C}}}'} $，$ {T}_{{{{D}}}'} $的理论坐标，各个投影点之间的理论距离为$ {L}_{{{T_{A'}}}\_ T_{{B'}}} $，${L}_{{{T_{A'}}}\_T_{{C'}}} $，$ {L}_{{{T_{A'}}}\_T_{{D'}}} $，$ {L}_{{{T_{B'}}}\_T_{{C'}}} $，$ {L}_{{{T_{B'}}}\_T_{{D'}}} $，$ {L}_{{{T_{C'}}}\_T_{{D'}}} $。提取无遮挡的3个靶标点在相机成像平面中的像素坐标，分别为（$ {\mu }_{{A}}， {v}_{{A}} $），（$ {\mu }_{{B}}， {v}_{{B}} $），（$ {\mu }_{{C}}， {v}_{{C}} $）。被遮挡的$ D $点的像素坐标为（$ {\mu }_{{D}}， {v}_{{D}} $），则通过几何比例关系可得

由式（1）−式（3）可得$ D $点的像素坐标（$ {\mu }_{{D}}， {v}_{{D}} $）。结合图像中已知的3个光斑的像素坐标，使用PNP算法求得光学靶标中心点O_t的空间坐标，实现对光学靶标的定位测量。

2.4.2   2个靶标点被遮挡时光斑像素坐标推算

如果光学靶标中的2个靶标点被遮挡，则在下一帧图像中的辅助矩形区域框内仅能找到2个光斑。对检测到的2个光斑进行标记，标记结果为A，B或A，D或B，C或C，D 4种情况。

以光斑标记结果A，B为例（被遮挡点为C，D点），通过式（1）求得4个靶标点的投影点$ {T}_{{{{A}}}'} $，$ {T}_{{{{B}}}'} $，$ {T}_{{{{C}}}'} $，$ {T}_{{{{D}}}'} $的理论坐标，则被遮挡的C点的像素坐标通过几何比例关系求得。

由式（1）和式（4）可得像素坐标的2个解（$ {\mu }_{{C}}'， {v}_{{C}}' $），（$ {\mu }_{{C}}''， {v}_{{C}}'' $）。计算这2个解距O₃（$\mu_{O_ 3}， v_{O_ 3} $）的欧氏距离，欧氏距离最小值对应点的坐标为C点的像素坐标，如图5所示。

图  5  2个靶标点遮挡情况下像素坐标推算

Figure  5.  Calculation of pixel coordinates when two target points are blocked

下载: 全尺寸图片 幻灯片

同理，被遮挡的D点像素坐标通过几何比例关系求得。

由式（1）和式（5）可得D点的像素坐标有2个解（$ {\mu }_{{D}}'， {v}_{{D}}' $），（$ {\mu }_{{D}}''， {v}_{{D}}'' $）。计算这2个解距O₄（$\mu_{O _4}， v_{O_ 4} $）的欧氏距离，欧氏距离最小值对应点的坐标为D点的像素坐标。

3.   掘进机定位试验

为验证光学靶标被遮挡情况下掘进机定位的精确性和稳定性，在山东能源集团新巨龙煤矿11302回采工作面搭建井下试验平台，掘进巷道方向为北偏东209.355°。位姿测量装置安装在EBZ260掘进机机尾的正上方，光学靶标朝向机尾方向，靶面尺寸为400 mm×360 mm。激光导引装置悬挂在后端巷道顶板上。通过人工调整使得激光导引装置悬挂方向与巷道掘进方向一致，此时相机坐标系与巷道坐标系一致。测试前通过试验标定出惯导坐标系与掘进机机身坐标系的安装偏角，以惯导系统输出的姿态角（含标定的安装偏角）与地测部门给出的巷道方向之差作为掘进机相对于巷道坐标系的航向偏角、俯仰角和横滚角，同时也是光学靶标坐标系相对于相机坐标系的航向偏角、俯仰角和横滚角。

掘进机组合式导航定位系统正常工作后，按照系统提供的掘进机位置坐标和姿态信息，将掘进机驾驶到距离激光导引装置15，25，40，60 m的测试点来采集光靶图像。试验过程中，用手对相应的靶标点进行遮挡，分为随机遮挡1个靶标点和遮挡同侧2个靶标点2种情况。为便于比较，每个测试点下掘进机的俯仰角、横滚角、航向偏角尽量调整为固定值，以没有遮挡情况下解算得到的定位结果作为基准值，并与不同遮挡情况下定位结果进行对比，结果见表1。其中x，y，z分别为沿巷道宽度方向、巷道掘进方向、巷道高度方向的定位结果，Δx，Δy，Δz分别为沿巷道宽度方向、巷道掘进方向、巷道高度方向的定位误差（测量值与基准值之差）。

表  1  不同距离下掘进机定位试验结果

Table  1.  Experimental results of roadheader positioning at different distances

掘进机距激光导引装置距离/m	姿态角/（°）	靶标点	定位结果/mm				定位误差/mm
			x	y	z		Δx	Δy	Δz
15	俯仰角：−0.11	A，B，C，D	304	14925	−903		−	−	−
		A，B，C	303	14900	−902		1	25	−1
		A，B，D	303	14920	−904		1	5	1
	横滚角：−0.35	A，C，D	304	14918	−902		0	7	−1
		B，C，D	304	14938	−904		0	−13	1
		A，B	305	14935	−903		−1	−10	0
	航向偏角：−1.14	C，D	303	14939	−904		1	−14	1
		A，D	305	14960	−904		−1	−35	1
		B，C	303	14890	−904		1	35	1
25	俯仰角：0.20	A，B，C，D	258	24951	−943		−	−	−
		A，B，C	259	24919	−939		−1	32	−4
		A，B，D	258	24972	−947		0	−21	4
	横滚角：0.11	A，C，D	258	24917	−940		0	34	−3
		B，C，D	258	24978	−947		0	−27	4
		A，B	259	24926	−941		−1	25	−2
	航向偏角：0.06	C，D	258	24924	−942		0	27	−1
		A，D	259	24977	−943		−1	−26	0
		B，C	258	24962	−943		0	−11	0
40	俯仰角：−0.12	A，B，C，D	49	40036	−869		−	−	−
		A，B，C	49	40069	−869		0	−33	0
		A，B，D	49	40052	−870		0	−16	1
	横滚角：−0.22	A，C，D	49	40007	−868		0	29	−1
		B，C，D	48	40015	−871		1	21	2
		A，B	50	40093	−872		−1	−57	3
	航向偏角：−1.07	C，D	48	40015	−870		1	21	1
		A，D	50	39974	−869		−1	62	0
		B，C	49	40023	−870		0	13	1
60	俯仰角：−0.24	A，B，C，D	16	60312	−997		−	−	−
		A，B，C	16	60367	−1000		0	−54	3
		A，B，D	16	60268	−994		0	44	−3
	横滚角：0.22	A，C，D	16	60356	−1000		0	−44	3
		B，C，D	16	60259	−994		0	53	−3
		A，B	16	60381	−999		0	−69	2
	航向偏角：−0.37	C，D	17	60345	−998		−1	−33	1
		A，D	16	60260	−997		0	52	0
		B，C	15	60262	−996		1	50	−1

下载: 导出CSV 

| 显示表格

从表1可看出：在靶标点被遮挡时，采用本文方法得到的沿巷道宽度方向、巷道高度方向最大定位误差分别为1，4 mm；沿巷道掘进方向最大定位误差在掘进机距激光导引装置5，25，40，60 m时分别为35，34，62，69 mm，试验结果满足掘进机定位实际需求（沿巷道宽度方向、巷道高度方向的定位误差影响截割断面的控制精度，通常要求小于50 mm；沿巷道掘进方向的定位误差影响进尺精度，通常要求小于300 mm^[21]）。

4.   结论

1） 在靶标点无遮挡情况下采集图像，将图像中光斑构造成辅助矩形区域框。当下一帧图像中部分靶标点被遮挡时，通过比较无遮挡靶标点的成像光斑与辅助矩形区域框顶点的欧氏距离，对无遮挡靶标点的成像光斑进行标记和匹配，进而确定被遮挡靶标点。根据已知靶标几何尺寸和惯导系统提供的靶标姿态信息，推导出投影后的靶标点与成像光斑的对应关系，进而求解出被遮挡靶标点对应的光斑像素坐标。

2） 在煤矿井下开展了掘进机定位试验，结果表明：光学靶标被遮挡情况下，通过推算被遮挡靶标点对应的光斑像素坐标，可以解决掘进机定位中断问题，保证了掘进机定位的实时性，且定位误差满足掘进机实际定位需求。