多质心有限质点法及其在工业机器人动力学建模的应用

刘目珅， 张飞斌， 王天杨， 褚福磊， 程卫东， 刘佑民

(1.清华大学 机械工程系，北京 100084； 2. 北京交通大学 机械与电子控制工程学院，北京 100044； 3. 北京航天发射技术研究所，北京 100084)

工业机器人动力学建模能够更好地获取工作过程中的动态特性，是分析机器人末端抖动，提高定位精度的理论基础，因此建立一个能够精准满足实际工况的动力学模型能为机械系统动态性能的评估和优化设计提供强有力的理论和技术支撑[1]。在工业机器人实际生产过程中，工业机器人关节处电流激励频率与杆件固有频率重合导致的工业机器人末端抖动现象时有发生，这就需要考虑工业机器人杆件柔性的精准建模，为有效抑制工业机器人末端抖动提供理论机理。当前工业机器人建模方法主要有Lagrange、Newton-Euler以及Kane法，但是以上方法多基于工业机器人杆件刚性假设，若对机器人模型考虑杆件柔性进行精准建模，常常会面临建模流程推导困难，求解效率低等问题[2]。

有限质点法[3]是一种基于向量式有限元[4-6]和固体力学，对结构进行数值分析的方法，该方法将模型在物理空间上分为有限个具有质量的质点，在时域上，将时间分解为一系列途径单元的集合，在每一个途径单元内结构的分析都是连续和线性的，对于质点采用基于牛顿运动学的控制方程描述运动状态。有限质点法通过逆向运动的概念分离单元刚体位移和节点纯变形，进而计算出虚拟的单元节点内力，再通过正向运动得到真实位置的单元节点内力，因此在本质上有限质点法是将几何线性和非线性都使用简便统一的框架进行求解[7]，在求解结构柔性变形上更有优势。

对工业机器人建模采用实体单元结构能够更真实的反映结构响应，有着更好的适应性和准确性。然而基于六面体单元的有限质点法[8-9]的逆向运动步骤较为繁琐，涉及到多次投影、求模以及向量夹角计算等。有限质点法以单元任一节点作为逆向平移参考点、逆向转动旋转中心，并且逆向转动参考平面未能够充分反映单元节点信息，不能充分将单元刚体位移与单元节点纯变形分离，尤其是工业机器人这种刚体大转动-柔性变形耦合模型，单元刚体位移对求解纯变形的影响更为严重，使部分节点内力值产生较大偏差，促使单元内其余节点内力与之对应，以达到单元内力平衡状态，因此在整个收敛过程中，单元节点内力值波动性增加，收敛性变差，甚至会造成节点内力值发散的情况。尽管Hou等[10]已经对基于六面体单元有限质点法的逆向运动步骤进行了有效的简化，但是未考虑逆向运动参考点和参考平面的选取对于节点内力收敛性的影响。

本文提出的多质心有限质点法与有限质点法相比采用了全新的逆向运动步骤，将平面外、内转动修改为形式相同、步骤简单的两次逆向转动；将单元平面质心作为逆向运动参考点，让每一个单元平面质心都参与到逆向转动所需的参考平面构造中，有效降低单元刚体位移对节点纯变形求解的影响，提高了对工业机器人这种刚体大转动-柔性耦合模型计算的收敛性。多质心有限质点法不同于传统有限单元法以能量法和连续介质力学作为理论基础，而是基于向量式结构力学[11]，以牛顿运动定律对质点的轨迹进行描述，对结构实现了真正意义上的离散，在本质上更适合处理工业机器人动力学问题。

基于本文方法对简单的悬臂梁模型编写MATLAB程序，将模型结果与理论公式以及ABAQUS软件计算结果对比，验证本文理论和算法的正确性。进一步将本文方法结合正运动学引入到工业机器人动力学建模中，基于MATLAB将建模程序与GUI工具箱建立联系，构建工业机器人动力学建模软件，将建模结果与实验数据相对比，验证本文方法对工业机器人动力学建模的准确性和实用性。

1 多质心有限质点法基本理论

1.1 单元节点位移

多质心有限质点法通过一系列质点和途径单元的集合，使用基于牛顿运动学的控制方程描述质点运动，是一种真正意义上的离散。对于结构的行为可以使用一组运动方程描述

