多自由度液压机械臂的虚拟分解建模及轨迹跟踪控制

钟凯澎，张 翼，邓 华

(1.中南大学 高性能复杂制造国家重点实验室，长沙 410083；2.中南大学 机电工程学院，长沙 410083)

0 引言

液压机械臂具有结构紧凑，负重比大，灵敏度高和抗干扰能力强等优点，被广泛应用于各类基础建设和维抢修作业工程中[1]。但多自由度液压机械臂的数学模型复杂，各关节的运动学和动力学耦合严重，增加了各关节控制器设计的难度。

多自由度液压机械臂一般具有闭链式结构和非独立轴系驱动形式[2]，一般通过忽略液压缸和传动杆等刚体的动力学特性，将系统简化为开链连杆形式再进行建模[3]。此方法可简化动力学建模过程，但忽略液压缸和传动杆的影响，会增加建模误差和影响控制器设计及控制性能。Ding等人提出利用拉格朗日原理和螺旋理论，对一种锻造操作机工作装置进行系统建模，该方法可以有效反映闭链结构的动力学特性[4]。但基于广义笛卡尔坐标系所建的数学模型中，各关节状态之间耦合严重，各关节的控制器很难独立设计，需要考虑各关节控制的相互作用，控制器参数调整困难。而且，惯性矩阵和科氏力矩阵均为时变矩阵，会导致正逆动力学的运算增多。有学者表明，基于广义笛卡尔坐标系下的串联空间机械系统模型在某些状态下存在惯性矩阵奇异的情况[5]，故以该类模型为基础的仿真可能存在误差。另外，对于液压驱动系统而言，不能忽略关节间和液压缸的摩擦力对系统带来的误差影响[6]。

近年，Zhu提出了一种针对多自由度，多闭链结构机器人的建模方法--虚拟分解理论[7]。该方法基于关节坐标系建模，避免了惯性矩阵奇异的情况，有效减少了正逆动力学的运算次数[8，9]。特别是采用虚拟分解法建立的动力学模型具有各关节运动解耦的形式，简化了控制器设计的难度，可直接针对各关节独立设计控制器[10，11]。

本文基于虚拟分解理论，对一种三自由度液压机械臂进行运动学、动力学和电液比例系统建模，并整合得到系统的各关节解耦的动力学模型。在此基础上针对每个关节独立设计PID控制器并进行了轨迹跟踪控制实验，结果验证了所提方法的有效性。

1 三自由度液压机械臂的虚拟分解

通过设置虚拟分解点，将三自由度机械臂虚拟分解为5个子系统，包括2个连杆子系统，3个闭链子系统，如图1(a)所示。分解后可用导向图表示分解系统，如图1(b)所示。

图1 工作装置虚拟分解示意图

闭链1、3、5子系统结构类似，可将其进一步分解为两个开链结构并联的子系统，如图2所示。

图2 闭链结构分解图

虚拟分解后，对各子系统刚体标记对应的固定坐标系，用于描述运动和力的方向。在每个闭链系统的两端增设两个坐标系

用于描述闭链内两条开链的合力作用，如图3所示。

图3 系统坐标系示意图

设坐标系{A}为图3中各刚体对应的坐标系。参照坐标系{A}，刚体的速度和角速度矢量分别为Av∈R3，Aω∈R3，则表示该刚体的速度/角速度矢量为：

同理，表示该刚体的所受到力矢量和转矩矢量分别为A f∈R3，Am∈R3，则力/转矩矢量为：

2 运动学

在同一个液压闭链系统内，角度和连杆边长的定义如图4所示。

图4 闭链内部关节角度关系

关节角qi1、qi2、qi3和xi1之间的关系可以为：

求得各闭链内液压缸长度与闭链关节角之间的关系后，所有分解后的刚体的速度矢量可以从基座推导到末端。该处基座的速度矢量为已知值，记为B0V=[0 0 0 0 0 0]T。因此，连杆与闭链间的速度转换可表示为：

