CN111597657B - 一种旋转关节型工业机器人模态参数和振动响应计算方法 - Google Patents

Abstract

本发明公开了一种旋转关节型工业机器人模态参数和振动响应计算方法，具体涉及旋转关节型工业机器人的振动特性分析与计算。主要包括：将工业机器人整体结构化整为零，拆分成具有传递关系的若干个元件；建立各个元件的传递矩阵和传递方程；根据元件传递链形成系统总传递矩阵和传递方程；根据首末元件的状态矢量确定系统边界条件；求解系统特征方程得到工业机器人各阶固有频率和固有振型；利用增广特征矢量正交性和模态叠加原理，求解机器人振动响应。本发明的机器人动力学建模与振动分析方法无需系统总体动力学方程、程式化建模程度高、系统矩阵阶次低、计算速度快，能够对工业机器人振动特性和响应进行精准描述和快速计算。

Description

一种旋转关节型工业机器人模态参数和振动响应计算方法

技术领域

本发明属于工业机器人动力学分析技术领域，具体涉及一种旋转关节型工业机器人模态参数和振动响应计算方法。

背景技术

工业机器人作为生产加工的重要工具和设备，其性能与动态特性紧密相关，从设计生产到使用，对工业机器人进行动态特性分析至关重要。尤其是近年来，随着机器人加工系统越来越多地被应用于航空航天高端制造领域，特别是在加工钛合金、镍基合金、复合材料等难加工材料时，机器人加工系统极易产生颤振，影响制造加工质量，严重时甚至会导致产品损坏，造成不可挽回的损失。因此，需要对机器人加工系统进行振动特性研究，计算其模态参数和振动响应，分析机器人加工系统的动力学性能，为制定工艺路线和加工颤振抑制策略提供理论指导。

在工业机器人结构设计过程中，常采用有限元仿真、模态分析等方法进行结构优化，已公开发明专利“一种工业机器人大臂结构优化方法，CN110705170A”发明了一种工业机器人大臂结构优化方法，对拓扑优化后的模型进行仿真模态分析，验证结构优化成果。该方法依赖于仿真软件进行分析优化，需要不断对结构进行修改与调整，设计优化周期长，且有限元分析依赖软件自带算法，无法根据实际需求对其进行改进计算。为了实现机器人的振动抑制，发明专利“一种空间柔性机械臂振动抑制算法，CN201610549912.3”在广义坐标系下建立机械臂系统的动力学模型，分解了动力学变量并设计了挠性振动控制器以实现振动抑制，该方法需要建立机械臂整体动力学模型，建模方法复杂，计算速度慢。综上所述，现有工业机器人振动特性分析存在结构优化依赖仿真分析、动力学方程复杂、振动响应计算速度慢等不足。本发明针对上述缺点，提出了一种旋转关节型工业机器人模态参数和振动响应计算方法。

发明内容

本发明所要解决的技术问题是针对上述现有技术的不足，提供一种旋转关节型工业机器人模态参数和振动响应计算方法。

为实现上述技术目的，本发明采取的技术方案为：

一种旋转关节型工业机器人模态参数和振动响应计算方法，包括如下步骤：

步骤一：提取待建模的工业机器人系统的主要结构元件，包括机械连杆和转动关节，并对各元件进行统一编号；

步骤二：建立物理坐标和模态坐标，定义物理坐标和模态坐标下的各个转动关节和机械连杆的状态矢量；

步骤三：建立机械臂杆件和转动关节的动力学模型，根据机械臂杆件和转动关节的基本参数，分别计算各机械臂杆件和转动关节的传递矩阵和传递方程；

步骤四：结合各机械臂杆件和转动关节的传递矩阵和传递方程，计算工业机器人的总传递矩阵和总传递方程；

步骤五：根据工业机器人实际边界特性，计算工业机器人边界条件；

步骤六：确定系统特征方程，求解特征方程得到工业机器人各阶固有频率与振型；

步骤七：根据工业机器人增广特征矢量，利用模态叠加原理和增广特征矢量正交性，计算得到机器人振动响应。

为优化上述技术方案，采取的具体措施还包括：

