悬臂式掘进机视觉导航与定向掘进控制技术

张旭辉，赵建勋，杨文娟，张 超

(1.西安科技大学 机械工程学院,陕西 西安 710054; 2.陕西省矿山机电装备智能监测重点实验室,陕西 西安 710054)

煤矿生产装备智能化是煤炭绿色安全高效发展的关键[1-2]。随着我国综采技术和配套设备的快速发展，“采掘失衡”矛盾日益突出，巷道掘进自动化、智能化是目前国内攻关的关键技术[3]。悬臂式掘进机是综掘工作面的重要装备，其智能化技术对目前其他形式的快速掘进装备发展具有代表性，自主定位、智能控制和远程监控等技术发展决定未来掘进生产的发展[4-5]。自动定向掘进是实现掘进工作面自动化的核心技术。

悬臂式掘进机的定向掘进机控制包括掘进机机身位姿测量和掘进机的定向纠偏控制两部分，前者是实现定向纠偏控制的基础。文献[6]利用超宽带技术，结合TW-TOF测距法实现了井下巷道中机身位姿测量，但脉冲信号在井下环境中易受干扰，且测量误差较大。文献[7]提出零速修正的掘进机惯性导航定位方法，利用二次曲线拟合法进行零速修正，得到较高精度的机身位姿，试验结果表明该定位方法可达到厘米级。文献[8]设计了一种基于雷达系统的井下设备定位系统，可利用井下环境生成雷达返回信号，获得井下设备轨迹及位置信息。文献[9]利用机器视觉测量方法，以十字激光器与激光标靶为特征，利用相机采集激光标靶图像,通过建立机身位姿视觉测量模型，实现对悬臂式掘进机机身位姿的检测。文献[10]采用机器视觉测量方法，以激光束为特征，建立了基于两点三线的掘进机位姿解算模型，解算得到巷道中机身位姿。文献[11]利用全站仪测量机身位姿，通过建立掘进机轨迹跟踪的数学模型，结合行走机构液压系统，提出了基于双层模糊控制的掘进机轨迹跟踪的控制方法。全站仪在使用过程中需要考虑移站问题，文献[12]根据激光测量原理研究了全站仪测站移站后的自主标定原理，构建了系统测量误差模型，验证了激光导向系统自主标定的可行性，可实现对掘进机的位姿测量。文献[13]利用悬臂式掘进行走机构运动学确定了机身位姿误差和机身移动速度间的关系，设计了掘进机行走控制系统，仿真验证了系统有效性。目前针对定向掘进控制系统的研究，以视觉、惯导和全站仪等测量方法为主要方式，通过建立掘进机轨迹跟踪控制模型实现对掘进机机身的跟踪控制。煤矿井下高粉尘、低照度工作环境对各测量方法提出了挑战，上述方法掘进机位姿测量的精度和可靠性尚待进一步研究和应用验证。

笔者提出一种基于机器视觉的悬臂式掘进机精确定位和轨迹跟踪控制的定向掘进方法，利用单目视觉测量原理，以井下激光指向仪为特征解决井下特征获取问题，通过建立悬臂式掘进机位姿视觉测量模型计算机身位姿;研究掘进机在井下巷道的纠偏机理，建立定向掘进运动控制模型，设计轨迹跟踪控制器实现对悬臂式掘进机定向掘进的过程控制。

1 悬臂式掘进机定向掘进控制方案

悬臂式掘进机自动定向掘进控制原理如图1所示。系统由悬臂式掘进机、防爆计算机、防爆工业相机、激光指向仪、DSP控制器和传感器构成;软件系统实现图像采集、特征提取、机身位姿解算、机身轨迹跟踪控制和数据存储显示等功能，可完成巷道中机身位姿精确测量以及掘进机轨迹跟踪控制。

图1 悬臂式掘进机自动定向掘进控制总体方案Fig.1 Overall scheme of automatic directional heading control for boom-type roadheader

