0 引言
目前，煤矿井下仍存在大量人工手动操控的悬臂式掘进机，导致巷道掘进效率低，成形质量差[1-2]。发展巷道快速掘进成套技术装备、提高掘进智能化水平已成为保障煤矿安全高效生产的先决条件[3-4]。其中，具有精确定位和自动纠偏控制功能的快速掘进机器人是实现巷道掘进智能化的关键[5-6]。
许多学者对煤矿井下掘进机定位与控制技术进行了深入研究。陶云飞[7] 提出了一种基于激光导向系统的掘进机位姿自动测量方法。杨文娟等[8] 提出了一种基于激光束和红外光斑特征的悬臂式掘进机机身及截割头位姿视觉测量系统。田原[9] 建立了掘进机惯性定位系统，将PHINS惯性导航系统用于掘进机自动定位，利用零速修正法提高其定位精度。吴淼等[10]提出了一种基于空间交汇测量技术的掘进机自主位姿测量方法。张旭辉等[11] 构建了虚拟设备及工作场景的模型，提出虚拟工作面的动态修正方法和设备的碰撞检测方法，实现了监测数据驱动下的虚拟交互式设备远程操控。朱伟等[12]设计了一种基于嵌入式软PLC技术的掘进机控制系统，实现了掘进机位姿监测与遥控控制。瞿圆媛等[13]建立了掘进机行走位姿偏差模型，在此基础上对掘进机行进纠偏调度规划与控制进行了研究。但上述掘进机控制技术主要以掘进机自主定位为基础，未考虑在井下高粉尘、低照度环境中掘进机定位与控制精度不高的问题。本文提出一种基于视觉测量的快速掘进机器人纠偏控制系统，对快速掘进机器人进行位姿调整，实现对快速掘进机器人的精确控制。
1 快速掘进机器人纠偏控制系统
1.1 快速掘进机器人总体结构
快速掘进机器人是集断面连续截割、支护、自动定位、无线遥控、快速装运和机械除尘等功能于一体的快速巷道掘进装备[14]。快速掘进机器人总体结构，由掘进机器人、临时支护机器人1&2(组成护盾结构)、钻锚机器人、锚网运输机器人、电液控平台、运输及通风除尘系统等组成，如图1所示。
快速掘进机器人通过前后盾体之间的推拉作用实现推进，并反复进行截割-推进过程，直至完成整个巷道的掘进工作。推进过程中通过视觉测量方法检测快速掘进机器人位姿并反馈给电液控平台，电液控平台根据位姿偏差，控制临时支护机器人上的水平和垂直伸缩油缸来调整整个系统方向、位置和姿态，保证下一阶段巷道的截割精度。
1-掘进机器人； 2-临时支护机器人1； 3-临时支护机器人2；
4-钻锚机器人； 5-锚网运输机器人； 6-电液控平台；
7-运输及通风除尘系统。
图1 快速掘进机器人总体结构
Fig.1 Structure of rapid tunneling robot
1.2 快速掘进机器人纠偏控制系统架构
快速掘进机器人纠偏控制系统由激光指向仪、防爆相机、防爆计算机、DSP控制器等组成，如图2所示。
图2 快速掘进机器人纠偏控制系统架构
Fig.2 Framework of deviation correction control system for rapid tunneling robot
利用安装在快速掘进机器人上的防爆相机，对后方的激光指向仪进行图像采集，通过以太网将采集的图像信息传输给防爆计算机，利用防爆计算机对图像进行预处理，根据位姿解算模型解算出快速掘进机器人位姿信息，并将解算出的位姿信息保存在MySQL数据库中。将解算的位姿信息与巷道设计轴线信息进行比较，计算出快速掘进机器人位姿与巷道设计轴线间的位姿偏差。根据位姿偏差的类型与大小，采用不同的控制策略计算出修正后控制量，并输出控制指令，数字信号的控制指令通过数模转换(D/A)模块转换成模拟信号，模拟信号经放大器放大后控制电磁比例阀的阀芯移动，实现对伸缩油缸的控制，根据各区域伸缩油缸的伸缩量不同，对快速掘进机器人进行位姿调整，实现纠偏控制。
2 快速掘进机器人位姿视觉测量
2.1 快速掘进机器人位姿视觉测量方案
快速掘进机器人位姿视觉测量由临时支护机器人、防爆相机和2个激光指向仪实现，如图3所示。防爆相机安装在临时支护机器人1的内壁顶端，2个激光指向仪安装在掘进后方巷道顶端，平行且指向巷道轴线方向。
图3 快速掘进机器人位姿视觉测量方案
Fig.3 Pose vision measurement scheme of rapid tunneling robot
快速掘进机器人上带有滤镜的防爆相机对巷道后方的激光指向仪进行图像采集，对图像进行去噪、增强、畸变校正等处理，利用霍夫变换提取直线特征，根据建立的位姿解算模型，得到防爆相机在激光指向仪坐标系下的位姿信息。通过快速掘进机器人坐标系与防爆相机坐标系间的转换关系，得到快速掘进机器人在激光指向仪坐标系下的位姿信息。利用全站仪测量激光指向仪在巷道坐标系下的绝对坐标，得到激光指向仪坐标系与巷道坐标系的转换关系，结合快速掘进机器人在激光指向仪坐标系下的位姿信息，最终得到快速掘进机器人在巷道坐标系中的位姿信息。
2.2 快速掘进机器人位姿解算
快速掘进机器人坐标系如图4所示，主要涉及巷道坐标系OXYZ、激光指向仪坐标系OjXjYjZj、防爆相机坐标系OcXcYcZc和快速掘进机器人坐标系OoXoYoZo。
图4 快速掘进机器人坐标系
Fig.4 Coordinate system of rapid tunneling robot
快速掘进机器人位姿解算模型如图5所示。通过巷道-激光指向仪-防爆相机-快速掘进机器人间的坐标转换关系，计算快速掘进机器人在巷道坐标系下的位姿。其中，Q1，Q2为2个激光光斑中心，L1，L3为2条激光束直线，L2为Q1和Q2连线，l1，l2，l3，q1，q2分别为L1，L2，L3，Q1，Q2在相机投影平面上的投影，Q3为激光束L1上的一点，Q4为激光束L3上的一点。
图5 快速掘进机器人位姿解算模型
Fig.5 Pose calculation model for rapid tunneling robot
位姿解算过程：建立防爆相机坐标系下l1，l2，l3的直线方程axi+byi+c=0(i=1,2,3)，其中，a，b为直线方程的系数，c为常数，xi，yi为防爆相机坐标系下点q1，q2在Xc轴和Yc轴上的坐标值，由l1，l2，l3的直线方程求出q1，q2的坐标
为防爆相机焦距。
图5中，平面Oc Q1 Q4，Oc Q1 Q2 ，Oc Q2 Q3的法向量为N1，N2，N3。有
V1=V3=N1×N3
(1)
V2=V1×N2
(2)
根据式(1)与式(2)求出防爆相机坐标系下L1的方向向量V1，L2的方向向量V2和L3的方向向量V3。
激光束L1与L3间的距离为M，投影平面上l1与l3间的距离为m。有
(3)
式中
为光斑中心Q1，Q2在防爆相机坐标系下的坐标。
已知激光指向仪尺寸参数，建立激光指向仪坐标系，求出激光指向仪坐标系下L1的方向向量W1，L2的方向向量W2，L3的方向向量W3，光斑中心Q1，Q2在激光指向仪坐标系下的坐标为p1，p2。
防爆相机坐标系到激光指向仪坐标系的旋转矩阵为R，平移矩阵为T，有
V=RW
(4)
Q=Rp+T
(5)
式中：V防爆相机坐标系下的方向向量,V=(V1，V2，V3)；W为激光指向仪坐标系下的方向向量，W=(W1，W2，W3)；Q为防爆相机坐标系下的坐标，
为激光指向仪坐标系下的坐标p=(p1，p2)。
由式(4)与式(5)求出防爆相机坐标系到激光指向仪坐标系的旋转矩阵、平移矩阵。根据旋转矩阵和平移矩阵得到激光指向仪坐标系下快速掘进机器人的位姿信息。通过巷道-激光指向仪-防爆相机-快速掘进机器人间的坐标转换关系，最终得到巷道坐标系下快速掘进机器人的位姿。
3 快速掘进机器人纠偏控制
3.1 控制策略
快速掘进机器人推进系统如图6所示。16个伸缩油缸均匀地布置在临时支护机器人的壳体内侧。水平方向的伸缩油缸分为上下和左右2个区域，上下区域的伸缩油缸用于调整竖直方向的位姿偏差和俯仰角，左右区域的伸缩油缸用于调整水平方向的位姿偏差和偏航角。针对不同偏差，调整相应区域伸缩油缸行程可实现快速掘进机器人的位姿控制。
图6 快速掘进机器人推进系统
Fig.6 Propulsion system of rapid tunneling robot
快速掘进机器人纠偏控制目标是使快速掘进机器人的机头位姿偏差保持在设计误差范围内。快速掘进机器人沿巷道设计轴线前进的过程中，利用位姿视觉测量方法检测快速掘进机器人的位姿，计算位姿偏差，在位姿偏差小于设计误差时保持当前推进状态，在位姿偏差大于设计误差时，根据偏差类型及大小选择合适的控制策略，控制快速掘进机器人在设计误差范围内正常运行。
快速掘进机器人姿态类型如图7所示。规定水平方向向左偏为负，向右偏为正；竖直方向向上偏为正，向下偏为负。图7(a)为快速掘进机器人轴线与巷道设计轴线间的位姿误差在设计误差范围内，此时不需要纠偏，可以继续保持当前控制量推动掘进机器人前进。图7(b)为快速掘进机器人向左偏，偏航角为负，应增大左区伸缩油缸油压，减小右区伸缩油缸油压，将其向右调整。图7(c)为快速掘进机器人向右偏，偏航角为正，应增大右区伸缩油缸油压，减小左区伸缩油缸油压，将其向左调整。图7(d)为快速掘进机器人向上偏，俯仰角为正，应增大上区伸缩油缸油压，减小下区伸缩油缸油压，将其向下调整。图7(e)为快速掘进机器人向下偏，俯仰角为负，应增大下区伸缩油缸油压，减小上区伸缩油缸油压，将其向上调整。
3.2 控制模型
快速掘进机器人推进过程如图8所示。临时支护机器人2固定，伸缩油缸的活塞杆向上推出，临时支护机器人1向上移动；临时支护机器人1固定，伸缩油缸的活塞杆收回，临时支护机器人2向上移动。
(a) 正常
(b) 左偏
(c) 右偏
(d) 上偏
(e) 下偏
图7 快速掘进机器人姿态类型
Fig.7 Pose types of rapid tunneling robot
图8 快速掘进机器人推进过程
Fig.8 Propulsion process of rapid tunneling robot
通过控制伸缩油缸中液压油的流向、压力和流量，实现对伸缩油缸运动方向、推力大小和运动速度的控制。
快速掘进机器人位姿控制系统模型如图9所示。系统采用PID控制方法，引入位姿视觉负反馈，形成闭环控制系统，控制各伸缩油缸的行程，以实现快速掘进机器人位姿的精确控制。
图9 快速掘进机器人位姿控制系统模型
Fig.9 Model of pose control system of rapid tunneling robot
输入巷道设计轴线，利用视觉测量方法测得掘进机器人位姿，将测得的位姿信息反馈到输入端形成闭环控制系统。测得的位姿与巷道设计轴线下的期望位姿进行比较，得到位姿偏差，PID控制器根据偏差类型与大小，选择相应的控制策略，并发送控制指令，对快速掘进机器人进行位姿调整。
4 实验与结果分析
搭建基于视觉测量的快速掘进机器人纠偏控制系统实验平台，如图10所示。实验以楼道模拟巷道，以烟雾制造器模拟工况中的粉尘，采用电动机驱动的履带式探测机器人模拟快速掘进机器人进行推进过程的位姿测量与控制实验。
实验平台的主要参数见表1。
1-矿用本质安全型激光指向仪； 2-烟雾制造器； 3-棱镜；
4-探测机器人； 5-惯导； 6-相机； 7-全站仪。
图10 基于视觉测量的快速掘进机器人纠偏控制系统实验平台
Fig.10 Experiment platform for deviation correction control of rapid tunneling robot based on visual measurement
表1 实验平台参数
Table 1 Parameters of experiment platform
参数值巷道尺寸(长×宽×高)/(m×m×m)20×2×3激光指向仪有效射程/m800MV-EM510C相机分辨率2 456×2 058探测机器人尺寸(长×宽×高)/(m×m×m)0.64×0.56×0.14惯导精度/(°)0.5全站仪精度/mm3
4.1 位姿视觉测量精度
建立位姿视觉测量系统，并通过全站仪与惯导对其进行位姿精度验证。选取机器人移动过程中14个位置作为测量点，利用位姿视觉测量系统测量移动机器人在巷道中的位姿。将全站仪测量得到的机器人位置和惯导测量得到的机器人姿态作为真实值，与位姿视觉测量系统得到的机器人位姿进行对比。通过对比机器人X方向与Y方向坐标、机器人偏航角变化来说明位姿视觉测量精度，结果如图11所示。
由图11可知，位姿视觉测量系统得到的位置信息与全站仪测量结果相吻合，X方向平均偏差为21.334 mm，Y方向平均偏差为34.154 mm；位姿视觉测量系统得到的姿态信息与惯导测量结果相吻合，偏航角平均偏差为0.493°。根据实际工况要求，位姿视觉测量精度满足行业标准，可以作为快速掘进机器人纠偏控制的基础。
4.2 机器人纠偏控制精度
将机器人位姿视觉测量结果作为纠偏控制系统的反馈信息，通过DSP控制器操控机器人沿掘进方向移动14 m，同时利用全站仪记录机器人移动过程中的三维坐标。机器人运动轨迹如图12所示。
图12中巷道中线表示机器人参考运动轨迹，折线为机器人实际运动轨迹。机器人从X=-20 mm，Y=0的起始位置向前运动。纠偏控制系统根据视觉测量的位姿信息对机器人两侧履带进行变速调整，使机器人不断改变运动方向，保证其一直在参考轨迹附近运动。结果表明：机器人在X方向的位置误差控制在30 mm之内，满足实际工况要求。
(a) 机器人X坐标值
(b) 机器人Y坐标值
(c) 机器人偏航角
图11 位姿视觉测量精度实验结果
Fig.11 Experimental results visual pose measurement accuracy
图12 机器人运动轨迹
Fig.12 Robot trajectory
5 结论
(1) 建立了快速掘进机器人位姿视觉测量方案，利用防爆相机拍摄激光指向仪图像，并对图像进行处理，根据位姿解算模型，解算出快速掘进机器人的位姿。
(2) 针对快速掘进机器人移动过程中出现的各种位姿偏差，提出对应的纠偏控制策略，将测量的位姿反馈给PID控制器进行纠偏控制以提高快速掘进机器人的控制精度。
(3) 实验结果表明，位姿视觉测量精度为X方向平均偏差21.334 mm、Y方向平均偏差34.154 mm、偏航角平均偏差0.493°，纠偏控制精度在30 mm以内。
[1] 张旭辉,张超,杨文娟,等.悬臂式掘进机可视化辅助截割系统研制[J].煤炭科学技术,2018,46(12):21-26.
ZHANG Xuhui,ZHANG Chao,YANG Wenjuan,et al.Research and development of visual auxiliary cutting system for cantilever roadheader[J].Coal Science and Technology,2018,46(12):21-26.
[2] 张登山.快速掘进系统研发及应用[J].煤炭科学技术,2015,43(增刊2):96-99.
ZHANG Dengshan.Development and application on rapid driving system[J].Coal Science and Technology,2015,43(S2):96-99.
[3] 王国法,刘峰,孟祥军,等.煤矿智能化(初级阶段)研究与实践[J].煤炭科学技术,2019,47(8):1-36.
WANG Guofa,LIU Feng,MENG Xiangjun,et al.Research and practice on intelligent coal mine construction(primary stage)[J].Coal Science and Technology,2019,47(8):1-36.
[4] 刘送永,张德义.巷道掘进机智能化技术研究现状及展望[J].工矿自动化,2019,45(10):23-28.
LIU Songyong,ZHANG Deyi.Research status and prospect of intelligentization technology of roadheader[J]. Industry and Mine Automation,2019,45(10):23-28.
[5] 杨健健,张强,吴淼,等.巷道智能化掘进的自主感知及调控技术研究进展[J].煤炭学报,2020,45(6):2045-2055.
YANG Jianjian, ZHANG Qiang, WU Miao, et al. Research progress of autonomous perception and control technology for intelligent heading[J]. Journal of China Coal Society,2020,45(6):2045-2055.
[6] 惠兴田,田国宾,康高鹏,等.煤巷掘进装备技术现状及关键技术探讨[J].煤炭科学技术,2019,47(6):11-16.
HUI Xingtian,TIAN Guobin,KANG Gaopeng,et al.Discussion on equipment technology status and key technology of roadway driving[J].Coal Science and Technology,2019,47(6):11-16.
[7] 陶云飞.掘进机位姿激光自动测量方法及系统研究[D].北京:中国矿业大学(北京),2017.
TAO Yunfei.Research on automatic measurement method and system of position and attitude of roadheader based on laser measurement system[D]. Beijing:China University of Mining and Technology(Beijing),2017.
[8] 杨文娟,张旭辉,马宏伟,等.悬臂式掘进机机身及截割头位姿视觉测量系统研究[J].煤炭科学技术,2019,47(6):50-57.
YANG Wenjuan,ZHANG Xuhui,MA Hongwei,et al.Research on 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.
[9] 田原.悬臂式掘进机惯性定位技术研究与试验[J].煤矿机电,2020,41(1):9-12.
TIAN Yuan. Research and test on inertial positioning technology of boom-type roadheader[J]. Colliery Mechanical & Electrical Technology,2020,41(1):9-12.
[10] 吴淼,贾文浩,华伟,等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法[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.
[11] 张旭辉,王妙云,张雨萌,等.数据驱动下的工业设备虚拟仿真与远程操控技术研究[J].重型机械,2018(5):14-17.
ZHANG Xuhui,WANG Miaoyun,ZHANG Yumeng, et al. Virtual simulation and remote control technology with data-driven for industrial equipment[J].Heavy Machinery,2018(5):14-17.
[12] 朱伟,王虹,李首滨,等.基于嵌入式软PLC的掘进机控制系统设计[J].工矿自动化,2020,46(2):100-106.
ZHU Wei,WANG Hong,LI Shoubin,et al.Design of roadheader control system based on embedded soft PLC[J].Industry and Mine Automation,2020,46(2):100-106.
[13] 瞿圆媛,宋林珂,吉晓冬,等.井下掘进机行进纠偏调度规划与控制研究[J].矿业科学学报,2020,5(2):194-202.
QU Yuanyuan,SONG Linke,JI Xiaodong,et al.Study on path planning and tracking of the underground mining road-header[J].Journal of Mining Science and Technology,2020,5(2):194-202.
[14] 葛世荣,胡而已,裴文良.煤矿机器人体系及关键技术[J].煤炭学报,2020,45(1):455-463.
GE Shirong,HU Eryi,PEI Wenliang. Classification system and key technology of coal mine robot[J]. Journal of China Coal Society,2020,45(1):455-463.