进一步地，步骤一中，将机械连杆等效为空间刚体元件，将转动关节等效为空间弹性铰元件，定义边界点为零，对转动关节与机械连杆按照实际安装顺序依次编号。

进一步地，步骤二中，各个转动关节和机械连杆在物理坐标、模态坐标下的状态矢量分别为：

式中，x、y、z表示物理坐标下惯性坐标系中联接点相对于其平衡位置的线位移；θ_x、θ_y、θ_z表示物理坐标下惯性坐标系中联接点相对于其平衡位置的角位移；m_x、m_y、m_z表示物理坐标下联接点处的内力矩；q_x、q_y、q_z表示物理坐标下联接点处的内力；X、Y、Z，Θ_x、Θ_y、Θ_z，M_x、M_y、M_z，Q_x、Q_y、Q_z分别为对应小写字母物理量的模态坐标，i,j表示相邻元件序号。

进一步地，步骤三具体为：

建立各转动关节的传递矩阵和传递方程，确定第j个转动关节输入端为I_j，输出端为O_j，第j个转动关节传递方程表示为：

Z_j,O＝U_jZ_j,I

式中，Z_j,O和Z_j,I分别为转动关节j的输出端和输入端的状态矢量，U_j为转动关节j的传递矩阵，

K_x、K_y、K_z为弹簧在x、y、z方向上的平移刚度，

K'_x、K'_y、K'_z为扭簧在x、y、z方向上的扭转刚度；

建立各连杆的传递矩阵和传递方程：

确定第i个机械连杆输入端为I_i，输出端为O_i，建立以输入端I_i为原点的连体坐标系I_ix_Iiy_Iiz_Ii，在连体坐标系中输出端O的坐标为(b₁,b₂,b₃)，质心C的坐标为(c_c1,c_c2,c_c3)，第i个机械连杆传递方程表示为

Z_i,O＝U_iZ_i,I

式中，Z_i,O和Z_i,I分别为连杆i的输出端和输入端的状态矢量，U_i为连杆i的传递矩阵，m为刚体的质量，ω为工业机器人系统的固有频率，

为刚体相对于输入点I的惯量矩阵，

(b₁,b₂,b₃)为刚体输出端O的坐标，(c_c1,c_c2,c_c3)为刚体质心C的坐标。

建立各关节转角对应的坐标变换矩阵：

进一步地，步骤四具体为：

计算工业机器人的总传递矩阵，由沿着系统传递方向的各元件的传递矩阵依次相乘得到：

U_all＝U_2nHx(θ_n)U_2n-1U_2n-2Hy(θ_n-1)U_2n-3…U₂Hz(θ₁)U₁

式中，Hx(θ)、Hy(θ)、Hz(θ)分别为绕x、y、z轴旋转关节角θ的坐标变换矩阵。

得到工业机器人的总传递方程：

Z_2n,0＝U_allZ_1,0

进一步地，步骤五具体为：

根据工业机器人实际边界特性，确定工业机器人边界条件

式中，Z_2n,0为第2n个元件输出端的状态矢量，Z_1,0表示第1个元件的输入端的状态矢量。

进一步地，步骤六具体为，将步骤五中的边界条件代入步骤四中的总传递方程，得到

由于边界条件Z_2n,0和Z_1,0中分别总有一半元素为零，删除对应的零元素化简上述方程，得到降阶的总传递方程

式中，

为删除Z_1,0中的零元素得到的6×1阶状态矢量，

为删除U_all中与Z_1,0中零元素对应的列和与Z_2n,0中非零元素对应的行得到的6×6阶矩阵，O₆为6×1阶全零矢量。

确定系统特征方程为

式中，det[·]表示矩阵的行列式，k为模态阶数；求解该方程即可得到工业机器人的各阶固有频率ω_k，再将各阶固有频率代入机械连杆和转动关节的传递矩阵和传递方程，计算得到各元件在模态坐标下的状态矢量，即可得到各阶固有振型；

进一步地，步骤七具体为，将系统的各阶固有频率ω_k代入各连杆体元件和转动关节铰元件的传递方程即可得到系统任意一点处的状态矢量，将机器人系统中所有机械连杆刚体元件输入端状态矢量中的线位移和角位移模态坐标取出，依次拼接即可得到系统各阶增广特征矢量

