0 引言

随着煤炭行业的不断发展，采掘失衡问题越来越突出。目前，煤矿井下巷道掘进主要由工人手动操作完成，截割断面的成形质量完全取决于人为因素，断面成形质量无法保证且巷道掘进效率低下。提高煤矿巷道掘进装备成形截割控制系统的自动化和智能化水平，是目前国内行业专家对解决采掘失衡问题达成的共识。李军利[1]对悬臂式掘进机的运动学、动力学及电液比例控制系统进行了研究。董磊等[2]对悬臂式掘进机回转机构动力学建模进行了研究。田劼等[3]对掘进机空间位姿的运动学模型进行了研究。王苏彧等[4]对悬臂式掘进机自主截割技术进行了研究，同时对自动截割轨迹规划进行了研究。高峰等[5]研究了掘进机截割臂摆速的自动控制。田劼等[6-7]提出了一种综合慢开、慢停与加减速控制及截割臂摆速分级自动调控方法。穆晶等[8]对截割断面的极限位置进行了分析。上述学者从悬臂式掘进机的运动学建模、轨迹规划以及自动截割控制等理论与技术方面提出了用于提高掘进机智能化水平的诸多方案，但是目前掘进装备智能化水平受制于井下设备位姿测量等技术水平，巷道的定向掘进精度和定形截割质量亟待提高。

煤矿巷道自动成形截割控制取决于掘进机机身定位和截割头位姿测量精度及稳定性。吴淼等[9]提出了一种基于激光测距的掘进机定位技术。杨文平等[10]通过全站仪测量到目标点的直线距离、水平角和垂直角，通过这些数据可求解掘进机位姿。但这2种方法的限制条件较多，无法满足实际工况要求。崔柳等[11]提出了基于一种UWB(Ultra Wide Band，超宽带)的掘进机姿态测量方法，该方法精度高，但操作复杂，难以实现实时自主测量。童敏明等[12]将三轴加速度传感器、单轴陀螺仪和超声波测距传感器进行数据融合来实现掘进机的定位,但掘进机定位和智能截割控制的精度和稳定性还有待提高。为此，本文在前期掘进机机身及截割头位姿视觉测量研究的基础上[13-16]，研究基于运动学解算的悬臂式掘进机器人空间关节轨迹规划理论及技术，设计了反馈线性化积分滑模控制器，提出了一种基于视觉反馈的悬臂式掘进机器人巷道成形智能截割控制系统,该系统能够实现悬臂式掘进机器人巷道成形截割过程的智能控制，定位精度高，稳定性好，可满足巷道成形质量需求。

1 控制系统总体方案

自动成形截割控制的前提是规划截割头运动轨迹，截割头运动轨迹与巷道断面类型、掘进工艺以及截割头外轮廓等密切相关。规划好截割轨迹后，以视觉测量得到的实际掘进机机身及截割部位姿为基础点，以截割轨迹上的点为控制目标，通过掘进机器人运动学逆解可以计算得到机器人各关节的运动控制量，以理论规划轨迹点和视觉测量位姿点构建反馈控制系统，实现悬臂式掘进机器人成形截割过程的动态控制。

悬臂式掘进机器人巷道成形截割控制系统总体方案如图1所示。在悬臂式掘进机器人上布局位姿测量传感器，包括安装在截割臂上的红外LED激光标靶、安装在已成形巷道顶部的2个激光指向仪，还有安装在机身上的捷联掼导系统、前向和后向2个防爆数字相机。系统利用前向防爆数字相机获得红外LED激光标靶图像，利用后向防爆数字相机获得2条激光线图像，基于单目视觉测量技术计算悬臂式掘进机器人机身位姿、截割头位姿，辅助捷联惯导等传感器实现悬臂式掘进机器人机身定位、定向。借助机器人D-H(Denavit-Hartenberg)解算和轨迹跟踪控制算法，可实现巷道成形截割视觉伺服控制。

图1 悬臂式掘进机器人智能巷道成形截割控制系统总体方案
Fig.1 Overall scheme of intelligent cutting control system for roadway forming of boom-type tunneling robot