系统采用单目视觉测量方法，通过构建井下巷道悬臂式掘进机机身位姿测量模型，求解机身实时位姿并传输到DSP控制器，获得机身当前位姿与期望位姿之间的位姿误差，利用DSP控制器输出控制指令控制掘进机移动，进行轨迹跟踪控制，同时进行掘进机机身及截割头实时位姿显示，为操控人员提供超挖预警报警。各模块原理如下:

(1)机身位姿视觉测量。根据掘进机、摄像机和激光指向仪在巷道中的位置关系构建巷道全局坐标系，固定在机身上的防爆工业相机采集后视巷道图像信息，通过图像处理、激光特征提取以及位姿解算等过程求解机身位姿数据。

(2)数据交互模块。系统工作时需要各模块之间协同工作，其中DSP控制器利用RS485串行通信模块与计算机通信，获取巷道参数和机身位姿数据，利用A/D模块采集流量、压力和电流等模拟量传感器数据，通过各数据库中数据交互，实现各工作模块之间的数据交互。

(3)轨迹跟踪控制。根据悬臂式掘进机轨迹纠偏控制策略构建掘进机定向掘进运动控制模型，比较实时反馈的机身位姿数据和机身移动期望位置得到机身位姿误差，利用DSP控制器D/A模块向掘进机行走机构液压系统发送控制指令，通过控制比例换向阀的开口，控制掘进机左右履带运动速度，使掘进机沿规划路径移动，通过不断调整机身位姿，实现对巷道中掘进机的轨迹跟踪控制。

2 悬臂式掘进机位姿视觉测量方法

2.1 掘进机机身位姿视觉测量方案

2.1.1 测量特征选取

图2(a)为井下高粉尘、低照度和多杂光的工作环境，普通视觉测量特征难以在该环境中准确识别测量特征，严重影响视觉测量的可靠性。图2(b)为巷道定位激光指向仪，具有体积小、结构简单、巷道环境中具有明显对比特征，适用于井下高粉尘的复杂工作环境。本文借助激光指向仪包含巷道坐标系位置信息的特点，以巷道布置的激光指向仪光斑和激光束为特征，为巷道中掘进机定位提供基准信息，建立掘进机位姿视觉测量模型，解算掘进机机身实时位姿。

图2 井下环境中激光指向仪特征图像Fig.2 Feature image of laser pointer in downhole environment

2.1.2 掘进机位姿视觉测量系统原理

悬臂式掘进机位姿视觉测量系统如图3所示，由防爆计算机、防爆工业相机和两激光指向仪组成。系统进行掘进机位姿测量之前，将防爆工业相机方向向后固定于掘进机机体上，在煤矿地测科确定的激光指向仪(包含巷道设计信息)基础上再平行固定一个激光指向仪，利用特殊设计的辅助装置保证两个激光束沿巷道设计方向平行。利用工业相机采集激光指向仪图像，传输到计算机进行图像预处理、激光点线特征提取，结合已建立的悬臂式掘进机机身位姿解算模型，解算掘进机在巷道坐标系中的位姿，实现悬臂式掘进机的位姿测量。机身位姿测量流程如图4所示。

图3 悬臂式掘进机视觉测量系统结构Fig.3 Visual measurement system structure of boom-type roadheader

图4 悬臂式掘进机机身位姿测量流程Fig.4 Flow chart of fuselage pose measurement of boom-type roadheader

2.2 机身位姿视觉测量模型

(1)

2.3 掘进机机身位姿解算方法

相机成像模型是对摄像机成像的一种几何描述，摄像机的成像模型一般为小孔成像模型[14]，该模型可以看作为线性几何模型。激光指向仪包括左右两激光光斑和两条激光束直线，结合两光斑的连线可将激光指向仪的两点三线特征简化为如图5所示的基于门形结构的掘进机机身位姿测量模型[15]。

图5 基于门形结构的机身位姿视觉测量原理Fig.5 Visual measurement schematic diagram of fuselage posture based on portal structure

