CN103513612B - 控制工业机器人与变位机协调运动的系统及方法 - Google Patents

Abstract

本发明公开了一种控制工业机器人与变位机协调运动的系统及方法，方法包括：S₁、建立连杆坐标系及正运动学模型；S₂、获得变位机的工作台坐标系与工业机器人的基坐标系的变换关系；S₃、从作业曲线上选取多个点，确定每个点的坐标及轴角；S₄、利用同一自变量建立第一三次样条插值函数及第二三次样条插值函数；S₅、设定工业机器人的速度；S₆、计算工业机器人当前的位移量，计算自变量的取值；S₇、计算位置插补量及变位机的轴角插补量；S₈、对工业机器人进行运动学逆解，以获得工业机器人的轴角插补量；S₉、控制工业机器人与变位机进行运动。本发明解决了工业机器人与变位机沿复杂空间轨迹协调运动的问题。

Description

控制工业机器人与变位机协调运动的系统及方法

技术领域

本发明涉及一种控制工业机器人与变位机协调运动的系统及方法，特别是涉及一种采用统一的参数变量对工业机器人及变位机的运动轨迹插补进行规划的系统以及一种利用所述系统实现的方法。

背景技术

在工业机器人进行焊接或切割等作业下，不但要求工业机器人能够严格按照特定的空间轨迹进行插补运动，所谓插补，即根据给定的曲线函数，在理想的轨迹上的已知点之间，进行数据点密化，确定一些中间点的方法，而且对工业机器人到达每个轨迹点时的姿态也有一定的要求。对于复杂的空间轨迹，很难保证工业机器人在进行轨迹插补的时候满足上述的要求。现有技术中较佳的方法是引入变位机系统，将工件放在变位机上，配合工业机器人系统进行协调运动。目前，现有的工业机器人与变位机系统能够进行简单的空间轨迹协调运动，比如直线，简单的圆弧等。但是，对于复杂的空间轨迹，主要还是依赖于工业机器人的离线编程技术获得轨迹的所有几何信息，然后再进行协调运动的插补算法，这对于目前主流的基于示教再现的机器人系统并不适用。

国内外的研究机构对于工业机器人和变位机系统协调运动也进行了大量的研究，上海交通大学的唐荣俊在其硕士毕业论文《焊接机器人与变位机的协调控制》中提出了一种分段三次埃米尔特插值算法来实现机器人与变位机系统协调运动，该算法能够保证整个协调运动的插补过程中机器人和变位机的运动速度均匀且插补点平滑，但是要求示教点必须分布均匀，这在实际的焊接过程中并不容易完全保证。河海大学的康艳军在其硕士毕业论文《弧焊机器人运动学规划及机器人/变位机的运动仿真》中提出一种根据焊缝特征约束，进行机器人与变位机系统协调运动的插补算法，该算法能够根据空间轨迹的特征进行自动的机器人与变位机的运动规划，但是必须要知道空间轨迹上每个点的几何特征，这在基于示教再现的机器人系统中很难实现。东南大学的唐创奇等人在期刊《工业控制机器人》中发表的《弧焊机器人与变位机协调运动的实现》一文，提出了一种根据空间轨迹的长度和焊接速度，获得焊接时间，然后根据这些信息先规划变位机的运动，再让机器人配合其运动的算法，但是该方法是基于时间的，即机器人与变位机的协调运动是提前就规划好的，在运动过程中不可以再进行速度的调整。

因此，现有的工业机器人与变位机的协调运动插补算法都存在一些缺点，具有一定的局限性，并不能很好地实现工业机器人与变位机沿复杂空间轨迹进行协调运动。

发明内容

本发明要解决的技术问题是为了克服现有技术中工业机器人与变位机的协调运动插补算法存在缺点、具有一定的局限性的缺陷，提供一种采用统一的参数变量对工业机器人及变位机的运动轨迹插补进行规划的系统以及一种利用所述系统实现的方法。

本发明是通过下述技术方案来解决上述技术问题的：

本发明的目的在于提供了一种控制工业机器人与变位机协调运动的方法，其特点在于，所述方法包括以下步骤：

S₁、分别建立所述工业机器人与所述变位机的连杆坐标系及正运动学模型；

