基于模糊滑模反步控制的自动装卸钻杆机械臂轨迹跟踪研究

李东民,王 通,杜 浩,赵柳杨,栾亨宣,步 宇,钟佩思

(1.山东科技大学 智能装备学院,山东 泰安 271000;2.山东科技大学 机械电子工程学院,山东 青岛 266590)

瓦斯突出仍是煤矿生产的重大安全问题,目前普遍采用钻机对突出煤岩进行钻孔以实现瓦斯抽排。钻孔施工时,钻杆装卸工作需通过人工手动完成,这种方式效率低且安全性差[1]。

现有机械臂轨迹跟踪控制方法多采用PID控制、自适应控制、鲁棒控制及滑模控制[2]。其中,滑模控制具有响应性高、扰动不敏感及物理实现简单等优点,故广泛应用于机械臂等非线性系统的控制;但该控制方式极易产生不连续性和抖振现象,极大地降低了机械臂控制精度[3]。Rebai等[4]针对非线性系统的自适应模糊控制问题,采用模糊逻辑逼近未知的非线性扰动,通过滑模技术合成自适应模糊控制器,并改进了滑模控制中的比例积分控制项,减小了系统抖振现象,使跟踪误差收敛到零。但该方法不能实现对非线性系统的全局跟踪和实时控制,存在一定的迟滞与波动。与模糊逻辑逼近未知的非线性扰动相比,扰动观测器(Disturbance observer,DOB)在处理扰动和不确定性时稳定性更强,为了估计机械臂在运动过程中的外界扰动,邹思凡等[5]设计了一种DOB并对扰动进行估算,在滑模控制律中加入扰动估计值对可观测的扰动进行补偿,并引入反演自适应控制律,对不可观测的扰动进行估计,以改善机械臂的跟踪性能。但是扰动估计值的选取会引发其他参数的随机变化,使得系统的稳定性不易度量。牛城波等[6]结合模糊DOB实现对未知扰动的观测和补偿,设计PID控制器实现对位移的动态跟踪,以实现机械臂系统的稳定。该控制方法着重于实现对外界扰动的观测,却采用常规PID进行扰动补偿,使得系统的鲁棒性不强,轨迹跟踪效果得不到保证。陈龙淼等[7]提出一种基于DOB的机械手旋转定位自适应滑模控制策略,引入DOB估计扰动,通过滑模控制的切换项补偿估计扰动,使得机械手姿态可跟随负载变化保持稳定。但是该控制策略以常规滑模控制为基础,在对外界扰动补偿的同时,其自身的抖振并未消除,控制系统的稳定效果得不到保证。秦武等[8]以非线性主动悬架系统为研究对象,建立主动悬架系统的天棚模型,采用DOB估计悬架系统中簧载受到的外界扰动,提出了基于天棚模型和DOB的滑模控制算法,验证了该控制方法对主动悬架系统具有较好的控制效果。但是该文并未考虑悬架的惯性因素和滑模控制的抖振问题。席雷平等[9]提出一种改进的幂次趋近律,并基于该趋近律设计相应滑模控制策略,对其趋近性能进行分析,有效抑制了机械臂控制系统存在的抖振,该文并没有考虑摩擦与外界扰动对机械臂控制系统的影响。席雷平等[10]、杨洋等[11]和梁骅旗等[12]均针对机械臂滑模控制中存在抖振的问题以及对外界扰动问题展开研究,设计了相应的DOB,实现对系统的不确定性和外部扰动的观测,并将改进后的滑模控制策略用于控制系统,使得控制系统在降低抖振的同时,实现相应的轨迹跟踪任务,但其仅对控制策略下的轨迹跟踪效果进行了分析,未对控制策略下的跟踪响应进行分析。

自动装卸钻杆机械臂在运动过程中经常遇到负载多变、高频振动以及“死点”等问题,并且井下施工过程存在粉尘、噪音及振动等扰动因素,导致现有自动装卸钻杆机械臂轨迹跟踪能力较差,极大影响了钻杆装卸施工效率。

