基于激光靶向跟踪的悬臂式掘进机位姿测量系统研究

薛光辉， 李圆， 张云飞，3

（1. 中国矿业大学（北京） 机电与信息工程学院，北京 100083；2. 煤矿智能化与机器人创新应用应急管理部重点实验室，北京 100083；3. 青岛鼎信通讯消防安全有限公司，山东 青岛 266111）

0 引言

煤炭是我国国民经济的基础，在当前和未来相当长时期内，煤炭作为我国主体能源的地位不会改变，一次能源占比在50%以上，90%以上的煤炭以井工方式开采。“采掘并重，掘进先行”是煤炭开采的原则，综掘智能化落后于综采智能化，采掘失衡矛盾突出。据统计，我国年煤巷掘进量约为12 000 km，绝大多数采用悬臂式掘进机完成。我国煤矿掘进工作面存在智能化程度不高、用人多、环境差、事故多发等突出问题，智能掘进已成为国家和行业关注的热点和痛点，掘进关键装备的智能化是当前卡脖子的问题[1-2]。悬臂式掘进机绝对位姿的快速准确测量是实现煤矿巷道智能掘进的前提和基础。

悬臂式掘进机位姿测量方法主要有2类：一类是通过机载高精度传感器如捷联惯导[3]、加速度和超声波传感器的融合测量[4]等实现自主测量的方法，使用方便，但属于相对测量，测量精度较低或成本较高；另一类是利用外置式测量装置，如激光跟踪仪、全站仪[5]、激光导向仪、机器视觉、iGPS[6]、UWB[7]等，测量精度高，受杂波干扰小，但布置繁琐，测量范围有限，受环境影响大。目前，利用激光的悬臂式掘进机位姿测量方法因可靠性高、成本低、无误差累计等优点深受业内专家学者的关注。周玲玲等[8]利用2对激光指向仪和激光标靶构建了悬臂式掘进机位姿实时检测系统。吴淼等[9]提出了一种基于空间交汇测量技术的悬臂式掘进机位姿测量方法，利用激光装置得到了悬臂式掘进机位姿参数。马一心等[10]提出了多台激光跟踪仪联合动态位姿测量方法，可实现悬臂式掘进机位姿精度评定。邓国华[11]利用激光指向测距仪和光测角仪测量悬臂式掘进机相对于指向激光的方向角和俯仰角，但测量过程中需要调动掘进机使激光发射到光测角仪上。Li Jianguo等[12]利用激光装置研究了金属矿井下铲运车的位置参数测量方法。Du Yuxin等[13]利用视觉测量原理和双十字激光源照射在标靶上的图像，实现了悬臂式掘进机相对于巷道的姿态参数和偏移参数的测量。张超等[14]将红外LED光源组成图像测量标靶，人为构造图像特征来解决特征选取问题，实现了位姿参数求解。

综上可知，基于激光的悬臂式掘进机位姿测量方法存在非绝对位姿测量、测量精度较低、使用繁琐和测量参数单一等问题，且多处于理论研究阶段，尚不能满足智能掘进需要。为此，本文在已有研究基础上[15-16]，优化了基于激光靶向跟踪的悬臂式掘进机位姿测量方法，设计了基于激光靶向跟踪的悬臂式掘进机位姿测量系统，实现了悬臂式掘进机在巷道大地坐标系中绝对位姿的一次性全参数实时测量。仿真和实验分析了该系统的误差分布规律，为悬臂式掘进机在巷道大地坐标系绝对位姿的快速准确测量提供思路。

1 系统组成与测量原理

1.1 系统组成

基于激光靶向跟踪的悬臂式掘进机位姿测量系统由激光跟踪装置和激光标靶组成，激光跟踪装置安装在后方巷道顶板处，激光标靶安装在掘进机机身上，如图1所示。

图1 悬臂式掘进机位姿测量系统组成Fig. 1 Composition of pose measurement system of cantilever roadheader