其中V_i ^k(i＝1,2,…,n)为第i个机械连杆元件第k阶增广状态矢量，由各体元件输入端状态矢量中的线位移和角位移向量组成的6×1列阵，增广特征矢量为6n×1阶列阵；

在关节处引入阻尼，工业机器人系统的强迫振动体动力学方程为

Mv_tt+Cv_t+Kv＝f

式中，

M为机器人系统质量参数矩阵，K为机器人系统刚度参数矩阵，C为机器人系统阻尼参数矩阵，v为系统的物理位移坐标列阵，f为系统的物理外力列阵，M_i、K_i、f_i分别为第i个机械连杆体元件的质量参数矩阵、刚度参数矩阵、外力列阵，v_i表征第i个机械连杆体元件的运动状态，为由其线位移和角位移组成的位移列阵，下标t表示对时间求导数；

每个体元件的质量参数矩阵和外力列阵为

式中，m_i为各体元件质量，

为第i个体元件质心坐标组成的反对称矩阵，

为体元件受到的外力主矢F_i对输入端I的力矩，D为外力的简化中心，即矩心，J_I,i为第i个体元件相对其输入端I的惯量矩阵；

假设阻尼为比例阻尼，阻尼参数矩阵与质量和刚度参数矩阵成正比，即

C＝αM+βK

式中，α、β为比例常数；

求解系统振动响应

增广特征矢量的正交性为

＜MV^k,V^p＞＝δ_k,pM_p,＜MV^k,V^p＞＝δ_k,pK_p

式中，＜MV^k,V^p＞为系统质量参数矩阵M与第k阶固有频率对应的增广特征矢量乘积与第p阶固有频率对应增广特征矢量的内积；M_p为系统的第p阶模态质量；

为系统的第p阶模态刚度；当k＝p时，δ_k,p＝1，当k≠p时，δ_k,p＝0；

模态叠加原理为

将增广特征矢量的正交性和模态叠加原理，代入工业机器人系统的强迫振动体动力学方程，得到解耦的广义坐标微分方程

式中，q^p(t)为系统广义坐标，

为系统广义速度，

为系统广义加速度，N为模态阶数；

通过数值积分方法，求解系统任意时刻的广义坐标q^p(t)，再将其反代入模态叠加原理方程，即可求解系统状态量的时间历程，即系统振动响应。

本发明的有益效果：

本发明一种旋转关节型工业机器人模态参数和振动响应计算方法，通用性好，建模灵活且程式化程度高，通过对组成工业机器人的每个元件独立建模，分别计算各元件的传递矩阵和传递方程后，按照实际连接顺序传递得到系统的总传递矩阵和总传递方程，同时拼接后形成的总传递矩阵的阶次不随着系统自由度的增加而增大，这就有效提高了计算速度，实现模态参数和振动响应的快速计算。

附图说明

图1工业机器人振动特性分析与振动响应计算的流程图；

图2为工业机器人动力学模型及其拓扑图；

图3为空间弹性铰模型示意图；

图4为一端输入一端输出的空间振动刚体模型示意图。

具体实施方式

以下结合附图对本发明的实施例作进一步详细描述，选用KUKA KR500-2830M型工业机器人，如图1所示：

步骤一：提取待建模的工业机器人的主要结构元件，包括机械连杆和转动关节，并对各元件进行统一编号。

如图2所示，根据工业机器人的各部件自然属性将工业机器人整体结构化整为零，拆分工业机器人的结构模型，提取待建模的工业机器人的主要结构特征，包括机械臂杆件和转动关节，一共分成12个元件，包含6个转动关节和6个机械连杆，将转动关节等效为铰元件，将机械连杆等效为一端输入一端输出的体元件；对工业机器人的12个连杆按照结构原有的组装顺序进行统一编号：0对应所有边界点；1、3、5、7、9、11分别对应转动关节1至转动关节6；2、4、6、8、10、12分别对应机械连杆1至机械连杆6。

步骤二：建立物理坐标和模态坐标，定义物理坐标和模态坐标下的各个转动关节和机械连杆的状态矢量。