为解决上述问题,本文设计了DOB估算井下扰动因素引起的扰动,并将滑模控制方法与模糊控制理论和反步控制理论相结合,设计了新型自动装卸钻杆机械臂轨迹跟踪控制系统,以降低在井下工作时的系统抖振,提升自动装卸钻杆机械臂轨迹跟踪的精度与稳定性。本文首先建立了带有扰动项的自动装卸钻杆机械臂动力学模型,并建立其关节空间与笛卡尔空间的映射关系。然后,设计自动装卸钻杆机械臂轨迹跟踪控制系统,通过DOB对扰动信号进行估算。之后,采用模糊控制规则对滑模控制进行描述,建立模糊滑模控制算法(Fuzzy sliding mode control,FSMC)。再次,结合反步法(Back-stepping control,BSC)逐步设计中间虚拟控制量,提出模糊滑模反步控制策略(Fuzzy sliding mode back-stepping control,FSMBSC),以提高对整个自动装卸钻杆机械臂的控制精度。最后,通过对比试验,分析SMCT、FSMBSC及FSMC对外界扰动的补偿效果,以检验自动装卸钻杆机械臂轨迹跟踪、系统响应性及稳定性等性能。

1 自动装卸钻杆机械臂动力学模型 的建立

由于外界扰动的影响,自动装卸钻杆机械臂实际运行中易产生轨迹误差,严重影响钻杆转移精度。为解决该问题,设计控制器对各关节的关节角位移、角速度进行实时控制,以严格满足期望轨迹对各关节角的控制要求[13],从而提高自动装卸钻杆机械臂关节轨迹跟踪的控制精度。

基于现有矿井钻机结构,设计了整体结构如图1所示的自动装卸钻杆机械臂,该机械臂主要包括底座、腰座、大臂、小臂、手腕、手爪等组成部分。其工作时,首先由手爪从钻杆库中抓取钻杆,并由大臂和小臂调整位姿后,将钻杆转移至待装卸位置,最后完成钻杆装卸。由该机械臂整体结构和工作原理可知,大臂与腰座相连的关节(定义为大臂关节)和大臂与小臂相连的关节(定义为小臂关节)对整个机械臂位姿调节起到主要作用,且承受较大负载力,严重影响钻杆转移与装卸操作的精确性,因此,本文重点对其展开轨迹跟踪研究。

图1 自动装卸钻杆机械臂结构示意图

为实现上述轨迹跟踪目标,建立带有扰动项的自动装卸钻杆机械臂动力学方程,如式(1)所示

(1)

(2)

(3)

(4)

(5)

由式(4)和式(5)建立自动装卸钻杆机械臂关节空间与笛卡尔空间的映射关系,通过对关节的轨迹跟踪分析,映射出自动装卸钻杆机械臂末端执行器的运动轨迹,以对末端执行器的后续运动进行分析。

2 自动装卸钻杆机械臂控制器的设计

根据自动装卸钻杆机械臂在煤矿井下的控制要求,设计其轨迹跟踪控制系统如图2所示。其控制原理为:(1)采用DOB对自动装卸钻杆机械臂运动过程中的扰动信号进行估算,并将估算值传送至滑模控制器;(2)运动点与滑模面采用模糊控制规则描述运动点与滑模面的距离,将该距离作为模糊控制的输入,切换项增益作为模糊控制的输出,建立FSMC算法,使得滑模控制的切换项增益可以根据井下扰动强度的不同实现自适应变换,保证系统运动点可以在有限的时间内到达滑模面;(3)综合应用FSMC与BSC,提出FSMBSC控制策略,以控制自动装卸钻杆机械臂运动轨迹,并补偿扰动信号,从而提高自动装卸钻杆机械臂装卸钻杆的控制精度。

图2 自动装卸钻杆机械臂轨迹跟踪控制系统

2.1 DOB的设计

基于自动装卸钻杆机械臂轨迹跟踪控制原理,构造DOB方程如式(6)所示

(6)

DOB估计误差的计算如式(7)所示

(7)

并且,DOB的误差值受式(8)影响[15]

(8)

并且,定义辅助变量如式(9)所示

(9)

(10)

(11)

结合式(6)和式(9)可求得DOB方程如式(12)和(13)所示

(12)

(13)

2.2 FSMC的设计

定义自动装卸钻杆机械臂的实际轨迹与期望轨迹的误差e如式(14)所示

e=qd-q

(14)

式中:qd表示各关节期望运动角位移;q表示各关节实际运动角位移。

定义滑模面函数[16]如式(15)所示

(15)

式中:s=[s1,s2,…,sn]T,Λ=diag([r1,r2,…,rn]),ri表示对应滑模面的斜率,ri>0,i=1,2,…,n。

对滑模面函数式(15)求微分可得式(16)

(16)

将式(16)的x2作为滑模控制的输出,故可将其等价为式(17)

(17)

(18)

(19)

