悬臂式掘进机行走轨迹及偏差感知方法

吴 淼，沈 阳，吉晓冬,2，王鹏江，姜 海，郑伟雄，李 悦

(1.中国矿业大学(北京) 机电与信息工程学院，北京 100083; 2.石家庄煤矿机械有限公司，河北 石家庄 050033)

巷道掘进是煤矿开采中效率最低、危险度最高的前端生产环节。我国国有煤矿每年巷道掘进量超过15 000 km，绝大部分采用以悬臂式掘进机为主要开采设备的综合机械化掘进(综掘)方式作业[1-2]。近年来，煤矿综掘工作面的智能化、无人化需求日益迫切，其中最关键的是巷道精准定向掘进[3-4]。掘进机姿态和位置信息的实时精准感知是掘进机自主行走控制和截割断面误差补偿的基础，是巷道定向掘进的先决条件。

目前，我国综掘普遍采用的掘进定向方法仍旧是传统的“激光指向仪”指示法[5]。煤矿地测部门人员利用全站仪确定指向仪在巷道顶板的安装位置，保证其指示方向与巷道设计中线方向完全一致，这样指向仪在大地坐标系下的绝对位置坐标和指向的绝对角度值即为已知。

激光指向仪发射的激光光斑是掘进定向的惟一依据，施工人员肉眼感知光斑位置，凭借经验操控掘进走向。每一轮或几轮断面截割完成后，人工测量光斑与巷道两帮的距离，以评估巷道走向是否存在偏差，并在此后掘进中做出相应调整，达到定向掘进的目的。

上述方式存在较多的问题:

(1)只能对巷道设计方向进行指示，与掘进机本身位姿参数无任何联系，无法为掘进机自主导控和巷道偏差补偿提供基准参数。

(2)无法解读激光光束所携带的基准信息，仅凭司机肉眼观察感知，凭经验操控向前掘进。

(3)工作面粉尘质量浓度大，作业现场光线差，司机视线受遮挡。

(4)属于巷道掘后检测，无法在掘进过程中实时感知掘进偏差。

(5)需多人员协助测量，危险系数高，劳动强度大，自动化水平低。

煤巷综掘无人化、智能化的发展趋势亟需发展巷道智能定向掘进感知技术。许多国内外学者都对掘进机机身位姿参数的感知方法做了研究。

贾文浩等[6]提出了一种基于室内定位系统(indoor Global Positioning System,iGPS)测量原理的掘进机定位方法,实现了在狭长封闭巷道内对掘进机位置的绝对精度检测。杜雨馨等[7-8]构建了一种基于机器视觉技术的掘进机机身位姿检测系统，通过对十字光线成像特征的分析，在实验室条件下实现了掘进机机身位姿的测量。陶云飞[9]将高精度全自动全站仪应用于掘进机的位姿自动检测中，通过测量安置在机身不同位置棱镜的坐标，解算出掘进机的位置、姿态信息，一定程度上实现了掘进机位姿参数的自动检测。符世琛等[10-11]提出了一种基于超宽带(Ultra Wide-Band，UWB)测距原理的掘进机定位定向方法，建立了相关解算数学模型及误差分布模型，为掘进机行走控制提供位置状态和位姿参数。冯大龙[12]较早的将捷联式惯导系统(捷联惯导)应用在掘进机的无人控制中，利用惯性测量单元测量掘进机相对于惯性空间的运动参数，使掘进机的无人作业成为了可能。

将多种传感器组合可以实现优势互补，以应对综掘工作面粉尘大、空间小、振动强、光线弱的恶劣环境。因此，很多学者提出了多传感器信息融合的掘进机位姿感知方法。毛君等[13]提出了将陀螺仪与全站仪组合的掘进机导向系统，实现了掘进机姿态和位置信息的连续获取。童敏明等[14]采用三轴加速度传感器、单轴陀螺仪和超声波测距传感器构建了掘进机定位系统，将机身位姿、偏转角度和位移等信息融合处理，得到了掘进机的位置信息。黄东等[15]提出了一种基于机器视觉/捷联惯导的掘进机位姿组合测量方法，捷联惯导测得姿态信息，单目视觉测得位置信息，从而实现掘进机实时位姿5个自由度的测量。