工业机器人是由12个元件组成的多自由度系统，系统中共有13个串联点和边界点，在力学系统中，任一点的状态矢量是一个列阵，表示该点的力学状态，其元素包括该点状态变量的线位移、角位移、内力、内力矩，在物理坐标下用z表示，在模态坐标下用Z表示，不同点的状态矢量用下标进行区别；此外，若某阶模态下系统的固有频率为ω，模态坐标和物理坐标可以进行相互转换，z＝Ze^iωt，式中i为虚数单位，状态矢量可以表示为

式中，x、y、z表示物理坐标下惯性坐标系中联接点相对于其平衡位置的线位移；θ_x、θ_y、θ_z表示物理坐标下惯性坐标系中联接点相对于其平衡位置的角位移；m_x、m_y、m_z表示物理坐标下联接点处的内力矩；q_x、q_y、q_z表示物理坐标下联接点处的内力；X、Y、Z，Θ_x、Θ_y、Θ_z，

M_x、M_y、M_z，Q_x、Q_y、Q_z分别为对应小写字母物理量的模态坐标，i,j表示相邻元件序号，如z_1,0表示第1个元件与边界接触点的物理坐标下的状态矢量，Z_2,3表示第2个元件与第3个元件的接触点的模态坐标下的状态矢量。

步骤三：建立机械臂杆件和转动关节的动力学模型，根据机械臂杆件和转动关节的基本参数，分别计算各机械连杆和转动关节的传递矩阵和传递方程。

(1)各转动关节的传递矩阵和传递方程：

如图3所示，对于转动关节，其包含伺服电机、减速机、传动齿轮等机械电气结构，将其等效为沿x、y、z方向平移和绕x、y、z方向扭转的空间弹性铰，在建模过程中不计质量，其质量全部归入实际装配位置相邻的机械连杆质量中，如铰元件1的质量归入体元件2，铰元件3的质量归入体元件4，铰元件5、铰元件7、铰元件9、铰元件11的质量均归入体元件6中。

确定转动关节的输入端为I_j，输出端为O_j，j＝1,3,5,7,9,11，转动关节的传递方程表示为

Z_j,O＝U_jZ_j,I

式中，U_j为铰元件j的传递矩阵，Z_j,O和Z_j,I分别为铰元件j输出端和输入端的状态矢量，K_x，K_y，K_z为弹簧在x、y、z方向上的平移刚度，K'_x，K'_y，K'_z为扭簧在x、y、z方向上的扭转刚度。

(2)各机械连杆的传递矩阵和传递方程：

对于机械连杆，确定机械连杆的输入端为I_i，输出端为O_i，i＝2,4,6,8,10,12，建立以输入端I_i为原点的连体坐标系

在连体坐标系中输出端O的坐标为(b₁,b₂,b₃)，质心C的坐标为(c_c1,c_c2,c_c3)，机械连杆的传递方程表示为

Z_i,O＝U_iZ_i,I

式中，U_i为体元件i的传递矩阵，Z_i,O和Z_i,I分别为体元件i输出端和输入端的状态矢量，J_I为刚体相对I_i点的惯量矩阵。

(3)各关节转角对应的坐标变换矩阵：

工业机器人共有6个关节，即每一个机械连杆对应绕一个转动关节进行轴向旋转，当工业机器人6个关节转角为θ₁,θ₂,θ₃,θ₄,θ₅,θ₆时，按传递方向，状态矢量Z_2,1、Z_4,3、Z_6,5、Z_8,7、Z_10,9、Z_12,11需先后通过坐标变换转换后进行传递：

Z_2,1＝Hz(θ₁)Z′_2,1

Z_4,3＝Hy(θ₂)Z′_4,3

Z_6,5＝Hy(θ₃)Z′_6,5

Z_8,7＝Hx(θ₄)Z′_8,7

Z_10,9＝Hy(θ₅)Z′_10,9

Z_12,11＝Hx(θ₆)Z′_12,11

定义机器人基坐标系oxyz如图4所示，关节1旋转轴与地面交点为坐标原点，垂直于地面向上为z轴正方向，坐标系原点指向如图4位姿下法兰盘中心方向为x轴正方向，按照右手螺旋定则确定y轴正方向。

