提综臂辅助旋铆机械手动力学建模及控制仿真

梁 栋，梁正宇，杨建成，徐振宇

（1.天津工业大学 机械工程学院，天津 300387；2.天津工业大学 天津市现代机电装备技术重点实验室，天津 300387；3.江苏金龙科技股份有限公司，江苏 苏州 215500）

多臂开口织机（简称为多臂织机）通过多臂实现开口，其在剑杆织机、喷气织机等无梭织机中应用广泛，常用于较为复杂的小花纹织物的织制，而提综臂则是多臂织机上实现凸轮开口的一个核心部件[1-2]。现阶段对提综臂的装配加工，主要采用旋铆工艺，由人工将提综臂相关零部件放至旋铆机上进行铆接。因人工操作效率低且误差较大，故存在铆接的产品一致性差、质量不稳定的问题。为实现该工艺的自动化，提高生产效率，保证加工质量，本文关注多臂织机提综臂的辅助旋铆加工，并针对提综臂的部分加工过程，采用先进并联机器人技术，提出新型辅助旋铆并联机器人。在此基础上，重点关注机器人的高效动力学控制策略，旨在保证机械手实现高速、高精度地抓放动作。

相比于传统串联机器人，并联机器人具有速度快、刚度大、结构稳定、运动惯性小、承载能力强、误差累积小等特点[3-4]。然而，并联机器人因其存在非线性闭环约束，其关节间的耦合作用较强。因此，并联机器人也具有控制难度大的特点，简单的电机位置控制（不考虑动力学因素）难以保证并联机器人在高速下的运动精度。对此，李占贤[5]提出基于伺服电动机的控制模型，通过增加速度前馈等环节，实现对并联机器人的准确控制。除此之外，基于逆动力学模型的控制方法因其良好的控制性能而备受青睐，设计者可以基于数学模型设计其相应的控制器来实现对机器人的运动控制，例如计算力矩控制[6-7]、神经网络控制[8]、模糊控制[9-10]、滑模控制[11]、模糊滑模控制[12]、模糊神经控制[13]等。动力学模型是动态特性分析和控制的基础，对于刚体动力学模型的建立，采用的方法则主要有牛顿-欧拉法[14-15]、虚功原理法[5-7]、拉格朗日方法[3-16]、KANE方法[17]等。在控制仿真实验的方法选择上，文献[18]采用Simulink与SimMechanics相结合的方法进行控制仿真实验，文献[19]采用Simulink与ADAMS联合仿真的方法对控制参数进行整定。

以提综臂的高效自动化加工为背景，本文采用并联机器人技术，提出一种辅助旋铆机械手，实现将提综臂零部件从传送带快速放至专用夹具，并对该机械手开展刚体动力学分析、控制策略设计及虚拟仿真实验，为其样机制造及实际应用奠定基础。首先，建立单支链的闭环矢量方程，并进行位置和速度分析；其次，基于拉格朗日方法建立机构的逆刚体动力学模型；接着，采用计算力矩控制方法，设计一种任务空间运动控制律；最后，借助ADAMS与Simulink平台开展联合虚拟仿真，整定控制参数。

1 辅助旋铆机械手结构简介

为实现多臂织机提综臂的自动化旋铆加工，提出一种辅助旋铆并联机械手，其主体运动执行机构的SolidWorks虚拟样机如图1所示。为便于观察，图1仅示出主要运动执行机构，其余附件未示出。该机构包含左、右两条相同支链，各支链均由一组平行四连杆机构驱动，保证了中间肘部关节拥有与驱动关节一致的运动规律，便于实现运动控制。两条支链各由一台伺服电动机驱动，且两个驱动转轴相互平行。各支链中肘部连接架与动平台则由一组4U（U表示虎克铰）子链相连接，其余均由转动副连接。

图1 机构示意图Fig.1 Diagram of mechanism