将两激光束简化为直线段L1和L3，且L1//L3。图中点P1和P2为两激光指向仪的光斑中心，P3和P4分别为激光指向仪所发出的激光线上的距离激光光斑一定距离的点，L2为过两光斑中心的直线。点q1和q2为两激光光斑中心点在图像坐标系上的投影点，l1，l2和l3为L1，L2和L3在图像坐标系上的投影直线。假设投影直线l1,l2和l3的直线方程为

aix+biy+ci=0 (i=1,2,3)

(2)

式中，ai，bi，ci分别为直线li的直线方程一般式的系数。

根据直线方程可得到投影直线l1，l2和l3的方向向量vi(-bi,ai,0)。图中相机光心Oc、图像平面中的投影直线l1，l2，l3和空间直线L1，L2，L3围成OcP2P3，OcP1P2，OcP1P4三个平面。设3个平面的法向量分别为N1，N2和N3。在平面OcP2P3中，设t1为投影直线l1上的任意一点，则可以得到平面OcP2P3的法向量N1为

N1=Oct1×V1

(3)

同理可确定平面OcP1P2和平面OcP1P4的法向量N2和N3。由于L1//L3，N3⊥L3，则可得N3⊥L1，N1⊥L1。可得空间直线L1，L2和L3的方向向量V1，V2和V3存在如下关系:

V3=V1=N1×N3

(4)

V2=V1×N2

(5)

V′2=k2P2/M2-k1P1/M1

(6)

(7)

设图像中l1和l3两投影直线交于消失点q3(x3,y3)，则根据消失点特性[16]可知向量Ocq3的单位向量与Zd轴的方向单位向量一致，可得Zd轴的方向向量nz为

(8)

通过nx和nz叉乘可确定Yd轴的方向向量ny为

(9)

(10)

则可计算得到相机在激光测量坐标系中的偏航角αc、横滚角βc、俯仰角γc为

(11)

(12)

相机在激光测量坐标系下的位置(Xc,Yc,Zc)为

(13)

(14)

将式(14)代入式(1)中，根据相机在激光测量坐标系下的位置和姿态角信息，结合通过事先标定得到的掘进机机体坐标系和相机坐标系之间的变换关系，可计算得到掘进机在激光测量坐标系下的位姿。利用全站仪测量激光指向仪在巷道坐标系下的坐标，获取激光测量坐标系和巷道坐标系之间的转换关系，最后便可求得掘进机在巷道坐标系下的偏航角、横滚角、俯仰角和位置信息。

3 悬臂式掘进机定向掘进控制模型

3.1 掘进机纠偏控制策略

巷道掘进施工中，悬臂式掘进机纠偏主要包括整平和对中，其中整平过程通过控制掘进机前后铲板和支撑的升降调整掘进机的角度;对中过程是控制掘进机左右履带运动使掘进机机身位置与期望位置重合，实现对机身位置的调整[17]。通过掘进机对中纠偏过程，实现对掘进机机身偏航角偏差α和位置偏差的纠偏控制。图6为掘进机的对中示意图，掘进机纠偏步骤如下:

(1)首先比较视觉测量得到的机身位姿数据和期望的机身位姿数据，确定机身偏航角偏差α和中心距偏差d。

(2)机身存在中心距为d时，当d>0时，控制掘进机左履带反转，右履带正转，使机身存在一定偏向角，然后控制左右履带同时正转，直至中心距小于限定值;当d<0时，则方向相反。

(3)机身存在偏航角偏差α时，当α>0时，控制掘进机左履带反转，右履带正转;α<0时，控制掘进机左履带正转，右履带反转，直至偏航角小于限定值。

通过上述掘进机纠偏控制步骤可实现巷道中掘进机的定向掘进控制。

3.2 悬臂式掘进机定向掘进运动模型

