七功能机械手非线性鲁棒控制方法研究 - 图文

浙 江 大 学 学 报（工学版）

学科代码：460?45

七功能液压机械手多输入多输出非线性鲁棒控制

方法研究

罗高生 顾临怡

（浙江大学流体动力与机电系统国家重点实验室，浙江省 杭州 310027）

摘 要：针对液压驱动的七功能主从液压机械手具有强非线性、未知外界干扰以及机械手多刚体系统与液压驱动系统强耦合的多输入多输出控制系统特性，提出了利用backstepping控制器设计方法和基于滑模控制的多输入多输出鲁棒控制器方法，使用李雅普诺夫稳定性定理证明闭环系统具有渐近稳定的控制性能。利用4500米深海主从液压机械手作为仿真模型和实验对象进行仿真和实验研究表明，所提出的控制方法具有很好的跟踪性能和鲁棒性。

关键词：七功能液压机械手；多输入多输出鲁棒控制；Backstepping；李雅普诺夫稳定性 中图分类号： 文献标识码：

Research on Nonlinear Robust Adaptive Control of

Double-screw-pair Driven elbow of 7-fun hydraulic manipulator

LUO Gao-sheng GU Lin-yi

（The State Key Lab of Fluid Power Transmission and Control, Zhejiang University, Hangzhou 310027, China）

Abstract：for control characters of double-screw-pair driven elbow of 7-fun hydraulic manipulator with strong

nonlinearity of hydraulic system, parameter uncertainties by temperature and pressure change of outer environment and unknown outer disturbance, a full state feedback robust adaptive control method was presented, with the combination of backstepping controller design method and robust adaptive method, and with the advantage of Lyapunov stability theory to prove the asymptotic stability of the control system with uncertainty disturbance. Simulation and experiments shows that the presented full state feedback robust adaptive controller has satisfied control character and robustness.

Keywords：elbow of 7-fun hydraulic manipulator; full state feedback robust adaptive control; backstepping;

Lyapunov stability

功能、六自由度液压机械手是一个高度耦合的多输入多输出非线性系统，各关节的运动力相互作用、相互耦合；机械手系统中的柯氏力和离心力与各关节的速度成平方关系，具有很强的非线性；六自由度机械手从手的多刚体系统再加上各关节的液压驱动系统，机械从手系系统与液压系统相互强耦合；另外，机械手的多刚体模型是非常的复杂，各关节的惯性张量、关节所受的摩擦力等均无法精确得到，因此很难对液压驱动的机械手的多刚体系统进行精确建模。

未加入执行器动态的机械手机械手控制系统已

经有相当的研究了，如基于PD的控制方法[103~105]，计算力矩的控制方法[106]，基于无源的控制方法[107][108][109],自适应控制方法[110]和鲁棒控制的方法[111][112]。但是如果加入执行器的动态，那么系统的阶数由原来的二阶系统变成了四阶系统，而且执行器的动态和机械手多刚体系统相耦合，因此系统的控制难度增加了，这些控制方法也就不再适用了。

本文针对液压机械手的建模不精确性和具有外界未知的扰动的系统特点提出全状态反馈多输入输出非线性鲁棒控制方法以解决深海七功能液压机械手从手的轨迹跟踪鲁棒控制问题。所提出的基于滑模控制的方法可以解决机械手从手建模的不精确和存在外界

基金项目：国家高技术研究发展计划（863计划）4500米级深海作业系统，项目编号：2008AA092301 通讯作者：罗高生，E-mail: kendysnow@zju.edu.cn

月

未知干扰的问题，利用backstepping[]解决液压驱动系统和机械手系统的耦合控制问题。

本文主要分为三个部分，第一部提出七功能液压机械手模型及其模型特点；第二部分讨论了七功能液压机械手的多输入多输出鲁棒控制方法；第三部分和第四部分分别以4500米深海主从液压机械手作为仿真模型和实验对象进行仿真和实验研究以验证所提出控制方法的跟踪性能和鲁棒性。

1 七功能液压机械手从手系统建模