悬臂式掘进机位姿测量系统结构与工作原理如图2所示。激光跟踪装置由激光测距模块、无线通信模块、处理器模块和二自由度云台等组成。激光标靶主要由激光靶面、倾角传感器、2个高分辨率相机、无线通信模块和机载工控机等组成，其中激光靶面由2个相互垂直的光学靶面组成，竖向靶面是一块半透半反射分光板，横向靶面是一块散射透光板。在无线通信模块引导下，激光跟踪装置的处理器模块控制二自由度云台俯仰或回转，使激光测距模块的激光束发射到激光标靶的竖向靶面目标区域并被反射至横向靶面上，在2个靶面上各形成一块光斑，并利用激光测距模块测得激光标靶和激光跟踪装置间的距离。激光标靶的相机1和相机2分别采集竖向靶面和横向靶面的光斑图像，输出至机载工控机，提取光斑中心坐标；激光标靶的无线通信模块将光斑中心坐标发送给激光跟踪装置，为其处理器模块控制二自由度云台提供信息，同时接收激光跟踪装置无线通信模块发来的测距值和二自由度云台的俯仰与偏向角度值，并传输给机载工控机；倾角传感器测出激光标靶的俯仰角并传输给机载工控机；机载工控机根据上述测量值解算出悬臂式掘进机绝对位姿。

1.2 位姿测量原理

掘进机机身坐标系{S}以掘进机机身中心点OS为原点，XS轴沿掘进机机身中线指向巷道掘进方向，ZS轴垂直掘进机机身向上；激光跟踪坐标系{J}以激光跟踪装置的回转和俯仰中心点OJ为原点，激光发射方向为XJ轴正向，回转轴向上为ZJ轴正向；激光标靶坐标系{B}以激光标靶的竖向靶面与横向靶面交线右顶点OB为原点，沿交线向左为YB轴正向，沿竖向靶面向上为ZB轴正向；巷道大地坐标系{H}以巷道中大地坐标已知点OH为原点，XH轴沿巷道中线指向掘进方向，ZH轴垂直水平面向上。掘进机位姿测量就是要确定掘进机在巷道大地坐标系中的位姿，如图3所示。

图3 悬臂式掘进机位姿测量系统坐标系及其转换关系Fig. 3 Coordinate systems of pose measurement system of cantilever roadheader and their transform relationships

设Jp为空间任一点p在激光跟踪坐标系中的齐次坐标，Hp为点p在巷道大地坐标系中的齐次坐标，即

式中：（Jpx,Jpy,Jpz）为点p在激光跟踪坐标系中的坐标；（Hpx,Hpy,Hpz）为点p在巷道大地坐标系中的坐标。

根据图3和式（1）有

其中：

同样地，激光标靶坐标系中任意一点q的坐标记为Bq， 其在激光跟踪坐标系中的坐标记为Jq，则有

式中：α1，β1，γ1分别为激光标靶坐标系沿激光跟踪坐标系ZJ轴、YJ轴和XJ轴旋转的角度；为激光标靶坐标系原点OB在激光跟踪坐标系中的坐标。

测量时，激光跟踪装置发射一束激光照射到激光标靶的竖向靶面上，经竖向靶面反射后又照射到激光标靶的横向靶面上，在2个靶面上形成2块激光光斑，如图3中的点A和a。设点A在激光标靶坐标系中的坐标为（0,yA,zA），在激光跟踪坐标系中的坐标为（d,0,0），其中d为激光跟踪装置坐标原点OJ与点A之间的距离。点a在激光标靶坐标系中的坐标为（-xa,ya, 0），点a关于YBOBZB平面的对称点为a′（xa,ya, 0），该点在激光跟踪坐标系中的坐标为（dc,0,0），其中dc为激光束到达竖向靶面经反射后到达横向靶面的距离，由激光跟踪装置中的激光测距模块测得。则有

为便于接收激光跟踪装置发射的激光，使激光标靶的2个正交靶面与水平面夹角呈45°，使用时激光标靶靶面开口应朝向巷道后方的激光跟踪装置。激光标靶底座上安装有倾角传感器，用于测量激光标靶与巷道大地坐标系水平面间的夹角γq，利用γq可计算得到γ1。