滑模控制器定义如式(20)～(22)示

u=ueq+usw

(20)

(21)

usw=Ksgn(s)

(22)

将式(20)带入式(1)中得式(23)

(23)

通过Lyapunov函数对滑模控制器的稳定性进行验证,如式(24)所示

(24)

结合式(2)和式(23)对V求导,得式(25)

sT(-(C+P)s+Δf-Ksgn(s)+Cs)=

sT(Δf-Ksgn(s))-sTPs≤-sTPs

(25)

为了对滑模控制中的抖振进行抑制,采用模糊控制规则描述运动点与滑模面的距离di,将di作为模糊控制的输入,切换项增益ki作为模糊控制的输出,建立FSMC算法,使得滑模控制的切换项增益ki可以根据井下扰动强度的不同实现自适应变换,使得系统运动点可以在有限的时间内到达滑模面。距离di的计算如式(26)所示

(26)

定义Am和Bm两个模糊集,设计7n条模糊规则,将di作为模糊输入,ki作为模糊输出。其中,模糊输入采用三角形和高斯型的隶属度函数,模糊输出采用三角形隶属度函数。模糊系统输出如式(27)所示

(27)

(28)

取自适应律为式(29)所示

(29)

从而,将模糊控制器与滑模控制器联立得式(30)

(30)

为证明其稳定性,设定Lyapunov函数设定如式(31)所示

(31)

结合式(23)对式(31)求微分,可得式(32)

(32)

将上述条件带入式(32)使得式(33)成立

(33)

式中:υ=diag[υ1,υ2,…,υn],pi>υi。

2.3 BSC的设计

(34)

(35)

对上述BSC建立Lyapunov函数V(ε,δ),使得ε,δ都趋近于0。其公式如式(36)和式(37)所示

(36)

(37)

(38)

对式(38)进行变换,求得控制律τi如式(39)所示

(39)

3 自动装卸钻杆机械臂运动仿真

3.1 两关节结构模型参数设置

根据打孔施工与自动装卸钻杆机械臂工作原理,建立自动装卸钻杆机械臂两个关节几何模型如图3所示。所述的两个关节即为大臂关节和小臂关节,并在图中标示为关节1与关节2。该两关节在钻杆装卸施工中,对大臂和小臂的位姿调整起到重要作用,且在施工过程中受力较大,故将其作为研究对象。

图3 两关节结构模型示意图

基于打孔施工实际工况,设置自动装卸钻杆机械臂结构参数如下:连杆长度L1=1.2 m,L2=1 m,连杆质量m1=5 kg,m2=4 kg,转动关节到连杆质心的距离Lc1=0.6 m,Lc2=0.5 m,关节转动惯量I1=0.3 kg·m,I2=0.25 kg·m,两个关节初始角均为30°,角速度和角加速度初始值均为0。

根据钻杆装卸实际施工流程,采用MATLAB/Simulink进行仿真研究。

其系统动力学方程如式(40)所示

(40)

期望输出轨迹为qd1=sinπt,qd2=cosπt。

两关节所受外界扰动均为τd1=τd2=5+0.15sint。

3.2 仿真参数的设置

模糊控制系统为单输入单输出系统,模糊输入和输出均采用三角形和高斯型的隶属度函数。基于实际工况并经参数整定后,得到FSMC的输入di和输出ki的隶属度函数曲线如图4所示。

图4 FSMC中输入di和输出ki隶属度函数

基于试验工况,设置DOB采样时间为0.01 s,仿真步数为200步,即仿真时长为2 s。对FSMBSC的仿真参数设置如下:k=diag([2,2]),P=diag([3,5]),ri=0.7,k1=diag([3,3]),k2=diag([5,5])。基于实际工况,设置关节1与关节2的输入信号分别为正弦信号和余弦信号,并采用固定步长求解器。采用S函数描述控制系统,采样时间设定为0.01 s,仿真步数设置为2 000步,即仿真时长为20 s。

3.3 系统仿真结果与分析

DOB对关节1和关节2的输出曲线分别如图5和图6所示,可见,所设计DOB准确测得外界扰动力矩,且只需约0.5 s即可趋近期望扰动值,故跟踪效果良好。

图5 关节1的扰动观测器输出

图6 关节2的扰动观测器输出

然后对关节1和关节2进行轨迹跟踪分析,将期望轨迹记为DT,滑模控制器轨迹记为SMCT,模糊滑模控制器轨迹记为FSMCT,模糊滑模反步控制器轨迹记为FSMBST,仿真结果如图7～10所示。