状态矢量的定义方法如下：①状态矢量Z_2,1定义在连体坐标系

上，状态矢量Z'_2,1定义在以体元件2输入端I₂为坐标原点，与工业机器人基坐标系oxyz坐标轴方向一致的坐标系I₂XYZ上，坐标系

与坐标系I₂XYZ之间仅相差关节1绕坐标系I₂XYZ的z方向的转角θ₁；

②状态矢量Z_4,3定义在连体坐标系

上，状态矢量Z'_4,3定义在以体元件4输入端I₄为坐标原点，与工业机器人基坐标系oxyz坐标轴方向一致的坐标系I₄XYZ上，坐标系

与坐标系I₄XYZ之间仅相差关节2绕坐标系I₄XYZ的y方向的转角θ₂；

③状态矢量Z_6,5定义在连体坐标系

上，状态矢量Z'_6,5定义在以体元件6输入端I₆为坐标原点，与工业机器人基坐标系oxyz坐标轴方向一致的坐标系I₆XYZ上，坐标系

与坐标系I₆XYZ之间仅相差关节3绕坐标系I₆XYZ的y方向的转角θ₃；

④状态矢量Z_8,7定义在连体坐标系

上，状态矢量Z'_8,7定义在以体元件8输入端I₈为坐标原点，与工业机器人基坐标系oxyz坐标轴方向一致的坐标系I₈XYZ上，坐标系

与坐标系I₈XYZ之间仅相差关节4绕坐标系I₈XYZ的x方向的转角θ₄；

⑤状态矢量Z_10,9定义在连体坐标系

上，状态矢量Z₁'_0,9定义在以体元件10输入端I₁₀为坐标原点，与工业机器人基坐标系oxyz坐标轴方向一致的坐标系I₁₀XYZ上，坐标系

与坐标系I₁₀XYZ之间仅相差关节5绕坐标系I₁₀XYZ的y方向的转角θ₅；

⑥状态矢量Z_12,11定义在连体坐标系

上，状态矢量Z₁'_2,11定义在以体元件12输入端I₁₂为坐标原点，与工业机器人基坐标系oxyz坐标轴方向一致的坐标系I₁₂XYZ上，坐标系

与坐标系I₁₂XYZ之间仅相差关节6绕坐标系I₁₂XYZ的x方向的转角θ₆。

步骤四：结合各机械臂杆件和转动关节的传递矩阵和传递方程，计算工业机器人的总传递矩阵和总传递方程；

各元件的传递方程为：

总传递方程为：

Z_12,0＝U_allZ_1,0

U_all＝U₁₂Hx(θ₆)U₁₁U₁₀Hy(θ₅)U₉U₈Hx(θ₄)U₇U₆Hy(θ₃)U₅U₄Hy(θ₂)U₃U₂Hz(θ₁)U₁

式中，U_all为总传递矩阵，由沿着系统传递方向的各元件的传递矩阵依次相乘得到。

步骤五：根据工业机器人实际边界特性，计算工业机器人边界条件；

确定边界条件，工业机器人的系统边界条件即为其第一个元件的输入端与最后一个元件的输出点的状态矢量，根据如图1所示的工业机器人位姿状态，铰元件1与地面0接触，受到约束，为固支点，其状态变量的线位移、角位移均为0，其受到的内力和内力矩未知，确定其状态矢量Z_1,0；体元件12属于自由端，不受约束，其受到的内力和内力矩均为0，线位移和角位移未知，确定其状态矢量Z_12,0。

式中，Z_12,0为第12个元件的输出端的状态矢量，Z_1,0表示第一个元件的输入端的状态矢量。

步骤六：确定系统特征方程，求解特征方程得到工业机器人各阶固有频率ω_k与振型；

根据总传递方程确定系统特征方程，将机器人的边界条件入式总传递函数，

化简去掉等号右边Z_1,0中的零元素得

去掉等号左边Z_12,0中的非零元素得O₆，对应划去U_all中与Z_1,0中零元素对应的列和与Z_12,0中非零元素对应的行，得到

确定系统特征方程