设Sn为空间任意一点n在掘进机机身坐标系中的齐次坐标，Bn为点n在激光标靶坐标系中的齐次坐标，则有

其中：

同时，设α，β，γ为掘进机在巷道大地坐标系中的偏向角、俯仰角和翻滚角，（x,y,z）为掘进机在巷道大地坐标系中的位置坐标，对应掘进方向位置、偏距和高度，则掘进机机身坐标系到巷道大地坐标系的转换矩阵为

由图3（b）所示的坐标转换关系可知：

将式（3）、式（5）、式（11）和式（12）代入式（13），并利用等式两边矩阵对应元素相等建立方程组，可解得掘进机在巷道大地坐标系中的绝对位姿，即掘进机位姿测量数学模型：

综上可知，悬臂式掘进机位姿测量与解算流程如下：

（1） 将激光标靶安装在掘进机机身上，并通过标定获得掘进机机身坐标系到激光标靶坐标系的转换矩阵。

（2） 将激光跟踪装置安装在巷道某位置，标定获得其在巷道大地坐标系中的位置

（3） 激光跟踪装置根据无线模块接收的信息，控制二自由度云台运动，使发射的激光束照射到激光标靶的竖向靶面上，利用此时的偏向角α0和俯仰角β0，结合步骤（2）得到的坐标利用式（3）求得巷道大地坐标系到激光跟踪坐标系的转换矩阵。同时，激光测距模块测得激光跟踪装置到激光标靶的距离dc。

（4） 利用相机1和相机2分别获取竖向和横向靶面上激光光斑的坐标（0,yA,zA）和（-xa,ya, 0），结合激光标靶内部倾角传感器测得的夹角γq，代入式（7）-式（9）和式（5），获得激光跟踪坐标系到激光标靶坐标系的转换矩阵。

（6） 利用式（14）解算出掘进机在巷道大地坐标系中的绝对位置（x,y,z）和姿态（α, β, γ）。

（7） 重复步骤（3）-步骤（6），可连续测量得到掘进机在巷道大地坐标系中的绝对位姿。

（8） 当超出测量范围时，将激光跟踪装置前移，然后重复步骤（2）-步骤（7），即可实现悬臂式掘进机在巷道掘进过程中的绝对位姿测量。

2 位姿测量误差仿真分析

根据建立的悬臂式掘进机位姿测量数学模型，分析悬臂式掘进机位姿测量系统中影响位姿解算精度的因素及位姿测量误差，确定系统各组成部分的结构尺寸和技术参数，建立系统仿真模型，分析掘进距离及激光跟踪装置安装位置对系统测量误差的影响规律，为系统现场应用和后续改进提供理论依据和数据参考。

2.1 精度影响因素

由悬臂式掘进机位姿测量数学模型可知，掘进机位姿参数解算与激光跟踪装置到激光标靶的距离、提取的激光光斑中心坐标、激光标靶倾角测量值、激光跟踪装置俯仰角与偏向角测量值有关，这些参数的测量误差均会影响掘进机位姿解算精度。

（1） 光斑中心坐标提取误差。现有的光斑视觉定位精度可达到像素级[17]，设计竖向、横向靶面的宽度均为 200 mm、高度均为 225 mm，以相机像素为960 dpi为例，每个像素点代表的实际尺寸为0.23 mm，考虑相机畸变标定后的残余误差，将激光标靶光斑中心坐标提取精度设为1 mm。

（2） 激光跟踪装置角度定位误差。激光跟踪装置偏向角和俯仰角跟踪的执行机构采用直流无刷伺服电动机，其定位误差与所用编码器精度有关。系统中选用的编码器为16 位，精度为0.005°。

（3） 激光测距误差。系统中采用SK80激光测距模块，最大测量范围为80 m，测量精度为2 mm。

（4） 激光标靶倾角测量误差。采用的倾角传感器分辨力为0.001°，全量程线性精度为0.005°。

2.2 仿真结果