防爆数字相机通过RJ45通信线与工业计算机相连，前向防爆数字相机采集固定于掘进机器人截割臂上的16点红外LED激光标靶图像，经过预处理、特征提取、空间三维坐标计算、坐标转换得到截割头姿态数据；后向防爆数字相机采集2个平行安装的激光指向仪的激光线，通过图像处理和位姿解算获得机身相对于激光指向仪的位姿数据。上述机身和截割头的位姿解算数据通过RS485传入DSP控制器，代入截割臂运动学方程，实时解算悬臂式掘进机器人截割头位置和姿态参数。智能控制系统运行时，首先设置巷道尺寸参数、掘进机外形参数，可选择不同的截割工艺，系统按照巷道和掘进机器人截割头尺寸参数自动规划巷道断面截割轨迹，轨迹数据通过串口发送给控制器，控制器利用截割臂的轨迹自动跟踪算法实时计算控制量，最后通过D/A模块输出的电流控制电控系统中的电液比例阀，从而实现掘进过程的截割位置修正。

2 掘进机器人截割轨迹规划

2.1 悬臂式掘进机器人坐标系统

悬臂式掘进机器人各坐标系分布如图2所示，其中截割头坐标系为OiXiYiZi，机身坐标系为OsXsYsZs，相机坐标系为OcXcYcZc，标靶坐标系为OwXwYwZw，机身坐标系原点Os是悬臂式掘进机器人的质心。

图2 悬臂式掘进机器人坐标系统
Fig.2 The coordinate system of boom-type tunneling robot

悬臂式掘进机器人的成形截割运动包括掘进机器人行走控制和截割头运动控制。前者由两履带液压马达的差速控制完成，后者涉及截割头伸缩量、回转台左右转动量和截割臂上下摆动量的控制。悬臂式掘进机器人的正运动学求解采用D -H模型，模型参数见表1。

2.2 掘进机器人截割头运动学正解

利用掘进机器人D -H参数将掘进机悬臂上各运动部件的角度变量转换到截割头的位姿坐标进行表示，显示出掘进机截割头相对于掘进机机身的位置和姿态，属于机器人运动学正解问题。悬臂式掘进机器人的截割头相对于其机身坐标系的变换矩为

表1 悬臂式掘进机器人的D -H参数
Table 1 D -H parameters of boom-type tunneling robot

连杆iai-1αi-1Diθi1L10H1θ12L2-90°0θ23H2-90°L3+d0400L40

注：1.ai-1为连杆长度，Li,Hi分别为图2中所示的各坐标系之间的位置参数，i=1,2,3,4。
2.αi-1为连杆扭曲角度。
3.Di为连杆偏置量，d为截割头伸缩量。
4.θi为连杆关节角度，θ1为回转关节的转动变量，θ2为截割臂的垂直摆动量。

(1)

记截割头中心点坐标向量则掘进机器人截割头中心点相对于机身坐标系的坐标向量为

(2)

因此，截割头中心点坐标为

(3)

2.3 掘进机器人截割头运动学逆解

将截割头的位姿信息转换为各关节的活动信息，即给出截割头要达到目标位姿所需要的关节变量。逆问题的求解是掘进机进行截割轨迹规划、运动控制和截割断面成形控制的基础。根据机器人学知识可知，截割头运动学正问题的解是唯一的，但逆问题的解存在不唯一或不存在的情况，因此，对最后的结果要进行判断。掘进机器人截割头空间位姿变换矩阵可以表示为

(4)

式中和p分别为掘进机器人截割头中心点坐标系相对于机身坐标系的旋转矩阵和平移矩阵，由捷联惯导和视觉测量系统测量得到。

令式(1)和式(4)相等，可以得到关节变量θ1和θ2：

(5)

2.4 掘进机器人关节空间轨迹规划

轨迹规划方法常用关节空间法和直角坐标系法。因井下巷道断面形状多为矩形，所以，本文以矩形断面为例，采用关节空间法进行巷道断面轨迹规划。综合考虑实际情况与截割效率，拟采用如图3所示的“S”形截割轨迹。在巷道掘进过程中，截割头的运动主要是水平回转和上下抬升，如AB段内的截割头水平移动，则回转关节的关节角θ1变化，BC段内的截割头做竖直移动,则抬升关节的关节角θ2变化。采用三次多项式轨迹规划法对其关节轨迹进行规划，确定轨迹光滑函数，以计算任意时刻的关节位置。

确定轨迹光滑函数时，需确定满足光滑函数的条件：

(1)已知起点位置及终点位置的关节角分别为θ0,θf，从起点移动至终点的时间为tf，它们之间的关系为

图3 矩形断面轨迹
Fig.3 Rectangular section locus

(6)

(2)截割过程中，各关节起点及终点速度为0，表达式为

(7)

(3)建立三次多项式:

θ(t)=k0+k1t+k2t2+k3t3

(8)

将式(6)、式(7)代入式(8)，可得到轨迹光滑函数的4个待定系数为

