基于等价输入干扰方法的钻孔机器人给进力跟踪控制

李旺年，陆承达，张幼振，宋海涛，田盛楠，黄恒宇，陈略峰，吴 敏

(1.中国地质大学(武汉)自动化学院，湖北 武汉 430074；2.中煤科工西安研究院(集团)有限公司，陕西 西安 710077)

钻孔机器人是一种特种作业机器人，可以在矿山、隧道、建筑等多领域完成各种钻孔作业，具有广泛的应用前景[1-4]。目前，我国预防瓦斯灾害的主要手段是瓦斯抽采钻孔机器人施工瓦斯抽采孔进行瓦斯预抽[5-7]。由于煤矿井下含煤地层地质力学环境复杂，负载阻力一般都是随机变化且难以准确测量的，因此，钻进过程是一个非线性、时变和不确定的过程，容易引起钻进参数的波动，造成给进系统运行状态的不稳定。

给进系统作为钻孔机器人的主要执行机构，其给进力的稳定跟踪控制是钻孔机器人的关键技术之一，对保证钻孔机器人的工作性能和钻孔的施工质量及施工效率起着至关重要的作用。给进系统采用液压给进，由液压泵向液压油缸提供高压油，通过液压油缸输出给进力，再由中间传动装置(拖板)带动连接在动力头上的钻具实现给进或起拔，其给进力大小由减压阀控制，实现加压或减压钻进。通过建立减压阀出口压力与预置阀门开度相关的机理模型，可以找出影响减压阀工作特性的主要因素[8]；建立减压阀的动态响应数学模型，可以分析基础振动幅值和频率对减压阀波动特性[9]；也有学者通过建立直动式比例减压阀的动态数学模型，研究PWM 控制信号谐波干扰信号、占空比突变及不同频率的颤振信号对比例减压阀控制品质的影响[10]。此外，国内很多学者针对比例阀门结构原理及其阀门控制研究现状进行了总结[11-12]，针对比例阀的工作原理，将进口流量和给定压力值，以给定电路作为控制输入，建立了比例阀控制模型，该模型给煤矿坑道钻进过程先导比例阀控制钻进过程给定压力提供了很好参考。

目前，国内外学者对钻孔机器人的控制策略进行了研究，Guo Yinan 等[13]提出了一种基于围岩感知的自适应控制方法，通过实时钻探数据获得岩石硬度，推荐最佳设定值，提高了钻机钻进效率。赵超泽[14]综合考虑了钻机的给进系统与回转系统，设计模糊控制器，根据回转压力及其变化率对给进压力进行调节，实现回转系统的自适应控制。王东升等[15]考虑钻进过程中煤层硬度对回转系统的影响，通过调整给进压力实现回转系统对岩层的自适应调节。A.K.A.Khaleel 等[16]以最佳的机械钻速为优化目标，通过优化钻压和转速的设定值，提高了钻机施工效率。

上述研究采用的PID 控制、模糊控制等方法，是通过钻机的状态来估计当前的钻进工况，进而调整钻机的给进力以克服地层的干扰，对钻机的给进系统和回转系统进行控制的方法。这些方法在应对非线性、不确定等复杂系统时存在一定的局限性。因此，研究一种新的控制方法，对提高钻孔机器人的稳定性和鲁棒性，具有重要的意义。

等价输入干扰方法是一种可以有效抵消外部干扰对系统的影响，提高系统鲁棒性的方法[17]。本文基于等价输入干扰方法，提出了一种钻孔机器人给进力跟踪控制策略。首先，基于钻进过程分析，建立了钻孔机器人给进系统控制数学模型；随后，基于等价输入干扰方法的控制结构设计了钻孔机器人的控制器，并与PID控制方法进行了对比仿真实验；最后，对实验结果进行了分析，证明了该方法的有效性和可行性。

1 钻孔机器人钻进过程分析

钻孔机器人主要由给进系统、回转系统、操纵台、上杆机械手、补杆装置、矿用隔爆兼本安型控制器、液压泵站、冷却系统、稳固装置和履带车体等组成。

