旋翼飞行机械臂固定时间滑模控制

刘宜成, 贺嘉辣, 严 文

(四川大学电气工程学院,成都 610000)

0 引言

旋翼无人机凭借结构简单、运动灵活,被广泛应用于航拍、侦察、军事等很多方面[1],在大部分场景中,旋翼无人机仅用于环境感知,缺乏与环境的主动交互[2]。

对于一些需要与环境进行接触的任务,如舰船补给过程中的物资空中调运、空中的目标物体抓取或者高压输电线的检修[3-4]等,依靠旋翼无人机就无法完成。为了能与外界进行物理交互,一般会在旋翼无人机上配备刚性工具(比如夹爪)或者n自由度的机械臂[5],从而增强旋翼无人机与外界交互的能力,这种机器人被称为旋翼飞行机械臂,在运输、救灾、高空作业等领域具有广泛的应用前景。

由于引入作业装置,旋翼飞行机械臂的控制面临着明显的困难。旋翼飞行机械臂动力学模型具有强非线性与欠驱动的特性,在任务执行过程中无法避免存在一些干扰[6-7],为旋翼飞行机械臂控制带来巨大的挑战。

滑模控制方法是非线性系统控制器设计中常用的一种控制方法,具有较强的鲁棒性,该方法被广泛应用于旋翼飞行机械臂的控制[8-9]。文献[10]对旋翼无人机姿态设计了滑模控制器,以抑制机械臂运动对旋翼飞行机械臂的耦合影响,并采用粒子群算法整定滑模参数从而完成旋翼飞行机械臂控制;文献[11]提出了一种适用于无人机的滑模自适应控制器,该控制器对环境干扰具有鲁棒性,可应用于位置和速度控制、航路点跟踪和视觉伺服;文献[12]通过线性化方法简化旋翼飞行机械臂模型,为旋翼飞行机械臂设计了外环滑模,内环PID的双环控制器,实现对两自由度飞行机械臂的控制,其性能优于传统PID双环控制;文献[13]通过拉格朗日方法建立了旋翼飞行机械臂动力学模型,将其投影至二维平面完成模型简化,通过自适应方法估计外部干扰,补偿至滑模控制器完成了旋翼飞行机械臂抓取工作。

上述文献将滑模控制方法应用于旋翼飞行机械臂并取得了不少的成果。随着机器人技术的不断发展,人们对旋翼飞行机械臂轨迹跟踪的准确性与快速性也提出了更高的要求,目前关于旋翼飞行机械臂控制的研究大多为渐近收敛或指数收敛,并没有关注系统收敛时间问题。此外,在滑模控制器设计过程中存在抖振问题,会影响旋翼飞行机械臂轨迹跟踪的精度,严重的情况下甚至会导致对象失控,因此需要对滑模控制器抖振问题进行处理。

本文研究的旋翼飞行机械臂由四旋翼飞行器与二自由度机械臂组成,利用拉格朗日法对系统整体进行建模得到旋翼飞行机械臂动力学模型,设计基于固定时间滑模观测器的旋翼飞行机械臂固定时间控制器。本文的主要改进之处有:设计固定时间滑模控制器,使得旋翼飞行机械臂可以快速跟踪期望轨迹;设计新型固定时间观测器快速估计旋翼飞行机械臂未建模动态与外部扰动组成的混合干扰,对旋翼飞行机械臂进行补偿,并采用饱和函数以及分段滑模面抑制滑模观测器抖振;为固定时间滑模控制器设计自适应趋近律以抑制控制器抖振。

1 旋翼飞行机械臂模型

旋翼飞行机械臂主要由四旋翼飞行平台与两关节机械臂组成,系统结构如图1所示。

图1 旋翼飞行机械臂结构

图1中:QI-XIYIZI表示惯性参考系坐标系FI,其中,O1-X1Y1Z1与O2-X2Y2Z2分别为机械臂关节1与关节2的坐标系F1,F2,坐标系原点分别为O1与O2,分别位于机械臂第1、第2连杆的质心位置;OB-XBYBZB表示机体坐标系FB,在四旋翼飞行平台重心上,ZB轴垂直飞行平台平面竖直向上。

1.1 运动学模型

图1中,四旋翼质心在惯性坐标系FI中位置表示为p=[x,y,z]T,飞行机械臂的姿态用Z-Y-X欧拉角表示,记为Φ=[φ,θ,ψ]T,机械臂的关节角η=[η1,η2]T,则运动学模型定义为

q=[pT,ΦT,ηT]T。

(1)

从飞行平台机体坐标系FB到惯性坐标系FI的旋转矩阵可以表示为

(2)

(3)

(4)

(5)

其中:BRI为旋转矩阵IRB的转置矩阵;矩阵T与Q分别为