由图7和图8可知,FSMBST-DT的轨迹跟踪误差为[-0.1,0.1],趋近于期望曲线,且其轨迹跟踪误差值明显小于FSMCT-DT和SMCT-DT。方差是描述曲线波动的重要参量[13],对两关节的轨迹误差曲线求方差可得,FSMBST-DT的跟踪误差比FSMCT-DT分别减小了32.6%和25%。

最后对各控制器进行跟踪响应性分析,由图9和图10可知,FSMBST的响应性最好,其次是FSMCT,而SMCT最差。关节1和关节2在FSMBST控制下具有较快的响应性,稳定时间约为0.3 s,且未发生超调,近似趋近于期望信号DT;而SMCT和SMCT两者响应时间明显落后于FSMBST,且均存在一定超调。

图7 关节1的位置跟踪误差

图8 关节2的位置跟踪误差

图9 关节1的位置跟踪响应图

图10 关节2的位置跟踪响应

由图11和图12可见,随着运动点位置的不断变换,模糊控制可使得切换增益k进行自适应的调整,从而减小抖振,并使得各关节的跟踪误差收敛。

图11 关节1的控制率增益变化

图12 关节2的控制率增益变化

由上述对FSMBST、FSMCT和SMCT的控制性能对比分析可得,FSMBST控制能够实现扰动补偿,有效降低由扰动引起的自动装卸钻杆机械臂运动跟踪误差,提高钻杆装卸施工精度。

图13 自动装卸钻杆机械臂试验平台

4 自动装卸钻杆机械臂轨迹跟踪试验

为进一步验证本文所提方法的有效性,基于系统建模与仿真结果,建立试验平台如图13所示,图中机械臂即为自动装卸钻杆机械臂。试验平台硬件包括伺服驱动器、伺服电机、编码器及I/O面板等。试验时,采用MATLAB/Simulink设计控制程序,并采用实时控制软件Quarc进行程序编译,将编译结果导出至I/O面板,再输送至伺服驱动器,进而驱动机械臂各关节运动[14,22]。其中,编码器实时检测各关节位置信号,并反馈至上位机,由上位机监控系统运行状态,实现自动装卸钻杆机械臂的运动控制。

基于自动装卸钻杆机械臂两关节的工况,设置试验时间为20 s,两关节的期望轨迹为qd1=sinπt,qd2=cosπt。控制器参数设置同仿真试验,即k=diag([2,2]),P=diag([3,5]),ri=0.7,k1=diag([3,3]),k2=diag([5,5]),采样时间设定为0.01 s。采用FSMBST与SMCT、FSMCT进行控制性能对比试验。

根据编码器采集到的实时数据,得到关节1和关节2的轨迹跟踪误差曲线如图14和图15所示。由图可见,三种控制方法均存在振荡现象,但FSMBST控制方式生成曲线的振荡幅度最小,控制效果最好。对两关节的轨迹误差曲线求方差可得,在关节1中,FSMBST-DT的跟踪误差比FSMCT-DT减小了28.2%;在关节2中,FSMBST-DT的跟踪误差比FSMCT-DT减小了22.6%,故本文方法降低了抖振,并提高了自动装卸钻杆机械臂轨迹跟踪精度。

图14 关节1的轨迹跟踪误差

图15 关节2的轨迹跟踪误差

5 结论

为提高矿井排瓦斯钻机装卸钻杆施工的安全性与生产效率,本文设计了自动装卸钻杆机械臂,并对其运动轨迹跟踪的控制性能展开研究,得到如下结论:

针对滑模控制易产生抖振的问题,本文采用DOB估算自动装卸钻杆机械臂的外界扰动,采用模糊控制规则描述运动点与滑模面的距离di,将di作为模糊控制的输入,切换项增益ki作为模糊控制的输出,建立FSMC算法,使得滑模控制的切换项增益ki可以根据井下扰动强度的不同实现自适应变换,使得运动点在有限的时间内到达滑模面,从而极大降低抖振程度。

针对现有机械臂轨迹跟踪精度与稳定性不高的问题,基于BSC与FSMC提出FSMBST控制算法,以补偿估算扰动。

针对自动装卸钻杆机械臂两关节跟踪误差进行了仿真和实验室试验。仿真结果显示,FSMBST-DT比FSMCT-DT分别减小了32.6%和25%;实验室试验得到,FSMBST-DT比FSMCT-DT分别减小了28.2%和22.6%。