CN108038286A

CN108038286A - 一种二自由度冗余驱动并联机器人的动力学建模方法

Info

Publication number: CN108038286A
Application number: CN201711242046.4A
Authority: CN
Inventors: 惠记庄; 武琳琳; 赵睿英; 罗丽; 张泽宇
Original assignee: Changan University
Current assignee: Changan University
Priority date: 2017-11-30
Filing date: 2017-11-30
Publication date: 2018-05-15
Anticipated expiration: 2037-11-30
Also published as: CN108038286B

Abstract

本发明属于工业机器人建模与仿真领域，公开了一种二自由度冗余驱动并联机器人的动力学建模方法，包括：获取二自由度冗余驱动并联机器人包含的三个子系统，建立每个子系统的动力学模型；确定二自由度冗余驱动并联机器人三个子系统之间的约束条件；根据二自由度冗余驱动并联机器人三个子系统之间的约束条件，以及每个子系统的动力学模型，得到二自由度冗余驱动并联机器人的动力学模型；能够解决现有具有多闭环结构的并联机器人在建模过程需要借助辅助变量，较难得到精确的动力学解析模型问题，以及无法系统化建立在非理想约束下的动力学模型的问题。

Description

一种二自由度冗余驱动并联机器人的动力学建模方法

技术领域

本发明属于工业机器人建模与仿真技术领域，尤其涉及一种二自由度冗余驱动并联机器人的动力学建模方法。

背景技术

动力学建模作为并联机器人动力学分析研究的一个重要内容，是机构的动力学特性分析以及动态控制研究的基础，因此，关于动力学建模的相关研究一直是并联机器人研究中的一个重点。由于并联机器人为一种闭环形式的多运动支链系统，其结构比较复杂，建模步骤繁琐、工作量大，而且有时需要借助动力学建模软件，从而导致并联机器人的动力学建模与控制在数学层面上脱节。因此，寻找一种系统、高效、简洁的动力学解析建模方法一直是很多研究学者在动力学建模方面急需解决的一个难题。

牛顿-欧拉法和拉格朗日乘子法是目前最常用的两种动力学建模方法。其中牛顿-欧拉法在并联机器人建模中相对直观，便于实现，但由于牛顿-欧拉法在建模过程中需要写出每个连杆的运动方程，对于结构复杂、自由度数目较多和构型特殊的并联机器人，在建模中系统未知量数目增多，迭代步骤复杂，工作量大。拉格朗日乘子法主要利用能量守恒的原理建立拉格朗日方程，从而避免了建模过程中的加速度和角加速度的计算，过程相对简单，建立的模型比较简洁并能够适用于控制器的设计，但是建立的模型只局部有效，并对求解的收敛性要求很高。

虽然上述方法能够实现并联机器人的动力学建模，但也存在一些不足之处，例如：无法以一种系统化的方法快速、高效地建立动力学解析模型；无法快速建立并联机器人在非完整约束条件下的解析模型；对存在非理想约束的并联机器人，其建模过程十分困难，如无法建立关节摩擦力的解耦非线性解析模型，因此，对建立并联机器人动力学模型特别是建立解析模型的新方法进行研究具有很大的必要性。

发明内容

针对上述技术问题，本发明的目的在于提供一种二自由度冗余驱动并联机器人的动力学建模方法，能够解决现有具有多闭环结构的并联机器人在建模过程需要借助辅助变量，较难得到精确的动力学解析模型问题，以及无法系统化建立在非理想约束下的动力学模型的问题。

为达到上述目的，本发明采用如下技术方案予以实现。

一种二自由度冗余驱动并联机器人的动力学建模方法，所述方法包括：

步骤1，获取二自由度冗余驱动并联机器人包含的三个子系统，建立每个子系统的动力学模型；

步骤2，确定二自由度冗余驱动并联机器人三个子系统之间的约束条件；

步骤3，根据二自由度冗余驱动并联机器人三个子系统之间的约束条件，以及每个子系统的动力学模型，得到二自由度冗余驱动并联机器人的动力学模型。

本发明技术方案的特点和进一步的改进为：

(1)步骤1具体包括如下子步骤：

(1a)二自由度冗余驱动并联机器人包含三个子系统，每个子系统为包含主动杆和被动杆的二杆机构，从而每个子系统的拉格朗日函数为该子系统主动杆的动能与该子系统被动杆的动能之和；