(6)

(7)

(8)

连杆i的质心在机体坐标系FB中线速度与角速度的表达式分别为

(9)

(10)

(11)

(12)

其中,S(·)表示向量的反对称矩阵。

1.2 动力学模型

本文采用拉格朗日方法对飞行机械臂建模,拉格朗日方程为

(13)

(14)

(15)

(16)

(17)

(18)

式中,M(q)矩阵大小为8×8,并且该矩阵为对称正定矩阵,将其写作分块矩阵形式为

(19)

将式(15)～(17)代入式(18),求得M(q)中各个矩阵块数学表达式为

(20)

旋翼飞行机械臂系统总势能为旋翼无人机势能与机械臂势能之和,即

(21)

式中:g=9.8 m/s2;e3=[0,0,1]T。将式(15)与式(21)代入式(14),可以得到系统拉格朗日函数为

(22)

将式(22)代入式(13)可得

(23)

(24)

(25)

将式(24)改写为矩阵形式

(26)

式中:τp=[τ1,τ2,τ3]T;τa=[τ4,τ5,τ6]T;τl=[τ7,τ8]T;G1,G2,G3分别为3×1,3×1与2×1的矢量矩阵。

2 控制器设计

旋翼飞行机械臂的控制结构如图2所示。

图2 飞行机械臂系统控制结构

由图2可知,控制器主要由固定时间观测器与固定时间滑模控制器组成。固定时间观测器主要用于估计外界干扰、未建模动态以及系统的复杂耦合对系统的影响;固定时间滑模控制器可使飞行机械臂在固定时间内收敛,具有更高的收敛速度与精度;采用饱和函数与自适应趋近律抑制滑模控制器抖振对系统的影响,提高飞行机械臂控制精度。

2.1 预备知识

引理1考虑如下系统[14]

(27)

式中,Rn表示实数集。

假设存在一个连续正定可导函数V(x),满足

(28)

式中,α,β,Δ>0,Δ代表系统未建模动态与外界干扰之和。则系统式(27)是固定时间稳定的,其收敛时间与收敛邻域可以表示为

(29)

(30)

引理2考虑如下标量系统[15]

(31)

(32)

2.2 固定时间观测器设计

考虑未建模动态与外界干扰的情况下,旋翼飞行机械臂系统模型式(23)可写为

G(q)+ΔG(q)=τ+τext

(33)

(34)

(35)

Uothers=M(q1)-1(τ-C(q1,q2)q2-G(q1))

(36)

D=M(q1)-1Δ

(37)

(38)

式中:β>1;k1,k2,k4>0;k3>L;z1为q2的估计值,估计误差σ1=z1-q2;z2为D的估计值,估计误差定义为σ2=z2-D;sat(xvar)定义如下

(39)

ε为极小的正整数;非线性函数χ(xvar,γ)定义为

(40)

式中:0<γ<1<γ0≤2;δ为一个任意小的正数;a与b定义为

(41)

函数χ(xvar)对xvar求偏导

(42)

非线性函数χ(xvar,γ)保证了观测器在接近平衡点时减缓收敛速度,配合饱和函数从而削弱传统滑模观测器的抖振问题,提高控制性能。此外,观测器式(38)可以在固定时间收敛到平衡点邻域内,且收敛时间To满足

(43)

证明。

对估计误差σ1与σ2进行求导

(44)

(45)

(46)

(47)

(48)

(49)

式中:

Δo=σ1(k2·sigγ(σ1)-k2·χ(σ1,γ))+

(50)

由式(45)与式(46)可知,当|σ1|≥max(δ,ε)时,Δo=0;当|σ1|

(51)

根据引理1与引理2,估计误差σ1可在时间To内收敛到原点附近充分小的邻域内,且收敛时间To满足

(52)

当σ1趋近于0时,由式(44)可得,σ2也将趋于0,即z2趋近于D。

2.3 固定时间控制器设计

旋翼飞行机械臂系统是一个欠驱动系统,可以划分为姿态、位置以及机械臂关节3个回路分别进行控制[11]。姿态回路主要用于控制φ,θ与ψ这3个通道;位置回路控制x,y和z这3个通道,并且向姿态回路输出参考值;机械臂控制回路主要用于控制机械臂关节角η1与η2。

根据式(26),在考虑到建模误差与外界干扰的情况下,飞行机械臂姿态动力学方程为

(53)

(54)

其中:ΔΦ为旋翼飞行机械臂姿态回路的广义干扰,包含旋翼飞行机械臂未建模动态与外部干扰,可由式(38)固定时间观测器观测得到,具体形式如下

(55)

定义姿态跟踪误差

eΦ=Φ-Φd

(56)

