CN110850834B - 并联机器人的建模方法、建模系统、控制方法及控制系统 - Google Patents

Abstract

本发明公开一种并联机器人的建模方法、建模系统、控制方法及控制系统，其中，所述并联机器人的建模方法包括建立并联机器人的无约束系统模型；将物理约束转换为特定数据形式引入，以获取满足给定物理约束的约束力解析数据；以及将所述约束力解析数据附加到所述无约束系统模型中，以获取所述并联机器人的动力学模型。利用本发明建立的动力学模型中约束力是解析解的表达式，并且建模的过程步骤清晰简洁，具有普遍性、系统性和简单性，而且经过仿真模拟分析，该通过该方法建立的动力学模型具有有效性和准确性。

Description

并联机器人的建模方法、建模系统、控制方法及控制系统

技术领域

本发明涉及机器人控制技术领域，特别涉及并联机器人的建模方法、建模系统、控制方法及控制系统。

背景技术

并联机器人是由动平台(或称为末端)和静平台(或称为支架)以及连接着它们的若干运动分支(或称为腿)组成的机械系统，具有动力性能好、承载能力强、精度高及刚度大等优点，目前在工业，医疗和航空等领域上得到了广泛应用，如高精度机床，太空探索，医学和微动装置。为了实现高精度控制，需要一个准确的并联机器人动力学模型。但由于并联机器人结构之间耦合关系十分复杂，其动力学模型通常是一个多参数耦合及高度非线性的复杂系统，这使得并联机器人的动力学分析和建模十分困难。对机器人进行动态建模常用的几种方法包括牛顿-欧拉法、拉格朗日-达朗贝尔法、凯恩法，Gibbs-Appell法、虚工原理和汉密尔顿原理等。

牛顿-欧拉法的思路比较简单，直观易懂，是最为常见的一种动力学建模方法之一，但在并联机器人的动力学建模中需要写出每个连杆的运动方程，随着并联机器人自由度数目的增加，所需要建立的方程个数会增加，使得推导过程复杂繁琐。拉格朗日法推导过程相对简单，建立的模型比较简洁，但是对于杆件数目比较多的并联机器人，则系统变量的数目会明显增多，迭代步骤复杂，建模工作量大。拉格朗日和达朗贝尔法相结合能够简洁的表达并联机构的闭环广义力，计算速度高于拉格朗日法，但是步骤复杂，计算量大。凯恩方法能得到形式相对简洁的动力学模型，但是建模思路相对于其它方法来说，比较抽象难懂。上述传统的并联机器人动力学建模方法在工程实际中虽得到了广泛的应用，但随着机构的复杂度增加或者当约束为非完整约束时，建模过程会变得十分困难。

并联机器人的动力学建模方法研究中，目前还缺少一种能够系统、简洁的动力学解析建模方法和控制方法。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种并联机器人的建模方法、建模系统、控制方法及控制系统，用于解决对于并联机器人进行动力学分析和建模时十分困难的技术问题。

为实现上述目的及其他相关目的，本发明提供一种并联机器人的建模方，所述并联机器人的建模方法包括：

建立并联机器人的无约束系统模型；

将物理约束转换为特定数据形式引入，以获取满足给定物理约束的约束力解析数据；以及

将所述约束力解析数据附加到所述无约束系统模型中，以获取所述并联机器人的动力学模型。

在一实施例中，所述建立并联机器人的无约束系统模型的步骤包括：

将并联机器人分割成若干独立的无约束子系统；

获取每个所述无约束子系统的无约束子系统模型；

将全部的所述无约束子系统的无约束子系统模型联立以形成所述并联机器人的所述无约束系统模型。

在一实施例中，利用拉格朗日法获取每个所述无约束子系统的无约束子系统模型。

在一实施例中，利用牛顿-欧拉法获取每个所述无约束子系统的无约束子系统模型。

在一实施例中，所述物理约束包括基座约束和末端执行器约束。

在一实施例中，所述并联机器人包括2自由度冗余驱动并联机器人。

为实现上述目的及其他相关目的，本发明还提供一种并联机器人的建模系统，所述建模系统包括：

无约束系统模型建立单元，用于建立并联机器人的无约束系统模型；

约束力解析数据获取单元，用于将物理约束转换为特定数据形式引入，以获取满足给定物理约束的约束力解析数据；

动力学模型获取单元，用于将所述约束力解析数据附加到所述无约束系统模型中，以获取并联机器人的动力学模型。

为实现上述目的及其他相关目的，本发明还提供一种并联机器人的控制方法，其所述控制方法包括：