(9)

根据截割头在A、B、C点的断面坐标，通过运动学逆解计算出3个位置的关节角分别为θa1，θb1，θc1，AB段和BC段的运动时间分别为t1、t2。在确定关节位置的光滑函数后，还可确定运动过程中的速度、加速度的时间函数。在进行截割头直线运动时，首先确定直线运动过程所需要的时间，再确定直线运动起点与终点的位置，便可根据插值结果确定任意时刻的关节位置。

AB段关节的位置函数为

(10)

AB段关节的角速度函数为

(11)

AB段关节的角加速度函数为

(12)

BC段关节的位置函数为

(13)

BC段关节的角速度函数为

(14)

BC段关节的角加速度函数为

(15)

3 基于视觉反馈的截割头轨迹跟踪控制

3.1 悬臂式掘进机器人视觉定位技术

悬臂式掘进机器人在截割头安装红外LED激光标靶，在机身上安装防爆数字相机，通过防爆数字相机采集激光标靶图像并进行一系列算法处理，获得掘进机器人截割头相对于机身的位姿坐标[14]。掘进机器人位姿解算过程如图4所示，利用防爆数字相机内参数对标靶图像进行校正处理，高斯滤波对图像进行滤波处理后，根据光斑特性提取特征点信息，计算红外LED激光标靶的4个顶点在图像坐标系下的坐标，利用四点透视原理得到红外LED激光标靶的三维空间坐标，最后结合经过标定获得的截割臂关节间的坐标转换参数，得到截割头的姿态角。

图4 悬臂式掘进机器人位姿解算
Fig.4 The pose estimation of boom-type tunneling robot

悬臂式掘进机器人机身位姿采用两点三线测量原理[15]，借助巷道激光指向仪(增加一个激光指向仪构成平行的2条激光线)，利用掘进机器人机身上安装的防爆数字相机采集激光线，利用改进的随机Hough变换对激光束进行直线检测，通过建立的基于两点三线的掘进机位姿解算模型获得掘进机器人机身相对巷道的位姿坐标。

3.2 反馈线性积分滑模轨迹跟踪控制器设计

截割轨迹表征了悬臂式掘进机截割头的中心点运动轨迹曲线。轨迹控制就是设计控制器实现截割头运动轨迹与预先规划轨迹的误差控制。悬臂式掘进机器人的系统动力学模型[17]为

(16)

式中：M(q)为悬臂式掘进机器人的惯性矩阵，q为悬臂式掘进机器人关节的广义坐标；为关节速度引起的哥氏力和向心力；G(q)为重力矩阵；为悬臂式掘进机器人系统中的不确定性部分,τ为控制转矩。

设状态矢量z=，令基于计算力矩法进行控制器设计，线性滑模控制律为

(17)

式中u为反馈控制器。

u的表达式为

(18)

式中：为对时间的二阶导数；Kd为微分系数矩阵；Kp为比例系数矩阵；为偏差变化率;e为位置偏差。

由式(17)和式(18)可以得到线性控制器为

τ=

(19)

将式(16)代入式(17)，得

(20)

设Yd=0，则式(20)变为

(21)

由于M(q)为非奇异矩阵，则

(22)

分别定义位置和加速度的跟踪误差为

(23)

由式(18)、式(22)、式(23)可得闭环系统跟踪误差满足：

(24)

式(19)的线性控制器假定了不确定项Yd=0。但在实际应用中，外界干扰不可避免，负载也是随机时变的。为了改善控制的鲁棒性，在线性反馈控制的基础上设计一个线性积分滑模控制器[18]，降低参数变化和外界扰动的影响。

普通的滑模面为

(25)

式中λ=diag(r1,r2),ri>0，i=1,2。

当普通滑模面的趋近率增大时能够提高响应速度，但同时会引起抖动，故引入积分滑模算法。

积分滑模面为

(26)

式中Λ1=diag(λ11,λ12)，Λ2=diag(λ21,λ22)，为正对角矩阵。

所以，上述反馈线性化积分滑模控制器为

(27)

式中：K=diag(μ1,μ2),μi>0，为滑模切换控制的控制增益；Ksgn(Si)用于补偿建模的误差与外界等引起的不确定扰动项。

(28)

4 实验验证