在步骤S₁中可根据DH原理(一种机器人系统坐标系的建立原理)或其他连杆坐标系的建立方式，建立所述连杆坐标系及所述正运动学模型。

S₂、根据坐标变换原理获得所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系；

具体可根据所述工业机器人与所述变位机的基座安装参数来确定所述变位机的基坐标系到所述工业机器人的基坐标系之间的变换关系，再根据所述变位机的正运动学模型，即得到所述变位机的工作台坐标系到所述变位机的基坐标系之间的变换关系，这样，利用坐标变换原理就可以求得所述变位机的工作台坐标系到所述工业机器人的基坐标系之间的变换关系。

S₃、确定安装在所述变位机的工作台上的工件的作业曲线，从所述作业曲线上选取多个点，所述多个点包括所述作业曲线的起点和终点，确定选取的每个点在所述变位机的工作台坐标系下的坐标以及所述工业机器人与所述变位机在选取的每个点处的轴角；

S₄、利用同一自变量建立所述多个点的第一三次样条插值函数以及变位机在选取的每个点处的轴角的第二三次样条插值函数，并获得每相邻的两个点之间的所述第一三次样条插值函数的三次多项式以及每相邻的两个点处所述变位机的轴角之间的所述第二三次样条插值函数的三次多项式；

所谓三次样条插值函数是一种分段函数，它在所述多个点分成的每个小区间(即每相邻的两个点之间形成的区间)上都是三次多项式，并且对于不同的小区间，其三次多项式之间的系数一般也是不同的，在步骤S₄中，利用所述多个点在所述变位机的工作台坐标系下的坐标值以及变位机在每个点处的轴角就能够计算出每相邻的两个点之间的三次多项式的系数。步骤S₄中利用所述第一三次样条插值函数对所述多个点进行插补运算，以对所述作业曲线进行拟合，并利用所述第二三次样条插值函数对所述多个点处的变位机的轴角进行插补运算。

S₅、通过定积分计算每相邻的两个点之间的曲线的长度以及所述作业曲线的总长度，设定所述工业机器人在沿每相邻的两个点之间的曲线运动时的速度、加速度以及加加速度；

所谓加加速度又称急动度，是描述加速度变化快慢的物理量。

S₆、根据设定的速度计算所述工业机器人当前的位移量，并根据所述当前的位移量计算每相邻的两个点之间的所述第一三次样条插值函数的三次多项式与所述第二三次样条插值函数的三次多项式中的自变量的当前取值；

利用速度变化曲线进行积分运算即可得到所述工业机器人运动时的当前的位移量，利用积分计算出的每相邻的两个点之间的曲线的长度就能够计算出经过的选取的点的曲线段总长度(即经过的每相邻的两个选取的点之间的曲线长度之和)，当前的位移量减去经过的曲线段的总长度即得到所述工业机器人在当前曲线段的位移量，再用所述工业机器人在当前曲线段的位移量除以当前曲线段的长度即得到三次多项式中自变量的当前取值。

S₇、根据自变量的当前取值计算当前所述工业机器人的位置插补量及所述变位机的轴角插补量；

将自变量的当前取值带入所述第一三次样条插值函数及所述第二三次样条插值函数在当前曲线段处的三次多项式，即可得到当前所述工业机器人的位置插补量及所述变位机的轴角插补量。

S₈、根据所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系，将所述位置插补量变换到所述工业机器人的基坐标系下，并对所述工业机器人进行运动学逆解，以获得所述工业机器人的轴角插补量；

S₉、利用控制器控制所述工业机器人与所述变位机分别按照所述工业机器人的轴角插补量与所述变位机的轴角插补量进行运动。

这样，利用本发明的所述方法就能够实现所述工业机器人及所述变位机沿任意空间轨迹进行协调运动，适用于示教类型的机器人控制系统，并且不需要知道空间轨迹上每个点的几何特征，示教点(即选取的点)也不需要均匀分布在空间轨迹上，而且在运动过程中可以任意改变所述工业机器人及所述变位机的运动速度，降低了空间轨迹插补算法的复杂度，很好地解决了工业机器人与变位机沿复杂空间轨迹协调运动的问题。