合理设计系统各装置尺寸对其工程应用具有重要意义。掘进机位姿测量时，激光标靶安装在掘进机机身上，外形尺寸过大不利于测量的灵活性和安装的便利性，过小则不能满足测量范围要求。以EBZ220型掘进机为例，按掘进方向测量范围为5～80 m计算，偏向角α的变化范围为-27.7 ～ 42.6°，要满足上述要求会使激光标靶的尺寸非常大，不利于其安装使用。本文系统设计的激光标靶尺寸为225 mm×200 mm（高×宽），在5～80 m范围内其偏向角α测量范围为-16.25～+16.25°。为满足测量要求，需要对激光跟踪装置和激光标靶的安装位置进行分析。激光标靶的较佳安装位置：将底座安装在掘进机机身上，使激光标靶坐标系的XB轴与掘进机机身坐标系的XS轴呈45°夹角，YB轴与YS轴平行。假定激光跟踪装置安装在激光标靶正后方（标靶正后）、巷道左帮顶板处（顶部左帮）、巷道右帮顶板处（顶部右帮）、激光标靶正后方顶板处（顶部正后）、与激光标靶同一水平面巷道左帮处（标靶左帮）和与激光标靶同一水平面巷道右帮处（标靶右帮）6个位置。由于光斑中心坐标提取误差服从正态分布[18]，而倾角传感器和激光测距模块经过修正后误差可忽略不计，其随机误差也服从正态分布，假定光斑中心坐标提取误差、倾角传感器和激光测距模块的测量误差分布分别为正态分布N（500，2）、N（500，1）和N（500，1），为分析在5～80 m测量范围内掘进机位姿误差分布规律，每次均加入随机误差源分别对6个位置进行1 000次仿真解算，取误差最大值作为掘进机位姿测量误差。在5～80 m测量范围内每隔5 m进行1次仿真，结果如图4所示。

图4 悬臂式掘进机位姿测量误差仿真曲线Fig. 4 The error simulation curves of pose measurement of cantilever roadheader

由图4可知，在5～80 m测量范围内，掘进机位姿测量误差呈现出以下规律：

（1） 偏向角误差小于1.30°，俯仰角误差小于1°，翻滚角误差小于0.03°；掘进方向位置误差小于5 mm，偏距和高度误差小于20 mm。

（2） 随着掘进距离增加，掘进机姿态误差均在一定范围内变化，且偏向角呈收敛趋势；偏距和高度误差随掘进距离增加呈线性增加趋势，但掘进方向位置误差不随掘进距离增加而增加。

（3） 激光跟踪装置安装位置对掘进机位姿测量误差有一定影响，标靶正后、标靶左帮和标靶右帮误差相对较小，其次为顶部正后，再次为顶部左帮和顶部右帮。激光跟踪装置安装位置为标靶正后、标靶左帮和标靶右帮时，偏向角误差小于1°，俯仰角误差小于0.7°，翻滚角误差小于0.03°，掘进方向位置误差小于5 mm，偏距和高度误差小于20 mm。激光跟踪装置安装在顶部左帮和顶部右帮时，偏向角误差小于1.30°，俯仰角误差小于0.90°，翻滚角误差小于0.03°，掘进方向位置误差小于5 mm，偏距和高度误差小于20 mm。激光跟踪装置安装在巷道顶部正后时，偏向角误差小于1.30°，俯仰角误差小于0.85°，翻滚角误差小于0.03°，掘进方向位置误差小于5 mm，偏距和高度误差小于20 mm。考虑到煤矿掘进工作面实际工况和遮挡问题，激光跟踪装置不宜安装在巷道两帮或中间较低位置，在允许情况下尽可能提高激光标靶安装高度。

3 实验与结果分析

3.1 实验系统搭建

以履带式机器人底盘模拟掘进机机身，以楼道长走廊环境模拟煤矿巷道，将激光标靶安装在履带式机器人底盘上，激光跟踪装置放置在三脚架上，搭建掘进机位姿测量实验系统，如图5所示。履带式机器人机身坐标系原点OS在激光标靶坐标系中的位置坐标标定结果为（-400，200，-230），掘进机机身坐标系在激光标靶坐标系中的偏向角α2为0，俯仰角β2为-45°。α0和β0分别为激光发射与激光测距模块的偏向角和俯仰角，为激光跟踪坐标系原点OJ在巷道大地坐标系中的位置参数，标定为（0,0,900）。