虽然上述研究对于掘进机位姿感知水平有了大幅提升，为巷道的精准定向掘进做出了很大贡献，但由于综掘工作面环境恶劣(高温、高湿、高粉尘、光线昏暗、底板起伏大)，灾害频发(瓦斯、冒顶、片帮、底臌)，装备繁多(掘进机、转载机、带式输送机、刮板输送机、临时支架、钻锚设备等)，空间狭小(狭长封闭空间)、工艺离散(掘-支-网-钻-锚-运六大主要工艺环节彼此独立)，目前还不能够在综掘流程中实现实时、自主、精准感知掘进机姿态和位置，生成行走轨迹为掘进机的动态调整提供参考的实用方法。

另外，悬臂式掘进机为主的综掘工作面是一种间断式的循环截割掘进，可以由断面截割补偿掘进机机身位姿的偏差。而上述研究只是检测掘进机的位姿参数，并未建立机身位姿参数与巷道定向掘进的关系，不能为截割断面误差补偿提供偏差参数。

再者，掘-支同步式快速掘进新工艺已在普遍探索之中，有可能成为一种发展趋势，新型快掘机组中的临时支护设备将严重遮挡综掘作业空间，造成 “外置式”位姿感知装置严重受限，因此 “机载式”的位姿感知方法成为必然需求[16-17]。

针对上述问题，笔者提出一种能够适用于量大面广的悬臂式掘进机的行走轨迹及偏差感知技术，可以实时、自主、准确、直接地获取掘进机行走轨迹，同时给出掘进机纵向轴线与巷道设计中线的偏差角、掘进机质心与巷道中线的偏差距离等位姿偏差信息。根据行走轨迹动态调整掘进机位姿并对位姿残余偏差进行反向截割补偿，保证巷道定向掘进精度。

1 偏差的描述及影响分析

获取掘进机机身位姿参数是为了给巷道精准掘进提供纠偏参数。以往的研究[18]已经提出了一套符合实际工况且精准、简便、易测的掘进机位姿参数体系，定义了横滚角、俯仰角、车前距、偏角、偏距5个掘进机机身位姿参数，它们可全面描述掘进机机身在煤巷内的位置和姿态。

偏角和偏距2个参数描述了悬臂式掘进机在掘进方位上的偏差，准确地反映了掘进机实际掘进方向偏离巷道设计中线的情况，是定向掘进最重要的参数。将偏角和偏距统称为掘进机方位偏差。

1.1 坐标系定义

1.1.1 地心惯性坐标系

地心惯性坐标系记为i系(Xi,Yi,Zi)，其坐标原点为地心，Zi轴沿地球自转轴指向北极，Xi,Yi轴在赤道平面内，分别指向惯性空间的两颗恒星。惯性坐标系不参与地球自转，其3个坐标轴在惯性空间内的指向固定不变。

1.1.2 大地坐标系

大地坐标系记为d系(Xd,Yd,Zd)，其坐标原点为地心，Zd轴沿地球自转轴指向北极，Xd,Yd轴在赤道平面内分别指向本初子午线和东经90°。大地坐标系固联在地球上，随地球自转。煤矿中巷道设计的位置和方向都是在大地坐标系中设计并标识的。

1.1.3 巷道坐标系

巷道坐标系记为h系(Xh,Yh,Zh)，其坐标原点在巷道设计中线上，并由掘进机初始位置确定，Xh轴指向巷道左侧，Yh轴沿巷道设计中线指向掘进方向，Zh轴竖直向上与Xh,Yh轴符合右手定则。

1.1.4 机体坐标系

机体坐标系记为b系(Xb,Yb,Zb)，其坐标原点在机身几何重心处，Xb轴沿机身横轴指向机身左侧，Yb轴沿机身纵轴指向机头方向，Zb与Xb,Yb轴符合右手定则。机体坐标系固联在掘进机机身，跟随掘进机移动。

1.2 掘进机方位偏差的描述

为简便起见，在上述巷道坐标系中只保留掘进机履带部分来表示掘进机机身。

1.2.1 偏角

掘进机机身中线与煤巷设计中线在煤巷底板平面XOY上投影的夹角α,定义为掘进机的偏角，如图1所示。偏角反映了掘进机实际朝向与煤巷设计方向之间的角度偏差值。

图1 偏角Fig.1 Deviation angle

1.2.2 偏距

机身中心点C与煤巷设计中线在煤巷底板平面XOY上投影的距离l，定义为掘进机的偏距，如图2所示。

图2 偏距Fig.2 Deviation distance

1.3 掘进机方位偏差与巷道定向掘进的关系

偏角和偏距体现了掘进机偏离巷道设计中线的情况，一旦出现偏差，将导致实际掘进方向偏离设计掘进方向，影响整个采区的正常生产，严重时还可能引发巷道内地质灾难，造成井下安全事故。李睿[18]通过激光位姿检测系统实现了偏角和偏距的检测，但不能形成掘进机动态行走轨迹，不能为掘进机前后调动提供实时参考。