根据上述任意一项所述的建模方法建立并联机器人的动力学模型；

将所述并联机器人的动力学模型视为无约束系统，将期望轨迹抽象成虚拟约束，将所述虚拟约束转换为特定数据形式引入，以获取所述并联机器人满足所述虚拟约束所需要的驱动力或力矩；

根据所述驱动力或力矩对所述并联机器人的轨迹进行跟踪控制。

在一实施例中，所述期望轨迹包括期望位置轨迹和期望速度轨迹。

为实现上述目的及其他相关目的，本发明还提供一种并联机器人的控制系统，所述控制系统包括：

动力学模型建立单元，用于根据权上述的建模方法建立并联机器人的动力学模型；

驱动力或力矩获取单元，用于将所述并联机器人的动力学模型视为无约束系统，将期望轨迹抽象成虚拟约束，并将所述虚拟约束转换为特定数据形式引入，以获取所述并联机器人满足所述虚拟约束所需要的驱动力或力矩；

轨迹跟踪控制单元，用于根据所述驱动力或力矩对所述并联机器人的轨迹进行跟踪控制。

本发明的建模方法，分析形式的并联机器人的运动方程不使用额外的辅助变量的情况下获得的(例如拉格朗日乘数或伪广义速度)，建立的模型是解析形式，并且能够得到系统约束力的解析表达形式，并且建模过程清晰简洁；

本发明的建模方法，将并联机器人分成几个支链子系统，采用运动学约束来描述各子系统和末端执行器、子系统与基座之间的物理连接，通过级联特性，该方法在做系统建模时具有普遍性，系统性和简单性；

利用本发明的建模方法，通过仿真分析可知各子系统的基座连接处固定且末端执行器轨迹重合，故该建模方法准确且有效；

本发明的控制方法，在得到准确的动力学模型后，将其视为无约束系统，同样利用U-K方程的级联性质，将期望轨迹以标准的二阶微分形式引入，求解额外约束力，通过仿真分析可知，各个子系统与基座的连接处的广义坐标始终未常数，视为固定不动，各个子系统的末端执行器运动轨迹重合，且良好地跟踪期望的轨迹，故该控制方法具有有效性和准确性。

附图说明

图1显示为本发明的并联机器人建模方法的流程示意图。

图2显示为本发明的二自由度冗余驱动并联机器人的结构示意图。

图3显示为本发明的二自由度冗余驱动并联机器人的一个子系统的结构示意图。

图4显示为本发明的并联机器人建模系统的框图。

图5显示为本发明的并联机器人控制方法的流程示意图。

图6显示为本发明的并联机器人控制系统的框图。

图7显示为本发明的实现并联机器人建模方法的第一服务装置的框图。

图8显示为本发明的实现并联机器人控制方法的第二服务装置的框图。

图9显示为本发明的动力学模型在仿真模拟时各子系统的末端轨迹图。

图10a-c显示为本发明的动力学模型在仿真模拟时各子系统基座连接处的坐标随时间的变化趋势图。

图11a-c显示为本发明的轨迹跟踪仿真模拟时各子系统末端执行器的坐标随时间的变化趋势图。

图12显示为轨迹跟踪仿真模拟时各子系统末端位置在X-Y平面的轨迹。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅图1-12。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本实施例以平面2自由度冗余驱动并联机器人为研究对象，结合Udwadia-Kalaba(U-K)方法方法和拉格朗日方程两种方法，建立了2自由度冗余驱动并联机器人的闭合运动方程。U-K方法在建立运动方程时不需要借助额外辅助变量，该方法最为显著的优点是建立的模型中约束力是解析解的表达式，并且建模的过程步骤清晰简洁，即首先利用牛顿-欧拉法或拉格朗日法建立无约束系统模型，然后将物理约束转化为Piffian标准形式(也即特定数据形式)，最后利用U-K方法获得满足给定约束的约束力解析解(也即约束力解析数据)并附加到无约束系统模型中。在得到动力学模型的基础上，引入期望约束轨迹进行轨迹跟踪控制，最后通过数值仿真，证明了该方法的准确性和有效性。

U-K理论的基本公式能被分成三部分。在解释U-K理论之前，简单介绍Moore-Penrose广义逆阵的基础知识。假设矩阵A的Moore-Penrose广义逆阵为A⁺,A⁺为满足以下方程组(Penrose方程组)的A的唯一解：

AA⁺A＝A

A⁺AA⁺＝A⁺

AA⁺＝(AA⁺)^T

A⁺A＝(A⁺A)^T (1)