虎克铰的布置：不失一般性，动平台形状设计为正方形，其对角线上的虎克铰的固定转轴两两平行，相邻的固定转轴两两垂直。动平台上4个虎克铰的空间排布形式如图2所示。中间肘部关节上虎克铰的固定轴线，两两相互垂直，与杆件另一端连接动平台的虎克铰上的固定轴线各自平行。由文献[20]可知，该机构具有y、z两个方向的移动自由度。

图2 动平台上虎克铰的布置Fig.2 Collocation of hook joints on moving platform

根据生产工艺要求，设计结果应达到如下目标：在最大加速度为3g的工况下，y、z2个方向的误差小于1 mm。由此设计的辅助旋铆机械手可以实现在多臂机提综臂旋铆加工过程中将零部件快速、精准地放至专用夹具，进而有效提高生产效率，保证加工质量。

2 动力学模型的建立

2.1 运动学分析

由于机构是依靠平行四连杆驱动，且相对布置的两杆相互平行，具有相同的运动规律。为简化分析，以驱动杆、肘部连接架、单被动杆和动平台构造闭环矢量链，如图3所示。以静平台中心点O为原点，建立惯性坐标系O-XYZ，设驱动杆与Y轴夹角为θi，G点坐标为[0yz]T，i为支链序号，且当i=1，sgn（i）=-1；i=2，sgn（i）=1，可得到以下矢量：

图3 闭环矢量链Fig.3 Closed-loop vector chain

由图3可建立闭环矢量方程，将所有杆件视为刚性杆，对向量DE求模，得到如下位置约束方程：

2.1 .1 位置逆解

对式（2）进行化简，可得

2.1 .2 位置正解

位置正解对于并联机构的控制具有重要意义，其通常需借助数值迭代法（如牛顿迭代法[21]）进行求解。由于牛顿迭代法对初始点的要求较高，若给定的初始点位置与实际初始点位置偏离较大，则可能会导致无法求得正确解，甚至无法收敛。因本文机构的结构特殊，可以得到其解析正解，这将有助于后续基于任务空间的高效控制策略设计。

对式（2）展开，分离出变量y、z，得到以下方程：

当i=1、i=2时，根据式（5）可得2个二元二次方程，联立消元整理后可得

根据一元二次方程求根公式解得

2.1 .3 速度分析

对约束方程式（2）关于时间求导得

由此可得输入与输出之间的运动映射关系为

对式（10）做矩阵变换，得到

式中：J为机构的运动雅可比矩阵，且

2.2 刚体动力学分析

刚体动力学建模旨在给出机构的尺度和惯性参数的条件下，建立驱动关节输入力（力矩）与末端动平台输出运动之间的映射关系，它是评价机构动态操作性能、物理样机设计、伺服电机选型以及运动控制策略设计的基础。本文采用Lagrange方法建立机器人的刚体动力学模型。

2.2 .1 结构简化及能量计算

由于求解完整的动力学模型较为复杂，且考虑到后续控制器运算的高效性，对部分杆件做适当简化，对一些非线性项进行忽略，在此利用以下假设：

（1）运动副为理想的，无因摩擦而引起的能量耗散；各杆件为刚体，不考虑在高速运动下惯性力引致的弹性变形。

（2）考虑到被动杆为轻质杆，忽略其转动时带来的惯性力，其质量按静力等效原则，以其质量的1/2分别等效到被动杆两端。

取驱动杆转轴平面为零势能面，以下为对单条支链各部分的能量分析。动平台的动能：

动平台的势能：

式中：m1为动平台质量；g为自然重力加速度（取9.8 m/s2）

主动杆组（包括驱动杆与辅助连杆）动能：由于驱动杆与辅助连杆具有相同的运动规律，故主动杆组的动能可写为

式中：I21为驱动杆绕基座转轴的转动惯量；I22为辅助连杆绕基座转轴的转动惯量。

主动杆组的势能：