(1)

结合显式中心差分公式，将运动方程改写为

(2)

式中：d为单元节点位移向量；n为迭代步数；h为步长； 节点集中载荷P=F+f。

当迭代步数n=0时，也就是运动时间t=t0=0时，对应第-1步虚拟的位移向量d-1为

(3)

接下来的迭代步(n≥1)都是以式(2)作为单元节点位置计算的控制方程，每一步计算所需要的节点内力f可以根据单元节点纯变形计算得出。

1.2 单元逆向运动

接下来在一个途径单元tn～tn+1时刻内对多质心有限质点法逆向运动步骤详细描述，对应迭代步数为第n步～第n+1步。

图1 六面体单元运动和变形示意图Fig.1 Motion and deformation of a hexahedral element

1.2.1 逆向平移

将tn+1时刻单元以单元质心位置矢量un+1为参考点整体逆向平移至tn时刻单元质心位置矢量un上，如图2所示，经过逆向平移的tn+1时刻单元节点记为a′-b′-c′-d′-e′-f′-g′-h′，结点i位置矢量d′i为

图2 逆向平移后的单元位置Fig.2 Element position after reverse translation

(4)

1.2.2 第一次逆向转动

将经过逆向平移的单元水平方向上的3个平面质心连接，构成三角形参考平面1′-4′-6′，其法向量为Vec′1，同理，tn时刻单元的参考平面1-4-6，其法向量为Vec1。构造的单元参考平面及其法向量，如图3所示。

图3 单元参考平面及其法向量示意图Fig.3 Reference plane and its normal vector of an element

Vec1与Vec′1的夹角θA及其法向量nvecA分别为

(5)

(6)

计算出第一次逆向转动的转动矩阵R′(-θA)

R′(-θA)=[1-cos(-θA)]V′2+sin(-θA)V′

(7)

以tn时刻的单元质心位置矢量un为旋转中心对节点a′-b′-c′-d′-e′-f′-g′-h′进行第一次逆向转动，如图4所示。获得新的单元节点位置为a″-b″-c″-d″-e″-f″-g″-h″，单元结点i位置矢量d″i为

图4 经第一次逆向旋转的单元位置Fig.4 Position of the element after the first reverse rotation

d″i=R′(-θA)d′i,i=a,b,c,…,g,h

(8)

1.2.3 第二次逆向转动

将经过第一次逆向转动的单元竖直方向上其余3个平面质心连接构造三角形参考平面2′-3′-5′，其法向量为Vec′2，同理，tn时刻单元的参考平面2-3-5，其法向量为Vec2。其余逆向转动步骤与第一次逆向转动相同，Vec2与Vec′2的夹角及其法向量分别为：θB和nvecB，第二次逆向转动的转动矩阵为：R″(-θB)。经过第二次逆向转动的单元位置及以tn时刻单元质心为原点的局部坐标系，如图5所示。单元节点位置为a‴-b‴-c‴-d‴-e‴-f‴-g‴-h‴，位置矢量d‴i为

图5 经第二次逆向旋转的单元位置及其局部坐标系Fig.5 Position of the element after the second reverse rotation and its local coordinate

d‴i=R″(-θB)d″i,i=a,b,c,…,g,h

(9)

(10)

需要特别说明的是，在整个逆向运动过程中，逆向平移是以单元质心为参考点整体逆向平移，逆向转动中单元节点都是绕同一旋转向量旋转相同的角度，保证了六面体单元的各个节点之间不会发生相对位移。将本文推导过程与有限质点法进行对比，主要差别如表1所示。由表1可知，本文方法有效降低了计算的复杂度，简化了计算流程。

表1 本文方法与有限质点法复杂度对比Tab.1 The complexity of the proposed method is compared with that of the finite particle method

1.3 单元节点内力

单元节点内力通过单元节点纯变形进行计算，计算方法可参考有限单元法[12]。如图5所示，以tn时刻单元质心作为原点建立局部坐标系O′-x1y1z1，坐标轴方向与全局坐标系保持一致，将全局坐标系下的单元节点位置矢量转化到局部坐标系中

(11)