式(4)中AUB表示速度和力矢量参考从坐标系{B}到坐标系{A}的转换矩阵。

则闭链内各连杆的运动速度可表示为：

式(5)中，xf=[1 0 0 0 0 0]T，zτ=[0 0 0 0 0 1]T。

末端的速度/角速度矢量可用相同方法求解，由于篇幅问题，不再一一讨论。

3 机电液耦合动力学

3.1 虚拟作用力

在上述章节中已通过运动学推导出被分解后各刚体的速度/角速度矢量。当刚体在自由运动时，参考自身坐标系{A}的合力/转矩方程为：

其中，MA∈R6×6为质量矩阵，CA∈R6×6为科氏加速度和离心加速度矩阵，GA∈R6为重力矩阵，详细求解过程可参考相关文献[6]。

根据虚拟分解理论，连杆和闭链间的虚拟作用力有以下关系：

由于闭链1、2结构类似，可用相同方法求解各个虚拟作用力。在同一闭链系统内，杆li1和杆li2、杆li3和杆li4的合力/转矩方程可通过公式(5)求得。设Tiη=[xiyi0 0 0zi]T为两条开链间的内部作用力，它的方向是从油缸指向连杆，并且以坐标系{Tic}为参考。闭链的内力作用有如下关系：

其中，α1+a2=1恒成立。根据油缸闭链的力驱动特性，有三个虚拟力被动，有：

联立式(7)和式(8)解方程可求得Tiη。故在不考虑摩擦等情况下，油缸的驱动力为：

为了使末端的位姿有更大的调整空间，与于闭链1和闭链2不同，闭链3在结构上增加了双摇臂结构，故单独列出闭链3的虚拟作用力求解过程。由图1(b)可知，两个零质量连杆此时只起到力传递的作用，设故各连杆有以下关系：

其中fF表示末端所受到的外部作用力。系统内共有6个虚拟力被动，即：

3.2 电液比例控制系统模型

由于三个驱动单元的工作原理相同，因此仅对其中一个电液比例控制系统展开讨论。阀控缸系统的基本方程如式(14)所示[12]：

式(14)中：i=1，3，5为第i个闭链，Kuq为电压流量增益系数，ui为输入电压，PS、PT分别为进油压力和回油压力，βe为油液的体积弹性模量，A1、A2为油缸大小腔的有效面积，Vt为油腔的等效容积，Qi1、Qi2为流进大小腔内的流量，Pi1、Pi2为大小腔内的压力，S(*)为符号函数。

对于液压机械臂而言，关节间已含多个以被动副形式存在的运动副，因而要整合所有运动副的摩擦力模型是十分困难的。因此，将所有的被动副摩擦力等效到主动副上[13]，获得一个等效的摩擦力模型，即：

式(15)中，fc为库仑摩擦系数，fv为粘性摩擦系数，fs为最大静摩擦系数。

忽略缸内液压油质量的影响，根据牛顿第二定律可得：

因此对于某一驱动连杆，动力学方程有：

3.3 输入输出的表达

式(17)的动力学模型大大降低了系统耦合的问题，其他关节的耦合影响均集中在作用力上。当考虑单个系统的运动时，连杆的速度矢量可表示为：

而对于空间坐标系下的动力学模型[3]如式(20)所示：

式(20)中=[θ1θ2θ3]T为简化后的连杆关节角。由于质量矩阵M(θ)，科氏加速度矩阵C(θ)，重力矩阵G(θ)是θ和θ的非线性函数，各关节的控制存在较强的耦合，控制器很难针对每个关节单独设计大大增加了控制器设计的难度。

因此采用式(19)的各驱动关节动力学模型的解耦形式，只需考虑系统存在一定扰动的情况下独立设计各驱动关节的控制器，很大程度上简化了控制器设计的难度。

4 控制器设计