钻孔机器人钻进过程中，钻进效率和钻进安全是两个首要考虑的因素。钻进效率由钻速决定，钻进安全是指钻孔机器人的给进力及回转扭矩需随钻孔深度和地层情况的变化工作在额定工况内。根据煤矿井下实际钻进工艺的分析，给进力是决定钻速的一个主要决策变量，钻孔机器人根据钻进工况及钻进参数实时监测，优化出目标钻速对应的给进力值。在钻进作业过程中，由控制器发出电流控制信号，改变减压阀的阀门开度，调节给进油缸进油腔的压力，实现给进系统输出给进力的改变，进而由钻杆和钻头传递动力，确保给进系统可以适应煤层负载变化，实现安全高效钻进。图1 为钻孔机器人进行瓦斯抽采孔施工的示意图。

图1 钻孔机器人施工瓦斯抽采孔Fig.1 Gas extraction hole construction with drilling robot

2 给进系统建模

在复杂地质工况钻进时，对钻孔机器人给进系统的冲击较大，容易造成系统的不稳定，因此，需要对给进系统进行建模。钻孔机器人给进系统的控制与执行元件主要由减压阀、液压油缸和中间传动装置组成，液压油缸输出的给进力由减压阀控制，在通过中间传动装置时产生能量消耗。因此，首先对减压阀进行动态建模，明确给进力的驱动方式，然后对整个给进系统进行控制建模。

2.1 减压阀动态建模

图2 为减压阀的物理结构示意图，在减压阀动态控制阀门出口压力的过程中，由于受到减压阀弹簧-阻尼结构影响，阀门预置量x0和 出口流量Qout会影响减压阀出口压力pout的大小，加上钻进过程负载(钻柱系统钻进地层产生的反作用力)影响，减压阀出口压力pout会产生波动。

图2 减压阀物理结构Fig.2 Physical structure of pressure reducing valve

根据先导比例减压阀工作原理，减压阀动态运动时，电磁力驱动阀芯运动产生惯性力、摩擦阻尼力和弹簧弹性力，可以通过控制减压阀的电流Im控制出口压力pout大小。忽略阀芯自身质量，减压阀动态方程可以表示为[18]：

式中：m为 阀芯质量；b为 阀芯黏性摩擦因数；x0为减压阀弹簧预压缩量；x为 阀芯位移(阀门开度)；k为弹簧刚度；ks为液动力弹簧刚度；Aout为 减压阀阀芯底面积；Im为减压阀电磁铁电流；kI为电流-力比例增益系数。

减压阀输出压力与进出口流量大小相关。动态过程中，受控腔的流量关系如下：

式中：V为减压阀出口受控腔容积；E为油液的体积弹性模量；Qin为进入负载的流量。

此外，Qout可表示为：

式中：Cd为阀芯流量系数；D为阀芯通径；ρ为油液密度。

由于减压阀阀门出口流量、压力为非线性函数，当阀门处于某平衡位置时，利用线性化公式可得：

式中：kQ为 流量增益；kc为流量-压力系数；

因此，可以得到减压阀出口压力pout和阀芯位移x之间的关系

2.2 给进系统控制模型

钻孔机器人给进系统中，减压阀、液压油缸以及中间传动装置属于串联关系，为了便于分析，先单独分析各个模块的力传导问题，然后建立给进系统控制模型。

由于减压阀模型中出口流量作为输出会与状态变量x之 间耦合，难以单独从式(5)建立出口压力pout与输入电流Im之间的可控的状态方程。选用传递函数可以将减压阀的控制输入和输出进行分离，得到两者之间的关系。因此，利用拉氏变换将输入和输出2 个变量分离出来。

将式(1)进行拉氏变换后为：

减压阀运动是一个缓变的过程，弹簧产生的加速度很小，加上减压阀质量较小，因此，产生的惯性力与弹簧弹力和电磁力相比很小；电磁阀控制过程中初始弹簧力远小于电磁力；为了控制需求，我们将弹簧产生的惯性力和弹簧预压缩量产生的弹力忽略，可得：