式中:Φ为旋翼飞行机械臂实际姿态;旋翼飞行机械臂期望姿态Φd=[φd,θd,ψd]T。

滑模面设计如下

(57)

式中:kΦ1,kΦ2,kΦ3>0;βΦ>1;eΦ为姿态估计误差。为了加快系统状态的收敛并降低系统的抖振,设计了如下固定时间收敛的自适应趋近律:

w(sΦ,eΦ)sgn(sΦ)

(58)

w(sΦ,eΦ)=k|eΦ|e-μ|sΦ|

(59)

其中,k,μ都为正数。当系统状态远离滑模面,即|sΦ|→∞时,χ(sΦ,γΦ),sigβΦ(sΦ)与sΦ这3项会使得系统快速接近滑模面;当系统状态接近滑模面时,sgn(sΦ)起主要作用,该项系数w(sΦ,eΦ)会随着系统趋于稳定而趋于0,从而达到抑制抖振效果。

设计飞行机械臂姿态回路控制策略为

(60)

按照式(53)～(59)可设计飞行机械臂位置回路控制器。飞行机械臂位置回路的控制律为

(61)

式中:pd为旋翼飞行机械臂平台期望位置;ep为旋翼飞行机械臂平台位置误差;sp为飞行机械臂位置环路的滑模面方程,形式与式(57)一致。w(sp,ep)形式如式(59)所示,pd=[xd,yd,zd]T,ep=p-pd。

(62)

式中:kp1,kp2,kp3>0;βp>1;0<γp<1。fp与Δp定义为

(63)

(64)

旋翼飞行机械臂是个欠驱动系统,期望偏航角ψd为给定条件,而期望横滚角φd与期望俯仰角θd则是由外环的输出决定,具体算式为

(65)

飞行机械臂关节回路控制器的控制律为

(66)

式中:ηd为旋翼飞行机械臂关节期望角度,ηd=[η1d,η2d]T;el为旋翼飞行机械臂的位置误差,el=η-ηd;sl为旋翼飞行机械臂位置回路的滑模面方程;w(sl,el)形式如式(59)所示,其中

(67)

式中:kl1,kl2,kl3>0;βl>1;fl与Δl分别定义为

(68)

(69)

飞行机械臂总体控制输入为τ=[τp,τa,τl]T。

2.4 稳定性分析

(70)

对式(70)进行求导

w(sΦ,eΦ)|sΦ|-w(sp,ep)|sp|-w(sl,el)|sl|

(71)

作如下定义

Δc=kΦ1(sigγΦ(sΦ)-χ(sΦ,γΦ))sΦ+kp1(sigγp(sp)-

χ(sp,γp))sp+kl1(sigγl(sl)-χ(sl,γl))sl

(72)

将式(72)代入式(71),得

Δc-w(sΦ,eΦ)|sΦ|-w(sp,ep)|sp|-w(sl,el)|sl|

(73)

当收敛时间大于To时,σ2=0,式(73)可以简化为

kq2((2Vc)(βΦ+1)/2+(2Vc)(βp+1)/2+(2Vc)(βl+1)/2)+Δc

(74)

式中,kqi=max(kΦi,kpi,kli),i=1,2。

下面分两种情况讨论。

1) 当2Vc≥1时,

(2Vc)(γΦ+1)/2+(2Vc)(γp+1)/2+

(2Vc)(γl+1)/2≥3·(2Vc)γc1

(75)

(2Vc)(βΦ+1)/2+(2Vc)(βp+1)/2+

(2Vc)(βl+1)/2≥3·(2Vc)βc1

(76)

其中,

(77)

2) 当2Vc<1时,

(2Vc)(γΦ+1)/2+(2Vc)(γp+1)/2+(2Vc)(γl+1)/2≥3·(2Vc)γc2

(78)

(2Vc)(βΦ+1)/2+(2Vc)(βp+1)/2+(2Vc)(βl+1)/2≥3·(2Vc)βc2

(79)

其中,

(80)

综合式(77)、式(80),根据引理1,该系统固定时间稳定,证明完毕。

3 仿真实验

3.1 仿真条件

旋翼飞行机械臂实物如图3所示。

图3 旋翼飞行机械臂实物图

图3中,旋翼飞行机械臂系统主要由四旋翼飞行器与2自由度机械臂组成,系统的主要参数如表1所示。其中,Ixx,Iyy,Izz分别为旋翼飞行机械臂x,y,z方向的转动惯量。旋翼飞行机械臂初始条件为:旋翼飞行机械臂初始位置p0=[0 m,0 m,0 m]T,初始姿态Φ0=[0 rad,0 rad,0 rad]T,机械臂两关节初始角度η0=[0 rad,0 rad]T。

表1 旋翼飞行机械臂参数

3.2 仿真结果