首先，对于一个无约束的机械系统，假设该系统有n个状态变量q＝[q₁,q₂,q₃...q_n]^T，根据牛顿力学或者拉格朗日力学写出该系统运动方程：

初始条件为：q(t＝0)＝q₀；

其中t∈R是时间，q∈Rⁿ是广义的坐标，

是广义速度,

是广义加速度，M(q，t)＝M^T(q，t)∈R^n×n是惯性矩阵，

包括由于重力，给定力(或压力)和科氏力/离心力引起的影响力。

其次，假设该系统存在m个约束(m≤n)

其中l＝1，2，3....m。上述约束被看做是一阶形式，将一阶形式的约束写成如下矩阵：

其中A(q，t)为m×n阶矩阵(m≤n)，C(q,t)为m×1阶列向量。要想得到约束的二阶形式，必须将一阶形式的约束对时间求一次导。(若给定约束是零阶的，则需要将其对时间求二次导。)一阶形式求导后如下式：

其中：

在式(5)中，我们将二阶项

放在等式左边，其余移项到等式右边，并设

联立式(5)和式(6),并写成矩阵形式：

其中A(q，t)为m×n阶矩阵，

为m×1阶列向量。

无约束状态下的运动方程为式(2)，然而当系统引入若干个外界约束时，该机械系统就成为“受约束机械系统”，其运动学方程为：

其中

为由于外加约束产生的额外约束力。假设式(7)中A(q，t)的秩≥1，也假定对称矩阵M(q,t)可逆。上述约束机械系统的约束运动方程的解析形式如下：

式中

B⁺(q，t)为B(q，t)的Moore-Penrose广义逆矩阵。我们对比式(8)和式(9)可得：

相比拉格朗日方法，U-K方程不使用额外的辅助变量求解。约束力为解析解且仅基于与“非约束系统”相同的系统状态变量q。

接下来我们将介绍本发明的并联机器人的动力学建模，图1示出了本发明的并联机器人建模方法的流程示意图。请参阅图1，所述并联机器人建模方法包括如下步骤：步骤S11、建立并联机器人的无约束系统模型；步骤S12、将物理约束转换为二阶标准微分形式(也可以是其他合适的特定数据形式，在本实施例中，采用二阶标准微分形式)引入，以获取满足给定物理约束的约束力解析解(也即约束力解析数据)；以及步骤S13、将所述约束力解析解附加到所述无约束系统模型中，以获取所述并联机器人的动力学模型。

下文将以2自由度冗余驱动并联机器人为例进行说明，其中，图2示出了一个二自由度冗余驱动并联机器人的结构示意图，如图2所示，其由3个子系统4组成。每个子系统4的一端A_i(i＝1，2，3)分别通过第一旋转关节1各自固定在支座(未图示)上，3个支座上的第一旋转关节1由3个电机提供驱动力矩，而每个子系统4的另一端(公共端E)通过第二旋转关节2连接在一起，并且在该连接处E固定有末端执行器。如图2所示，在本实施例中，所述每个子系统分别包括两个连杆41和42，两个连杆41和42通过第三活动关节3连接。

在步骤S11中，由于直接写出2自由度冗余驱动并联机器人的动力学方程十分困难，本实施例根据U-K方程的级联特性，采用层级建模的方法得到2自由度冗余驱动并联机器人的动力学方程，因此本实施例所述建立并联机器人的无约束系统模型的步骤包括：将并联机器人分割成若干独立的无约束子系统；获取每个所述无约束子系统的动力学方程；将全部的所述无约束子系统的动力学方程联立成矩阵形式，以获取所述并联机器人的所述无约束系统模型。下面将具体进行介绍。

在步骤S11中，以图2所示的2自由度冗余驱动并联机器人为例，可将2自由度冗余驱动并联机器人分割成3个子系统(图3示出了一个子系统的结构示意图)，通过拉格朗日力学，我们容易得到子系统的动力学方程(也即无约束子系统模型，下文也做此理解)。需要说明的是，在其他实施例中，也可以通过牛顿-欧拉法获取各子系统的动力学方程。

拉格朗日力学主要以两个基本方程为基础：其一是针对直线运动；其二是针对旋转运动。我们先定义拉格朗日函数为：

L＝K-P (11)

其中：L为拉格朗日函数，K为系统的总动能，P为系统的总势能。K和P可以用任何方便的坐标来表示。于是系统的动力学方程式，即拉格朗日方程如下：