图5 悬臂式掘进机位姿测量实验系统Fig. 5 Experimental system for pose measurement of cantilever roadheader

3.2 实验结果

利用全站仪标定出履带式机器人在模拟巷道中的位姿，再利用搭建的实验系统测量掘进机位姿。测量过程中，微调激光跟踪装置的姿态角，使激光束入射到激光标靶竖向靶面的不同位置，以观察激光跟踪装置测量激光标靶的位姿精度变化情况。为了避免实验误差，进行了10组不同位姿参数的测量实验。第1-6组实验中履带式机器人在巷道大地坐标系下的位姿参数：x=4 510.0 mm，y=-328.0 mm，z=368.0 mm，α=-2.000°，β=0.500°，γ=0.001°。第7-10组实验中履带式机器人在巷道大地坐标系下的位姿 参 数：x=5 820.0 mm，y=196.0 mm，z=435.0 mm，α=-2.500°，β=1.000°，γ=0.001°。实验结果见表1。

表1 实验结果Table 1 The experimental results

根据表1，绘制掘进机位置误差和姿态误差曲线，如图6所示。由图6可知，掘进方向位置x、偏距y和高度z测量绝对误差均小于5 mm，偏向角α测量绝对误差小于1°，俯仰角β测量绝对误差小于0.6°，翻滚角γ测量绝对误差可忽略不计，实验结果优于仿真结果。

图6 位姿测量误差实验曲线Fig. 6 The experimental curves of pose measurement error

3.3 讨论

定向掘进及断面自动截割成形是煤矿巷道智能掘进的题中之义。定向掘进的要义是控制好掘进巷道的中线和腰线。巷道中线控制需要知道掘进机的偏向角和偏距，而巷道腰线控制需要准确测量掘进机在巷道大地坐标系中的俯仰角和高度。仿真和实验结果均表明，本文系统偏向角测量误差较大，偏距和高度随掘进距离增加呈线性增加趋势。随着掘进距离增加，为保证掘进机偏距和高度的测量精度，需要将激光跟踪装置前移，自主快速标定其在巷道大地坐标系中的位姿。另外，煤矿井下环境复杂，粉尘大，照明不足，系统的有效性和环境适应性需要在现场环境下检验。因此，下一步应重点研究掘进机偏向角测量精度的改进措施、移站后自主快速标定技术和环境适应性等。

4 结论

（1） 基于激光靶向跟踪的悬臂式掘进机位姿测量系统主要由激光跟踪装置和激光标靶组成。激光跟踪装置发射激光到激光标靶并在其2个靶面上形成光斑，且保持激光跟踪。激光标靶接收激光跟踪装置发射的激光束，根据2个靶面上的光斑位置和激光测距模块的测距信息，依据巷道大地坐标系、激光跟踪装置坐标系、激光标靶坐标系和掘进机机身坐标系之间的转换关系求解掘进机在巷道中的绝对位姿，实现了悬臂式掘进机在巷道坐标系中绝对位姿的全参数实时测量。

（2） 建立了悬臂式掘进机位姿测量数学模型，分析了系统的误差影响因素，仿真研究了系统的位姿测量误差分布规律，结果表明，该系统能够满足掘进机位姿检测精度要求，为工程应用提供支撑。

（3） 搭建了位姿测量实验系统，开展了其在模拟巷道中的位姿测量实验，分析了系统的测量精度，结果表明，掘进方向位置、偏距和高度测量绝对误差均小于5 mm，偏向角测量绝对误差小于1°，俯仰角测量绝对误差小于0.6°，翻滚角测量绝对误差可忽略不计，实验结果优于仿真结果。

（4） 煤矿井下粉尘大，照明条件差，悬臂式掘进机位姿测量系统的环境适应性还需后续现场实验验证。仿真和实验研究表明，掘进机偏向角测量误差稍大，为保障测量结果满足工程需要，系统测量范围有限，提高偏向角测量精度及移站后的自主标定是下一步研究重点。