求解该方程获得机器人系统各阶固有频率ω_k，再连立各元件传递方程，得到各阶频率对应的全部连接点的状态矢量，即可求解系统各阶振型。

步骤七：根据工业机器人增广特征矢量，利用模态叠加原理和增广特征矢量正交性，计算得到机器人振动响应。

具体为：(1)拼接系统增广特征矢量

将系统的各阶固有频率ω_k，代入各元件传递方程即可得到系统任意一点处的状态矢量，将6个刚体的输入端线位移和角位移模态坐标拼接成系统各阶增广特征矢量

k为阶数，此增广特征矢量为36×1阶列阵；

(2)计算系统体动力学方程

计算工业机器人的振动响应，在关节处引入阻尼，工业机器人系统的强迫振动体动力学方程为

Mv_tt+Cv_t+Kv＝f

式中

M为系统质量参数矩阵，K为系统刚度参数矩阵，C为系统阻尼参数矩阵，v为系统的位移坐标列阵，f为系统的外力列阵，M_i、K_i、f_i分别为第i(i＝1,2,…,6)个体元件的质量参数矩阵、刚度参数矩阵、外力和外力矩列阵，v_i表征第i个体元件的运动状态，由体元件的位移和角位移组成的位移列阵，下标t表示对时间求导数，此处体元件2、4、6、8、10、12顺序对应1、2、3、4、5、6。

每个体元件的质量参数矩阵和外力列阵为

即可拼接系统质量参数矩阵M和系统外力列阵f。式中，m_i为各体元件质量，

为第i个体元件质心坐标组成的反对称矩阵，

为体元件受到的外力主矢F_i对输入端I的力矩，D为外力的简化中心，即矩心，J_I,i为第i个体元件相对其输入端I的惯量矩阵,此处体元件2、4、6、8、10、12顺序对应1、2、3、4、5、6。

假设阻尼为比例阻尼，阻尼参数矩阵与质量和刚度参数矩阵成正比，即

C＝αM+βK

式中，α、β为常数。

(3)利用增广特征矢量的正交性：

＜MV^k,V^p＞＝δ_k,pM_p,＜MV^k,V^p＞＝δ_k,pK_p

式中，＜MV^k,V^p＞为系统质量参数矩阵M与第k阶固有频率对应增广特征矩阵乘积与第p阶固有频率对应增广特征矩阵的内积；M_p为系统的第p阶模态质量；

为系统的第p阶模态刚度；ω_p为第p阶固有频率；当k＝p时，δ_k,p＝1，当k≠p时，δ_k,p＝0。

模态叠加原理

强迫振动体动力学方程变为解耦的广义坐标微分方程

式中，q^p(t)为t时刻的系统广义坐标，

为系统广义速度，

为系统广义加速度，N为模态阶数。

(4)求解系统振动响应

通过数值积分方法，求解系统任意时刻的广义坐标，利用模态叠加原理求解系统状态量的时间历程，即系统振动响应。

实施例2

方案基本相同，唯一区别在于，当在实际工业应用中工业机器人将在法兰处安装末端执行器时，为计算工业机器人加工系统的振动特性和振动响应，在现有工业机器人传递矩阵U_all的基础上，左乘末端执行器的传递矩阵U_E，即可得到整体的新传递矩阵U_all-New＝U_EU_all，即可按步骤五至七进行系统振动特性和振动响应计算。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (7)

1.一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于，包括如下步骤：

步骤一：提取待建模的工业机器人系统的主要结构元件，包括机械连杆和转动关节，将转动关节和机械连杆分别等效为铰元件和体元件，并对各元件进行统一编号；

步骤二：建立物理坐标和模态坐标，定义物理坐标和模态坐标下的各元件的状态矢量；

步骤三：建立机器人各转动关节铰元件和各机械连杆体元件的传递矩阵和传递方程；

建立机器人各转动关节铰元件的传递矩阵和传递方程，确定第j个转动关节输入端为I_j，输出端为O_j，第j个转动关节传递方程表示为

Z_j,O＝U_jZ_j,I

式中，Z_j,O和Z_j,I分别为转动关节铰元件j的输出端和输入端的状态矢量，U_j为转动关节铰元件j的传递矩阵；