其中F_i为x_i方向的所有外力之和，T_i为产生q_i方向旋转的所有外力矩之和，x_i和q_i为系统变量。为了得到运动方程，首先需要推导系统的能量方程，然后根据上式(12)对拉格朗日函数式(11)求导。

由于系统水平，所以重力势能始终不变，选取系统所在平面为0势能面,故P＝0。系统总动能K为两个连杆的动能K₁和K₂之和。两个连杆L_ai和L_bi均具有质量分布，将其质量等效看做在质点上，在计算动能时需要考虑转动惯量。图3所示的第i个子系统的参数如下表1所示：

表1第i个子系统的参数(i＝1,2,3)

根据第i个子系统的参数，写出两连杆质点速度如下：

所以系统总动能为：

对拉格朗日函数(14)求导可得：

在式(15)和式(16)中，选取q_i＝[q_ai，q_bi，x_ai，y_ai]^T为系统第i(i＝1,2,3)个机械臂子系统的状态变量，通过拉格朗日方法，得出第i个机械臂子系统的运动方程的矩阵形式如下：

式中M_i和C_i是第i个子系统的质量矩阵和离心力/科氏力，τ_i是由组装在第i个主动关节中的制动器施加的外部扭矩。M_i和C_i如下所示：

其中：

C_i13＝C_i14＝C_i22＝C_i23＝C_i24＝C_i33＝C_i34＝C_i43＝C_i44＝0；

选取系统状态变量为q＝[q_a1，q_b1，x_a1，y_a1，q_a2，q_b2，x_a2，y_a2，q_a3，q_b3，x_a3，y_a3]^T，将这3个没有约束的子系统的运动方程联立成矩阵形式：

在步骤S12中，将物理约束转换为二阶标准微分形式引入，所述物理约束包括基座约束和末端执行器约束，作为示例，在下文中，二阶标准微分形式例如可采用二阶Piffian标准微分形式，下面将分两个方面进行阐述。

在步骤S12中，一方面将3个子系统的一端利用约束各自固定在一个基座上，假设第i个子系统的末端主动关节为A_i,对应基座A′_i的坐标为(x(A′_i)，y(A′_i))。由于主动关节通过第一旋转关节1与基座连接，因此第i个主动关节和基座之间的运动约束可表示为：

x(A_i)＝x(A_i)，y(A_i)＝y(A_i) i＝1，2，3 (20)

为了得到约束的二阶形式，将式(19)对时间t进行两次微分，得到二阶约束如下：

其中，

另一方面，把3个子系统的另一端E_i(i＝1,2,3)通过第二旋转关节2连接起来，将末端执行器固定在连接处E。设x(E_i)和y(E_i)表示末端执行器E在第i个子系统中的广义坐标。所以这3个子系统的运动学约束为：

同样为了得到约束的二阶形式，将式(22)对时间t进行两次微分，得到二阶约束如下：

其中：

我们已经得到了支座约束和末端执行器约束的二阶形式，然后在式(19)上同时引入支座约束和末端执行器约束，通过U-K方程，得出2自由度冗余驱动并联机器人的动力学方程如下(上述步骤S13)：

其中：

A＝(A₁ A₂)；

b＝(b₁ b₂)；

综上所述，本实施例中，2自由度冗余驱动并联机器人的动力学建模过程总结如下：

首先，将2自由度冗余驱动并联机器人拆分为3个独立的无约束子系统，根据拉格朗日方程，分别写出独立子系统的动力学方程，然后将3个方程联立成矩阵形式，作为并联机器人的无约束系统模型(对应上述步骤S11)；接着，同时引入末端执行器和基座的约束，将约束写成二阶Piffian标准微分形式，利用U-K方程，得到理想的2自由度冗余驱动并联机器人的约束力解析解(对应上述步骤S12)；将获取的约束力解析解附加到并联机器人的无约束系统模型，得到完整的2自由度冗余驱动并联机器人的动力学模型(也即上述步骤S13)。

如图4所示，本发明的实施例还提供一种并联机器人的建模系统100，该建模系统包括无约束系统模型建立单元101，用于建立并联机器人的无约束系统模型；约束力解析解获取单元102，用于将物理约束转换为二阶标准微分形式引入，以获取满足给定物理约束的约束力解析解；动力学模型获取单元103，用于将所述约束力解析解附加到所述无约束系统模型中，以获取并联机器人的动力学模型。

需要说明的是，在本实施例中，上述各单元的功能详见上文中的各步骤，在此不做赘述。