PID控制器具有结构简单，易于实现，和鲁棒性强等优点，其广泛应用于各类工业控制系统。而PID控制器的难点在于其参数获得和优化问题，特别对于多关节运动耦合比较严重的系统。PID参数一般是通过半经验半实验的方式获得，但由于液压机械臂的工作区域大，功率输出大，如直接在实验样机上操作，容易因操作不当导致事故。因此，准确的机电液动力学模型可以为控制器参数的选取提供一个可靠的仿真平台。

PID控制算式采用如下离散形式[11]表示：

其中，e(k)为控制偏差，KP为比例系数，KI为积分系数，KD为微分系数。

在设计控制器时，基于式(19)的数学模型，首先针对各个驱动关节设计合适的初始位置和运动速度，通过所建动力学模型计算出各个驱动连杆的驱动力。然后根据驱动力的范围确定fk的上下界，并将fk看成扰动。采用试凑法获得各驱动单元的PID参数。在试凑时，对参数调整实行先比例、再积分，后微分的整定步骤。在获得每个独立PID控制器参数后，再进行基于完整模型的控制仿真，结合实验，分别调整出三组品质较为良好的PID参数，各驱动单元的PID参数如表1所示。

表1 各驱动单元的PID参数

5 模型验证与控制实验

5.1 模型验证实验

本文的试验样机为某中小型反铲式挖掘机器人。在上述的建模过程中，关于系统建模所涉及到的结构及参数，可以借助Adams、SolidWords等辅助仿真软件进行估算或辨识得到。本实验采用了模拟量输出型拉线传感器测量液压缸长度，传感器精度为0.02%FS，采样周期为100Hz。

下面将分别对实验样机的三个机电液耦合动力学模型进行验证，即单独对闭链1、3、5系统对应的比例阀输入电压信号。实验前先确定阀芯电压死区，各单元输入的对应电压信号表2所示，系统模型预测输出和液压缸实际位置输出比较如图5～图7所示。

表2 各闭链的输入电压

图5 闭链1系统模型验证

图6 闭链3系统模型验证

图7 闭链5系统模型验证

从图5～图7可以看出，所建立的机电液耦合动力学系统数学模型能较好地反映该挖掘机器人各关节的输入和输出情况。

5.2 轨迹跟踪实验

为了模拟实际工况，规划一组末端轨迹进行实验。通过几何关系和逆运动学的方法，推导出和各驱动单元的轨迹和末端轨迹，如图9～图11中实线线条所示，实际跟踪效果用虚线表示。

图8 闭链1系统轨迹跟踪

图9 闭链3系统轨迹跟踪

图10 闭链5系统轨迹跟踪

图11 末端轨迹跟踪

从关节空间位置跟踪图9～图11可知，闭链3和闭链5单元的跟踪轨迹效果良好，仅有较小滞后，三个驱动油缸系统之间仍然存在一定的液压耦合问题，导致油源会先流向压力较小油缸的现象，故闭链1驱动单元的跟踪轨迹存在一定的误差。从末端轨迹跟踪图11可知，系统可以良好地跟踪目标轨迹。

6 结语

1）本文对三自由度液压机械臂进行虚拟分解建模，并结合液压系统模型和摩擦模型获得系统的各关节解耦的机电液耦合动力学模型。该模型能考虑到液压缸等结构的动力学特性，使数学模型更为准确。

2）动力学模型验证实验表明，采用虚拟分解法建模，良好地反映液压机械臂的动力学特性，可以为后续的控制工作提供一个可靠的仿真模型。

3）对所建模型进行整合优化，获得了各关节驱动缸独立的动力学模型，控制解耦，使得控制器设计更为简单。

4）针对单关节独立设计了三个PID控制器并进行了末端轨迹跟踪控制实验，结果表明末端能有效跟踪目标位置。所方法可应用于其他多闭链机构，为多闭链机构的动力学建模与控制提供参考。