较佳地，步骤S₉之后还包括一步骤S₁₀：判断所述工业机器人的当前的位移量是否等于所述作业曲线的总长度，若是，则结束流程，若否，则返回步骤S₆。

较佳地，步骤S₅中设定的所述速度、加速度以及加加速度均是可调节的。

本发明的目的在于还提供了一种控制工业机器人与变位机协调运动的系统，其特点在于，所述系统包括：

一模型建立模块，用于分别建立所述工业机器人与所述变位机的连杆坐标系及正运动学模型；

一坐标系变换模块，用于根据坐标变换原理获得所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系；

一选取模块，用于确定安装在所述变位机的工作台上的工件的作业曲线，从所述作业曲线上选取多个点，所述多个点包括所述作业曲线的起点和终点，确定选取的每个点在所述变位机的工作台坐标系下的坐标以及所述工业机器人与所述变位机在选取的每个点处的轴角；

一函数建立模块，用于利用同一自变量建立所述多个点的第一三次样条插值函数以及变位机在选取的每个点处的轴角的第二三次样条插值函数，并获得每相邻的两个点之间的所述第一三次样条插值函数的三次多项式以及每相邻的两个点处所述变位机的轴角之间的所述第二三次样条插值函数的三次多项式；

一速度规划模块，用于通过定积分计算每相邻的两个点之间的曲线的长度以及所述作业曲线的总长度，设定所述工业机器人在沿每相邻的两个点之间的曲线运动时的速度、加速度以及加加速度；

一自变量计算模块，用于根据设定的速度计算所述工业机器人当前的位移量，并根据所述当前的位移量计算每相邻的两个点之间的所述第一三次样条插值函数的三次多项式与所述第二三次样条插值函数的三次多项式中的自变量的当前取值；

一插补量计算模块，用于根据自变量的当前取值计算当前所述工业机器人的位置插补量及所述变位机的轴角插补量；

一运动学逆解模块，用于根据所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系，将所述位置插补量变换到所述工业机器人的基坐标系下，并对所述工业机器人进行运动学逆解，以获得所述工业机器人的轴角插补量；

一控制器，用于控制所述工业机器人与所述变位机分别按照所述工业机器人的轴角插补量与所述变位机的轴角插补量进行运动。

较佳地，所述系统还包括一判断模块，用于判断所述工业机器人的当前的位移量是否等于所述作业曲线的总长度，若是，则不再操作，若否，则重新启用所述自变量计算模块。

较佳地，所述速度规划模块设定的所述速度、加速度以及加加速度均是可调节的。

本发明的积极进步效果在于：本发明能够实现工业机器人及变位机沿任意空间轨迹进行协调运动，适用于示教类型的机器人控制系统，并且不需要知道空间轨迹上每个点的几何特征，示教点也不需要均匀分布在空间轨迹上，而且在运动过程中可以任意改变工业机器人及变位机的运动速度，降低了空间轨迹插补算法的复杂度，很好地解决了工业机器人与变位机沿复杂空间轨迹协调运动的问题。

附图说明

图1为本发明的一较佳实施例的控制工业机器人与变位机协调运动的系统的结构示意图。

图2为本发明的一较佳实施例的控制工业机器人与变位机协调运动的方法的流程图。

具体实施方式

下面结合附图给出本发明较佳实施例，以详细说明本发明的技术方案。

如图1所示，本发明的控制工业机器人与变位机协调运动的系统包括一模型建立模块1、一坐标系变换模块2、一选取模块3、一函数建立模块4、一速度规划模块5、一自变量计算模块6、一插补量计算模块7、一运动学逆解模块8、一控制器9以及一判断模块10。

所述模型建立模块1首先根据DH原理或其他建立连杆坐标系的方式，分别建立工业机器人和变位机的连杆坐标系和正运动学模型和其中，表示工业机器人的工具坐标系相对工业机器人的基坐标系的齐次矩阵，表示变位机的工作台坐标系相对变位机的基坐标系的齐次矩阵。以DH原理建立的工业机器人的正运动学模型公式如下：

${}_{tool}^{base}T=T_1^0\·T_2^1\·T_3^2\·\mathrm{...}T_n^{n-1}\·T_F^n\·T_{tool}^F;$

其中，i＝1,2...n为工业机器人的第i个关节到底i-1个关节的齐次变换矩阵，n为工业机器人的轴的个数；