需要说明的是，上述的各功能单元，实际实现时可以全部或部分集成到一个物理实体上，也可以物理上分开。且这些单元可以全部以软件通过处理元件调用的形式实现；也可以全部以硬件的形式实现；还可以部分单元通过处理元件调用软件的形式实现，部分单元通过硬件的形式实现。此外这些单元全部或部分可以集成在一起，也可以独立实现。这里所述的处理元件可以是一种集成电路，具有信号的处理能力。在实现过程中，上述方法的各步骤或以上各个模块可以通过处理器元件中的硬件的集成逻辑电路或者软件形式的指令完成。

需要说明的是，如图7所示，本实施例的并联机器人建模方法还可以通过第一服务装置300实现，所述第一服务装置300包括相互连接的存储器303和处理器301，所述存储器301存储有计算机程序，该程序被所述处理器301执行时实现所述并联机器人的建模方法。需要说明的是，当需要和外部进行通信时，所述第一服务装置300还包括通信器302，所述通信器302与所述处理器301连接。

上述的处理器301以及下文将要介绍的处理器401可以是通用处理器，包括中央处理器(CentralProcessingUnit，简称CPU)、网络处理器(NetworkProcessor，简称NP)等；还可以是数字信号处理器(DigitalSignalProcessing，简称DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，简称ASIC)、现场可编程门阵列(Field－ProgrammableGateArray，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件；上述的存储器303以及下文将要介绍的第二处理器402可能包含随机存取存储器(RandomAccessMemory，简称RAM)，也可能还包括非易失性存储器(non-volatilememory)，例如至少一个磁盘存储器。

需要说明的是，上述存储器303以及下文中将要介绍的存储器403中的计算机程序可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，电子设备，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。

本发明实施例中还可以提供一种第一计算机存储介质，其存储有计算机程序，该程序被处理器执行时实现所述的机器人的建模方法；所述第一计算机存储介质包括所有形式的非易失性存储器、介质和存储器设备，包括例如：半导体存储器设备，例如EPROM、EEPROM和闪存设备；磁盘，例如内部硬盘或可移动盘；磁光盘；以及CD-ROM和DVD-ROM盘。

在得到了准确的2自由度冗余驱动并联机器人动力学模型后，为了让并联机器人完成期望功能，我们就必须对其进行轨迹控制，使其按期望轨迹运行。传统的轨迹跟踪控制大多是利用位置和速度传感器获得末端执行器的位置和速度，然后和给定的轨迹进行比较从而获得跟踪误差。最后采用不同控制方法(如PID、鲁棒控制、自适应控制等)将误差控制在合理范围内，从而实现轨迹跟踪。本实施例采用一种新的2自由度冗余驱动并联机器人轨迹跟踪控制思路，从U-K方法出发，将所需要跟踪的位置(或速度)轨迹(也即期望轨迹)抽象成虚拟的约束，然后将该约束转化为Piffian标准形式，再利用U-K方程获得机器人满足该轨迹约束所需要的驱动力矩，从而实现机器人的轨迹跟踪控制。具体地，请参阅图5，所述控制方法包括以下步骤，步骤S21、建立并联机器人的动力学模型；步骤S22、将所述并联机器人的动力学模型视为无约束系统，将期望轨迹抽象成虚拟约束，将所述虚拟约束转换为二阶标准微分形式引入，以获取所述并联机器人满足所述虚拟约束所需要的驱动力或力矩；步骤S23、根据所述驱动力或力矩对所述并联机器人的轨迹进行跟踪控制。

在步骤S21中，可以按照上文介绍的并联机器人建模方法建立并联机器人的动力学模型，详见上文，再次不做赘述。

在步骤S22中，先将2自由度冗余驱动并联机器人看做“无约束系统”。假设：

将式(24)看做“无约束系统”，改写成无约束形式：

基于式(26)动力学方程，我们将期望的轨迹以二阶约束的形式引入。现在我们期望末端执行器E的x坐标和y坐标分别为时t的函数f_x(t)和f_y(t)，由于3个子系统已经在末端执行器处约束在一起，故只需在一个子系统的末端引入约束：

将式(27)对时间t求二次导，将二阶微分写成矩阵形式的约束：

若式(26)动力学模型要满足约束式(28)，就需要给该“无约束系统”添加额外的约束力/力矩，假设额外的约束力/力矩为

根据U-K方程可知：

其中：

至此，我们计算出跟踪期望轨迹所需要提供的额外力/力矩

在步骤S23中，可以根据获取的额外力、力矩对所述并联机器人的轨迹进行跟踪控制。

需要说明的是，在本实施例中，所述期望轨迹包括期望位置轨迹和期望速度轨迹。

如图6所示，本发明的实施例介绍一种并联机器人的控制系统200，所述控制系统包括动力学模型建立单元201，用于上述的所述的建模方法建立并联机器人的动力学模型；驱动力或力矩获取单元202，用于将所述并联机器人的动力学模型视为无约束系统，将期望轨迹抽象成虚拟约束，并将所述虚拟约束转换为二阶标准微分形式引入，以获取所述并联机器人满足所述虚拟约束所需要的驱动力或力矩；轨迹跟踪控制单元203，用于根据所述驱动力或力矩对所述并联机器人的轨迹进行跟踪控制。

需要说明的是，在本实施例中，图6中各单元的功能详见上文中的各步骤，在此不做赘述。各功能单元实际实现时可以全部或部分集成到一个物理实体上，也可以物理上分开。且这些单元可以全部以软件通过处理元件调用的形式实现；也可以全部以硬件的形式实现；还可以部分单元通过处理元件调用软件的形式实现，部分单元通过硬件的形式实现。此外这些单元全部或部分可以集成在一起，也可以独立实现。这里所述的处理元件可以是一种集成电路，具有信号的处理能力。在实现过程中，上述方法的各步骤或以上各个模块可以通过处理器元件中的硬件的集成逻辑电路或者软件形式的指令完成。

需要说明的是，如图8所示，本实施例的并联机器人控制方法还可以通过第二服务装置400来实现，所述第二服务装置400包括相互连接的存储器403和处理器401，所述存储器401存储有计算机程序，该程序被所述处理器401执行时实现所述并联机器人的建模方法。需要说明的是，当需要和外部进行通信时，所述第二服务装置400还包括通信器402，所述通信器402与所述处理器401连接。

上述的处理器401和存储器403的详细介绍详见上文中处理器301和存储器303的介绍，在此不做赘述。

本发明实施例中还可以提供一种第二计算机存储介质，其存储有计算机程序，该程序被处理器执行时实现所述的机器人的控制方法；所述第二计算机存储介质包括所有形式的非易失性存储器、介质和存储器设备，包括例如：半导体存储器设备，例如EPROM、EEPROM和闪存设备；磁盘，例如内部硬盘或可移动盘；磁光盘；以及CD-ROM和DVD-ROM盘。

下面将通过数值仿真分析来验证所述出的建模方法和控制方法的可行性，具体从两方面来说，分别是(一)动力学建模仿真和(二)轨迹跟踪仿真。

(一)动力学建模仿真

通过之前的动力学分析，并结合拉格朗日方程和Udwadia-Kalab方程得到了理想的无关节摩擦的2自由度冗余驱动并联机器人的动力学方程。在本小节，我们将通过数值模拟的方法来验证所提出的建模方法的有效性。本实施例中三个子系统的动力学参数如表(2)所示。

表2子系统连杆动力学的参数

在进行数值模拟之前，还需要知道每个子系统各个连杆之间的初始角度值和三个基座的位置坐标，在这里我们假设初始的速度/角速度和加速度/角加速度在初始时刻都为0，并选取任意一组满足约束的初始值进行仿真。选择初始状态q₀＝[1.3015,2.1752,0,0.25,2.9105,1.4593,0.43,0,2.9810,1.8776,0.4269,0.5005]^T。另外，我们给该系统一个输入力矩，以验证三个子系统的末端轨迹是否重合以及与基座连接处是否固定，故令式(18)中的外加力矩τ＝[0.1cos(πt),0,0,0,0,0,0,0,0,0,0,0]^T,其仿真结果图如图9和图10所示。

图9为三个子系统末端位置在X-Y平面上的轨迹。我们将原系统拆分为3个子系统后，三个系统末端可以各自独立运动。但在引入末端约束后，由图9可知，三个子系统在外加力矩τ的作用下，轨迹重合。

我们在建立系统动力学模型时，基座与子系统也是通过引入约束来进行固定的，而不是一开始就将其固定在三个基座上。仿真结果图10展示了三个子系统基座连接处的坐标随时间的变化趋势。三个子系统与基座的连接处通过引入约束后，在外加力矩τ的作用下，坐标相对于时间的变化为常数，可知其固定在基座处不动。

由仿真结果可知，运用本文提出的建模方法，子系统的基座连接处固定且末端执行器轨迹重合，故表明该方法准确，有效。

(二)轨迹跟踪仿真

我们期望末端执行器E的x坐标和y坐标分别为时t的函数f_x(t)和f_y(t)，在行轨迹跟踪控制数值仿真时，设f_x(t)＝x_e1+0.01cos(t)，f_y(t)＝y_e1，其中理想x坐标变化曲线为余弦函数，理想y坐标变化曲线为常数，合成理想的末端运动轨迹为水平往复运动的直线。根据式(27)和(28)可得：

将式(30)对时间t求二次导，将二阶微分写成矩阵形式的约束：

式(30)中(x_e1，y_e1)是选取的初始的基准点的位置坐标，根据初始位置而定，为常数。

在进行期望的轨迹跟踪时，对于式(24)中的外加力矩τ，首先将其看做0，利用U-K方程求出

此时

就是满足期望约束要提供的外加力矩τ。轨迹跟踪仿真的仿真初始状态和子系统动力学参数和动力学建模仿真相同。根据式(26)和式(31),数值仿真结果如图11和图12所示。

图11表示3个子系统末端执行器的x轴坐标和y轴坐标随时间的变化趋势。根据式(30)，预期的轨迹为末端执行器x轴为余弦函数，y轴为静止，由仿真结果图11可知，各子系统x坐标和y坐标与理想轨迹重合，满足期望约束。

图12为三个子系统末端位置在X-Y平面上的轨迹。我们将原系统拆分为3个子系统后，三个系统末端可以各自独立运动，但在引入末端约束和期望轨迹约束后，由图12可知，三个子系统在外加力矩τ的作用下，末端执行器跟踪期望的水平直线，并且3个子系统轨迹重合。

运用本文提出的建模方法和轨迹跟踪控制方法，三个子系统与基座的连接处的广义坐标始终未常数，视为固定不动，三个子系统的末端执行器运动轨迹重合，且良好地跟踪期望的轨迹。

综上所述，在本实施例中，我们提出了一种基于U-K方程的新型建模方法。该方法利用U-K方程的级联性质建立并联机器人的运动方程。该方法最为显著的优点是分析形式的并联机器人的运动方程是在没有辅助变量的情况下获得的(例如拉格朗日乘数或伪广义速度)，建立的模型是解析形式，并且能够得到系统约束力的解析表达形式。该方法将并联机器人分成几个支链子系统，采用运动学约束来描述各子系统和末端执行器、子系统与基座之间的物理连接。通过级联特性，提出的方法在做系统建模时具有普遍性，系统性和简单性。在得到准确的动力学模型后，将其视为无约束系统，同样利用U-K方程的级联性质，将期望轨迹以标准的二阶微分形式引入，求解额外约束力。最后通过数值仿真证明了本文提出的建模方法和轨迹跟踪控制方法的有效性和准确性。

在本文的描述中，提供了许多特定细节，诸如部件和/或方法的实例，以提供对本发明实施例的完全理解。然而，本领域技术人员将认识到可以在没有一项或多项具体细节的情况下或通过其他设备、系统、组件、方法、部件、材料、零件等等来实践本发明的实施例。在其他情况下，未具体示出或详细描述公知的结构、材料或操作，以避免使本发明实施例的方面变模糊。

在整篇说明书中提到“一个实施例(one embodiment)”、“实施例(anembodiment)”或“具体实施例(a specific embodiment)”意指与结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中，并且不一定在所有实施例中。因而，在整篇说明书中不同地方的短语“在一个实施例中(in one embodiment)”、“在实施例中(inan embodiment)”或“在具体实施例中(in a specific embodiment)”的各个表象不一定是指相同的实施例。此外，本发明的任何具体实施例的特定特征、结构或特性可以按任何合适的方式与一个或多个其他实施例结合。应当理解本文所述和所示的发明实施例的其他变型和修改可能是根据本文教导的，并将被视作本发明精神和范围的一部分。

还应当理解还可以以更分离或更整合的方式实施附图所示元件中的一个或多个，或者甚至因为在某些情况下不能操作而被移除或因为可以根据特定应用是有用的而被提供。

另外，除非另外明确指明，附图中的任何标志箭头应当仅被视为示例性的，而并非限制。此外，除非另外指明，本文所用的术语“或”一般意在表示“和/或”。在术语因提供分离或组合能力是不清楚的而被预见的情况下，部件或步骤的组合也将视为已被指明。

如在本文的描述和在下面整篇权利要求书中所用，除非另外指明，“一个(a)”、“一个(an)”和“该(the)”包括复数参考物。同样，如在本文的描述和在下面整篇权利要求书中所用，除非另外指明，“在…中(in)”的意思包括“在…中(in)”和“在…上(on)”。

本发明所示实施例的上述描述(包括在说明书摘要中所述的内容)并非意在详尽列举或将本发明限制到本文所公开的精确形式。尽管在本文仅为说明的目的而描述了本发明的具体实施例和本发明的实例，但是正如本领域技术人员将认识和理解的，各种等效修改是可以在本发明的精神和范围内的。如所指出的，可以按照本发明所述实施例的上述描述来对本发明进行这些修改，并且这些修改将在本发明的精神和范围内。

本文已经在总体上将系统和方法描述为有助于理解本发明的细节。此外，已经给出了各种具体细节以提供本发明实施例的总体理解。然而，相关领域的技术人员将会认识到，本发明的实施例可以在没有一个或多个具体细节的情况下进行实践，或者利用其它装置、系统、配件、方法、组件、材料、部分等进行实践。在其它情况下，并未特别示出或详细描述公知结构、材料和/或操作以避免对本发明实施例的各方面造成混淆。

因而，尽管本发明在本文已参照其具体实施例进行描述，但是修改自由、各种改变和替换意在上述公开内，并且应当理解，在某些情况下，在未背离所提出发明的范围和精神的前提下，在没有对应使用其他特征的情况下将采用本发明的一些特征。因此，可以进行许多修改，以使特定环境或材料适应本发明的实质范围和精神。本发明并非意在限制到在下面权利要求书中使用的特定术语和/或作为设想用以执行本发明的最佳方式公开的具体实施例，但是本发明将包括落入所附权利要求书范围内的任何和所有实施例及等同物。因而，本发明的范围将只由所附的权利要求书进行确定。

Claims (5)

1.一种并联机器人控制方法，其特征在于，所述并联机器人包括2自由度冗余驱动并联机器人，其由3个子系统组成，每个所述子系统的一端分别通过第一旋转关节各自固定在基座上，3个基座上的所述第一旋转关节由3个电机提供驱动力矩，而每个所述子系统的另一端通过第二旋转关节连接在一起，并且在该连接处固定有末端执行器，所述控制方法包括：

将并联机器人分割成三个独立的无约束子系统；

获取每个所述无约束子系统的无约束子系统模型；

将全部的所述无约束子系统的无约束子系统模型联立以形成所述并联机器人的无约束系统模型；

将基座约束和末端执行器约束转换为二阶Piffian标准微分形式引入，以获取满足给所述基座约束和末端执行器约束的约束力解析数据；以及

同时将所述基座约束和末端执行器约束的约束力解析数据附加到所述无约束系统模型中，以获取所述并联机器人的动力学模型；

将所述并联机器人的动力学模型视为无约束系统，将期望轨迹抽象成虚拟约束，将所述虚拟约束转换为二阶Piffian标准微分形式引入，以获取所述并联机器人满足所述虚拟约束所需要的驱动力或力矩；

根据所述驱动力或力矩对所述并联机器人的轨迹进行跟踪控制。

2.根据权利要求1所述的并联机器人控制方法，其特征在于，利用拉格朗日法获取每个所述无约束子系统的无约束子系统模型。

3.根据权利要求1所述的并联机器人控制方法，其特征在于，利用牛顿-欧拉法获取每个所述无约束子系统的无约束子系统模型。

4.根据权利要求1所述的并联机器人控制方法，其特征在于，所述期望轨迹包括期望位置轨迹和期望速度轨迹。

5.一种并联机器人控制系统，其特征在于，所述并联机器人包括2自由度冗余驱动并联机器人，其由3个子系统组成，每个所述子系统的一端分别通过第一旋转关节各自固定在基座上，3个基座上的所述第一旋转关节由3个电机提供驱动力矩，而每个所述子系统的另一端通过第二旋转关节连接在一起，并且在该连接处固定有末端执行器，所述控制系统包括：

无约束系统模型建立单元，用于建立并联机器人的无约束系统模型；

约束力解析数据获取单元，用于将基座约束和末端执行器约束转换为二阶Piffian标准微分形式引入，以获取满足给所述基座约束和末端执行器约束的约束力解析数据；

动力学模型获取单元，用于同时将所述基座约束和末端执行器约束的约束力解析数据附加到所述无约束系统模型中，以获取所述并联机器人的动力学模型；

驱动力或力矩获取单元，用于将所述并联机器人的动力学模型视为无约束系统，将期望轨迹抽象成虚拟约束，并将所述虚拟约束转换为二阶Piffian标准微分形式引入，以获取所述并联机器人满足所述虚拟约束所需要的驱动力或力矩；

轨迹跟踪控制单元，用于根据所述驱动力或力矩对所述并联机器人的轨迹进行跟踪控制。

Images