按照拉格朗日能量法建立的七功能机械手多刚体系统的动态方程[]如下：

ddt(?K?q?)??K?q???G (1) 即：

M?q??V(q,q?)?Mq???C(q,q?)q????G (2)

其中：

V(q,q?)?D(q)?q?2??B(q)?q?q??(3) 其中K为机械手多刚体系统的总动能，q?R6x1为机械手的广义关节角度坐标变量；M(q)?R6X6为质量矩阵；D(q)?R6X6为离心力系数矩阵；B(q)?R6x15为

柯氏力系数矩阵；?q?2??R6X1广义关节角速度平方项；?q?q???R15X1为广义关节角速度交叉乘积项。 液压机械手多刚体系统控制模型具有以下特性[121]：

性质6.1 惯性矩阵正定对称，即M(q)?M(q)T，因此其逆M(q)?1存在；

性质6.2 ?Mm，MM，使得Mm?M(q)?MM??；

性质6.3 C(q,q?)q?中的元素为q?TNi(q)q?，Ni(q)对称有界的矩阵。即Ni(q)??；

?性质6.4 ????Rnx1，使得?TM?(q)?2C(q,q?)??0； 性质6.5 C(q,q?)具有右侧线性交换的特性，即C(q,x)y?C(q,y)x，?x,y?Rnx1，q?液压机械手的关节空间；

性质6.6 ?CM?0，C(q,x)?CMx， ?q,x?Rnx1；

性质6.7 ?TM?0，使得G(q)?TM， ?q?Rnx1；

性质6.8 ?q?M?0，使得q??q?M； 液压系统对液压机械手多刚体系统的驱动力矩为：

??d(q)(A1P1?A2P2) (6-4)

其中，P1，P12?R6x，为分别对应于机械手各关

节驱动油缸或者马达两腔的油液压力；

A1?diag(A1i)，A2?diag(A2i)，i?1..6为机械手各驱动油缸或者马达两端的有效作用面积,

d(q)?diag(d6x6i)?R 是机械手各关节执行器对机械手相应关节的驱动力臂矩阵，力臂矩阵具有如下性质：

性质6.9 ?dM?0，dm?0使得dm?d(q)?dM，?q?机械手的关节空间；

液压缸的力学模型

假设不考虑液压缸的内漏和外漏，机械手各关节执行器压力腔和回油腔的动态方程[82]分别为为：

V1(x)?P?1??A1x??Q?x1??A1q??Q1 (6-5) e?qV2(x)?P?2?A2x??Q?x2?A2q??Q2 (6-6) e?q其中，x?R6x1，为油缸活塞位移或者马达的角位移，且油缸活塞的位移与关节角度关系为：

x(q)??x6x1，

i?1..6，且?x??x??q?daig?i??q??R6x6i(qi)??R；i?V1(x)，V2(x)?R6x6为电液比例阀到液压缸或马达两容

腔的有效容积，分别为

V1(x)?Vh1?A1diag?xi? V2(x)?Vh2?A2diag?xi?

其中Vh1?diag?Vh1i??R6x6，i?1..6

Vh2?diag?Vh2i??R6x6，i?1..6，它们是液压机械手各执行器的压力腔和回油腔的的初始容积；Q1，

月

V1(x)和V2(x)Q2?R6x1为流入和流出油缸腔体的流量，

具有以下性质：

性质6.10 ?VM?0，Vm?0，使得

Vm?Vj(x)?VM，j?1..2，?x?机械手各关节操作空间范围内各执行器的位移。

说明 6.1 深海液压机械手从手驱动中肘关节和手腕旋转关节是由摆动油缸和摆线液压马达驱动的，可通过修正相应的系数，以使得他们的驱动方程与油缸的驱动方程一致。

比例阀的模型

电液比例阀在不考虑比例阀的死区时其模型[82]为

Q1?Kq1g1(P1,sgn(xv))xv

Q2?Kq2g2(P2,sgn(xv))xv

(6-7)

V1(x)??x??Q1??A1??Q1 P1??A1xq?e?q

(6-12b)