(1b)根据每个子系统的拉格朗日函数以及Euler-Lagrange方程，得到每个子系统的动力学模型。

(2)子步骤(1a)具体为：

第i个子系统的拉格朗日函数为第i个子系统主动杆的动能E_ai与第i个子系统被动杆的动能E_bi之和L_i＝E_ai+E_bi；

其中，

其中，I_ai表示第i个子系统中主动杆相对于连杆质心的惯性力矩，I_bi表示第i个子系统中被动杆相对于连杆质心的惯性力矩，m_ai表示第i个子系统中主动杆的质量，m_bi表示第i个子系统中被动杆的质量，r_ai表示第i个子系统中主动杆质心相对于关节点的距离，r_bi表示第i个子系统中被动杆质心相对于关节点的距离，q_ai表示第i个子系统中主动杆的转动角度，表示第i个子系统中主动杆的转动角速度，q_bi表示第i个子系统中被动杆的转动角度，表示第i个子系统中被动杆的转动角速度，l主动连杆或被动连杆的长度。

(3)子步骤(1b)具体包括：

(1b1)令q_i＝[q_ai q_bi]^T为第i个子系统的广义坐标，τ_i＝[τ_ai τ_bi]^T为第i个子系统的关节力矩，根据Euler-Lagrange方程：

得：

其中，

其中，τ_ai表示第i个子系统中主动杆的驱动关节力矩，τ_bi表示第i个子系统中被动杆的驱动关节力矩，表示第i个子系统中主动杆的转动角加速度，表示第i个子系统中被动杆的转动角加速度；

(1b2)将(1)和(2)写成矩阵形式，得到并联机器人第i个子系统的动力学模型：

其中，M_i(q_i，t)为质量惯性矩阵，为科式力矩阵，定义为：

t表示时间变量。

(4)步骤2具体包括：

确定二自由度冗余驱动并联机器人三个子系统之间的约束方程：

其中，(A_1x，A_1y)表示第一个主动关节处的坐标，(A_2x，A_2y)表示第二个主动关节处的坐标，(A_3x，A_3y)表示第三个主动关节处的坐标；

将所述约束方程写为二阶矩形式为：

其中，q＝[q_a1 q_b1 q_a2 q_b2 q_a3 q_b3]^T，

(5)步骤3具体包括如下子步骤：

(3a)根据二自由度冗余驱动并联机器人三个子系统之间的约束条件以及每个子系统的动力学模型将约束条件带入Udwadia-Kalaba方程中得：

其中，表示M(q，t)的二分之一次方，B(q，t)＝A(q，t)M^-1/2(q，t)，B⁺(q，t)表示B(q，t)的Moor-Penrose广义逆，M^-1(q，t)表示M(q，t)的负一次方；

式中，M(q，t)∈R^6×6为并联机器人的质量矩阵，为并联机器人的科氏力矩阵，q∈R⁶为并联机器人的关节角向量，τ(t)∈R⁶为并联机器人的关节力矩，其定义分别为

(3b)从而得到二自由度冗余驱动并联机器人的动力学模型：

本发明以二自由度冗余驱动并联机器人作为研究对象，利用层级堆聚建模方法建立其动力学解析模型，将并联机器人分割为三条开式支链，以支链作为子系统，利用拉格朗日方法很容易的建立了子系统的运动方程；根据子系统之间的约束条件建立机器人动力学解析模型，并求解了机器人的动力学逆问题，得到的驱动力模型适用于后续的控制器设计，层级堆聚建模方法与传统建模方法的结合使建模速度得到了提高；以拉格朗日方法为验证平台，从理论上论证了建立的动力学模型的正确行及有效性；最后借助MATLAB仿真软件，利用数值仿真的方法验证了模型的正确性并对其进行了逆向动力学仿真。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的二自由度冗余驱动并联机器人在工作平面内的结构示意图；

图2为本发明实施例提供的一种二自由度冗余驱动并联机器人的动力学建模方法的流程示意图；

图3为本发明实施例提供的二自由度冗余驱动并联机器人建模过程示意图；

图4为本发明实施例提供的并联机器人子系统结构示意图；