建立各机械连杆体元件的传递矩阵和传递方程，确定第i个机械连杆输入端为I_i，输出端为O_i，建立以输入端I_i为原点的连体坐标系

在连体坐标系中输出端O的坐标为(b₁,b₂,b₃)，质心C的坐标为(c_c1,c_c2,c_c3)，第i个机械连杆传递方程表示为

Z_i,O＝U_iZ_i,I

式中，Z_i,O和Z_i,I分别为机械连杆体元件i的输出端和输入端的状态矢量，U_i为机械连杆体元件i的传递矩阵；

步骤四：结合工业机器人各机械连杆和转动关节的传递矩阵和传递方程，建立其总传递方程为

Z_2n,0＝U_allZ_1,0

式中，U_all为机器人的总传递矩阵，由沿着系统传递方向的各机械连杆和转动关节元件的传递矩阵依次相乘得到；

步骤五：根据工业机器人实际边界特性，确定工业机器人边界条件

式中，Z_2n,0为第2n个元件输出端的状态矢量，Z_1,0表示第1个元件的输入端的状态矢量；X、Y、Z分别表示模态坐标下惯性坐标系中联接点相对于其平衡位置沿xyz三方向的线位移，Θ_x、Θ_y、Θ_z分别表示模态坐标下惯性坐标系中联接点相对于其平衡位置绕xyz三方向的角位移，M_x、M_y、M_z分别表示模态坐标下联接点处xyz三方向的内力矩，Q_x、Q_y、Q_z分别表示模态坐标下联接点处的xyz三方向内力；

步骤六：确定系统特征方程，求解特征方程得到工业机器人的各阶固有频率与固有振型；

将步骤五中的边界条件代入步骤四中的总传递方程，得到

由于边界条件Z_2n,0和Z_1,0中分别总有一半元素为零，删除对应的零元素化简上述方程，得到降阶的总传递方程

式中，

为删除Z_1,0中的零元素得到的6×1阶状态矢量，

为删除U_all中与Z_1,0中零元素对应的列和与Z_2n,0中非零元素对应的行得到的6×6阶矩阵，O₆为6×1阶全零矢量；

确定系统特征方程为

式中，det[·]表示矩阵的行列式，k为模态阶数；求解该方程即可得到工业机器人的各阶固有频率ω_k，再将各阶固有频率代入机械连杆和转动关节的传递矩阵和传递方程，计算得到各元件在模态坐标下的状态矢量，即可得到各阶固有振型；

步骤七：拼接工业机器人增广特征矢量，利用模态叠加原理和增广特征矢量正交性，计算得到机器人系统的振动响应，包括：

将系统的各阶固有频率ω_k代入各机械连杆体元件和转动关节铰元件的传递方程即可得到系统任意一点处的状态矢量，将机器人系统中所有机械连杆体元件输入端状态矢量中的线位移和角位移模态坐标取出，依次拼接即可得到系统各阶增广特征矢量

其中

为第i个机械连杆体元件第k阶增广状态矢量，由各机械连杆体元件输入端状态矢量中的线位移和角位移向量组成的6×1列阵；

在关节处引入阻尼，工业机器人系统的强迫振动体动力学方程为

Mv_tt+Cv_t+Kv＝f

式中，

M为机器人系统质量参数矩阵，K为机器人系统刚度参数矩阵，C为机器人系统阻尼参数矩阵，v为系统的物理位移坐标列向量，f为系统的物理外力列向量，M_i、K_i、f_i分别为第i个机械连杆体元件的质量参数矩阵、刚度参数矩阵、外力列向量，v_i为第i个机械连杆体元件的位移列向量，下标t表示对时间求导数；

每个体元件的质量参数矩阵和外力列向量为

式中，m_i为第i个机械连杆体元件质量，

为第i个体元件质心坐标组成的反对称矩阵，

为第i个机械连杆体元件受到的外力主矢F_i对输入端I的力矩，D为外力的简化中心，即矩心，J_I,i为第i个体元件相对其输入端I的惯量矩阵；I₃为三阶单位矩阵；

