水下机器人-机械臂系统的滑模自抗扰控制

李小岗, 王红都, 黎 明, 刘 鑫

(1. 中国海洋大学 工程学院, 山东 青岛 266100; 2. 四川警学院 侦查系, 四川 泸州 646000)

水下机器-机械臂系统(Underwater Vehicle-Manipulator System, UVMS)模型具有高度非线性、强耦合、水动力学参数变化等特点, 且其在作业过程受到复杂海洋多源干扰的作用, 这些都会严重影响UVMS 的操作和定位精度等指标[1]。因此, 有必要设计具有干扰抑制与补偿能力的复合控制器, 以达到预期的抗扰等性能指标要求。

目前, UVMS 已经有了较为成熟的建模与控制方法[2-4]。Fossen[2]采用牛顿-欧拉公式建立了UVMS 的模型, Tarn[3]提出了一种新的基于欧拉-朗格朗日方程和牛顿-欧拉公式的模型建立公式。这两种经典的建模方法为基于模型的控制奠定了基础。Mahesh[4]研究了UVMS 的协调控制, 将水下机器人和机械臂系统的相互影响进行了系统性的阐述。随着对控制精度要求的提高, 需要开发更加高效、精确的控制系统, 在此基础上许多学者提出了众多新的控制方法[5]。Huang[6]结合干扰观测器, 提出了基于模型的自适应控制; 针对UVMS 的动态不确定性和时变外部扰动, Dai[7]将扩张卡尔曼滤波器引入补偿系统, 将基于模型的力矩控制器与H∞鲁棒补偿跟踪控制器结合设计了间接自适应控制器; Londhe[8]将单输入模糊控制器应用于UVMS的轨迹跟踪问题; Wang[9]采用离散延迟估计来处理未知的水动力干扰, 并针对UVMS 的闭环系统进行了分析。Huang[10]将运动学和动力学控制器相结合, 设计了基于动态模型的动力学控制器相结合, 设计了基于动态模型的自适应控制器, 提高了UVMS 操纵轨迹的精确性。这些方法都能达到较好的控制效果, 但是需要模型精确已知或者模型过于复杂。

图1 UVMS 的动态模型示意图Fig. 1 Schematic of the dynamic model of UVMS

韩京清[11]借鉴PID 控制不依赖于模型的优点,提出了自抗扰控制技术(Active Disturbance Rejection Control, ADRC)。其核心思想是将系统的内部不确定性与外部扰动集总为“广义总扰动”, 引入了非线性扩张状态观测器(Extended State Observer, ESO)对其进行估计; 并且在外部信号输入端加入跟踪微分器(Tracker Differential, TD)对输入信号安排过渡过程,使得自抗扰控制具有快速的响应过程和良好的抗干扰能力[12-14]。但是其参数难以整定, 且非线性函数难以工业化的难点在很大程度上限制其工业应用。为了解决以上难点, Gao 等[15]用基于带宽的思想提出了线性自抗扰控制器, 采用PD 控制器替换原来的非线性误差反馈, 并将PD 控制器和线性扩张状态观测器(Linear Extended State Observer, LESO)的参数与其带宽联系起来, 提出了结构相对精简的线性自抗扰控制器(Linear Active Disturbance Rejection Controller, LADRC), 该控制器极大减少了需要调节的参数。因此, LADRC 技术为具有多输入多输出的AUVMS 高精度控制设计带来了可能。

本文针对UVMS, 将复杂系统模型转变为简单的积分串联系统, 利用LESO 可对包括建模误差、耦合、内外部干扰、海洋多源干扰在内的集总干扰进行估计并补偿, 引入滑模控制器(Sliding Mode Control, SMC)增加系统的鲁棒性, 建立了滑模自抗扰控制器(Sliding Mode Active Disturbance Rejection Control, SM-ADRC), 考虑用简单的控制结构解决UVMS 的复杂控制问题, 最后仿真结果证明了该方法的有效性。首先建立了UVMS 的动力学模型, 将UVMS 分成6+n个子系统, 针对每一个子系统, 将内部不确定性、外部干扰和其他子系统的耦合影响集总为总扰动, 设计LESO 进行估计, 再利用估计误差建立滑模面, 给出UVMS 的SM-ADRC 设计方法,并进行 Lyapunov 稳定性分析。在此基础上得到UVMS 的SM-ADRC 设计方法, 并给出仿真结果, 与传统LADRC 与SMC 的控制结果进行对比分析。

1 UVMS 动力学分析

1.1 UVMS 的动力学模型

作者将整个UVMS 系统视为刚体, 根据Fossen[2]的建模方法建立如下UVMS 动力学模型:

式中向量q= [X,Y,Z,φ,θ,Ψ,θ1,θ2, …,θn]T由船体的位置[X,Y,Z]T, 航向角[φ,θ,Ψ]T, 机械臂关节角度向量 [θ1,θ2, …,θn]T(n为机械臂关节个数, 本文选取n=3), 因此系统总自由度为水下机器人本体的6 个自由度和机械臂关节的自由度之和, 即6+n。斜对称正定矩阵M(q)是包括附加质量的系统质量矩阵, 为,C(q,)表示包括附加质量的科氏力和向心力部分[15],D(q,)表示水动力阻尼部分,G(q)表示船体的浮力与重力之间的总和作用,F(q,)表示系统中水下机器人与机械臂相对运动间的耦合影响,τc是整个系统的控制输入,τd为难以测量的未知扰动。根据UVMS的固有特征, 系统(1)中的系数矩阵有以下特性[16-19]:

式中f为建模误差, 参数不确定性, 水动力阻尼影响和未知干扰等的总和, 即:

定义系统维数为m= 6+n, 因此系统模型(4)可展开为:

其中系数矩阵[aii]m×m(q)和[cii]m×m(q,q˙ ),y=q为系统输出向量,u=τc为系统控制输入向量,f为总扰动向量。

图1 给出UVMS 的动态模型示意图, 定义图中坐标轴Ie(X,Y,Z)为惯性坐标系,Ib(xb,yb,zb)为机身坐标系,Ii(xi,yi,zi),i=1, 2, …,n为机械臂关节自身坐标系, 其中n为机械臂关节个数, 本文只考虑机械臂关节绕Z轴旋转。

1.2 水动力参数分析

假设UVMS 的构成为圆柱体, 则其在水下所受的黏性曳力动力学模型为[20]:

式中vr为被控对象与洋流的相对速度,ρ为水密度(海水密度为1.02～1.07),Ds为线性表面摩擦系数,Dsvr即为线性表面摩擦力。A(α)为被示性曲面前部的投影横截面积,α为冲角。ο ()为阶数大于等于三阶的高阶相, 通常可忽略。Cd(Rn,α)为曳引系数, 由雷诺数Rn决定。而雷诺系数可以由以下公式求得:

式中d是垂直于流体速度vb方向的物体的特征长度,μ为流体黏性系数。

升力垂直于流动方向。对于水下运动物体, 它们可以被建模为[20]:

式中Cl为升力系数, 由雷诺系数与冲角决定。表1给出了圆柱体雷诺数函数的升力系数与曳力系数。

表1 UVMS 的升曳系数Tab. 1 Drag force and lift force of the UVMS

当刚体在重力作用下浸入流体中时, 必须考虑两个主要的力: 重力和浮力。后者是唯一的流体静力学效应, 它不是机身和液体之间相对运动的函数。

定义如下向量:

为重力加速度向量,V为机身的体积,m为质量。则物体浮重为W=, 所受浮力为B=。作用在机身的重心=[xG,yG,zG]的重力在机身坐标系可表示为:

式中Rt为转移矩阵, 作用于浮心=[xB,yB,zB]的浮力在机身坐标系可表示为:

因此在机身坐标系内的重力和浮力作用下的力/力矩矢量可表示为:

由于假设重力作用在刚体上是一致的, 所以质心和重心是重合的。

用欧拉角可以将gRB表示为:

式中s(.)和c(.)表示正弦与余弦函数。

当一个刚体在流体中运动时, 必须考虑围绕着它的流体的附加惯性, 也就是由于流体的运动而加速的惯性。这种效应在工业机器人中可以忽略, 因为空气的密度比移动机械系统的密度轻得多。然而在水下时水下机器人的密度相比, 水的密度ρ≈1 000 kg/m3。在0 度时净水密度为1 002.68 kg/m3; 含盐量为3.5%的海水密度为1 028.48 kg/m3。

当物体具有一定速度地通过流体时, 在物体的支撑下, 周围的流体也会加速。因此, 会产生方向相反的力施加在物体上, 即附加质量力, 它可以由为一个6×6 的矩阵表示。由于附加质量是物体表面几何形状的函数, 所以对于刚体的6×6 惯性矩阵, 其性质各不相同。例如惯性矩阵不一定是正定的。

以沿xb方向的线性加速度所产生的水动力为例,定义:

同样地, 可定义所有与6 个力/力矩分量[X,Y, Z,K,M,N]T和6 个线性/角加速度相关的其余元素, 这些元素可以被分组到附加的质量矩阵MA中。对于完全浸入水中的物体则有MA>0, 并且对于理想液体(无杂物), 物体移速较慢且无海浪或洋流时有[21]:

附加质量也增加了科氏力和向心力, 可以用矩阵表示为:

物体完全浸入水中, 运动速度较低, 且具有水下航行器常见的3 个对称面, 则可以考虑以下矩阵MA和CA的结构:

其中[u,v,w]与[p,q,r]分别对应船体的位置[X,Y,Z]与航向角[φ,θ,Ψ]方向上的速度。