V2(x)??x??Q2?A2??Q2 P2?A2xq

?e?q

(6-12c)

(6-8)

其中，xv?R6x1为阀芯的位移；Kq1，Kq2?R6x6，Kq1?diagKq1i，Kq2?diagKq2i，

????，，g2(P2,sgn(xv))?R6x6g1(P，1,sgn(xv))?diag?g1i(P1i,sgn(xvi))?g2(P2,sgn(xv))?diag?g2i(P2i,sgn(xvi))?，i?1..2。

g1(P1,sgn(xv))??Ps?P1ig1i(P,sgn(x))??1ivi?1i?Pr?P

xvi?0xvi?0假设6.1 ??M?0，使得???M。

?为外界干扰和海水对深海液压机械手运动阻尼产生的干扰，由此可知假设6.1是合理的假设。

由方程(6-12)可知由于液压系统的存在，使得系统的阶数由原来机械手刚体系统的2阶扩大为4阶系统，液压系统与机械系统相互耦合，系统具有很强的非线性。传统的线性系统设计方法基本上不可能满足

?，P1，P2大范围变化的要求，通过坐状态变量q，q标变化的反馈线性化的方法也满足不了系统的要求。

本节以Backstepping作为设计方法，通过适当定义几个Step之间的虚拟控制量进行控制器的反演设计。同时，考虑到机械手从手系统受到的外界干扰和模型建模不精确问题，采用滑模控制的方法，将模型的不精确和外界干扰抑制，并采用参考模型输入的方式，通过设置参考模型状态的初始值，使闭环控制系统跳过滑模过渡阶段，并使系统的跟踪速度更快收敛。

i?1..6

Step 1

以机械手从手系统方程(6-12a)作为控制目标，设计控制器使得状态q渐进收敛于qd。

定义?为虚拟控制，并定义如下虚拟控制量：

(6-9) ??P?Prxvi?0g2i(P2i,sgn(xvi))??2i??Ps?P2ixvi?0i?1..6

(6-10)

其中，Ps为液压系统的供油压力；Pr为液压系统的回油压力。

阀芯位移与输入电压之间的关系为： xv?Kuu

(6-11)

其中Ku?diag?Kui??R6x6， Ku为正定对称的对角矩阵，u?R6x1的控制电压输入矢量。

~?d(q)(AP?AP)?d(q)P?d(q)(P?P)???d??1122LLdL

(6-13a)

~~为期其中，?d为期望的机械手从手控制力矩，?望力矩与实际控制力矩的误差。

~PL?A1P 1?A2P2?PLd?PL

(6-13b)

2 深海液压机械手从手多输入多输出全

状态反馈鲁棒控制方法研究

具有外界干扰的深海七功能液压机械手总的系统动态方程为：

???C(q,q?)q????G(q)???d(q)(A1PM(q)q1?A2P2)?G??

(6-12a)

PL为等效的控制压差，PLd为期望的等效控制压

~差，PL为期望等效控制压差与等效控制压差的误差。

并且，使以下控制量的等式成立：

?d?d(q)PLd

即，

(6-13c)

PLd?d?1(q)?d

(6-13d)

月

则有：

P~?1L?d(q)?~

(6-13e)

由性质6.9可知， d?1(q)是存在的，因此由??~d和

可以得到PLd和P~L。

定义如下变量：

~跟踪位置误差， q?q?qd

(6-14a)

相对参考速度误差，

s?~q???~q?q??q?r

(6-14b)

其中，qq?r为相对参考速度：r?q?d??~

q

(6-14c)

其中~q，q?r，s?R6x1分别为位置跟踪误差、参考速度、参考速度误差，??diag??i?，i?1..6，是正定的对角矩阵，为控制器设计变量。由方程(6-14b)可知，s包含了跟踪位置误差和跟踪速度误差变量，它是位置跟踪误差和速度跟踪误差的综合体现，如果

s等于0，那么就可以选择?，使得~q?，~q以指定的指数收敛速度收敛到0。

将系统(6-12a)方程两侧减去?M?q?r?Cq?r项，得到