式中：m21为驱动杆的质量；m22为辅助连杆的质量；r21为驱动杆质心到其转轴中心的距离；r22为辅助连杆质心到其转轴中心的距离；h为驱动杆转轴中心与辅助连杆转轴中心的z向垂直距离。

中间关节的动能：

中间关节的势能：

式中：I3为中间关节的绕驱动轴的转动惯量；r3为中间关节质心到驱动轴的距离；m3为中间肘部关节的质量。

2.2 .2 建立系统的逆刚体动力学模型

考虑到机器人机构为含闭环约束的多体系统，采用第一类Lagrange方程：

式中：L为Lagrange能量函数；qi（i=1，2，…）为广义坐标；τi为广义力；fj为第j个约束方程；λj为第j个Lagrange乘子。选取系统广义坐标向量为：

根据机器人闭环矢量方程，以杆长为约束，可列得如下约束方程：

将简化的模型代入动能表达式，求取系统总动能为：

系统总势能为：

将上述式子分别代入式（19），求得各广义力：

广义力τ1：

广义力τ3：

因机构仅有主动关节处安装伺服电机，故τ3=0且τ4=0，将其代入式（26）和（27）得到方程组：

对式（28）方程组进行消元，求得拉格朗日乘子，其可表征机构的广义约束力：

式中：|J1|表示矩阵J1的行列式。

将求得的拉格朗日乘子代入上面动力学方程组，整理得到：

式中：

3 动力学控制策略

与一般机械系统一样，当机器人结构及其相关参数确定后，其动态特性可由系统动力学方程来描述。因此，可通过基于系统数学模型设计相应的控制器来实现对机器人的运动控制。对于具有强耦合性、非线性的机械系统而言，PD（P表示比例系数，D表示微分系数）反馈控制是最为简单且行之有效的控制方法[7]。为实现高速下的机器人运动控制，本文计及系统动力学因素，对辅助旋铆机械手设计具有PD补偿和非线性补偿的计算力矩控制器。其中，PD补偿是对惯性项进行补偿，非线性项是对重力项、离心力与科氏力项进行补偿。在实际应用中，计算力矩实质上是通过求出驱动电机的输出力矩，进而通过改变电机的输出电压（电流）实现对机械系统的精确控制。

在本文研究中，暂不考虑摩擦力带来的影响，后期可在样机调试中对这部分力矩进行辨识/补偿。

根据系统逆动力学方程，可以得到驱动力矩与末端执行器运动的映射关系为：

在输入力矩中，离心力与科氏力项以及重力项，均采用理论值作为补偿，惯性项则采用PD进行补偿，设计任务空间控制输入力矩为：

式中：e=xd-x；Kp为位置增益对角矩阵；Kd为速度增益对角矩阵；x为末端执行器实际位置向量；xd为末端执行器理论位置向量。

假设控制输入力矩与驱动力矩相同，可以得到：

上式化简后得到一个关于时间的常系数二阶齐次线性微分方程：

根据常系数二阶齐次微分方程解的形式可知，误差函数可由指数函数线性表达。因此，通过调整PD参数，可使误差渐进趋近于0，即末端轨迹的控制精度可以得到保证。控制原理框图如图4所示。

图4 计算力矩控制原理框图Fig.4 Diagram of computed torque control（CTC）

4 基于Simulink与ADAMS的联合仿真实验

为了验证设计的动力学控制策略的合理性与有效性，以及便于直观地观察机器人控制效果与运动状况，现结合控制策略与虚拟样机技术进行联合虚拟仿真。通过联合仿真，可以在虚拟环境下调试并发现设计中存在的问题，并及时对控制参数做出修正与调整，降低样机研发成本，为未来样机制造提供理论依据和技术储备。