动态的掘进机行走轨迹和方位偏差信息可以在掘进机向截割断面行走过程中实时为掘进机机身位姿调整提供参考，以尽可能减小掘进机方位偏差。由于巷道内空间有限以及驱动掘进机行走的液压马达难以精确控制，掘进机机身方位偏差很难彻底消除。可根据残余掘进机方位偏差信息调整巷道截割断面的位置以反向补偿巷道掘进偏差，从而达到巷道精准定向掘进的目的。

因此，需要研发一种可以自主动态感知掘进机行走轨迹、同时给出掘进机偏角和偏距的位姿感知系统。

2 行走轨迹及偏差感知系统组成

掘进机行走轨迹及偏差感知系统主要由激光偏距感知系统、捷联惯导、二维里程计、解算主机等组成，如图3所示。

2.1 激光偏距感知系统

激光偏距感知系统在以往基础上加以改进，由一台扇形激光发射器、一台激光标靶组成，如图3所示。地测人员按照巷道设计要求将扇形激光发射器精确安装在后方巷道顶部中线上(已知精确偏距亦可)，发出的扇形激光作为巷道设计中线基准传递媒介。激光标靶平行安装在掘进机机身已知位置且与机身中线垂直。扇形激光在激光标靶上形成线型光斑，被光敏元件感知，通过内部控制器解算，得到掘进机相对于巷道中线的偏差距离。偏距感知系统仅在掘进初始位置使用，提供巷道坐标系下初始位置的横坐标。

图3 系统组成Fig.3 System compositions

2.2 捷联惯导

捷联惯导由三轴光纤陀螺仪、三轴加速度计以及解算芯片组成，安装在掘进机机身顶部中心，可自主测得大地坐标系下掘进机相对于正北方向的航向角，还可解算出掘进机在大地坐标系下的航向、姿态、速度、位置等信息[19]。其航向角及姿态角的测量精度很高，而速度、位置的误差随时间增加会逐渐增大。捷联惯导使用前需输入当地的经纬度坐标，经过3 min的初始自对准(自对准过程中保证掘进机静止)，自主寻得掘进机相对于正北方向的航向角，以及初始的俯仰角和横滚角;初始自对准完成后，可实时精准测得掘进机在大地坐标系下的运动信息，从而实现“静态对准，动态测量”。

(1)

(2)

式中，L，g和ωie分别为当地纬度、重力加速度和地球自转角速率;ωiecosL和ωiesinL分别为地球自转角速度在北向和天向的分量。

(3)

整合可得

(4)

式(4)展开为

(5)

(6)

式中，γ，θ，φ分别为载体的滚转角、俯仰角和航向角。

将Tij的值与式(6)中的元素对应，即可求得掘进机的初始航向和姿态信息。

经过上述粗对准过程，由于陀螺仪和加速度计会受到角晃动和线晃动的干扰以及惯性器件本身的测量误差，导致捷联惯导获得的航向角和姿态角存在一定的失准角误差。通过进一步的滤波精对准过程，可减小失准角误差的影响，得到精准的航向、姿态信息[20-21]。

初始自对准完成后，掘进机开始向前行走，可用四元数法对掘进机进行实时姿态角更新[22]。

在捷联惯导解算中常用四元数表示载体的姿态。四元数由1个实数部分和3个虚数部分组成，它将载体的三维姿态表示为载体绕固定点的旋转，可完整描述载体的姿态信息。四元数Q的形式为

Q=q0+q1i+q2j+q3k

(7)

式中，q0，q1，q2，q3为实数;i，j，k既是相互正交的单位向量，又是虚数单位。

捷联惯导姿态更新可由下式表示:

Q(tk+1)=Q(tk)⊗q(h)

(8)

式中，Q(tk+1)和Q(tk)分别为tk+1时刻和tk时刻的姿态四元数;⊗为四元数的乘法;q(h)为[tk，tk+1]时间段内的姿态变化四元数。

(9)

式中，Ф为机体坐标系tk时刻和tk+1时刻的等效旋转矢量;Ф为Ф的模。

(10)

利用式(6)，(10)中对应元素相等，可得姿态角与四元数的关系为

(11)

θ=arcsin(2q0q1+2q2q3)

(12)

(13)

这样得到了掘进机在大地坐标系下动态的航向角、横滚角和俯仰角。由于高精度的捷联惯导价格昂贵，而掘进机行走轨迹及偏差的感知只需利用其航向信息，因此，可研发航向角单一参数测量装置及其解算模型，大大降低成本，满足综掘工作面的需求。