通过对悬臂式掘进机纠偏分析可知，掘进机是通过左右两侧履带做差速运动实现的运动控制。为建立掘进机机身的运动学模型，假设:① 悬臂式掘进机在二维平面内运动;② 悬臂式掘进机左右两侧履带与地面完全接触;③ 两侧履带中心连线与掘进机前后运动方向垂直[18]。则悬臂式掘进机运动学模型如图7所示。

图7中坐标系XOZ为巷道二维平面坐标系;k点为参考点，xk，yk为该点的坐标;θ为悬臂式掘进机的航向角;坐标系X0O0Z0为以掘进机质心为原点的掘进机二维平面坐标系，设(x0,y0)为掘进机质心在巷道坐标系XOZ中的坐标;vL和vR为掘进机运动时其左右履带速度;b为单侧履带的宽度，D为机身宽度。则机身在巷道坐标系XOZ中的位姿可表示为P=[x0,y0,θ]T。根据图中的几何关系可推得悬臂式掘进机运动学模型为

(15)

设机身运动线速度为v，角速度为ω，则左右履带速度和机身之间速度可表示为

(16)

结合式(15)，(16)可得掘进机从当前位置移动到k点时的运动学方程为

(17)

(18)

其中，xe为掘进机在巷道中的横向误差;ye为掘进机在巷道中的纵向误差;θe为掘进机航向角误差。其位姿误差微分方程描述为

(19)

式中，ωA为当前位姿状态下的机身运动角速度;vA为当前位姿状态下的机身运动线速度;vB和ωB为期望位姿状态下的机身运动线速度和角速度;偏航角误差θe=θB-θA。

4 轨迹跟踪控制器设计

4.1 掘进机定向掘进控制系统

悬臂式掘进机通过左右两侧履带差速运动实现机身的移动，属于典型的非完整移动机器人。对于非完整移动机器人系统需要设计控制输入作用的控制器，使机器人可以以给定的速度沿规划轨迹运动。掘进机定向掘进控制系统具有高度非线性，经典线性控制理论难以直接应用于系统的控制器设计。Back-stepping方法[19]是一种非线性系统的稳定设计理论，针对非线性系统中的参数不确定性，通过逐步修正算法设计控制器，可实现系统的全局调节。因此，论文采用Back-stepping方法，采用根据悬臂式掘进机运动学模型设计轨迹跟踪控制器，悬臂式掘进机定向掘进控制系统结构如图8所示。

图8 悬臂式掘进机定向掘进控制结构框Fig.8 Block diagram of directional heading control structure of boom-type roadheader

系统输入掘进机期望位姿Pr=[xr,yr,θr]T和跟踪控制时的期望运动速度，即参考速度qr=[vr,ωr]T;输出为掘进机当前时刻位姿Pc=[xc,yc,θc]T。通过设计系统速度控制器使当前位姿与期望位姿之间的位姿误差Pe确定掘进机的运动控制速度Pc，通过反复调节控制使得掘进机位姿误差Pe趋近于0。

4.2 掘进机轨迹跟踪控制器设计

笔者结合Back-Stepping设计方法，根据Lyapunov函数设计具有全局收敛特性的掘进机轨迹跟踪控制器。分别以Pr和qr为掘进机期望位姿及速度，则根据式(19)掘进机位姿微分方程有

(20)

引理:对任意x∈R，并且|x|<∞，则有f(x)=xsin(arctan(x))≥0，当且仅当x=0时，f(x)=0成立[20]。根据上述引理可知，悬臂式掘进机轨迹跟踪控制器设计步骤如下:

第1步:根据掘进机位姿微分方程可知，当xe=0时可选Lyapunov函数为

(21)

对式(21)微分可得

(22)

根据上述引理可知，当θe=a=arctan(vrye)时可使ye收敛，此时若xe收敛于0，则θe收敛到a，便有ye收敛于0。掘进机轨迹跟踪的控制律输入为v和ω，可取偏向角误差变量为

(23)

第2步:在Vy基础上选择全局Lyapunov函数V为

(24)

对上式微分可得

(25)