(6-12a)方程的变体方程：

M(?q??q??r)?C(q??q?r)???Mq??r?Cq?r?G?? (6-15a)

将方程(6-14b)带入方程(6-15a)中，得到：

Ms??Cs???Mq??r?Cq?r?G??

(6-15b)

对(6-15b)系统取李雅普诺夫函数为：

V?112sTMs

(6-16)

对V1求时间导数则有：

V?1?sTMs??1T?2sMs (6-17)

带入系统方程(6-15b)，得到：

V?1?sT(??Mq??1r?Cq?r?G?Cs??)??2sTMs (6-18a)

V?1?sT(??Mq???)?1r?Cq?r?G??2sT(M?2C)s (6-18b)

由性质6.4得到12sT(M??2C)s?0，(6-18b)方程则为： V?1?sT(?d?Mq??r?Cq?r?G)?sT??sT?~

(6-19)

取以下期望的控制律：

?d?M?q??r?C?q?r?G??Ksgn(s)

(6-20)

其中，M

?，C?，G?是对M，C，G的估计。 K?diag?ki?，i?1..6为正定的对角矩阵，是控

制器设计参数；sgn(s)??sgn(si)??R6x1。

将方程(6-20)带入方程中(6-19)中得到：

V?1?sT(M~?q??C~q??G~)?Ksgn(s)?sT??sT?~

(6-21)

其中， M~?M??M,C~?C??C,G~?G??G

由性质6.2，6.6，6.7和假设6.1可以得到：

V?1???si(ki?M~?q?~q?~r?Cr?G~i??Mi)?sT? (6-22)

取控制参数：

k~~i?M?q?q?~r?Cr?Gi??i??Mi

(6-23)

其中????i?，i?1..6是可以选择的正的列矢量。 则有：

V?1???s??sTii?~，i?1..6

(6-24)

如果sT?~?0时，即sTd(q)P~L?0，有： V?1??sT??0

(6-25)

由LaSalle-Yoshizawa定理可知：

s?0

(6-26)

月

将方程(6-26)带入方程(6-14b)中，可以得到q?qd，系统渐近稳定。

此时有：

QLd?(A1V1?1A1?A2V2?1A2)?x1?~??q(PLd?KpLPL?d(q)s)?q?e?q??Ksgn(s)?q??r?C?r?GPLd?d?1(q)?d?d?1(q)M

Step 2

?? (6-34)

(6-27)

其中，KpL?diagKpLi，i?1..6为正定的对角矩阵，是控制器设计参数。

将控制律(6-34)带入系统方程（6-32）中，得到：

??~~?0，即sTd(q)P在这一步中，为了使sT?L?0，

~也就是只要使PL?0，这样就可以使s?0，并且获得实际的控制输入。

~对PL?PL?PLd两端求时间导数，得：

~~?PL??KpLPL?d(q)s

(6-35)

~???P PL?PLLd

(6-28)

~???AP?? PL?(A1P122)?PLd (6-29)

取系统(6-12)的李雅普诺夫函数为：

11~~V?V1?sTMs?PLPL

22

(6-36)

对系统的李雅普诺夫函数（6-36）取时间导数，

并带入方程（6-24）、（6-35），得到：

说明6.2 为防止PLd不可微分，这个可以采用

?kskitanh(ii)代替kisgn(si)，但此时的系统不是全局

?渐进稳定而仅是一个局部稳定的系统，系统仅收敛于一个六维超球域。 ~???AP?? PL?(A1P122)?PLd~T~???V??PVLPL 1

(6-30)

?s????s???iiii~~~~?sTd(q)PL?PLTKpLPL?PLTd(q)s~~ ?PLTKpLPL

(6-37)

?0带入系统的压力动态方程(6-12b)，(6-12c)到方程

(6-30)中，可以得到：

~由LaSalle-Yoshizawa定理可知s?0,PL?0，将它带入系统方程(6-14b)和(6-13d)中，可以得到