2.3 二维里程计

二维里程计主要由编码器、测量轮、支架、张紧装置组成，如图4所示，安装在掘进机底部，处于捷联惯导的正下方，其两测量轮的对称线与掘进机纵轴线重合，测量轮与掘进机纵轴线呈45°，如图5所示。两测量轮跟随掘进机的前进转动，带动编码器旋转，得到测量轮的旋转圈数，通过计算可得掘进机在单位时间内沿机身纵轴前进方向和横轴方向的里程增量。二维里程的精准感知有效解决了传统里程计无法感知掘进机发生侧向滑动的难题。

图4 二维里程计结构Fig.4 Structure diagram of two-dimensional odometer

图5 二维里程计安装位置Fig.5 Installation location diagram of two-dimensional odometer

3 行走轨迹及偏差感知系统原理

3.1 系统原理

巷道中线方向角以大地坐标系为基准，而捷联惯导输出大地坐标系下的航向角，两者的差即为掘进机的偏角，如图6所示。二维里程计通过编码器可实时感知掘进机在机身纵轴和横轴2个方向的里程增量。将偏角和里程增量信息结合，利用航位推算算法对二维里程增量进行累加，可得掘进机在巷道坐标系下的行走轨迹，其纵坐标为掘进距离，横坐标为偏距，并将所得的位置、方向、姿态等信息发送至远端显示，为掘进纠偏提供基准参数。在实际工况下，也可以用上述偏角和激光测出的偏距作为输入值计算掘进纠偏量对巷道两帮进行修正，实现误差补偿。系统原理如图7所示。

图6 巷道坐标系与大地坐标系关系Fig.6 Schematic diagram of the relationship between the road-way coordinate system and the geodetic coordinate system

图7 系统原理Fig.7 Diagram of system principle

设由二维里程测量装置所得里程增量分别为ΔSOD1和ΔSOD2，根据图5中测量装置与掘进机的位置关系，将两里程增量合并，可得掘进机机体坐标系二维里程增量矢量ΔSOD为

(14)

将机体坐标系里程增量转换到巷道坐标系h系:

(15)

将巷道坐标系下的里程增量累加，可得掘进机在巷道中的轨迹坐标:

(16)

式中，Ph0为h系下掘进机的位置坐标，其横坐标由激光偏距感知系统提供的初始偏距，纵坐标设为0;ΔPh(i-1)掘进机位置坐标增量。

其中，初始位置P0的横坐标由激光偏距感知系统提供的初始偏距确定，纵坐标设为0。

3.2 系统流程

本系统的流程如下:掘进机行走前，捷联惯导进行3 min的初始对准，自主感知掘进机的初始航向角;激光偏距感知系统提供初始偏距信息，作为掘进机初始位置的横坐标，其初始纵坐标记为0。掘进机开始向前行走后，捷联惯导、二维里程计将测得的原始数据实时传输到机载控制器进行位姿参数计算，得到掘进机实时位置坐标，横坐标为实时偏距，纵坐标为竖直掘进距离，同时可得掘进机的偏角、航向角和姿态角等信息。掘进机行驶至巷道断面开始掏槽前，通过偏角和偏距等信息判断掘进机位姿偏差程度，若偏差较大，对掘进机位置、方向进行粗调整后，再进行掏槽;掏槽完成后，根据此时的偏角和偏距调整截割参数以反向补偿掘进机位姿偏差，保证巷道精准掘进。图8为掘进机实时行走轨迹及偏差示意图。

图8 掘进机行进轨迹及偏差示意Fig.8 Diagram of roadheader trajectory and deviation

4 实验验证

为了验证所提出的掘进机行走轨迹及偏差感知系统的有效性，并探究其对行走轨迹追踪和掘进方位偏差感知精度，搭建了行走轨迹及偏差感知精度验证实验系统，在模拟煤矿综掘工作面真实工况的条件下对本文所提系统输出的偏角、偏距、轨迹坐标精度进行了实验验证。

4.1 实验配置

行走轨迹及偏差实验验证系统主要包括激光偏距感知系统、捷联惯导、二维里程计、机载控制器、井上和井下可视化远程控制平台、全站仪等。

(1)激光偏距感知系统。激光偏距感知系统中的激光标靶和扇形激光发射器均为自主研发，如图9，10所示。其基本性能参数见表1。

图9 激光标靶Fig.9 Laser target

(2)光纤捷联惯导。捷联惯导选用光纤型捷联惯导，如图11所示。其基本性能参数见表2。