对时刻的单元建立形函数，建立方式和表达式形式与传统有限元方法相同，形函数坐标系记为O″-rst，如图6所示。

图6 八节点六面体单元形函数坐标系Fig.6 Eight-node hexahedral element shape function coordinate

八节点六面体单元的形函数Ni表达式为

(12)

式中：r,s,t分别为形函数表达式参数；ri=±1，si=±1，ti=±1为形函数坐标系内节点i的坐标，i=a,b,c,…,g,h。

将局部坐标系内坐标转换到形函数坐标系中，坐标系的转换通过雅可比矩阵J来实现

(13)

对雅可比矩阵求逆，得到形函数关于局部坐标的导数为

(14)

(15)

(16)

利用单元变形描述的虚功与应力应变描述的虚功相等这一原理建立等式

(17)

式中：Va为形函数积分参数； δ为变分符号。

(18)

(19)

式中，R′(θA)和R″(θB)分别为第一次和第二次正向运动转动矩阵。

1.4 步 长

对结合显示中心差分推导出的控制方程来说，步长的大小尤为重要，一般在问题的分析中，对步长的要求主要有两方面：①物理行为方面，在结构分析过程中，使用足够的数据点来满足计算精度的要求；②数值计算方面，差分公式是函数微分的一种近似计算，必然有一定误差，所以要避免迭代计算中的误差累计造成的数据发散。

根据上述对步长的要求得知，步长必须要小于一个步长临界值hd，才能得到一个收敛、准确的计算结果。然而模型类型不同其步长临界值也各异，并且精确求解临界步长也是困难的。丁承先等以简单的杆单元为对象，提出了一种有效的估算临界步长值的方法。

1.5 质量和加速度

六面体单元质量分布遵循一个原则：将单元质量均分到各个节点上，再采用集成的方式就可得到单元节点质量，此时的单元节点只考虑质量，忽略节点尺寸[13]。

单元节点的加速度同样可以通过差分的形式给出

(20)

2 结构算例

2.1 悬臂梁模型

为验证本文提出的多质心有限质点法的准确性，现以简单的悬臂梁模型为例，基于本文方法使用MATLAB软件进行编程，对结构的变形进行分析。

使用六面体单元对悬臂梁模型划分网格，网格数量为320个，单元节点为525个，如图7所示。悬臂梁长、宽、高分别为0.5 m，0.1 m和0.1 m。材料类型为弹性，弹性模量为E=206 GPa，密度为Rho=7 800 kg/m3，泊松比为rp=0.29。分析时间TE=0.05 s，步长为h=1×10-6s，末端集中载荷P=-3 N，采用斜坡-平台方式缓慢加载，加载时间rtime=0.005 s，并使用阻尼系数收敛计算结果，阻尼系数为zeta=100。当前模型变形为小变形，所以依然适用于材料力学理论公式

图7 悬臂梁六面体单元模型Fig.7 Hexahedral elements of cantilever model

(21)

式中：L为悬臂梁的长；I为截面惯性矩， 对于正方形截面，I=B4/12，B为正方形截面的边长。

沿悬臂梁中线各节点的竖直方向的位移结果，如图8所示。横坐标为中线各节点与固定端的距离，纵坐标为中线各节点竖直方向上产生的位移，可以看出竖直方向上位移与材料力学理论公式及采用C3D8R六面体单元的ABAQUS软件的计算结果基本保持一致。

图8 悬臂梁中线各节点竖直方向位移结果Fig.8 Vertical displacement results of each node in the middle line of cantilever beam

本文方法和有限质点法对悬臂梁自由端中心节点竖直方向位移变化过程的收敛性对比，如图9所示。以理论公式结果为标准值，在相同网格数量前提下，本文方法、有限质点法、理论公式以及ABAQUS软件的自由端中心节点竖向方向位移的对比结果，如表2所示。

表2 结果对比Tab.2 Results contrast

图9 本文方法和有限质点法对悬臂梁自由端中心 节点竖直方向位移变化过程的收敛性对比Fig.9 Convergence comparison between the method in this paper and the finite mass method on the vertical displacement change process of the center node of the free end of the cantilever beam

由以上结果证明本文方法有着良好的准确性和收敛性。

2.2 工业机器人动力学模型