图5为本发明实施例提供的并联机器人主动关节角度变化仿真结果示意图一；

图6为本发明实施例提供的并联机器人被动关节角度变化仿真结果示意图一；

图7为本发明实施例提供的并联机器人主动关节角速度变化仿真结果示意图一；

图8为本发明实施例提供的并联机器人被动关节角速度变化仿真结果示意图一；

图9为本发明实施例提供的并联机器人主动关节约束力仿真结果示意图；

图10为本发明实施例提供的并联机器人被动关节约束力仿真结果示意图；

图11为本发明实施例提供的并联机器人末端执行器的运动轨迹示意图；

图12为本发明实施例提供的并联机器人主动关节角度变化仿真结果示意图二；

图13为本发明实施例提供的并联机器人被动关节角度变化仿真结果示意图二；

图14为本发明实施例提供的并联机器人主动关节角速度变化仿真结果示意图二；

图15为本发明实施例提供的并联机器人被动关节角速度变化仿真结果示意图二；

图16为本发明实施例提供的并联机器人驱动力矩变化仿真结果示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例首先进行机器人介绍：本实施例采用一种十分常见的少自由度并联机器人——固高科技GPM2012系列的平面二自由度冗余驱动并联机器人作为研究对象进行分析。

如图1所示为二自由度冗余驱动并联机器人在工作平面内的结构示意简图。在工作空间内建立如图1所示的直角坐标系，其中A₁、A₂、A₃表示并联机器人的主动关节，在直角坐标系中的坐标分别为A₁(0，0.250)，A₂(0.433，0)，A₃(0.433，0.500)，单位为m，q_a1、q_a2、q_a3表示三个主动关节的转角。B₁、B₂、B₃表示并联机器人的被动关节，q_b1、q_b2、q_b3表示三个被动关节的转角。并联机器人的末端工作装置位于图中的C点，其坐标表示为(x，y)。各连杆长度相等，即L₁₁＝L₁₂＝L₂₁＝L₂₂＝L₃₁＝L₃₂＝l＝0.244m。

本发明实施例提供一种二自由度冗余驱动并联机器人的动力学建模方法，如图2所示，所述方法包括：

步骤1，获取二自由度冗余驱动并联机器人包含的三个子系统，建立每个子系统的动力学模型。

步骤2，确定二自由度冗余驱动并联机器人三个子系统之间的约束条件。

具体的，如图3所示，二自由度冗余驱动并联机器人可以看作为三个具有相似结构的开式支链加上末端的约束关系组成的。因此，若将末端C处的约束去掉，则并联机器人被分割为3个相似结构的开式支链。将这三个开式支链分别作为并联机器人分割出来的三个子系统，采用传统的拉格朗日动力学建模方法可以很容易建立3个子系统的动力学模型。通过末端的约束关系，将三个子系统组合在一起，利用层级堆聚建模方法建立具有约束关系的并联机器人动力学解析模型。

本发明实施例提供的并联机器人的子系统建模过程为：二自由度冗余驱动并联机器人的子系统可以看成为一个二杆机构，如图4所示，定义主动连杆和被动连杆的质量分别为m_a、m_b，长度均为l。质心位置分别在距主动关节A与被动关节B的ra、rb处，主动连杆和被动连杆相对于连杆质心的惯性力矩为I_a、I_b。

设该并联机器人的各个连杆均为理想的刚体，由于并联机器人在平面上运动，因此重力势能的影响可以忽略不计，则并联机器人子系统的拉格朗日函数就等于主动杆的动能加上被动杆的动能。E_ai(i＝1，2，3)、E_bi(i＝1，2，3)分别表示该子系统的主动连杆和被动连杆的动能，则：

其中，(x_cai，y_cai)和(x_cbi，y_cbi)分别为主动杆和被动杆在直角坐标系中的质心坐标，其值分别为：

其中，(x_ai，y_ai)为主动关节处的坐标，对上式求导，可得：

将式(4)和式(5)代入到式(1)中得：

则子系统的拉格朗日函数L_i为：

L_i＝E_ai+E_bi。 (7)

令q_i＝[q_ai q_bi]^T，τ_i＝[τ_ai τ_bi]^T为子系统的关节力矩，根据

Euler-Lagrange方程：

得：

其中，