???x?x?1?1?11~??AP???A2V2(x)A2????eA1VA1Pqq(xq)Q??A2V2?(x)Q2?0q，系统渐近稳定。 122???e?A1V1(x)A11?d1?q?q??由流量方程(6-7)，(6-8)可知，xv与流量具有相同的符号。因此，由方程(6-7)，(6-8) ，(6-33)，(6-34)，(6-31)

(6-11)可以解得：

将方程(6-31)带入方程(6-30)中，得到：

?1?1?1?x~u?K[AVKg(P,sgn(Q))?AVKq2g2(P2,sgn(QLd)??1?1?1?1u11q111Ld22????eA1V1(x)Q1?A2V2(x)Q2?PLdPL???eA1V1(x)A1?A2V2(x)A2q?q (6-38)

(6-32)

定理6.1 由控制律方程（6-38）、（6-21）所控制

由方程(6-7)~ (6-11)可知，Q1，Q2是与实际输入

的深海七功能主从液压机械手控制可以实现具有未知

u直接联系的，中间没有其他动态，因此，可以定义

干扰和参数不确定的机械手系统（6-12）的全局渐进

以下控制量作为作为实际的控制输入，并通过计算得

稳定控制。

到实际的控制u。

??????QL?A1V1?1(x)Q1?A2V2?1(x)Q2

??

(6-33)

3 深海液压机械手从手多输入多输出全状态反馈鲁棒控制器仿真研究

仿真参数给定

为了使得PL?0，取以下期望的控制律，即：

~

月

仿真中，机械手从手系统采用第六章研究所获得的4500米ROV深海主从七功能液压机械手从手的多刚体系统模型中的液压驱动系统与多刚体系统的机械连接及各关节的液压缸参数。液压驱动系统见第二章图 2.13 从手机械手液压原理图所描述的比例阀阀控缸和阀控马达系统。

机械手从手多刚体系统与液压驱动系统的耦合系数如下列方程(6-39)和(6-40)所描述：

??x1??q?1???x????q?????????????? ?????1??(0.046cos(q1?1.47019)?0.229)2d1(q1)?0.229?10.021068cos(q1?1.47019)?0.054557

d2(q2)?0.3031?

(1.6501(0.240123?0.04242cos(q2?1.0360.096709?0.04242cos(q2?1.0367)(1.61812(0.2477?0.3708cos(q3?2.011))2d3(q3)?0.3091?0.099?0.3708cos(q3?2.011)

?x2?q2?x3?q3106(0.045cos(q5?2.0658)?0.228)2d5(q5)?0.228?19(2280cos(q5?2.0658)?6001)

其中方程(6-38)和(6-39)对角线上的第四个和第六个单元分别对应机械手从手的摆动油缸驱动肘关节和摆线液压马达驱动的手腕旋转关节。

液压缸系统的系数矩阵定义如下述方程(6-41)，(6-42)，(6-43)，(6-44)所描述的系统。

压力腔油缸有效作用面积矩阵为：

1.042e?6?x5?q5 (6-39)

其中，矩阵中的其他单元定义如下：

?x10.010534sin(q1?1.47019)?? ?q10.054557?0.021068cos(q1?1.47019)?x221.21sin(q2?1.0376)? ?q296709?42420cos(q2?1.0376)?x36.18sin(q3?2.011)? ?q311009?4120cos(q3?2.011)?x53.42sin(q5?2.0658)?? ?q56001?2280cos(q5?2.0658)力矩矩阵系数矩阵定义如下：

A1?diag(A1i)?1.07074e-3?2.826e-3??2.826e-3??4.81e-3??1.07e-3?(1e-4?

回油腔油缸作用面积矩阵定义如下：

(6-41)

A2?diag(A2i)?1.38474e-3?2.022e-3??2.022e-3??4.81e-3??1.3847e-3??

?d1(q1)???d2(q2)????d3(q3) (6-42) d(q)?diag(di)???7.1e?3??从比例阀到油缸压力腔的有效油液容腔矩阵为： ?d5(q5)???

1??

(6-40)

V1(x)?Vh1?A1diag?x(q)?

其中，矩阵中的其他单元定义如下：

月

?A11x1(q1)?1e?5??A12x2(q2)?1e?5??A13x3(q3)?1e?5?

A23x3(q3)?0.00072595544?0.000006066511009?4120cos

A14x4(q4)?1e?5

???A15x5(q5)?1e?5?1e?4q6?1e?5??2??3A24x4(q4)?0.000245?0.00005108(q4??)

4A25x5(q5)?0.00000415426001?2280cos(q5?2.0658)?0.0

1、 其他仿真参数

其中，矩阵中的其他单元定义如下：

(6-43)

表6.1为系统方程（6-12）已知的常量，表6.2为控制器参数。 A11x1(q1)?0.0002864229?0.001070740.054557?0.021068cos(q1?1.47019)表6.1 深海主从七功能液压机械手控制系统从手系统

参数

A12x2(q2)?0.00000282696709?42420cos(q2?1.0367)?0.000688131参数 值

700Mpa A13x3(q3)?0.00000847811009?4120cos(q3?2.011)?0.00072063参数 值 110mA/V ?e Ku

Ps 2.1?107N/m2 Kq 1.852?10?10m3/(N/m23A14x4(q4)?0.00005108(q4??)

4表6.2 深海主从七功能液压机械手控制系统从手控制

器参数

值 diag[100，500,200，50，50，20] diag[2,2,10,10,10,10]

A15x5(q5)?0.00028481684?0.00000321226001?2280cos(q5?2.0658)参数 K 从比例阀到油缸回油腔的有效油液容腔矩阵为： V2(x)?Vh2?A2diag[x(q)] ? ?A21x1(q1)?1e?5diag[100,2000,500,20,20,10] KpL ??A22x2(q2)?1e?5?A23x3(q3)?1e?5 ??A24x4(q4)?1e?5?2、 深海液压机械手从手多输入多输出全状态

?A25x5(q5)?1e?5反馈鲁棒控原理图 ?(1e?4)?(1e?5)?(1e?4)/(2*pi)*q6??控制原理图见附件图A.5~A.8。由于控制系统较

其中，矩阵中的其他单元定义如下：

(6-44)

为复杂，没有将所有的控制原理图一一列上，如有需

要可另行提供。

3、 仿真曲线

A21x1(q1)?0.001384740.054557?0.021068cos(q1?1.47019)?0.00026033仿真中分别对不具有外界扰动图6.1~图6.4和具有外界扰动图6.5~图6.8的情形分别进行了仿真和对比。另外，仿真中使用了说明6.2的方法代替控制器

A22x2(q2)?0.000737077?0.00000202296709?42420cos(q2sgn?1.0367)??0.09。 中的函数，边界层厚度

月

图6.1 位置跟踪曲线

图6.2 位置跟踪误差

图6.3 控制输入电压

月

图6.4 控制输入力矩

手腕摆动关节处增加100Nm外界干扰负载时的仿真曲线：

图6.5 手腕摆动关节增加100Nm外界干扰力矩时的位置跟踪

曲线

月

图6.6 手腕摆动关节增加100Nm外界干扰力矩时的跟踪误差

图6.7手腕摆动关节增加100Nm外界干扰力矩时的控制输入电压

图6.8 手腕摆动关节增加100Nm外界干扰力矩时的控制输入力矩

由图6.1和图6.2系统位置跟踪效果较好，图6.3和图6.4可知系统的控制输入平滑，对比图6.1~图6.4和图6.5~图6.8可知，在受到外界扰动时，控制器具有较好的鲁棒性。

全状态鲁棒自适应控制器试验及结果分析

实验设备和控制器参数

1、 实验装置

实验装置采用4.3.3.1节相同的实验平台。

2、 系统参数和控制器参数

系统采用表6.1中的系统参数，其余的系统参数为方程(6-38)~(6-41)所描述，控制器参数见表6.3， 表6.3 深海主从七功能液压机械手控制系统从手控制

器参数

参数 K ? 值 diag[700，900，800，600,500] diag[5,5,20,10,20]

本文来源：https://www.bwwdw.com/article/7f33.html