为工业机器人的法兰到工业机器人的第n个关节的齐次变换矩阵；

为工业机器人的末端工具坐标系到工业机器人的法兰的齐次变换矩阵；

同理，建立的变位机的正运动学模型公式如下：

${}_{postioner\_table}^{postioner\_base}T=T_1^0\·T_2^1\·T_3^2\·\mathrm{...}T_m^{m-1}\·T_{postioner\_table}^m;$

其中，i＝1,2...m为变位机的第i个关节到第i-1个关节的齐次变换矩阵，m为变位机的轴的个数。

为变位机的工作台坐标系到变位机的第m个关节的齐次变换矩阵。

所述坐标系变换模块2会通过标定或机器人示教等方式，根据坐标变换原理获得所述变位机的工作台坐标系到所述工业机器人的基坐标系的齐次变换矩阵。如果知道所述变位机的基坐标系到所述工业机器人的基坐标系之间的齐次变换矩阵则所述变位机的工作台坐标系到所述工业机器人的基坐标系可以表示如下：

$T_{postioner\_table}^{base}=T_{postioner\_base}^{base}\·T_{postioner\_table}^{postioner\_base};$

其中可以由变位机的轴角获得，在此就不再赘述。

在将工件安装在所述变位机的工作台上之后，所述选取模块3会确定安装在所述变位机的工作台上的工件的作业曲线，从所述作业曲线上选取多个点，所述多个点包括所述作业曲线的起点和终点。

具体会首先控制所述变位机运动，使得作业曲线上的第一个点处于合适的工作位置，然后控制所述工业机器人运动，使所述工业机器人的工具末端以合适的姿态到达该作业点。这里的合适的位置往往是根据加工工艺的需要确定的，例如对于焊接来说，作业曲线就是需要焊接的焊缝曲线，当焊缝处于水平位置时为最合适的位置。

所述选取模块3接着确定选取的每个点在所述变位机的工作台坐标系下的坐标(笛卡尔位姿)以及所述工业机器人与所述变位机在每个点处的轴角，记在第一个选取的点处轴角分别为θ_{1_robot}和θ_{1_postioner}，其中θ_{1_robot}和θ_{1_postioner}分别为一组维数为n和m的工业机器人和变位机的当前轴角。

具体可由上述的变位机的工作台坐标系与工业机器人的基坐标系之间的变换关系，获得各个选取点在变位机的工作台坐标系下的笛卡尔位姿，假设第i个选取点用P_i表示，P_i为一组工业机器人和变位机的轴角，则根据所述模型建立模块1建立的连杆坐标系和正运动学模型，可以获得在该选取点处，工业机器人的工具坐标系相对工业机器人的基坐标系的齐次变换矩阵和变位机的工作台坐标系相对变位机的基坐标系的齐次变换矩阵

根据坐标变换原理，即可得到该选取点在变位机的工作台坐标系中的位姿矩阵^{postioner_table}P_i；

${}^{postioner\_table}{P}_i=inv{(}_{postioner\_table}^{postioner\_base}T)\·inv\left(T_{postioner}^{base}\right)\·T_{tool}^{tase};$

其中inv表示矩阵求逆函数。

所述函数建立模块4会建立以u为自变量的分段三次多项式样条插值函数来拟合各个示教点，u的变化范围从0到1，当自变量u从0逐步增大到1的过程中，作业曲线的插补点也刚好从一个选取点过渡到下一个选取点，且采用该拟合方法可以保证在各个选取点处速度连续。

具体推导过程如下：由三次分段多项式样条插值函数的定义可知，假设S_i(u)是第i个轨迹区间[0，1]上是一个三次多项式，可假定具体方程为：

S_i(u)＝A_iu³+B_iu²+C_iu+D_i,(u∈[0,1],i＝0,1,…,n-1)

公式中S_i,A_i,B_i,C_i,D_i均为包含3个元素的列向量，例如，A_i＝[a_ix a_iy a_iz]^T。

注意到S_i″(u)在区间[0，1]上是一次多项式，如果S_i″(u)在第i个轨迹区间[0，1]的两个端点的取值已知，设S_i″(0)＝M_i,S_i″(1)＝M_i+1，则有