为了验证悬臂式掘进机器人巷道成形智能截割控制系统的可靠性，搭建了实验平台，如图5所示。掘进机器人采用1∶5缩小的EBZ160悬臂式掘进机器人。实验台上安装的传感器及位置：红外标靶固定在掘进机截割臂上；2台防爆数字相机采用加装有窄波滤镜的MV-EM130M工业相机；超声波传感器采用GUC6矿用本安型超声波物位传感器。采用KJD127矿用隔爆兼本安型计算机完成传感器数据处理和显示、视觉测量和轨迹规划等功能。悬臂式掘进机器人改造时各运动关节均安装了位移传感器，采用自主研发的基于DSPF2812的运动控制器完成悬臂式掘进机器人各运动关节的轨迹跟踪控制。

图5 悬臂式掘进机器人巷道成形智能截割控制系统实验平台
Fig.5 Experimental platform of intelligent cutting control system for roadway forming of boom-type tunneling robot

在实验台上进行掘进机器人轨迹控制精度及稳定性实验。截割臂回转关节和抬升关节的轨迹跟踪控制结果对比如图6所示。实验结果表明，回转关节的角度跟踪误差平均值小于0.39°，抬升关节的角度跟踪误差平均值小于0.45°，视觉伺服控制下的掘进机器人截割臂回转关节和抬升关节的实际运动位置与所规划的轨迹基本相符，且截割头截割轨迹跟踪控制稳定。

图6 关节空间轨迹跟踪控制结果对比
Fig.6 Comparison of control results of trajectory tracking of joint space

采用Vicon视觉跟踪仪实时检测截割头轨迹跟踪控制过程的位姿变化，与理论规划的截割头运行轨迹进行对比，轨迹跟踪控制结果如图7所示。从图7可看出，视觉伺服控制下的悬臂式掘进机器人的轨迹跟踪误差小于25.61 mm，且轨迹跟踪过程表现稳定。考虑实际井下工况及掘进工作特点，本文提出的掘进机器人巷道成形智能截割控制系统可以满足现场对巷道成形控制的要求。

图7 轨迹跟踪控制结果
Fig.7 The control results of trajectory tracking

5 结论

对掘进机器人视觉定位、巷道截割面轨迹规划和智能控制等理论和技术进行了研究，提出了悬臂式掘进机器人巷道成形智能截割控制系统，解决了基于视觉反馈的井下掘进机器人巷道成形智能截割控制问题。

(1)结合巷道断面截割和掘进工艺要求，提出了一种基于三次多项式的掘进机器人关节轨迹规划方法，在构建的悬臂式掘进机器人坐标系统基础上，通过悬臂式掘进机器人运动学逆问题求解确定轨迹光滑函数，从而得到了任意时刻的机器人各关节的运动控制量。

(2)以构建的悬臂式掘进机器人系统动力学模型为基础，提出了一种基于计算力矩法的悬臂式掘进机器人线性积分滑模控制器。以巷道成形轨迹面为目标，以视觉测量得到的实际掘进机机身及截割部位姿作为反馈量，以理论规划轨迹点和视觉测量位姿点经过差值计算和运动学逆解得到机器人各关节的运动控制量，通过控制器驱动截割臂关节到达相应的位置。

(3)搭建了悬臂式掘进机器人巷道成形智能截割控制系统实验平台，对掘进机器人轨迹控制精度及稳定性进行实验，结果表明，该系统能够实现悬臂式掘进机器人成形截割过程的动态控制，视觉伺服控制的轨迹跟踪误差可满足巷道成形质量需求。

参考文献：(References)

[1] 李军利.悬臂式掘进机断面自动成形理论与控制策略研究[D].太原：太原理工大学,2009.

[2] 董磊,任家骏,王喜胜,等.悬臂式掘进机回转机构的建模与动力学仿真[J].煤炭科学技术,2009,37(6):83-85.

DONG Lei,REN Jiajun,WANG Xisheng,et al.Model establishing and dynamics simulation of rotary mechanism for boom-type roadheader [J].Coal Science and Technology,2009,37(6):83-85.

[3] 田劼,王苏彧,穆晶,等.悬臂式掘进机空间位姿的运动学模型与仿真[J].煤炭学报,2015,40(11):2617-2622.

TIAN Jie，WANG Suyu，MU Jing，et al．Spatial pose kinematics model and simulation of boom-type roadheader[J]．Journal of China Coal Society,2015,40(11):2617-2622.

[4] 王苏彧,吴淼.基于PCC的纵轴式掘进机自主截割控制系统研究[J].煤炭工程,2016,48(6):132-135.

WANG Suyu,WU Miao.Autonomous cutting control system of longitudinal roadheader based on PCC[J]．Coal Engineering，2016,48(6):132-135.

