基于视觉测量的快速掘进机器人纠偏控制研究

2020-09-27张旭辉周创张超谢楠张楷鑫刘博兴

工矿自动化 2020年9期

张旭辉，周创，张超，谢楠，张楷鑫，刘博兴

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

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.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

图5中，平面OcQ1Q4，OcQ1Q2，OcQ2Q3的法向量为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)

已知激光指向仪尺寸参数，建立激光指向仪坐标系，求出激光指向仪坐标系下L1的方向向量W1，L2的方向向量W2，L3的方向向量W3，光斑中心Q1，Q2在激光指向仪坐标系下的坐标为p1，p2。

防爆相机坐标系到激光指向仪坐标系的旋转矩阵为R，平移矩阵为T，有

V=RW

(4)

Q=Rp+T

(5)

由式(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) 正常

图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-棱镜；

表1 实验平台参数Table 1 Parameters of experiment platform

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之内，满足实际工况要求。