S_i″(u)＝(1-u)M_i+uM_i+1

对函数S_i″(u)进行一次和二次积分分别可得

$S_i^{\′}\left(u\right)=\∫S_i^{\′\′}\left(u\right)du=-\frac{1}{2}{(1-u)}^2{M}_i+\frac{1}{2}{u}^2{M}_{i+1}+{\tilde{C}}_i$

$S_i\left(u\right)=\∫S_i^{\′}\left(u\right)du=\frac{1}{6}{(1-u)}^3{M}_i+\frac{1}{6}{u}^3{M}_{i+1}+{\tilde{C}}_{i}u+{\tilde{D}}_i$

公式中可由位置点S_i(0)＝r_i,S_i(1)＝r_i+1求得，即

$\left\{\begin{array}{c}\frac{1}{6}{M}_i+{\tilde{D}}_i=r_i\\ \frac{1}{6}{M}_{i+1}+{\tilde{C}}_i+{\tilde{D}}_i=r_{i+1}\end{array}\right.$

求得

$\left\{\begin{array}{c}{\tilde{C}}_i=r_{i+1}-r_i-\frac{1}{6}{M}_{i+1}+\frac{1}{6}{M}_i\\ {\tilde{D}}_i=r_i-\frac{1}{6}{M}_i\end{array}\right.$

带入原表达式，可得

$S_i\left(u\right)=\frac{1}{6}{(1-u)}^3{M}_i+\frac{1}{6}{u}^3{M}_{i+1}+(r_{i+1}-r_i-\frac{1}{6}{M}_{i+1}+\frac{1}{6}{M}_i)u+r_i-\frac{1}{6}{M}_i,(u\∈\[0,1\],i=0,1,\mathrm{...},n-1)$

其一阶导数为

$S_i^{\′}\left(u\right)=-\frac{1}{2}{(1-u)}^2{M}_i+\frac{1}{2}{u}^2{M}_{i+1}+(r_{i+1}-r_i-\frac{1}{6}{M}_{i+1}+\frac{1}{6}{M}_i),(u\∈\[0,1\],i=0,1,\mathrm{...},n-1)$

其二阶导数为

S_i″(u)＝(1-u)M_i+uM_i+1,(u∈[0,1],i＝0,1,…,n-1)

对于函数S(u)，共有n+1个未知量M₀,M₁,…,M_n，即需要构造n+1个与M₀,M₁,…,M_n相关且独立的方程。同时，注意到函数S(u)在各型值点处一阶导数连续，有

S_i′(1)＝S′_i+1(0)

即

M_i+4M_i+1+M_i+2＝6(r_i-2r_i+1+r_i+2),(u∈[0,1],i＝0,1,…,n-2)

这样，得到关于未知量M₀,M₁,…,M_n的n-1个方程，要确定该n+1元方程组的解，还需要两个约束条件。通常，选取自然边界条件S₀″(0)＝S″_n-1(1)＝0，即

$\left\{\begin{array}{c}{S}_0^{\′\′}\left(0\right)=M_0=0\\ S_{n-1}^{\′\′}\left(1\right)=M_n=0\end{array}\right.$

于是，得到n+1阶矩阵形式表达的方程组

由上述的方程组即可解出M₀,M₁,…,M_n，带入S_i(u)的表达式即可得到每段三次多项式的插值函数，即在变位机的工作台坐标系下，作业曲线的每个轨迹区间的三次多项式插值函数。

所述函数建立模块4会以相同的自变量u建立选取的各个点处变位机的轴角的分段三次多项式样条插值函数。

具体推导过程如下：设变位机的每个轴在示教选取各个点时依次经过一序列轴角θ₀,θ₁,…,θ_n，为了获得一阶、二阶连续的插补轨迹，在每相邻的两个轴角之间用三次多项式进行插补。选取与在变位机的工作台坐标系下进行位置轨迹样条插补相同的自变量u,u∈[0,1]。

由分段三次多项式样条插值函数的定义可知，θ_i(u)在第i个轴角区间[0，1]内是一个三次多项式，即方程可写为

θ_i(u)＝a_iu³+b_iu²+c_iu+d_i,(u∈[0,1],i＝0,1,2,…,n-1)

注意到θ_i″(u)在区间[0，1]上是一次多项式，如果θ_i″(u)在第i个轴角[0，1]的两个端点的值已知，设

θ_i″(0)＝m_i,θ_i″(1)＝m_i+1

则有

θ_i″(u)＝(1-u)m_i+um_i+1

对函数θ_i″(u)进行一次和二次积分分别可得

${\mathrm{\θ}}_i^{\′}\left(u\right)=\∫{\mathrm{\θ}}_i^{\′\′}\left(u\right)du=-\frac{1}{2}{(1-u)}^2{m}_i+\frac{1}{2}{u}^2{m}_{i+1}+{\tilde{c}}_i$

${\mathrm{\θ}}_i\left(u\right)=\∫{\mathrm{\θ}}_i^{\′}\left(u\right)du=\frac{1}{6}{(1-u)}^3{m}_i+\frac{1}{6}{u}^3{m}_{i+1}+{\tilde{c}}_{i}u+{\tilde{d}}_i$

公式中可由轴角点θ_i(0)＝θ_i,θ_i(1)＝θ_i+1求得，即

$\left\{\begin{array}{c}\frac{1}{6}{m}_i+{\tilde{d}}_i={\mathrm{\θ}}_i\\ \frac{1}{6}{m}_{i+1}+{\tilde{c}}_i+{\tilde{d}}_i={\mathrm{\θ}}_{i+1}\end{array}\right.$

求得

$\left\{\begin{array}{c}{\tilde{c}}_i={\mathrm{\θ}}_{i+1}-{\mathrm{\θ}}_i-\frac{1}{6}{m}_{i+1}+\frac{1}{6}{m}_i\\ {\tilde{d}}_i={\mathrm{\θ}}_i-\frac{1}{6}{m}_i\end{array}\right.$

带入原表达式，得

${\mathrm{\θ}}_i\left(u\right)=\frac{1}{6}{(1-u)}^3{m}_i+\frac{1}{6}{u}^3{m}_{i+1}+({\mathrm{\θ}}_{i+1}-{\mathrm{\θ}}_i-\frac{1}{6}{m}_{i+1}+\frac{1}{6}{m}_i)u+{\mathrm{\θ}}_i-\frac{1}{6}{m}_i,(u\∈\[0,1\],i=0,1,\mathrm{...},n-1)$

其一阶导数为

${\mathrm{\θ}}_i^{\′}\left(u\right)=-\frac{1}{2}{(1-u)}^2{m}_i+\frac{1}{2}{u}^2{m}_{i+1}+({\mathrm{\θ}}_{i+1}-{\mathrm{\θ}}_i-\frac{1}{6}{m}_{i+1}+\frac{1}{6}{m}_i),(u\∈\[0,1\],i=0,1,\mathrm{...},n-1)$

其二阶导数为

θ_i″(u)＝(1-u)m_i+um_i+1,(u∈[0,1],i＝0,1,…,n-1)

对于函数θ(u)，共有n+1个未知量m₀,m₁,…,m_n，即需要构造n+1个与m₀,m₁,…,m_n相关且独立的方程。同时，注意到函数θ(u)在各型值点处一阶导数连续，有

θ_i′(1)＝θ′_i+1(0)

即

m_i+4m_i+1+m_i+2＝6(θ_i-2θ_i+1+θ_i+2),(u∈[0,1],i＝0,1,…,n-2)

这样，得到关于未知量m₀,m₁,…,m_n的n-1个方程，要确定该n+1元方程组的解，还需要两个约束条件。通常，选取自然边界条件θ₀″(0)＝θ″_n-1(1)＝0，即

$\left\{\begin{array}{c}{\mathrm{\θ}}_0^{\′\′}\left(0\right)=m_0=0\\ {\mathrm{\θ}}_{n-1}^{\′\′}\left(1\right)=m_n=0\end{array}\right.$

于是，得到n+1阶矩阵形式表达的方程组为

由上述方程组即可解出m₀,m₁,…,m_n，带入θ_i(u)的表达式即可得到每段三次多项式的插值函数。

而所述速度规划模块5根据上述求得每个轨迹区间的三次多项式，通过定积分计算每相邻的两个点之间的曲线的长度(即每个轨迹区间的轨迹长度)和所述作业曲线的总长度(即各段轨迹长度的总和)，并设定所述工业机器人在沿每相邻的两个点之间的曲线运动时的速度、加速度以及加加速度。

所述自变量计算模块6根据设定的速度计算所述工业机器人当前的位移量，利用积分计算出的每相邻的两个点之间的曲线的长度(即经过的每段轨迹的长度)就能够计算出经过的选取的点的曲线段总长度(即经过的每段轨迹的长度之和)，当前的位移量减去经过的曲线段的总长度即得到所述工业机器人在当前曲线段的位移量，再用所述工业机器人在当前曲线段的位移量除以当前曲线段的长度即得到三次多项式中自变量u的当前取值。

所述插补量计算模块7会根据自变量u的当前取值计算当前所述工业机器人的位置插补量及所述变位机的轴角插补量，具体地，将自变量u带入到当前曲线段处的三次多项式中，就可以分别得到所述工业机器人的位置插补量以及所述变位机的轴角插补量。

所述运动学逆解模块8会根据所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系，将所述位置插补量变换到所述工业机器人的基坐标系下，并对所述工业机器人进行运动学逆解，以获得所述工业机器人的轴角插补量。

所述控制器9则控制所述工业机器人与所述变位机分别按照所述工业机器人的轴角插补量与所述变位机的轴角插补量进行运动。

所述判断模块10则判断所述工业机器人的当前的位移量是否等于所述作业曲线的总长度，若是，则说明作业曲线的空间轨迹规划已经到达终点，此时就不再进行操作，若否，则说明作业曲线的轨迹规划还没有完成，此时重新启用所述自变量计算模块6。

在本发明的具体实施过程中，所述速度规划模块5设定的所述速度、加速度以及加加速度均是可调节的。

如图2所示，本发明利用本实施例的控制工业机器人与变位机协调运动的系统实现的控制工业机器人与变位机协调运动的方法包括以下步骤：

步骤101、分别建立所述工业机器人与所述变位机的连杆坐标系及正运动学模型。

步骤102、根据坐标变换原理获得所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系。

步骤103、确定安装在所述变位机的工作台上的工件的作业曲线，从所述作业曲线上选取多个点，所述多个点包括所述作业曲线的起点和终点，确定选取的每个点在所述变位机的工作台坐标系下的坐标以及所述工业机器人与所述变位机在选取的每个点处的轴角。

步骤104、利用同一自变量u建立所述多个点的第一三次样条插值函数以及变位机在选取的每个点处的轴角的第二三次样条插值函数，并获得每相邻的两个点之间的所述第一三次样条插值函数的三次多项式以及每相邻的两个点处所述变位机的轴角之间的所述第二三次样条插值函数的三次多项式。

步骤105、通过定积分计算每相邻的两个点之间的曲线的长度以及所述作业曲线的总长度，设定所述工业机器人在沿每相邻的两个点之间的曲线运动时的速度、加速度以及加加速度。

步骤106、根据设定的速度计算所述工业机器人当前的位移量，并根据所述当前的位移量计算每相邻的两个点之间的所述第一三次样条插值函数的三次多项式与所述第二三次样条插值函数的三次多项式中的自变量的当前取值。

步骤107、根据自变量的当前取值计算当前所述工业机器人的位置插补量及所述变位机的轴角插补量。

步骤108、根据所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系，将所述位置插补量变换到所述工业机器人的基坐标系下，并对所述工业机器人进行运动学逆解，以获得所述工业机器人的轴角插补量。

步骤109、利用控制器控制所述工业机器人与所述变位机分别按照所述工业机器人的轴角插补量与所述变位机的轴角插补量进行运动。

步骤110、判断所述工业机器人的当前的位移量是否等于所述作业曲线的总长度，若是，则结束流程，若否，则返回步骤106。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，但这些变更和修改均落入本发明的保护范围。

Claims (6)

1.一种控制工业机器人与变位机协调运动的方法，其特征在于，所述方法包括以下步骤：

S₁、分别建立所述工业机器人与所述变位机的连杆坐标系及正运动学模型；

S₂、根据坐标变换原理获得所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系；

S₃、确定安装在所述变位机的工作台上的工件的作业曲线，从所述作业曲线上选取多个点，所述多个点包括所述作业曲线的起点和终点，确定选取的每个点在所述变位机的工作台坐标系下的坐标以及所述工业机器人与所述变位机在选取的每个点处的轴角；

S₄、利用同一自变量建立所述多个点的第一三次样条插值函数以及变位机在选取的每个点处的轴角的第二三次样条插值函数，并获得每相邻的两个点之间的所述第一三次样条插值函数的三次多项式以及每相邻的两个点处所述变位机的轴角之间的所述第二三次样条插值函数的三次多项式；

S₅、通过定积分计算每相邻的两个点之间的曲线的长度以及所述作业曲线的总长度，设定所述工业机器人在沿每相邻的两个点之间的曲线运动时的速度、加速度以及加加速度；

S₆、根据设定的速度计算所述工业机器人当前的位移量，并根据所述当前的位移量计算每相邻的两个点之间的所述第一三次样条插值函数的三次多项式与所述第二三次样条插值函数的三次多项式中的自变量的当前取值；

S₇、根据自变量的当前取值计算当前所述工业机器人的位置插补量及所述变位机的轴角插补量；

S₈、根据所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系，将所述位置插补量变换到所述工业机器人的基坐标系下，并对所述工业机器人进行运动学逆解，以获得所述工业机器人的轴角插补量；

S₉、利用控制器控制所述工业机器人与所述变位机分别按照所述工业机器人的轴角插补量与所述变位机的轴角插补量进行运动。

2.如权利要求1所述的方法，其特征在于，步骤S₉之后还包括一步骤S₁₀：判断所述工业机器人的当前的位移量是否等于所述作业曲线的总长度，若是，则结束流程，若否，则返回步骤S₆。

3.如权利要求1或2所述的方法，其特征在于，步骤S₅中设定的所述速度、加速度以及加加速度均是可调节的。

4.一种控制工业机器人与变位机协调运动的系统，其特征在于，所述系统包括：

一模型建立模块，用于分别建立所述工业机器人与所述变位机的连杆坐标系及正运动学模型；

一坐标系变换模块，用于根据坐标变换原理获得所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系；

一选取模块，用于确定安装在所述变位机的工作台上的工件的作业曲线，从所述作业曲线上选取多个点，所述多个点包括所述作业曲线的起点和终点，确定选取的每个点在所述变位机的工作台坐标系下的坐标以及所述工业机器人与所述变位机在选取的每个点处的轴角；

一函数建立模块，用于利用同一自变量建立所述多个点的第一三次样条插值函数以及变位机在选取的每个点处的轴角的第二三次样条插值函数，并获得每相邻的两个点之间的所述第一三次样条插值函数的三次多项式以及每相邻的两个点处所述变位机的轴角之间的所述第二三次样条插值函数的三次多项式；

一速度规划模块，用于通过定积分计算每相邻的两个点之间的曲线的长度以及所述作业曲线的总长度，设定所述工业机器人在沿每相邻的两个点之间的曲线运动时的速度、加速度以及加加速度；

一自变量计算模块，用于根据设定的速度计算所述工业机器人当前的位移量，并根据所述当前的位移量计算每相邻的两个点之间的所述第一三次样条插值函数的三次多项式与所述第二三次样条插值函数的三次多项式中的自变量的当前取值；

一插补量计算模块，用于根据自变量的当前取值计算当前所述工业机器人的位置插补量及所述变位机的轴角插补量；

一运动学逆解模块，用于根据所述变位机的工作台坐标系与所述工业机器人的基坐标系之间的变换关系，将所述位置插补量变换到所述工业机器人的基坐标系下，并对所述工业机器人进行运动学逆解，以获得所述工业机器人的轴角插补量；

一控制器，用于控制所述工业机器人与所述变位机分别按照所述工业机器人的轴角插补量与所述变位机的轴角插补量进行运动。

5.如权利要求4所述的系统，其特征在于，所述系统还包括一判断模块，用于判断所述工业机器人的当前的位移量是否等于所述作业曲线的总长度，若是，则不再操作，若否，则重新启用所述自变量计算模块。

6.如权利要求4或5所述的系统，其特征在于，所述速度规划模块设定的所述速度、加速度以及加加速度均是可调节的。