在此通过MATLAB软件中的Simulink模块与ADAMS软件中的Control模块实施联合虚拟仿真实验。主要思路是：在Simulink中按照图4搭建控制模型，起到模拟控制器的作用；在ADAMS软件中导入机器人的SolidWorks虚拟样机，起到模拟机构运动的作用。最终搭建的Simulink-Adams联合控制仿真模型如图5所示。

图5 Simulink-ADAMS联合控制仿真模型Fig.5 Simulink-ADAMSintegrated control simulation model

根据工作任务需求，设计“门字形”轨迹作为末端执行器的运动轨迹，并采用修正梯形加速度运动规律[4]，取最大加速度amax＝3g。仿真中，给出机器人的尺度及质量参数如下：l1=0.06 m，l2=0.44 m，l3=0.07 m，l4=0.09 m，l5=0.515 m，l6=l7=0.060 m，r21=0.185 m，r22=0.22 m，r3=0.465 m；m1=0.90 kg，m21=1.16 kg，m22=0.10 kg，m3=1.03 kg，I21=0.06 kg·m2，I21=0.11 kg·m2，I22=0.006 5 kg·m2，I3=0.22 kg·m2。通过试凑法，整定控制参数为：Kp=diag（5 000，5 000），Kd=diag（2 000，2 000）。需要说明的是，由于该模型忽略了系统中的摩擦力等影响，最佳控制参数还需在实际中进行调试，在此仅是提供一个参考的范围。

根据ADAMS输出的仿真动画，可以看到联合仿真下，机构在不同时刻的运动状态如图6所示。

图6 机构在不同时刻的运动位姿Fig.6 Poses of mechanism in different time

将仿真实验结果导出，通过理想轨迹与仿真轨迹的对比，可得到机械手运动过程中末端执行器位置跟踪及误差的变化情况，分别如图7和图8所示。主动关节控制输入力矩如图9所示。

结合图7和图8发现，机械手在y向的跟踪误差控制在0.3 mm以内，在z向的跟踪误差也能控制在0.65 mm以内。由于z向仅是控制机械手的上下移动，并不影响y向的定位，所以使用该控制策略，理论上满足设计要求。另外由图9可看出，控制力矩峰值大小为37.82 N·m，且力矩在轨迹跟踪过程中波动平缓，无振荡和突变，这将有利于实际控制。综上，对于高速运动的辅助旋铆并联机械手而言，利用设计的任务空间计算力矩控制方案，可以获得较高的运动精度以及较好的控制性能，同时亦为其他高速并联机器人的运动控制提供了技术参考。

图7 末端执行器轨迹对比Fig.7 Comparison of trajectory of end-effector

图8 末端执行器位置跟踪误差Fig.8 Position tracking errorsof end-effector

图9 主动关节控制力矩Fig.9 Control torques of active joints

5 结论

（1）在对辅助旋铆机械手结构分析基础上，构造了单支链的闭环矢量链。运用闭环矢量法，建立了单支链的位置约束方程，得到了位置正逆解的解析表达式；通过求导法，建立了速度映射模型。

（2）基于第一类Lagrange方程，并结合系统闭环约束方程，建立了机械手的刚体动力学模型，其中的Lagrange乘子可表征系统广义约束力。通过消元，进一步得到了驱动力矩与末端执行器参考点运动之间的函数映射关系。

（3）设计了任务空间计算力矩控制策略，考虑了由PD补偿来实现对惯性项的补偿，由离心力、科氏力项以及重力项的理论值作为非线性补偿。经推导可知，选择合适的PD参数，可使跟踪误差渐进趋近于0。

（4）搭建了Simulink-Adams联合虚拟仿真模型。仿真实验结果表明，在最大加速度为3g时，设计的任务空间动力学控制策略可使末端执行器的y向跟踪误差在0.3 mm以内，z向的跟踪误差也能控制在0.65 mm以内，且控制力矩波动平缓，符合设计预期。需指出，本文给出的PD参数仅为参考值，实际应用时仍需结合实体样机进行调试试验。