则可得悬臂式掘进机轨迹跟踪控制律为

(26)

其中，(xe,ye,θe)T为掘进机的位姿误差;(vr,ωr)为掘进机的参考速度;k1和k2为常数。掘进机轨迹跟踪控制律以掘进机位姿误差和参考速度为输入，以掘进机运动的控制速度为输出，通过控制器的反复调节，使掘进机的位姿误差最终趋近于0。

5 测量系统性能评价及实验结果

5.1 机身位姿视觉测量实验

5.1.1 悬臂式掘进机位姿视觉测量实验

搭建了悬臂式掘进机位姿视觉测量实验平台，进行功能验证和测量性能评价，如图9所示。平台由悬臂式掘进机模型、2个激光指向仪、MV-EMV510M防爆工业相机和防爆计算机等组成。

图9 悬臂式掘进机机身位姿测量实验平台Fig.9 Boom-type roadheader body posture measurement experimental platform

悬臂式掘进机位姿视觉测量精度测试实验步骤如下:

(1)搭建平台时，将防爆计算机固定于悬臂式掘进机机体上，使相机采集激光指向仪激光图像，在原有激光指向仪旁再平行固定一个激光指向仪，为掘进机机身位姿视觉测量提供测量基准。

(2)通过软触发方式控制防爆摄像机采集激光指向仪光斑和光束图像，并在计算机中进行图像预处理、激光光斑中心定位、激光束直线特征提取以及位姿解算等过程，计算得到掘进机在激光测量坐标系中的位姿。

(3)将悬臂式掘进机移动至不同位置，利用全站仪获得掘进机机身在激光测量坐标系下的位姿实际值，与视觉测量系统测量结果进行对比，获取掘进机位姿视觉测量误差。

设定两激光之间距离为55 cm，根据上述实验步骤进行实验，得到实验结果和误差如表1，图10，11所示。

表1 悬臂式掘进机机身位姿测量部分实验结果Table 1 Experimental results of boom-type roadheader body posture measurement

图10 机身姿态角测量误差Fig.10 Error of measurement of fuselage attitude Angle

实验结果表明本文所述的悬臂式掘进机位姿视觉测量方法可有效实现机身位姿的获取。图10为测量得到的掘进机姿态角角度误差，其中机身偏航角角度误差小于0.3°，俯仰角角度误差小于0.25°，横滚角角度误差小于0.3°。图11为测量得到掘进机位置误差，x，y，z轴方向的误差分别为20，25,18 mm。

图11 机身位置测量结果误差Fig.11 Error of airframe position measurement

5.1.2 悬臂式掘进机可视化截割井下测试

基于笔者提出的机身定位方法研发了悬臂式掘进机可视化截割控制系统，图12为在山西煤矿井下环境中搭建的悬臂式掘进机可视化辅助截割系统试验平台，平台由掘进机位姿视觉测量系统、捷联惯导和防爆计算机等组成。以多传感器实现机身和截割头位姿测量，借助机器人控制方法实现结果过程的可视化导引和超挖预警。

图12 悬臂式掘进机可视化辅助截割系统试验平台Fig.12 Visual auxiliary cutting system test platform of boom-type roadheader

该系统在某矿回风掘进巷道进行了悬臂式掘进机可视化辅助截割测试。测试时移动安装在机身上的防爆工业相机来模拟机身的移动，通过人工测量方式获取相机在巷道中的移动数据为真实值，最后将人工测量结果与视觉测量结果比较获取机身位姿测量误差。表2及图13为得到的测量结果及误差，考虑井下环境中机身姿态角角度的真实值测量困难，故仅给出计算得到的掘进机姿态信息。

表2 悬臂式掘进机井下位姿测量部分实验结果Table 2 Part of experimental results of underground pose measurement of boom-type roadheader

图13 悬臂式掘进机位置测量误差Fig.13 Measurement error of boom-type roadheader