3.2.1 干扰观测器仿真

为了验证本文所提新型固定时间观测器算法的优越性,将本文算法与文献[16]所提固定时间干扰观测器算法在二阶系统进行对比仿真,二阶系统如下

(81)

式中:x1,x2为系统状态变量;u为系统输入;Δdis=3sin 0.3t+2cos 0.2t+sin 0.25t-4。

本文新型固定时间观测器参数选择如下:k1,k2,k3,k4都取值为10;β=5/3;γ=3/5。

实验结果如图4所示。

图4 实验结果

从图4可以看出,本文设计的新型固定时间观测器可以快速跟踪上干扰值。相比于文献[16]算法,本文算法收敛速度更快,收敛过程中抖振更小,超调幅度更小,可以快速跟踪上干扰,具有更好的性能,并且在收敛稳定时,收敛误差可以达到10-5数量级,满足工程使用所需,实验证明了该观测器算法的有效性。

3.2.2 旋翼飞行机械臂控制器仿真

为验证本文对旋翼飞行机械臂设计的控制器的有效性,在上述相同初始条件和模型参数的情况下,将本文算法与文献[12]提出滑模PID混合控制算法进行对比仿真,定义未知外部干扰和旋翼飞行机械臂本身建模不确定性组成混合干扰为Δp=[0.4sint+0.5sin(2t+π/4),0.2cost+0.8sin(1.5t+π/3),0.3cos 2t+0.7sin(t+π/5)]T,ΔΦ=[0.4sin 0.5t+0.5sin(2t+2),0.6cos 0.8t+0.8sin(1.7t+π/3),1.5cos(2t+3)+3.5sin(t+π/5)]T,Δl=[2sin 0.5t+0.5sin(2t+2),cos(0.8t+11)+0.6sin(t+π/3)]T。

控制器与观测器参数选择如下:k1,k2,k3取值为10,k4=1,k=3,β=5/3,γ=3/5,kΦ1,kΦ2,kΦ3取值为5,kp1,kp2,kp3取值为5,kl1,kl2,kl3取值为5,βΦ,βp,βl取值为3,γΦ,γp,γl取值为1/3,ε=0.005,γ0=0.000 5,μ=10。

旋翼飞行机械臂期望位置pd=[3 m,4 m,2 m],期望偏航角ψd=π/4,期望机械臂关节角度ηd=[π/5,π/4]。仿真结果如图5～8所示。

图5 旋翼飞行机械臂位置与偏航角跟踪曲线

图5中,控制器在x,y,z方向上都在2 s内实现稳定跟踪并且跟踪误差收敛到零的附近,而文献[12]算法收敛速度明显慢于本文算法。

从图6可看出,当系统收敛稳定时,由于存在未建模动态与干扰,本文算法的收敛精度明显优于文献[12]算法收敛精度,说明本文算法对未建模动态和扰动具有良好的抑制能力,在飞行机械臂的轨迹跟踪上有较好效果。

图6 旋翼飞行机械臂位置与偏航角误差曲线

图7与图8展示了本文算法对机械臂关节的控制效果,在外部干扰与耦合的影响下,本文算法有较快的收敛速度与动态性能,控制精度可达10-4。

图7 旋翼飞行机械臂关节角度跟踪曲线

图8 旋翼飞行机械臂关节角度误差曲线

图9展示了旋翼飞行机械臂关节1控制器输出τ7,在采用了饱和函数以及自适应方法后,旋翼飞行机械臂控制器的控制输出抖振幅度减小,且在系统稳定时几乎无抖振,对比文献[12]算法控制器输出抖振明显降低,说明本文算法对抖振有较好的抑制效果。

图9 旋翼飞行机械臂关节1控制器输出

4 结论

针对旋翼飞行机械臂在飞行过程中稳定性的问题,提出了一种新型的基于固定时间滑模观测器的滑模控制算法,实现了旋翼飞行机械臂稳定飞行。首先,使用D-H法与拉格朗日方法建立了旋翼飞行机械臂运动学与动力学模型;随后,为旋翼飞行机械臂系统设计了新型固定时间观测器估计影响系统稳定的未建模动态与外界干扰;最后,将旋翼飞行机械臂分为姿态、位置以及机械臂子系统进行控制,降低了控制器设计难度,结合固定时间理论设计了旋翼飞行机械臂的固定时间滑模控制器,利用Lyapunov稳定性理论证明了系统可以在固定时间收敛到平衡点附近邻域内。仿真实验证明了本文算法对于旋翼飞行机械臂镇定和轨迹跟踪任务均具有较好效果。下一步工作将会考虑把本文算法应用于已搭建好的旋翼飞行机械臂平台,结合视觉完成旋翼飞行机械臂的抓取实验。