将式(9)和式(10)写成矩阵形式，可以得到并联机器人子系统的动力学模型为：

其中，M_i(q_i，t)为质量惯性矩阵，为科式力矩阵，其定义为：

由于并联机器人三个子系统的结构相似，因此，三个子系统的动力学模型也是相似的。

本发明实施例提供的并联机器人的约束条件为：

由于二自由度冗余驱动并联机器人的三条开式支链在末端处联结，因此，通过并联机器人的运动学关系可以很容易得到该并联机器人的约束方程：

对约束方程(14)进行二阶求导并改写为二阶矩阵形式：

令并联机器人的关节向量为q＝[q_a1 q_b1 q_a2 q_b2 q_a3 q_b3]^T，则二阶约束矩阵A_4×6可以表示为：

矩阵b_4×1为：

本发明实施例提供的并联机器人的动力学模型为：

该并联机器人的子系统动力学模型为式(11)，二阶约束方程为式(15)，将式(15)带入到Udwadia-Kalaba方程中得：

式中，M∈R^6×6为并联机器人的质量矩阵，C∈R^6×6为并联机器人的科氏力矩阵，q∈R⁶为并联机器人的关节角向量，τ∈R⁶为并联机器人的关节力矩。其定义分别为

改写式(18)，得到并联机器人系统的动力学模型为：

通过式(11)和式(18)可以推导出该并联机器人所受的理想约束力为：

进一步的，本发明实施例提供的并联机器人逆动力学求解过程为：

若并联机器人的各个关节角度q、关节角速度以及关节角加速度已知，整理式(19)可得：

其中，I∈R^6×6为单位矩阵。

并联机器人具有三个驱动关节，对机器人进行逆向动力学建模，求解机器人的驱动力矩。首先选取末端执行器的坐标q^e＝(x，y)作为并联机器人动力学模型的独立变量，表示并联机器人的关节角速度向量，表示并联机器人末端执行器的运动速度向量，则与之间的关系可表示为：

其中矩阵W为Jacobian矩阵，其定义为：

其中：

b_ix＝l cos(q_ai+q_bi)， (24)

b_iy＝l sin(q_ai+q_bi)， (25)

d_ix＝l cos q_ai+l cos(q_ai+q_bi)， (26)

d_iy＝l sin q_ai+l sin(q_ai+q_bi)， (27)

令τ_s为应施加给并联机器人的驱动力矩，则：

τ_s＝W^Tτ。 (29)

式(22)对时间求导得：

令矩阵S为主动关节与末端执行器之间的Jacobian矩阵，主动关节的驱动力矩τ_ac与τ_s之间的关系可表示为：

τ_s＝S^Tτ_ac， (31)

其中，

联立式(29)和式(31)，可得该并联机器人主动关节的驱动力矩为：

其中，为矩阵S^T的广义逆矩阵。

一、基于拉格朗日方法的模型验证

拉格朗日法是常用的并联机器人动力学建模方法之一，《并联机器人——建模、控制优化与应用》中利用拉格朗日方法建立了该二自由度冗余驱动并联机器人的动力学模型：

其中，A为一阶约束矩阵，λ为拉格朗日乘子，A^Tλ为系统的约束力。

将代入(22)，可以得到如下的关系式：

由于式(22)对于任意速度向量都成立，因此AW＝0，根据矩阵的相关知识，有如下的关系：

W^TA^T＝0。 (35)

所以，将式(33)两端同时乘以W^T并消去约束力可以得到机器人的动力学模型为：

将层级堆聚建模方法得到的并联机器人动力学模型式(18)两边同时乘以矩阵W^T，得：

其中，B＝AM^-1/2。根据伪逆的性质可得：

将上式带入到(37)中，得：

将式(35)代入上式(39)，得：

对逆向动力学模型式(21)，有：

令s＝[I-M^1/2B⁺AM^-1]^-1，则利用伪逆的性质可得：

将式(38)代入上式，得：

将s代入到式(41)中：

将上式两端同时乘以矩阵W^T，得：

由于W^TA^T＝0，所以上式变为：

通过对比式(40)和式(36)，式(46)和式(36)可知，利用Udwadia-Kalaba理论的层级堆聚方法建立的并联机器人动力学模型和利用拉格朗日法建立的动力学模型是等效的。

