Study on visual servo control system for cutting of cantilever roadheader
未来较长时间内煤炭仍将是我国主体能源［1］，但我国因巷道掘进困难而导致采掘比例严重失调［2］。 巷道掘进工作中环境复杂、司机视野差，通过掘进机司机手动操作进行煤巷掘进作业无法准确控制掘进机的运行位姿，导致巷道掘进过程中出现巷道成形质量差、掘进效率低的问题。 因此，实现掘进机的信息化、自动化和智能化对减轻工人劳动强度、提高开采效率具有重要意义［3］。 目前国内外针对悬臂式掘进机的自动截割控制的研究现状如下：日本三井三池公司针对截割控制研发了截割功率自适应控制系统［4］；毛君等［5］建立了断面仿形截割控制的数学模型；吴淼等［6］利用激光测距仪和倾角传感器设计掘进机位姿检测系统；王苏彧等［7］根据液压缸伸缩量与截割头的位置关系，利用倾角传感器及旋转编码器测量液压缸的伸缩量计算得到截割头位置，并提出了掘进机记忆截割控制方法；毛清华等［8］利用行程传感器、超声和惯导等多传感器检测掘进机机身空间位姿，可有效准确地检测掘进机的空间位姿。 目前针对悬臂式掘进机所建立的控制系统中，大多利用位移传感器的间接式位移传感器及角度编码器来反馈掘进机截割头位姿信息，但利用位移传感器间接式测量方式对安装精度有较高的要求，且在振动环境下，现有方法的测量稳定性和传感器寿命较低。 笔者采用非接触式视觉测量方法［9］，利用视觉伺服控制技术，根据视觉测量得到的信息作为视觉伺服的信息控制掘进机关节驱动器实现掘进机自动截割。
视觉伺服系统是利用视觉测量信息作为反馈的控制系统［10］，该系统由视觉测量系统、机器人和运动控制系统组成，利用视觉测量系统获取环境信息并反馈到控制器中，并根据控制策略对机器人进行控制。 视觉测量相比其他传感器具有测量范围广、信息量大等特点，可以快速、有效地获得环境信息。视觉伺服分为基于位置的视觉伺服、基于图像的视觉伺服［11］。 基于位置伺服控制系统根据建立的目标与摄像机之间的数学模型将图像信息转换成世界坐标系中的位置信息，并传给机器人关节控制器控制机器人运动［12］。 基于图像的视觉伺服在二维图像上通过计算图像的雅克比矩阵得到位置误差，该方法对标定误差和空间模型误差不敏感，但伺服过程中容易出现图像雅克比矩阵奇异点。
将视觉伺服控制技术用于悬臂式掘进机的自动截割控制中，利用视觉测量方法实现对截割头位置的检测和精确定位，并建立掘进机运动学模型实现掘进机的自动截割控制。 采用位置反馈型机器人视觉控制技术以及动态look-and-move 的结构［13］方式建立悬臂式掘进机视觉伺服截割控制系统，利用视觉进行位置反馈的掘进机视觉伺服控制结构示意如图1 所示。
图1 掘进机视觉伺服控制结构
Fig.1 Visual servo control structure of roadheader
系统主要任务是根据对截割头规划的截割轨迹，利用固定在机体上的工业相机连续拍摄固定在截割臂上标靶图像并进行快速图像处理，计算得到截割头位姿，在对掘进机进行运动学建模基础之上，计算截割头在巷道中位姿，并将得到的三维截割头位置信息反馈到控制器中，再根据截割头位姿调整策略利用关节控制器和伺服放大器对掘进机液压缸进行控制。 与传统的利用位移传感器获取截割头位置信息相比，视觉测量方法通过非接触测量可以快速、准确地获得有效信息，不仅不需要改变掘进机的结构加装传感器，并且具有较好的测量稳定性和准确性。
悬臂式掘进机视觉伺服系统由悬臂式掘进机、DSP 控制器、CCD 工业相机、红外标靶机载计算机和各传感器组成。 利用视觉进行位置反馈的悬臂式掘进机自动截割控制系统总体方案如图2 所示，系统包括视觉测量模块、截割头位姿解算模块、掘进机运动控制模块、数据存储显示模块等。
图2 掘进机视觉伺服截割控制系统总体方案
Fig.2 General scheme of visual servo cutting control system forroadheade
如图2 所示，掘进机视觉伺服控制系统采用机载计算机+DSP 控制器的架构，系统依靠人工在计算机中输入规划路径信息，采用视觉测量方法测量截割头相对于机身的位置信息，利用超声波传感器和捷联惯导测量并计算得到掘进机在巷道空间中的机身位姿。 根据截割头相对于机身位姿信息及掘进机在巷道中的位姿信息，利用D-H 方法进行运动学正向解算得到在巷道空间下截割头的位姿。 将解算得到的巷道空间下的截割头位姿反馈到DSP 控制器中，并与规划的路径信息比较得到截割头的误差信息，根据误差信息采用反变换法对掘进机逆运动学解得到截割头需摆动的关节角度。 采用PID 控制方法，利用DSP 控制器输出控制命令通过电液比例阀驱动升降油缸及回转油缸来实现对截割臂摆动的控制。 并将解算得到的位姿数据保存到MYSQL数据库中，在巷道断面中用图形的方式显示截割头截割轨迹，监视系统工作时的运动状态。
软件系统采用多线程方式实现多任务并行控制，其中在视觉测量模块中完成图像处理及位姿解算；在位姿解算模块中计算截割头的位置信息；在数据存储显示模块中完成各数据的存储及截割路径的显示功能。 在运动控制模块中根据得到的误差信息利用DSP 的D／A 模块发出控制信号实现掘进机的截割控制，各模块共同工作实现掘进机的视觉伺服截割控制。
悬臂式掘进机视觉测量系统如图3 所示，视觉测量模块由机载计算机、CCD 工业相机、工业以太网以及红外LED 标靶组成。 视觉测量模块采用单目视觉测量技术，进行截割头的位姿计算，得到截割头相对于机身的位姿。
图3 悬臂式掘进机视觉测量系统
Fig.3 Vision measuring system of cantilever roadheade
利用摄像机采用软触发的形式实时采集红外标靶的图像，将采集到的图像通过千兆网GigE 接口传输到机载计算机中。 通过视觉测量模块实现了悬臂式掘进机截割头的位姿解算。 为保证实时性，设定视觉测量的周期不超过100 ms。 由于测量系统时根据图像的信息计算得到掘进机截割头的空间位置，因此视觉测量系统中的图像分析处理过程直接影响系统的控制精度。 为保证控制系统的控制精度，对测量系统的图像进行预处理，减小由于图像噪声等问题引起的系统误差。 对采集得到的图像进行去噪及图像增强预处理，如图4 所示。
图4 图像处理流程
Fig.4 Flow of image processing
根据图4 中图像处理流程，对输入的图像利用高斯滤波器去除图像中的噪声干扰，利用图像二值化对图像进一步处理，获取图像中的光斑区域，再在各区域中利用灰度质心方法提取图像中的光斑中心坐标［14-16］。 最后根据文献［9］中掘进机截割头姿态测量方法，利用摄像机外参标定计算并得到截割头相对于掘进机机身的位姿。
图5 为根据掘进机运动学模型所建悬臂式掘进机的坐标系。 其中坐标系O0 X0 Y0 Z0、O1 X1 Y1 Z1、O2X2Y2Z2、O3X3Y3Z3、O4X4Y4Z4、OcXcYcZc、OwXwYwZw分别为悬臂式掘进机基坐标系、回转关节坐标系、抬升关节坐标系、伸缩关节坐标系、截割头坐标系摄像机坐标系及标靶坐标系，OdXdYdZd为巷道断面坐标系，以截割断面左下角作为原点，竖直方向为Zd 轴方向，Xd轴方向垂直于截割面，Yd轴方向平行于截割断面水平向左；系统工作时，首先将掘进机初始姿态的基坐标系与巷道断面坐标系各方向保持一致，并设初始姿态下掘进机的各方向旋转角度均为0。 将捷联惯导系统固定在掘进机机体上，实时测量掘进机机身位姿，即横滚角、俯仰角及偏航角的角度变化量。 利用安装在机身上的超声波传感器测量掘进机机身距离两侧煤壁以及截割断面的距离，结合掘进机姿态角，可以计算得到掘进机基坐标系原点巷道断面坐标系下的Xd、Yd和Zd轴方向下的坐标。 然后根据姿态计算方法可以得到基坐标系O0 X0 Y0 Z0 相对于巷道断面坐标系的变换矩阵d0T：
图5 掘进机坐标系
Fig.5 Roadheader coordinate system
式中：α、β、γ 分别为掘进机的横滚角、俯仰角及偏航角的变化量；SX、SY、SZ 分别为机体坐标系原点在巷道断面坐标系下的各个方向下的坐标。
得到掘进机截割头坐标系与巷道断面坐标系之间的转换关系之后，可以计算得到任意时刻截割头在断面的位置，并在轨迹显示模块中显示截割头的轨迹。 同时，根据截割头实际位置与规划掘进机运动轨迹的误差信息，利用反变化法对掘进机逆运动学求解，计算得到任意时刻的驱动关节角度。
悬臂式掘进机视觉伺服截割控制系统的闭环系统结构如图6 所示。 为实现控制系统的精确控制并且具有一定稳定性，采用PID 控制方法［17］对掘进机进行控制。 控制系统采用闭环控制，利用视觉反馈得到的掘进机截割头位姿测量，经过位姿解算模块得到截割头于截割断面上的位置信息。 据规划的截割头轨迹及与计算得到位姿的偏差经过PID 控制器的调控，利用控制器发出指令经过比例放大器及电液比例阀控制液压缸伸缩，使掘进机截割头到达期望位置。
图6 掘进机闭环控制系统结构
Fig.6 Roadheader closed-loop control system structure
根据文献［18］可知，通过分析比例放大器、电液比例阀、液压缸等各环节特性可以得到掘进机视觉伺服截割控制系统传递函数数学模型，在截割头的视觉测量部分中，可以将视觉测量部分看作是一个比例环节，在理想情况下，位姿反馈系数为1。 掘进机自动截割控制系统传递函数模型如图7 所示。
图7 掘进机截割控制系统传递函数模型
Fig.7 Transfer function model of tunneling control system of roadheader
首先对视觉测量系统进行精度试验，采用视觉测量系统对截割头进行跟踪定位，采集悬臂运动过程中标靶的图像，并计算截割头姿态角。 搭建矿用悬臂式掘进机截割头姿态视觉测量测量试验平台如图8 所示，选用与EBZ160 型掘进机比例为1 ∶5 的掘进机模型，图中①为MV-EM130M 工业相机并加波长为850 nm 的窄波滤镜，图中②采用300 mm×300 mm 的方形标靶，标靶上均匀布置16 个红外LED 灯。
图8 矿用悬臂式掘进机截割头姿态视觉测量试验平台
Fig.8 Experimental platform for visual measurement of cutting head attitude of mine cantilever roadheade
悬臂式掘进机试验平台主要参数如下：
由于在掘进机工作环境下会存在杂光的干扰，并且LED 灯光的波长大于900 nm，因此在工业相机前加滤镜去除杂光。 采集标靶图像如图9 所示，图9a 为利用工业相机直接拍摄红外标靶的图像，图9b为相机加装滤镜后对红外标靶所拍摄的图像。
图9 采集标靶图像
Fig.9 Acquisition of target images
在不同位置下利用视觉测量传感器对截割头的俯仰角进行实时测量，设定图像采集时间间隔为100 ms，利用视觉测量模块实时测量截割臂8 s 内的姿态角的变化，并采用精度为0.05°的倾角传感器测量掘进机截割臂姿态角的真实值。 为方便计算设定掘进机位于巷道中间位置，并且当截割头水平且位于掘进机中间位置时设定截割头回转角及升降角分别为0°和-90°。 得到的试验结果及试验误差如图10、图11 所示。
图10 截割头俯仰角测量结果
Fig.10 Measurement result of pitch Angle of cutting head
图11 截割头俯仰角测量结果误差
Fig.11 Error of pitching angle of cutting head
根据试验结果可知，2 次利用倾角传感器测量的掘进机截割臂的俯仰角度分别为-61.88°及-90.03°。 使用视觉测量系统测量截割头2 次位置的截割头角度时，截割头的最大误差为±0.4°。
悬臂式掘进机视觉伺服截割控制试验平台如图12 所示。 测试系统由悬臂式掘进机模型、计算机、MV-EMV130M 防爆工业相机、300 mm×300 mm 的红外标靶、捷联惯导、超声波传感器、DSP 控制器以及掘进机组成。 在悬臂式掘进机视觉伺服截割控制试验中，通过固定在掘进机机身上的防爆工业相机采集固定在掘进机截割臂上的红外标靶的图像，使用加装在镜头前的滤镜去除杂光，经过图像预处理、光斑中心定位及截割头位姿结算计算得到截割头坐标系相对于机身坐标系之间转换关系。 放置两挡板于机身两侧及机身前端模拟煤壁，利用超声测距传感测得机身距煤壁的距离。 融合捷联惯导及超声波传感器测得数据计算得到截割头坐标系与巷道断面坐标系之间的转换关系。 并将得到的数据通过RS485 反馈到DSP 控制器中，控制器根据规划路径与截割头实际位姿之间的误差输出控制命令通过电液比例阀驱动升降油缸及回转油缸控制截割头运动。 为保证系统的实时性，设定相机采集图像时间间隔为100 ms。 预先在巷道断面坐标系中规划掘进机截割头的运动轨迹。 当掘进机机身为静止时，截割头在巷道断面的截割路径为弧线，其轨迹规划示意如图13 所示。
图12 悬臂式掘进机视觉伺服截割控制试验平台
Fig.12 Test platform for visual servo cutting control of cantilever roadheade
图13 轨迹规划示意
Fig.13 Schematic of trajectory planning
按实际巷道等比例设定模拟巷道的尺寸为1 000 mm×1 400 mm，在断面坐标系下设以A（50，-77，191.5）为起点，根据图13 中规划轨迹进行自动截割，其中点B 点坐标为（50，-1 223，191.5）、C 点坐标为（50，-1 243，411.6）和D 点坐标为（50，-56，411.6）。 根据截割头的解算模块可计算得到截割头位于A、B、C 和D 点时，截割头的回转角及升降角的理论值分别为（-34°，-69°）、（28°，-69°）、（28°，-82°）以及（-34°，-82°），使掘进机的截割部空载运行，根据规划的截割轨迹截割断面，通过精度为0.01°的倾角传感器和精度为0.05°的角度编码器测量得到截割头垂直摆角和水平摆角的实际值，掘进机截割头截割轨迹如图14 所示，红线为掘进机截割头截割时的实际运行轨迹，蓝线为掘进机的规划截割轨迹，如图14 可以看出截割头运动轨迹可以稳定地按照规划的截割路径运动。 试验结果及试验误差如图15 所示。
图14 掘进机截割头截割轨迹
Fig.14 Cutting track of cutting head ofroadheader
图15 中，根据图15a 和图15b 中截割头姿态角误差可知，截割头运动过程中回转角以及升降角的角度误差在0.7°以内，则代入截割头位姿解算模块可以得到截割头中心点位置误差在18 mm 以内，换算到全尺寸掘进机满足巷道施工标准的误差允许范围，表明在利用视觉伺服的掘进机控制系统中，掘进机截割头可以按照规划的截割轨迹进行自动截割，并且截割轨迹满足巷道掘进的精度要求，对实现悬臂式掘进机自动化及智能化具有重要意义。
图15 掘进机截割头位姿误差
Fig.15 Position error of heading head of roadheader
1）煤矿悬臂式掘进机的自动截割控制是掘进工作面控制的瓶颈问题，将视觉位姿测量技术用于掘进机位姿测量，建立悬臂式掘进机截割控制模型，构建巷道断面成形轨迹视觉伺服控制系统，为巷道掘进自动化提供了新的思路。
2） 视觉伺服截割控制系统按照巷道断面规划截割轨迹，多传感器融合获得机身位置，工业相机采集红外LED 标靶获得截割头位姿，系统结合机身和截割头位姿按照D-H 坐标进行正逆求解，获得截割头相对巷道的轨迹控制点和显示坐标，实现了截割头轨迹跟踪和位姿可视化显示。
3）搭建掘进机视觉伺服控制试验平台，验证截割头视觉测量和视觉伺服控制性能，结果显示：视觉伺服控制下，截割头良好跟踪预先规划的轨迹，截割头中心点位置误差在18 mm 之内，初步实现悬臂式掘进机的自动成形截割控制。
试验中获得机身位置是相对巷道的坐标，尚存在受片帮、巷道成形质量等方面影响的问题，课题组采用多激光束特征合作标靶的悬臂式掘进机机身位姿测量方法已经取得良好效果，有力支撑掘进机视觉伺服控制系统，为全面解决巷道成形截割问题奠定了技术基础。
［1］ 毛善君.“高科技煤矿”信息化建设的战略思考及关键技术［J］. 煤炭学报，2014，39（8）：1572-1583.MAO Shanjun. Strategic thinking and key technologyof informatiza⁃tion construction of high-tech coal mine［J］.Journal of China Coal Society， 2014，39（8）：1572-1583.
［2］ 杨仁树.我国煤矿岩巷安全高效掘进技术现状与展望［J］.煤炭科学技术，2013，41（9）：18-23.YANG Renshu. Present status and outlook on safety and high effi⁃cient heading technology of mine rock roadway in China［J］. Coal Science and Technology， 2013，41（9）：18-23.
［3］ 张旭辉，刘永伟，毛清华，等.煤矿悬臂式掘进机智能控制技术研究及进展［J］.重型机械，2018（2）：22-27.ZHANG Xuhui， LIU Yongwei， MAO Qinghua， et al. Research and progress on intelligent control technology of boom-type road⁃header in coal mine［J］.Heavy Machinery， 2018（2）：22-27.
［4］ 刘永伟. 矿用悬臂式掘进机器人控制系统研制［D］.西安：西安科技大学，2018.LIU Yongwei. Design of the Control system for the robot of the robot of boom-type roadheader in mine coal［D］.Xi’an：Xi’an Univeresity of Science and Technology，2018.
［5］ 毛 君，吴常田，孙 觅.悬臂式掘进机仿形截割控制初探［J］.中国工程机械学报，2007（3）：322-224，328.MAO Jun，WU Changtian，SUN Mi. Investigation into profilecutting control of boom-typed excavators［J］. Chinese Journal of Construction Machinery，2007（3）：322-224，328.
［6］ 吴 淼，贾文浩，华 伟，等.基于空间交汇测量技术的悬臂式掘进机位姿自主测量方法［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.
［7］ 王苏彧，吴 淼.纵轴式掘进机记忆截割控制方法研究［J］.煤炭技术，2016，35（11）：266-268.WANG Suyu WU Miao. Research on control method of memory cutting for longitudinal roadheader［J］. Coal Technology， 2016，35（11）：266-268.
［8］ 毛清华，张旭辉，马宏伟，等.多传感器信息的悬臂式掘进机空间位姿监测系统研究［J］.煤炭科学技术，2018，46（12）：41-47.MAO Qinghua，ZHANG Xuhui，MA Hongwei，et al. Study on spa⁃tial position and posture monitoring system of boom - type roadheader based on multi sensor information［J］. Coal Science and Technology， 2018，46（12）：41-47.
［9］ 张旭辉，刘永伟，杨文娟，等.矿用悬臂式掘进机截割头姿态视觉测量系统［J］.工矿自动化，2018，44（8）：63-67.ZHANG Xuhui，LIU Yongwei，YANG Wenjuan，et al. Vision meas⁃urement system for cutting head attitude of mine-used boom-type roadheader［J］. Industry and Mine Automation， 2018，44（8）：63-67.
［10］ 贾丙西，刘 山，张凯祥，等.机器人视觉伺服研究进展：视觉系统与控制策略［J］.自动化学报，2015，41（5）：861-873.JIA Bingxi，LIU Shan，ZHANG Kaixiang， et al. Survey on robot visual servo control：vision system and control strategies［J］. Acta Automatica Sinica，2015，41（5）：861-873.
［11］ HUTCHINSON S，HAGER G D，CORKE P I. A tutorial on visual servo control ［ J ］. IEEE Transactions on Robotics and Automation， 1996， 12（5）：651-670.
［12］ 徐 德.单目视觉伺服研究综述［J］.自动化学报，2018，44（10）：1729-1746.XU De. A tutorial for monocular visual servoing［J］. Acta Auto⁃matica Sinica，2018，44（10）：1729-1746.
［13］ CONTICELLI F， ALLOTTA B. Two-level visual control of dy⁃namic look - and - move systems. ［ J］. IEEE International Conference on Robotics and Automation， 2000，11（10）：3784-3789.
［14］ 彭碧霞.车牌图像自动识别系统预处理算法研究［J］.武汉大学学报（工学版），2006（3）：131-134.PENG Bixia. Study on method of pre-processing on vehicle license plate image recognition system［J］. Journal of Wuhan Uni⁃versity， 2006（3）：131-134.
［15］ 王 宇.基于灰度质心法的雷达干扰源定位方法研究［J］.自动化技术与应用，2016， 35（2）：63-66.WANG Yu. Study on interference localization method for warming radar based on center of gravity method［J］. Acta Automatica Sin⁃ica，2016， 35（2）：63-66.
［16］ 李道萍，杨 波.高精度光斑中心定位算法［J］.光学仪器，2018，40（4）：20-25.LI Daoping YANG Bo. High-precision center location algorithm of light spot［J］. Journal of Optical Instruments，2018，40（4）：20-25.
［17］ 张 军，焦生杰，叶 敏，等.基于双闭环PID 的挖掘机泵扭矩控制策略［J］.中国工程机械学报，2012，10（3）：316-320.ZHANG Jun，JIAO Shengjie，YE Min，et al. Torque control strat⁃egy for double-closed-loop PID-basedexcavator pumps［J］. Chi⁃nese Journal of Construction Machinery，2012，10（3）：316-320.
［18］ 田 劼，王红尧，吴淼.掘进自动截割成形控制系统动态特性分析［J］.工矿自动化，2015，41（6）：40-44.TIAN Jie，WANG Hongyao，WU Miao. Dynamic characteristics a⁃nalysis of automatic cutting and profiling control system for road⁃header［J］. Industry and Mine Automation，2015，41（6）：40-44.