首先对建模对象RB08A3机器人进行实验，发现存在明显抖动现象，在工业机器人连杆4末端安装加速度传感器，测得运动过程中执行末端加速度数据，并记录J2，J3关节电流变化值，如图10所示。

图10 工业机器人及加速度传感器布置位置Fig.10 Industrial robot and acceleration sensor placement

为分析抖动现象的原因，首先将关节电流数据分别做频域分析，结果如图11所示。

图11 关节电流频域图Fig.11 Frequency domain diagram of joint current

对实验测得的加速度数据进行频域分析，可知机器人抖动是因为在13.5 Hz和27.5 Hz左右的电流激励频率和工业机器人固有频率重合。下面采用多质心有限质点法对工业机器人进行动力学建模，建模步骤如图12所示。

图12 工业机器人动力学建模流程图Fig.12 Flow chart of industrial robot dynamics modeling

工业机器人结构基本参数，如表3所示。使用六面体单元对机器人划分网格，建立的物理模型如图13所示，所需单元个数为960。

表3 RB08A3机器人结构参数Tab.3 RB08A3 robot structure parameters

图13 工业机器人物理模型Fig.13 Physical model of industrial robot

设定机器人动力学建模基本参数： 运动时间为 2.0 s，迭代步长为1/3 125 s，单元材料密度为7.8×103，弹性模量为206 GPa，泊松比为0.29，根据单元的连接类型，确定单元节点质量，运动状态为J2和J3关节转动，其余关节保持固定。

为方便计算，引入固定的整体坐标系和随机械臂旋转的坐标系[14]，结合模型的单元与节点信息，以及关节输出角度变化值，根据正运动学求解出机器人模型刚体运动的位置，然后对模型使用多质心有限质点法进行分析，得到执行末端变形量。进而将变形量叠加到执行末端，重复上述步骤的计算，直至分析时间达到运动时间。使用式(20)可以根据执行末端位置求解出末端加速度变化。

基于MATLAB软件将工业机器人动力学建模程序与MATLAB软件中GUI工具箱连接，搭建工业机器人动力学建模软件，软件主界面如图14所示。

图14 软件主界面Fig.14 Main interface of software

进而对仿真测得的执行末端z和y方向上的加速度数据频域分析，并与实验数据对比，对比结果如图15和图16所示。

图15 建模与实验在z方向上的加速度频域图Fig.15 Modeling and experiments in the z direction acceleration frequency domain diagram

图16 建模与实验在y方向上的加速度频域图Fig.16 Modeling and experiments in the y direction acceleration frequency domain diagram

在z方向上的建模结果具有13.5 Hz和28.5 Hz，相对实验结果14.0 Hz和27.5 Hz的误差分别为-3.6%和3.6%。在y方向上的建模结果具有13.5 Hz，相对实验结果14.0 Hz的误差为-3.6%。根据以上结果可以得知，建模与实验结果在低阶固有频率基本重合，高阶能量趋势基本相同。

3 结 论

本文提出多质心有限质点法，将单元质心不仅作为逆向平移的参考点，也作为逆向转动的旋转中心，使每个单元平面都参与到参考平面的构造中，有效减弱了单元刚体位移对节点纯变形求解的影响，进而解决了工业机器人这种刚体大转动-柔性变形耦合模型中因单元节点纯变形计算偏差，造成的节点内力值收敛性差的问题。

与传统有限质点法相比，多质心有限质点法的两次逆向转动，除选取的参考平面节点不同以外，都是采用简单的平面外转动的旋转方式，大幅度减少了逆向运动过程中的向量求模、叉乘、投影等步骤次数，有效简化了计算流程，使得整体框架更为统一。

对简单悬臂梁模型使用多质心有限质点法自编程序建模分析，并将分析结果与有限质点法、理论公式和ABAQUS软件分析结果对比，结果表明本文方法有着良好的准确性和收敛性。

将多质心有限质点法结合正运动学对工业机器人动力学建模，通过MATLAB软件将建模程序与GUI图形化界面结合，构造基于多质心有限质点法的工业机器人动力学建模软件，并将建模结果与实验结果相对比，结果表明使用本文方法的建模结果能够有效模拟出工业机器人因电流激励频率与固有频率重合产生的抖动频率。