对于质量为m, 长度为L, 截面半径为r的圆柱形刚体, 其附加质量系数可选取为:

在[20]中可以找到关于圆柱在流体中运动的附加质量效应的详细的理论和实验讨论, 它表明附加质量矩阵是状态相关的, 它的系数是圆柱运动距离的函数。

流体的黏度也会对物体产生耗散阻力和升力,即为式(1)中的D（q,q˙）q˙ 项。一种常见的简化方法是只考虑线性和二次阻尼项, 并将这些项组合成一个矩阵D（q,q˙ ）, 并有:

这个矩阵的系数也被认为是常数。对于完全浸入水中的物体, 可以做以下进一步的假设

需要注意的是当假定阻尼矩阵为对角结构时就意味着忽略耦合耗散项。

2 滑模自抗扰控制器

系统(7)是一个高维不确定系统。为了方便进行控制设计, 根据其具体维数将UVMS 分成m个子系统, 针对每一个子系统, 将建模误差, 外部干扰和其他子系统的耦合影响等集总为总扰动, 设计LESO进行估计, 再利用估计误差建立滑模面, 构造滑模控制器。在此基础上得到针对整个 UVMS 的SM-ADRC 设计方法。

2.1 线性扩张观测器

以系统(7)中的第一个子系统为例:

式中为ξ1子系统(25)的广义总扰动, 包含了内部的不确定性和外部未知干扰, 假设其未知但有界。则系统(26)可重新写作:

现将总扰动ξ1扩展为系统的状态变量x3= ξ1,同时假设其微分有界, 并定义=w, 则可以列出所对应的二阶系统的状态空间表达式:

基于状态方程(29)可以建立如下观测器:

式中l1,l2,l3为观测器增益, 加入扩张状态量的观测器(31)称为LESO。若观测器增益选为:

可使得s3+β1s2+β2s+β3满足Hurwitz 条件。而且可以通过改变β来调整特征根的位置[13], 如将其配置于s3+ β1s2+ β2s+ β3= (s+ ω1o)3, 则有:

式中ω1o为第一个子系统的LESO 的观测器带宽。

2.2 LESO 收敛性及估计误差分析

将(31)中观测器增益取为(33)中对应值, 在做拉氏变换, 可以求得状态变量的估计值z1,z2,z3的传递函数:

定义观测器跟踪误差为:

因此可以解出误差的传递函数:

一般性地将式(37)中输入输出Y1(s),U1(s)取为幅值为K的阶跃信号即Y1(s)=K/s,U1(s)=K/s来对系统的稳态进行分析, 则可求得稳态误差为:

再考虑LESO 的估计误差的动态特性。由(30)和(31)可得LESO 对系统(9)的跟踪误差方程:

假设作用于此系统的扰动有界, 定义式(39)的状态转移为[22]:

其中，e= [e1,e2,e3]T,η= [η1,η2,η3]T,Λ= diag[,1]T。将(41)代入(39)可得:

通过2.1 节的配置已使得Ao的特征值均具有负实部:

因此由式(42)不难得出当ω1o增大时总扰动项ξ1的影响会随之减小, 且LESO 的跟踪误差逐渐减小。

2.3 子系统的滑模变结构控制器

引理1[23]对于V:[0,∞)∈R不等式:

同样以UVMS 的第一个子系统为例, 并基于上节所述的总扰动项有:

定义系统期望输出为y1d, 且假设y1d有界。LESO 估计误差为:-y1d, 定义滑模面为:

式中c>0。

基于LESO(31)可建立如下滑模控制器:

针对系统(47), 选择自抗扰滑模控制器(48)、(49)中的参数k>1/2, 则其闭环系统的跟踪误差将收敛至某邻域, 且其收敛速度与LESO 的参数ωo和k有关。

证明:

引入Lyapunov 函数:

由引理1 可解得:

由于V(t) =s2>0, 所以当t→∞有:

可以看到当LESO 对扰动的跟踪误差逼近于0,k足够大时:

因此当k>1/2 时跟踪误差eˆ 逐渐收敛到邻域D= ｛≤ φ｝且其收敛速度取决于LESO 增益ωo和滑模控制器增益k。

由以上可知系统的稳定性与收敛速度由滑模参数k和观测器参数ωo决定。其余子系统的控制器及其稳定性可由同样的方法建立与验证。

2.4 微分跟踪器设计

考虑当输入信号为非光滑信号时, 避免出现数字计算中的高频振荡, 和为状态变量的提取安排过渡过程, 采用积分链式微分器提取输入信号的微分,通过每层的积分环节对扰动进行抑制。该有限时间收敛二阶阶微分器(TD)为:

式中v0(t)为输入信号, 状态量x1在加速度的限制≤r之下, 能快速的跟踪输入信号v0(t), 且r越大,其跟踪速度越快, 同样当x1充分的接近v0(t)时, 另一状态量x2(t) =x˙1(t)可作为输入信号的近似微分。

2.5 UVMS 的SM-ADRC 设计

综上, 完整的滑模自抗扰控制结构图由图2 给出,图中yd1与yd2分别为参考输入的跟踪信号与其微分信号。其他8 个子系统可以以同样的方法建立控制器, 针对AUVMS 的完整SM-ADRC 的结构图如图3 所示。

图2 SM-ADRC 结构图Fig. 2 Structure of SM-ADRC

图3 UVMS 的SM-ADRC 结构图Fig. 3 Frame of SM-ADRC for UVMS

3 仿真结果与分析

基于UVMS 的工作特点, 在本节对带有两连杆(连接关节n=3)的UVMS 的航行轨迹, 机械臂旋转轨迹进行跟踪实验, 测验控制器对UVMS 的航向角切换频率要求。并且将实验结果与传统滑模控制, 自抗扰控制进行对比。

在仿真实验中, 9 个子系统的LESO 的参数分别为ωo=[150, 80, 135, 420, 556, 894, 240, 779, 368]。微分跟踪器参数r均在集合[20, 30]取值 , 滑模控制器中参数c在集合[2, 3]中取值, 参数k=[45, 36, 22, 44,59, 71, 46, 59, 88] 对应每个子系统。定义UVMS 的初始位置为原点, 即[X, Y, Z] =[0, 0, 0]。为了验证控制器对位置的快速变化的跟踪能力, 将UVMS 的机械臂关节角度在0 到5 秒内期望角度变化轨迹设定为:

并同时考虑UVMS 的位置快速变化时的跟踪效果和航向角变化状态。为了使仿真效果尽可能的接近水下环境, 作者将外部干扰设计为一阶高斯-马尔科夫模型来模拟洋流的运动学特征。其表达式如下:

式中vc为设定的洋流速度,αc和βc为洋流的运动方向,εc、εα和εβ为较小的正实数,ωv、ωα和ωβ分别为不同频率的高斯白噪声。

在图4 和图5 中可以看到, 滑模自抗扰控制器可以很好地避免传统滑模控制的抖振现象, 实现更精确的跟踪轨迹, 并且在位置陡然变化时能较传统线性自抗扰控制器和滑模控制器更快的跟踪位置变化,有更平稳快速的响应过程, 图5 中[Ex,Ey,Ez]分别指代对UVMS 的位置[X,Y,Z]的跟踪误差。在图6 中可以看到3 个控制器都能跟踪机械臂的摆动角度轨迹进行但在图7 中, [Eθ1,Eθ2,Eθ3]分别对应在各个控制器的控制下关节角的跟踪误差, 从中可以看出, 机械臂关节角度轨迹跟踪结果显示滑模自抗扰控制器能更快更精准地跟踪机械臂摆动的轨迹, 在不同工况(机械臂不同的摆动频率)下均能达到非常良好的轨迹跟踪效果, 跟踪误差小于LADRC 与SMC, 显示了SM-ADRC 优良的鲁棒性。为了公平地验证所设计的控制的控制效果, 在仿真中3 个控制器的所产生的控制输入在图8 给出, 控制输入的消耗在一定程度上可以反映控制对能量的消耗, 如电机的耗电量。在图8 中可以看到与传统滑模控制与LADRC 相比, 要达到图 4～图7 的控制效果, 文中所设计的SM-ADRC所许需的控制输入量是最少的, 这说明了SM-ADRC 可以依靠更小的能量消耗而达到更好的控制效果, 具有更高的工业化的能力。

图4 UVMS 位置变化轨迹Fig. 4 Trajectory of UVMS’s position

图5 UVMS 位置跟踪误差Fig. 5 Error in the UVMS’s position tracking

图6 机械臂关节角度轨迹跟踪结果Fig. 6 Angular trajectory tracking of manipulators

图7 UVMS 机械臂期望关节角轨迹的跟踪误差Fig. 7 Error in tracking the UVMS manipulator angular Trajectory

图8 不同控制器的控制输出对比Fig. 8 Comparison of the control outputs of three controllers

4 结论

作者提出的滑模自抗扰控制器具有良好的响应过程, 并通过线性扩张观测器对扰动的实时估计,很大程度上消除了传统滑模控制器的抖振现象。通过滑模控制器代替传统线性自抗扰控制器中的PD控制器, 在系统输入信号快速切换时能保持整个系统的稳定, 并且控制过程中的超调现象大幅度减小,此外, 作者所设计的控制器与传统滑模控制器和线性自抗扰控制器相比能用更少的控制量达到更好的控制效果, 这在一定程度上保证了所设计的控制器的可进行实际工业化。最后通过仿真实验验证了提出的控制器有效性。