[5] 高峰,王苏彧,高娟,等.掘进机截割臂摆速自动控制方法研究[J].工矿自动化,2011,37(8):38-41.

GAO Feng,WANG Suyu,GAO Juan,et al.Research of automatic control method of swing speed of cutting arm of roadheader[J].Industry and Mine Automation,2011,37(8):38-41.

[6] 田劼,王红尧,吴淼.掘进自动截割成形控制系统动态特性分析[J].工矿自动化,2015,41(6):40-44.

TIAN Jie,WANG Hongyao,WU Miao.Dynamic characteristics analysis of automatic cutting and profiling control system for roadheader[J].Industry and Mine Automation,2015,41(6):40-44.

[7] 田劼,吴淼.煤矿掘进机自动截割成形控制方法研究[J].煤矿机械,2015,36(5):54-57.

TIAN Jie,WU Miao.Research on automatic cutting shaping control method of coal mine roadheader[J]．Coal Mine Machinery，2015,36(5):54-57.

[8] 穆晶,杜毅博,田劼,等.悬臂式掘进机截割断面极限位置分析[J].工矿自动化,2011,37(10):67-69.

MU Jing,DU Yibo,TIAN Jie,et al.Analysis of extreme position of cutting section of boom-type roadheader [J].Industry and Mine Automation,2011,37(10):67-69.

[9] 吴淼,贾文浩,华伟，等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[J].煤炭学报,2015,40(11):2596-2602．

WU Miao,JIA Wenhao,HUA Wei,et al.Autonomous measurement of position and attitude of boom-type roadheader based on space intersection measurement[J].Journal of China Coal Society,2015,40(11):2596-2602．

[10] 杨文平,胡鹏,樊纲.掘进机自动定向技术探究[J].煤矿机械,2016,37(8):46-48.

YANG Wenping,HU Peng,FAN Gang.Research on automatic orientation technology of excavator [J].Coal Mine Machinery,2016,37(8):46-48.

[11] 崔柳,徐会军,刘治翔,等.基于超宽带定位的掘进机姿态监测仿真与实验研究[J].煤炭学报,2017,42(增刊1):267-274.

CUI Liu,XU Huijun,LIU Zhixiang,et al．Simulation and experiment on the attitude monitoring of roadheader based on ultra wide band[J].Journal of China Coal Society,2017,42(S1):267-274．

[12] 童敏明,杜雨馨,李高军,等.多传感器的掘进机定位系统研究[J].煤矿机械,2013,34(6):146-148.

TONG Minming,DU Yuxin,LI Gaojun,et al.Study on positioning system of roadheader by multi-sensor[J]．Coal Mine Machinery,2013,34(06):146-148.

[13] 张旭辉,刘永伟,杨文娟,等.矿用悬臂式掘进机截割头姿态视觉测量系统[J].工矿自动化,2018,44(8):63-67.

ZHANG Xuhui,LIU Yongwei,YANG Wenjuan,et al.Vision measurement system for cutting head attitude of mine-used boom-type roadheader [J].Industry and Mine Automation,2018,44(8):63-67.

[14] YANG Wenjuan，ZHANG Xuhui，MA Hongwei，et al.Infrared LEDs-based pose estimation with underground camera model for boom-type roadheader in coal mining[J].IEEE Access,2019,7:33698-33712.DOI:10.110/ACCESS.2019.2904097.

[15] 杨文娟,张旭辉,马宏伟,等.悬臂式掘进机机身及截割头位姿视觉测量系统[J].煤炭科学技术,2019,47(6):50-57.

YANG Wenjuan,ZHANG Xuhui,MA Hongwei,et al.Position and posture measurement system of body and cutting head for boom-type roadheader based on machine vision [J].Coal Science and Technology，2019,47(6):50-57.

[16] 杨文娟,马宏伟,张旭辉.悬臂式掘进机截割头姿态视觉检测系统[J].煤炭学报,2018,43(增刊2):581-590.

YANG Wenjuan,MA Hongwei,ZHANG Xuhui.Attitude measurement system of cutting head for boom-type roadheader based on vision measurement [J].Journal of China Coal Society,2018,43 (S2):581-590.

[17] 刘永伟.矿用悬臂式掘进机器人控制系统研制[D].西安:西安科技大学,2018.

[18] 胡盛斌,陆敏恂.空间三关节机器人模糊积分滑模控制[J].计算机仿真,2012,29(2):162-166.

HU Shengbin，LU Minxun.Fuzzy integral sliding mode control for three-links spatial robot [J].Computer Simulation,2012,29(2):162-166.