二、动力学模型仿真

在MATLAB环境中利用ode15i函数对该并联机器人的动力学模型(28)进行数值仿真。设置并联机器人的仿真初始条件为：各个关节的初始角度为q_a1＝78.08°，q_a2＝164.14°，q_a3＝275.09°，q_b1＝-125.18°，q_b2＝-78.42°，q_b3＝-107.65°；初始速度向量为初始加速度向量为给定控制力矩为τ＝[1，0，1，0，1，0]^T。该并联机器人的动力学相关参数值如表1所示。

表1 GPM2002并联机器人各连杆的动力学参数

利用层级堆聚建模方法建立的动力学模型的仿真结果如下：图5-6为该并联机器人在力矩驱动的条件下主动关节和被动关节的角度变化，从图中可以看出，在控制力矩的作用下，机器人的各个关节的角度变化连续、平稳、收敛，没有出现角度突变的现象。图7-8为该并联机器人在控制力矩的作用下主动关节和被动关节的角速度变化曲线，从图中可以观察到，各个关节的角速度变化连续，无不规则变化，未出现尖点等畸形情况，说明机构的运动是稳定的。图9-10为并联机器人各个关节处的约束力变化曲线，从图中可以观察到，约束力随着时间的变化而变化，其中约束力最大为约1.2N，并且主动关节1和被动关节1相对于其他关节，所受的约束力较大。

三、逆向动力学仿真

采用MATLAB仿真软件对该并联机器人的逆动力学模型(21)进行数值仿真，其动力学参数如表1所示。若使并联机器人作一个圆周运动，设定其圆心为(0.2165，0.250)，半径为0.06m，运动周期为2s。通过运动学逆解，在MATLAB中通过编程可以得到并联机器人各个主动关节和被动关节的运动状态及主动关节所需驱动力矩的大小。如图11所示为并联机器人末端在工作空间中的运动轨迹。

通过运动学逆解，得到并联机器人3个主动关节和3个被动关节的角度变化曲线，如图12、图13所示，当末端执行器按照上述轨迹运动时，并联机器人的各个关节的角度变化连续，平稳，未出现无规律的波动现象。图14、图15为并联机器人主动关节及被动关节的角速度变化曲线，各个关节的角速度变化比较平稳，未出现断点、畸形等情况，并且各个关节角速度的最大值比较接近，说明当该并联机器人进行高速运动时，机构能够保持其运动的稳定性。图16为根据并联机器人逆向动力学模型得到的并联机器人三个主动关节的驱动力矩变化曲线，从图中可以观察到主动关节1的所需的驱动力矩比其他两个关节的力矩稍大，但总体来说力矩在各个关节的分别较为均匀，说明机构在运动时，三个主动关节受力比较稳定。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims

1.一种二自由度冗余驱动并联机器人的动力学建模方法，其特征在于，所述方法包括：

步骤1，获取二自由度冗余驱动并联机器人包含的三个子系统，每个子系统为包含主动杆和被动杆的二杆机构，建立每个子系统的动力学模型；

2.根据权利要求1所述的一种二自由度冗余驱动并联机器人的动力学建模方法，其特征在于，步骤1具体包括如下子步骤：

(1a)二自由度冗余驱动并联机器人包含三个子系统，从而每个子系统的拉格朗日函数值为该子系统主动杆的动能与该子系统被动杆的动能之和；

(1b)根据每个子系统的拉格朗日函数值以及Euler-Lagrange方程，得到每个子系统的动力学模型。

3.根据权利要求2所述的一种二自由度冗余驱动并联机器人的动力学建模方法，其特征在于，子步骤(1a)具体为：

第i个子系统的拉格朗日函数值为第i个子系统主动杆的动能E_ai与第i个子系统被动杆的动能E_bi之和L_i＝E_ai+E_bi；

其中，

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>E</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>I</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>q</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>I</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>I</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>m</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msup> <mi>l</mi> <mn>2</mn> </msup> <msup> <mi>sin</mi> <mn>2</mn> </msup> <msub> <mi>q</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>m</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msup> <mi>l</mi> <mn>2</mn> </msup> <msup> <mi>cos</mi> <mn>2</mn> </msup> <msub> <mi>q</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>q</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>m</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mi>r</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <msup> <mi>sin</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>q</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>m</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msubsup> <mi>r</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <msup> <mi>cos</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>q</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> <mo>;</mo> </mrow>