将式(5)进行拉氏变换后为：

通过式(8)可得：

将式(9)代入式(7)中，可以将阀门开度消去

按照式(6)、式(8)、式(10)，可以画出减压阀电流控制传递函数方框图，如图3 所示。

图3 减压阀电流控制传递函数方框图Fig.3 Transfer function for current control of pressure reducing valve

由传递函数方框图(图3)可以看出，给定电磁铁电流Im(s)为可控制输入量，减压阀出口压力pout(s)为控制输出量，减压阀阀门进口压力pin(s)和流入负载流量Qin(s)为外部干扰量。与式(1)相比，减压阀的控制量与外部干扰量被很好地分离。因此，当pin(s)=0，Qin(s)=0时，控制系统的传递函数为：

令K0=k+ks，进而控制系统的闭环传递函数为：

获得减压阀的出口压力以后，通过驱动液压油缸带动中间传动装置提供给进系统的给进力。然而，在液压油缸驱动中间传动装置的过程中，伴随着能量的损耗，给进系统提供的给进力会减小。本文以ZDY4500LFK 电控钻机为例，计算其给进系统的传动效率，从理论最大输入进给力考虑给进系统的传动效率[19]，其理论最大输入给进力为：

式中：pmax为液压油缸最大给进压力(对应减压阀出口压力pout最大值)；A2为液压油缸有杆腔面积。

液压油缸的最大输出进给力Fmax可表示为：

式中：ηmax为液压油缸的机械效率；p21为给进系统的回油背压；A1为液压油缸无杆腔面积。

然而，液压油缸带动中间传动装置(拖板)时会产生摩擦力：

式中：u0为 拖板与给进机身导轨间的摩擦因数；l为拖板长度；h1为动力头中心距拖板与给进机身导轨接触表面的距离；h2为给进油缸距拖板与给进机身导轨接触表面的距离。

由此可计算得到给进系统的传动效率η为：

通过ZDY4500LFK 电控钻机参数计算得到，给进系统的理论传动效率为77%，而钻机实测得到的传动效率为76%，两者很接近。

因此，在钻孔机器人钻进过程中，考虑给进系统的传动效率因素，可以得到给进系统控制模型的传递函数F(s)为：

3 给进力跟踪控制问题

钻孔机器人钻进过程中是通过控制减压阀电流来改变减压阀阀门开度，调整减压阀出口压力(油压)，即给进系统液压油缸的进油腔压力。在实际给进系统中，减压阀出口压力可以测量，从而通过液压油缸和中间传动装置可以计算得到孔口实际给进力。

此外，钻柱在钻进过程中受到给进力和负载扰动的共同作用，由于传感器受限等原因，给进系统的状态无法直接测量，不能通过状态反馈进行扰动抑制。

因此，通过孔口实际给进力和控制输入量建立观测器来重构给进系统的状态，设计基于等价输入干扰方法的钻孔机器人给进力跟踪控制策略。

如图4 所示，钻孔机器人给进力跟踪控制系统由给进系统、内部模型、状态反馈控制器、状态观测器和等价输入干扰(Equivalent-Input-Disturbance，EID)估计器组成。系统的外环由内部模型、状态反馈控制器和给进系统构成，实现对孔口推荐给进力的跟踪控制；系统的内环由给进系统、状态观测器和等价输入干扰估计器组成，实现对给进系统所受负载扰动的抑制。具体内部模型用于跟踪参考信号，状态反馈控制器用于系统镇定，状态观测器用于重构给进系统的状态，EID 估计器用于补偿给进系统所受的总扰动。

图4 钻孔机器人给进力跟踪控制系统Fig.4 Force tracking control system of drilling robot

3.1 控制系统描述

内部模型用于精确跟踪参考输入，关系式为：

式中：xr(t)为 内部模型的状态；e(t)=r(t)-yˆ(t)为跟踪误差。当参考输入r(t)精 确已知时，参数Ar和Br可直接确定。

由于孔口实际给进力无法测量，使用Luenberger全维状态观测器，重构被控对象的状态。