假设阻尼为比例阻尼，阻尼参数矩阵与质量和刚度参数矩阵成正比，即

C＝αM+βK

式中，α、β为比例常数；

求解系统振动响应

增广特征矢量的正交性为＜MV^k,V^p＞＝δ_k,pM_p,＜MV^k,V^p＞＝δ_k,pK_p

式中，＜MV^k,V^p＞为系统质量参数矩阵M与第k阶固有频率对应的增广特征矢量乘积与第p阶固有频率对应增广特征矢量的内积；M_p为系统的第p阶模态质量；

为系统的第p阶模态刚度；ω_p为系统的第p阶固有频率；当k＝p时，δ_k,p＝1，当k≠p时，δ_k,p＝0；

模态叠加原理为

式中，v(t)为t时刻系统的物理位移坐标列向量，q^k(t)为第k阶系统广义坐标；

将增广特征矢量的正交性和模态叠加原理，代入工业机器人系统的强迫振动体动力学方程，得到解耦的广义坐标微分方程

式中，q^p(t)为第p阶系统广义坐标，

为第p阶系统广义速度，

为第p阶系统广义加速度，＜f,V^p＞为系统的外力列向量f与第p阶固有频率对应增广特征矢量V^p的内积，N为模态阶数；

通过数值积分方法，求解系统任意时刻的广义坐标q^p(t)，再将其反代入模态叠加原理方程，即可求解系统状态量的时间历程，即系统振动响应。

2.根据权利要求1所述的一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于步骤一所述的转动关节等效的铰元件为由弹簧和扭簧并联组成的空间弹性铰元件，机械连杆等效的体元件为一端输入一端输出空间刚体元件。

3.根据权利要求1所述的一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于步骤二所述的各个转动关节铰元件和机械连杆体元件在物理坐标、模态坐标下的状态矢量分别为

式中，x、y、z表示物理坐标下惯性坐标系中联接点相对于其平衡位置的线位移；θ_x、θ_y、θ_z表示物理坐标下惯性坐标系中联接点相对于其平衡位置的角位移；m_x、m_y、m_z表示物理坐标下联接点处的内力矩；q_x、q_y、q_z表示物理坐标下联接点处的内力；X、Y、Z，Θ_x、Θ_y、Θ_z，M_x、M_y、M_z，Q_x、Q_y、Q_z分别为对应小写字母物理量的模态坐标，i,j表示相邻元件序号。

4.根据权利要求2所述的一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于步骤三所述的各个转动关节铰元件的传递矩阵为

式中，

K_x、K_y、K_z为弹簧在x、y、z方向上的平移刚度，

K'_x、K'_y、K'_z为扭簧在x、y、z方向上的扭转刚度。

5.根据权利要求1所述的一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于步骤三所述的各个机械连杆体元件的传递矩阵为

式中，m为机械连杆体元件的质量，ω为工业机器人系统的固有频率，

为机械连杆体元件相对于输入点I的惯量矩阵，J_x、J_y、J_z分别为机械连杆体元件对于连体坐标系x轴、y轴和z轴的转动惯量，J_xy、J_xz、J_yz分别为机械连杆体元件对于连体坐标系x轴、y轴和z轴的惯性积，

(b₁,b₂,b₃)为机械连杆体元件输出端O的坐标，(c_c1,c_c2,c_c3)为机械连杆体元件质心C的坐标。

6.根据权利要求1所述的一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于步骤四所述的机器人总传递矩阵为

U_all＝[U_2nH(θ_n)U_2n-1][U_2n-2H(θ_n-1)U_2n-3][U_2n-4H(θ_n-2)U_2n-5]…[U₄H(θ₂)U₃][U₂H(θ₁)U₁]

式中，H(θ_i)分别为机械连杆体元件绕关节轴旋转关节角θ的坐标变换矩阵，i＝1,2,3,...,n。

7.根据权利要求6所述的一种旋转关节型工业机器人模态参数和振动响应计算方法，其特征在于所述的坐标变换矩阵H(θ)有三种形式，分别为绕x轴、y轴和z轴进行旋转，对应的坐标变换矩阵Hx(θ)，Hy(θ)，Hz(θ)分别为

Images