结果表明，该方法可以在掘进工作面高粉尘、水雾以及杂光的环境下稳定获取激光图像特征，计算出悬臂式掘进机机身位置信息。由于井下巷道地势环境影响视觉标定精度，以及恶劣环境等影响，系统的测量误差与实验室相比有所增大，X，Y，Z轴方向的误差分别为±38，±40，±40 mm。值得注意的是，考虑了视觉测量可能存在的遮挡、丢帧等问题，可视化截割系统中组合了低成本MEMS惯导，多传感器融合保证了悬臂式掘进机机身定位的精度和稳定性。

5.2 掘进机轨迹跟踪控制实验

为了验证定向掘进控制系统的可行性和稳定性，搭建定向掘进控制实验平台，如图14所示,为方便对履带转动速度的控制，采用电机驱动的履带式移动机器人代替悬臂式掘进机进行实验。实验平台由履带式移动机器人、内置DSP控制器、MV-EMV510M防爆工业相机、YHJ-800激光指向仪和计算机组成。

图14 定向掘进控制实验平台Fig.14 Directional heading control experiment platform

实验时以楼道模拟巷道环境，确定巷道坐标系，首先采用全站仪测量履带机器人和激光测量坐标系在巷道坐标系中的位姿，并对相机进行外参标定。在运动过程中，借助机身位姿视觉测量系统实时测量机器人位姿，并将机身位姿数据反馈至DSP控制器内，根据当前机身位姿误差确定输出控制命令，控制左右履带转动，通过不断调整使机身移动至期望位置。

设定以巷道中线为掘进机定向移动的参考轨迹，利用全站仪测量得到移动机器人在巷道平面坐标系的初始位置为(-0.4，0)，偏航角为-5.21°，设移动机器人运动终点为(0，5)，偏航角为0°，机身位置误差允许范围在3 cm以内，角度误差允许范围在0.2°以内。根据上述实验步骤进行实验，如图15，16所示为轨迹跟踪控制结果及误差。

图15 机身轨迹跟踪结果Fig.15 Airframe trajectory tracking results

图15机身轨迹跟踪结果中，蓝色线为设定的机身参考运动轨迹，红色线为机身的实际运动轨迹，机身从起始位置开始运动时存在起始位置偏差，随着运动进行，机身逐渐沿参考轨迹进行运动，通过不断调整机身运动方向，使机身最终运动至期望点。由图16机身轨迹跟踪误差可得，机身位置和偏航角误差最终都收敛，且机身在X方向的位置误差为±15 mm以内，在Y方向的位置误差为±20 mm以内，机身偏向角角度误差在0.15°以内，均满足允许范围。

图16 机身轨迹跟踪误差Fig.16 Fuselage trajectory tracking error

6 结 论

(1)提出基于图像测量的激光指向仪精确定位方案，由于相机安装于机身且两束激光线构建门型三线特征实现机身位姿测量，与相关研究对比，具有不易脱靶、标靶移动少等优点。

(2)基于单目视觉测量原理建立悬臂式掘进机位姿视觉测量模型，以2个激光指向仪为参考基准，实现掘进机机身的相对位姿测量，具有系统简单经济的优势。实验表明，利用视觉测量方法进行机身位姿测量时，姿态角角度误差在0.3°以内，位置误差在25 mm以内。实际在井下工业试验时，考虑了视觉测量存在的遮挡、丢帧等问题，组合了低成本MEMS惯导，获得了稳定的测量效果。

(3)根据悬臂式掘进机运动特点设计纠偏策略，构建了悬臂式掘进机定向掘进运动控制模型，采用Back-stepping方法实现掘进轨迹的跟踪控制，对实现综掘工作面自动化提供了借鉴。轨迹跟踪控制实验表明，控制器按照视觉测量反馈的信息控制机身沿设定的参考轨迹运动，运动至终点时，掘进机机身偏航角角度误差在0.15°以内，机身位置误差在20 mm以内，可在误差允许范围内实现对移动机器人的轨迹跟踪控制。