图10 扇形激光发射器Fig.10 Fan laser transmitter

表 1 激光偏距感知系统性能参数Table 1 Performance parameters of laser deviation distance perception system

表2 捷联惯导性能参数Table 2 Performance parameters of strapdown inertial navigation

(3)二维里程测量装置。二维里程计为自主研发，如图12所示。其中编码器选用光电多圈高精度绝对值编码器，测量轮选用大承重的全向轮，基本参数见表3，4。

表3 编码器性能参数Table 3 Performance parameters of Encoder

图12 二维里程计Fig.12 Two-dimensional odometer

(4)全站仪。全站仪作为掘进机位姿参数基准参考测量仪器，基本参数见表5。全站仪和3个棱镜的布置方式如图13所示，测量棱镜 1，2，3 固定在掘进机机身不共线的3个位置上，其中棱镜1固定在激光标靶中点上部，可用棱镜1的位置变化代表掘进机的运动。全站仪放置在掘进机正后方，可测得全站仪与机载棱镜的距离S、水平角β、垂直角γ，并通过式(17)完成棱镜相对于全站仪的坐标测量工作[23]。

图13 全站仪及棱镜布置Fig.13 Layout of total station and prisms

表4 测量轮性能参数Table 4 Performance parameters of measuring wheel

表5 全站仪性能参数Table 5 Performance parameters of total station

(17)

根据以上原理测得3个棱镜的相对全站仪的坐标，并将它们转换到巷道坐标系，得到3个棱镜在巷道坐标系下的坐标分别为(xh1,yh1,zh1),(xh2,yh2,zh2)和(xh3,yh3,zh3)。全站仪测得的基准机身偏角α′可通过式(18)求出;基准轨迹为棱镜1的水平坐标，其中横坐标xh1为基准偏距。

(18)

4.2 实验方案

实验地点地理位置为北纬37.92°,东经114.52°。以掘进机起点为坐标原点，初始朝向为巷道设计方向，按照一般巷道外形尺寸在地面确定掘进机移动边界，并建立巷道坐标系。通过井上可视化控制平台操控掘进机按井下实际工况所需操纵掘进机行走，同时系统传感器所采集到的原始数据在机载控制器计算后，通过光纤传输回井上可视化控制平台，实时显示出掘进机行进轨迹及方位偏差值，并将所测数据保存。

掘进机行走过程中，通过全站仪测量掘进机机身上3个棱镜坐标，以作为掘进机机身位姿基准参考点。全站仪进行坐标测量时掘进机需要短暂停止。人为不定时控制掘进机停止，并记录相应时刻，可得到一系列掘进机位姿基准参考点。一共进行5组实验。

4.3 实验结果

图14为掘进机实际轨迹与系统测量显示轨迹对比图，左边红色轨迹为掘进机实际轨迹，右图为本文所提系统测得轨迹在远程可视化控制平台的显示，从定性的角度来看本文所提系统能够较好地感知掘进机机身的实时位姿。将以本文所提方法测得的5组位姿数据与相对应基准参考点数据进行比较分析，结果见表6。

图14 掘进机实际轨迹与系统测量显示轨迹对比Fig.14 Comparison diagram of the actual trajectory of roadheader and the measured trajectory of the system

表6 测量误差统计特性Table 6 Statistical characteristics of measurement errors

偏角测量误差的最大值为0.104°，均值为0.052°，方差为0.100°;偏距(横坐标)测量误差的最值为0.084 m，均值为0.048 m,方差为0.077 m;纵坐标测量误差的最大值为0.099 m，均值为0.055 m,方差为0.068 m。可以得出，本文所提方法的偏差感知精度满足巷道定向掘进对于掘进机定位定向要求，可使所掘巷道达到国家煤矿井巷工程验收标准[24-25]。

5 结 论

(1)基于已往研究，在巷道坐标系下描述了偏角和偏距，阐述了掘进机方位偏差与巷道定向掘进之间的关系。

(2)搭建了一种可以自主、动态感知掘进机行走轨迹、偏角和偏距的位姿感知系统，提出了基于激光偏距感知、捷联惯导、二维里程计的掘进机方位偏差感知方法及相应的自主精准定向掘进策略。

(3)实验结果表明，所提方法实现了掘进机机身偏角0.052°、偏距(横坐标)0.048 m、纵坐标0.055 m的感知精度，验证了所提方法的可行性及优越性，感知精度满足巷道定向掘进对于掘进机定位定向要求。

(4)基于所提系统开发了远程可视化操控平台，实现了掘进机位姿状态在井上、下的实时显示。