式中：(t)为 系统的观测状态；uf(t)为 状态反馈；L为待设计的观测器增益；yˆ(t)为观测器的输出。

状态反馈控制律设计为：

式中：Kr与Kc为状态反馈控制器增益。

考虑到被控对象存在负载扰动，建立基于估计与补偿的控制结构，实现高精度的给进力跟踪控制。

3.2 基于等价输入干扰方法的控制结构

首先阐述EID 的存在性[20]。

定义1：令控制输入u(t)=0，并且初始状态满足x(0)=xo(t)。如果对于 ∀t≥0，被控对象的输出y(t)和yo(t)满足y(t)≡yo(t)，则扰动de(t)称为扰动d(t)的等价输入干扰。

根据稳定逆的概念，给出等价输入干扰存在性的定义。

定义2：在扰动d(t)的作用下，如果系统输出yo(t)满足yo(t)∈Φ，则在被控对象的输入端存在扰动d(t)的等价输入干扰de(t)，并且de(t)∈Φ，集合 Φ定义为

式中：αi(t)为时间t的多项式函数；ωi(≥0)和φi为常量，i=1,2,···,n。

根据EID 的定义，被控对象式(18)改写为：

根据文献[21]，构建EID 估计值如下：

由于估计的扰动容易受到输出测量噪声的影响，使用一阶低通滤波器B(s)限制扰动估计的带宽，即扰动估计为：

低通滤波器B(s)的状态空间方程为：

式中：xf(t)为低通滤波器B(s)的状态；(t)为滤波后的扰动估计值。通常低通滤波器的截止频率ωf大于扰动最高频率ωd的5～10 倍。

得到最终系统的控制输入。

钻机给进力跟踪控制问题描述为：基于减压阀的运动模型(式(18))，设计状态反馈控制器 {Kr,Kc}、状态观测器增益L和EID 估计器增益Kp，使系统在控制律(式(27))的作用下稳定，同时具有满意的跟踪与扰动抑制性能。

4 控制器设计与优化

为了简化系统的稳定性分析，考虑外部信号r(t)=0，d(t)=0 。定义xe(t)=x(t)-xˆ(t)为状态观测误差，由式(19)、式(20)、式(23)、式(26)和式(27)得到闭环系统的状态空间方程如下：

控制器的设计分为两个步骤：首先假设外部干扰全部由内环的EID 估计器补偿，进行状态反馈控制器{Kr,Kc}的设计；然后，根据设计好的状态反馈控制器，进行状态观测器L和EID 估计器Ke的设计。

4.1 状态反馈控制器的设计

为考虑负载扰动全部由系统内环的EID 估计器补偿，建立外环系统的状态空间方程：

式中：

则外环系统(式(29))渐近稳定，并且状态反馈控制器增益为：

证明：参照文献[22](定理3.1)的证明。

4.2 闭环系统的镇定

则闭环系统(式(28))渐近稳定，并且状态观测器增益和EID 估计器增益为

证明：参照文献[22](定理3.2)的证明。

4.3 步骤设计与优化

5 仿真验证

钻孔机器人减压阀模型仿真参数见表1。

表1 减压阀模型仿真参数Table 1 Simulation parameter of pressure reducing valve model

根据表1 所给参数，得到仿真模型如下：

为了确保煤矿井下工作面安全回采，某煤矿井下设计了孔深60～120 m 的工作面顺层孔用以预抽工作面瓦斯。在施工过程中，使用ZDY4500LFK 电控钻机采用加压钻进方式进行施工，给进压力由减压阀进行实时控制。本文以其中一个钻孔为例，钻孔设计方位189.1°，倾角-11.5°，由于煤层较软，钻进过程中实测给进压力为1～2 MPa。为了验证所提方法在煤矿井下钻进过程控制中的适用性，因此，选择的输入参数和干扰函数根据钻进过程的实际情况确定。

在某煤矿井下回风巷碎软煤层实际钻进施工时，给进力为5～7 kN，因此，给定参考信号：

选择Ar=-0.001，Br=1。