4.根据权利要求2所述的一种二自由度冗余驱动并联机器人的动力学建模方法，其特征在于，子步骤(1b)具体包括：

<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mfrac> <mi>d</mi> <mrow> <mi>d</mi> <mi>t</mi> </mrow> </mfrac> <mfrac> <mrow> <mo>&part;</mo> <msub> <mi>L</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&part;</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> </mrow> </mfrac> <mo>-</mo> <mfrac> <mrow> <mo>&part;</mo> <msub> <mi>L</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&part;</mo> <msub> <mi>q</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> </mrow> </mfrac> <mo>=</mo> <msub> <mi>&tau;</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mi>d</mi> <mrow> <mi>d</mi> <mi>t</mi> </mrow> </mfrac> <mfrac> <mrow> <mo>&part;</mo> <msub> <mi>L</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&part;</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mfrac> <mo>-</mo> <mfrac> <mrow> <mo>&part;</mo> <msub> <mi>L</mi> <mi>i</mi> </msub> </mrow> <mrow> <mo>&part;</mo> <msub> <mi>q</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mfrac> <mo>=</mo> <msub> <mi>&tau;</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>

得：

<mrow> <mo>(</mo> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mi>cosq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> <msub> <mover> <mi>q</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mi>sinq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>&CenterDot;</mo> <msubsup> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&tau;</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

其中，β_i＝m_bilr_bi，

其中，M_i(q_i,t)为质量惯性矩阵，为科式力矩阵，定义为：

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <mo>+</mo> <mn>2</mn> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mi>cosq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mi>cosq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> <mo>+</mo> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mi>cosq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> <mtd> <msub> <mi>&gamma;</mi> <mi>i</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mi>i</mi> </msub> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <msub> <mi>sinq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <mo>+</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&beta;</mi> <mi>i</mi> </msub> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> <msub> <mi>sinq</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

t表示时间变量。

5.根据权利要求1所述的一种二自由度冗余驱动并联机器人的动力学建模方法，其特征在于，步骤2具体包括：

将所述约束方程写为二阶矩形式为：

其中，q＝[q_a1 q_b1 q_a2 q_b2 q_a3 q_b3]^T，

6.根据权利要求1所述的一种二自由度冗余驱动并联机器人的动力学建模方法，其特征在于，步骤3具体包括如下子步骤：

(3a)根据二自由度冗余驱动并联机器人三个子系统之间的约束条件以及每个子系统的动力学模型(i＝1,2,3)，将约束条件带入Udwadia-Kalaba方程中得：

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&CenterDot;&CenterDot;</mo> </mover> <mo>=</mo> <mi>&tau;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mi>M</mi> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> </msup> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <msup> <mi>B</mi> <mo>+</mo> </msup> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&times;</mo> <mo>&lsqb;</mo> <mi>b</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>,</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>-</mo> <mi>A</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <msup> <mi>M</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>&tau;</mi> <mo>-</mo> <mi>C</mi> <mo>(</mo> <mrow> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>,</mo> <mi>t</mi> </mrow> <mo>)</mo> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mo>&rsqb;</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>

其中，表示M(q,t)的二分之一次方，B(q,t)＝A(q,t)M^-1/2(q,t)，B⁺(q,t)表示B(q,t)的Moor-Penrose广义逆，M^-1(q,t)表示M(q,t)的负一次方；

式中，M(q,t)∈R^6×6为并联机器人的质量矩阵，为并联机器人的科氏力矩阵，q∈R⁶为并联机器人的关节角向量，τ(t)∈R⁶为并联机器人的关节力矩，其定义分别为

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>M</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>M</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>M</mi> <mn>3</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>&tau;</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&tau;</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&tau;</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&tau;</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>q</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>,</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mn>1</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mn>1</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>2</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mn>2</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>C</mi> <mn>3</mn> </msub> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>,</mo> <msub> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mn>3</mn> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msub> <mi>q</mi> <mi>i</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>a</mi> <mi>i</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>q</mi> <mrow> <mi>b</mi> <mi>i</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>

(3b)从而得到二自由度冗余驱动并联机器人的动力学模型：