实测给进压力拟合曲线如图5 所示，经过计算得到，现场压力数据在拟合曲线10%上下波动，数据波动主要由外部干扰引起，因此，将此波动幅值比例作为给定扰动信号的幅值比例。

图5 减压阀阀门出口压力现场数据拟合曲线Fig.5 Field data fitting curve for outlet pressure of pressure reducing valve

因此，设计给定扰动信号为：

选择低通滤波器

从而，Af=-101,Bf=100,Cf=1。

5.1 设计与仿真

设计调节参数，α=1，β=1×10-24，δ=1×10-1，γ=1，μ=1×10-4，ε=1，得到控制器如下：

图6 所示为基于等价输入干扰方法的钻孔机器人给进系统控制结果，可以看出，所提方法保证了系统的稳定运行，且实现了较好的跟踪与扰动抑制性能。由图7 可知，稳态跟踪误差的峰峰值为 4×10-5kN，稳态扰动估计误差的峰峰值为0.1 mA。

图6 钻孔机器人给进系统给进力跟踪控制结果Fig.6 Force tracking control results of drilling robot feed system

图7 钻孔机器人给进系统跟踪误差与扰动抑制误差Fig.7 Tracking error and disturbance suppression error of drilling robot feed system

5.2 与PID 控制方法的对比

在给进系统使用PID 控制器镇定系统，PID 控制器设计如下：

为了获得相同的控制输入，取PID 控制器的增益分别为：KP=1×10-5,KI=1×10-4,KD=1×10-9。

图8 所示为所提控制方法与PID 控制方法的对比结果。在瞬态时，PID 控制器实现了更快速地跟踪参考信号，但其使用了一个较大的控制力，这在实际中容易造成执行器饱和等现象。PID 控制器是单自由度控制方法，在未施加干扰时实现了令人满意的跟踪性能，然而其不具备扰动估计与补偿的功能，因此，PID控制器的扰动抑制性能受限。与PID 控制器相比，所提方法基于扰动估计与补偿的二自由度控制结构获得了较好的控制性能。

图8 与PID 控制方法的给进系统给进力跟踪控制对比结果Fig.8 Comparison results of Force tracking control of feed system with PID control method

由图9 及其放大图可知，在稳态时(10～30 s，30～40 s，40～50 s)跟踪误差为0.4 kN，比所提方法大104倍，因此，所提方法具有更好的跟踪与扰动估计性能。

图9 与PID 控制方法的给进系统跟踪误差对比结果Fig.9 Comparison results of tracking error of the feed system with PID control method

选取积分平方误差(Integrated Square Error,ISE)、时间乘绝对误差积分准则(Integrated Time and Absolute Error,ITAE)和均方根误差(Root Mean Square Value,RMSE)量化所提控制方法的性能。

由表2 中的数据可知，所提EID 控制方法的跟踪误差更小，可以有效解决煤矿井下复杂地层不确定性扰动影响钻孔机器人工作性能和钻孔施工质量及效率的问题。

表2 EID 与PID 控制方法的误差数据对比Table 2 Comparison of error data of EID and PID control methods kN

6 结论

a.根据钻孔机器人的钻进施工工艺，基于电液先导比例减压阀的工作原理，建立了减压阀的运动控制数学模型，得到了控制输入量电流和控制输出量减压阀出口压力的映射关系，并在明确给进力驱动方式的基础上对整个给进系统进行了控制建模。

b.提出基于等价输入干扰方法的钻孔机器人给进力跟踪控制策略，建立基于等价输入干扰估计与补偿的控制结构，设计状态反馈控制器、状态观测器和等价输入干扰估计器增益，使系统具有较好的跟踪与扰动抑制功能。

c.利用Matlab 软件数值仿真研究可知，所提方法稳态时跟踪误差为 4×10-5kN，扰动估计误差为0.1 mA，研究结果对提高钻孔机器人适应复杂煤层负载变化，保证其工作性能和安全高效施工提供了控制理论基础。通过仿真说明所提方法能够使钻孔机器人的给进系统实现给进力的较好的跟踪性能，下一步将结合现场实际对所提方法开展工程应用研究。