CN110900612B - 一种位姿同步的六轴工业机器人轨迹平顺方法 - Google Patents

Abstract

一种位姿同步的六轴工业机器人轨迹平顺方法，采用圆弧曲线对位置轨迹过渡，并采用四元数B样条对姿态轨迹过渡。过渡后的位置和姿态轨迹均具有高阶连续性，可同时约束位置和姿态的过渡误差，且过渡后的位置轨迹和姿态轨迹具有参数同步性。包括两个步骤：步骤1、根据位置和姿态的过渡误差阈值、连续性条件和位姿同步条件确定过渡参数；步骤2、根据过渡参数分别计算位置和姿态的过渡轨迹。本发明提出的平顺方法能够生成平滑且精度可控的六轴机器人轨迹，进而提高六轴机器人的作业效率和精度。

Description

一种位姿同步的六轴工业机器人轨迹平顺方法

技术领域

本发明属于工业机器人自动控制技术领域，具体涉及一种位姿同步的六轴工业机器人轨迹平顺方法。

背景技术

六轴工业机器人被广泛应用于激光切割、焊接、喷涂等复杂应用中。复杂机器人轨迹大多是由离线编程软件生成的线性轨迹和圆弧轨迹组成。在每个轨迹点处，线性轨迹的位姿(位置和姿态)均不具有速度和加速度的连续性，造成机器人在执行过程中的停顿、振动和轨迹误差等问题。

为了解决线性轨迹的不连续问题，目前机器人控制***中大多采用拐角过渡方法。现有位置轨迹的拐角过渡研究中，多采用圆弧、抛物线或多项式曲线来构造过渡轨迹；姿态轨迹的拐角过渡研究中，基于四元数B样条的方法能够提高姿态的连续性。但现有研究存在两个主要问题：一是无法同时约束位置轨迹和姿态轨迹的过渡误差；二是不具备位置和姿态的参数同步性，进而导致位置和姿态的几何形状随速度的改变而改变。

中国发明专利(申请号201610075346.7)提出了一种工业机器人应用的过渡轨迹规划方法，采用抛物线实现关节空间和笛卡尔空间的轨迹过渡；中国发明专利(申请号201710291306.0)提出了一种通过过渡等级来控制过渡区域大小的拟合过渡方法。但以上过渡方法均无法约束位置和姿态的过渡误差，且轨迹形状随速度改变而改变。因此，提供一种简单有效、带误差约束、位姿同步的六轴工业机器人轨迹平顺方法很有必要。

发明内容

本发明提供了一种针对六轴工业机器人的位姿同步的平顺过渡方法，采用圆弧曲线对位置轨迹过渡，并采用四元数B样条对姿态轨迹过渡，该方法计算简单，位置轨迹和姿态轨迹具有参数同步的连续性，且位置轨迹和姿态轨迹的过渡误差可控，能够有效的提高六轴工业机器人的作业效率和质量。

本发明所采用的技术方案是：

一种位姿同步的六轴工业机器人轨迹平顺方法，包括如下步骤：

步骤1、根据过渡误差约束、连续性条件和位姿同步性确定每个位置点和姿态点的过渡参数，其中过渡参数用于确定过渡轨迹的起点和终点；

步骤2、根据过渡参数构造位置和姿态的过渡轨迹；

按上述方案，所述步骤1具体包括如下步骤：

步骤1.1、将六轴工业机器人的线性轨迹的位置点用三维点P_i(x_i,y_i,z_i)表示，姿态点用四元数q_i(q_i,1,q_i,2,q_i,3,q_i,4)表示，其中序号i＝1,2,…N,N为总的轨迹点个数(N≥3)。

步骤1.2、遍历除首尾端点外的内部轨迹点P_i(x_i,y_i,z_i),i＝2,3,…N-1，根据位置点的过渡误差阈值δ_p，计算每个位置点P_i(x_i,y_i,z_i)的过渡圆弧的最大位置过渡参数：

首先，计算线段P_i-1P_i的长度L_i-1和线段P_iP_i+1的长度L_i,若长度L_i-1或长度L_i太短(小于距离阈值)，则该轨迹段不需平顺；否则计算线段P_i-1P_i和线段P_iP_i+1的夹角

若夹角

接近0度或180度，则该轨迹段也不需平顺，将位置轨迹的不需平顺段做标记，以下先来介绍需平顺段的过渡方法；

然后，设r_i,1和r_i,2为两个位置过渡参数，过渡圆弧的起点可表示为E_i,0＝P_i+r_i,1(P_i-1-P_i),终点可表示为E_i,1＝P_i+r_i,2(P_i+1-P_i)，根据过渡的对称性条件建立两个位置过渡参数之间的关系：r_i,1L_i-1＝r_i,2L_i；

最后，根据圆弧过渡误差阈值δ_p以及相邻过渡轨迹不相交的条件计算位置过渡参数r_i,1的上界：

步骤1.3、遍历除首尾端点外的轨迹点P_i,i＝2,3,…N-1，根据姿态点的过渡误差阈值δ_o，计算每个姿态点q_i(q_i,1,q_i,2,q_i,3,q_i,4)的过渡四元数B样条的最大姿态过渡参数：

首先，计算姿态点q_i-1旋转到姿态点q_i的旋转轴U_i-1及旋转角θ_i-1,并计算姿态点q_i旋转到姿态点q_i+1的旋转轴U_i及旋转角θ_i；若旋转角θ_i-1或旋转角θ_i接近0度，则该轨迹段也不需平顺，将姿态轨迹的不需平顺段做标记，以下先来介绍需平顺段的过渡方法；

然后，设姿态过渡四元数B样条具有5个控制点，其中第一和第二控制点f_i,0,f_i,1位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，第三控制点f_i,2与姿态点q_i相同，第四和第五控制点f_i,3,f_i,4位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，即：

以上控制点的分布能够保证过渡轨迹与原线性轨迹的G²连续性。其中s_i,1和s_i,2为两个姿态过渡参数，c为与位姿同步相关的比例系数；根据姿态过渡的对称性条件建立两个位置过渡参数之间的关系：s_i,1θ_i-1＝s_i,2θ_i；

最后，根据姿态过渡误差阈值δ_o、姿态过渡轨迹对称、相邻过渡轨迹不相交的条件计算姿态过渡参数s_i,1的上界：

步骤1.4、计算满足轨迹点误差且位姿同步的过渡参数t_i,1,t_i,2和比例系数c:

首先根据位置轨迹和姿态轨迹同步条件计算比例系数

然后根据位置和姿态轨迹是否可平顺的标记确定位姿同步过渡参数的取值：

若位置轨迹和姿态轨迹均标记为可平顺，则取位置和姿态的过渡参数最小值作为位姿同步的过渡参数，即：

若位置轨迹和姿态轨迹均不可平顺，则不需对位置轨迹和姿态轨迹进行平顺；

若位置轨迹或姿态轨迹之一可平顺，则可单独采用步骤1.2或步骤1.3的方法平顺。

按上述方案，遍历除首尾端点外的轨迹点P_i,i＝2,3,…N-1，所述步骤2针对可平顺段进行平顺，具体包括如下步骤：

步骤2.1、根据过渡参数构造位置轨迹的过渡圆弧，圆弧轨迹表示为：

步骤2.2、根据过渡参数和比例系数构造姿态轨迹的过渡曲线，其中控制点如步骤1.3中定义，节点向量为U＝[0,0,0,0,0.5,1,1,1,1]，

为四元数B样条的累加和基函数，四元数B样条曲线表示为：

通过本发明所构思的以上技术方案，与现有技术相比，本发明具有的优势：

1、提出的机器人位姿平顺方法能够提到机器人轨迹的连续性，其中位置轨迹为G¹连续，姿态轨迹为G²连续，能够避免机器人执行过程中的停顿和振动；

2、提出的考虑误差约束的平顺过渡方法能够同时控制位置轨迹和姿态轨迹的过渡误差，有利于高精度的机器人应用；

3、提出的位姿同步的平顺过渡方法保证了位置轨迹和姿态轨迹的参数同步，将几何平顺和速度规划区分开，进而避免机器人轨迹随速度的改变而改变。

4、提出的轨迹平顺方法适合于位置轨迹和姿态轨迹的单独过渡或同时过渡，进而适用于工业机器人的多种应用场景。

附图说明

图1为本发明实施例六轴工业机器人位置轨迹过渡示意图；

图2为本发明实施例六轴工业机器人姿态轨迹过渡示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施案例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施案例仅仅用以解释本发明，并不用于限定本发明。

本发明为一种位姿同步的六轴工业机器人轨迹平顺方法，其中位置轨迹采用圆弧过渡，姿态轨迹采用四元数B样条过渡，计算过渡轨迹主要包括计算过渡参数和构造过渡轨迹两个主要步骤。

步骤1、根据过渡误差约束、连续性条件、和位姿同步性确定每个位置点和姿态点的过渡参数；

步骤1.1、六轴工业机器人的线性轨迹由一系列轨迹点组成，每个轨迹点包含位置点和姿态点信息。位置点可用三维点P_i(x_i,y_i,z_i)表示，姿态点可用四元数q_i(q_i,1,q_i,2,q_i,3,q_i,4)表示，其中序号i＝1,2,…N,N为总的轨迹点个数(N≥3)。

步骤1.2、遍历除首尾端点外的内部轨迹点P_i(x_i,y_i,z_i),i＝2,3,…N-1，根据位置点的过渡误差阈值δ_p，计算每个位置点P_i(x_i,y_i,z_i)的过渡圆弧的最大位置过渡参数：

首先计算线段P_i-1P_i的长度L_i-1＝|P_i-1P_i|，线段P_iP_i+1的长度L_i＝|P_iP_i+1|,设置距离误差阈值ε，如ε＝1e-6mm,若长度L_i-1<ε或L_i<ε,则认为距离太短，将索引i标记为位置不需平顺段；否则计算线段P_i-1P_i和线段P_iP_i+1的夹角

若夹角

接近0度或180度，则将索引i标记为位置不需平顺段；不需平顺段将在步骤1.4进行处理，以下为需平顺段的过渡方法；

设r_i,1和r_i,2为两个位置过渡参数，用于确定表示过渡圆弧的起点和终点位置。如图1所示，圆弧的起点可表示为E_i,0＝P_i+r_i,1(P_i-1-P_i),终点可表示为E_i,1＝P_i+r_i,2(P_i+1-P_i)。要求过渡圆弧左右对称，即r_i,1L_i-1＝r_i,2L_i，则过渡轨迹与线性轨迹的误差最大点位于图1所示的点Q_i，因此可根据圆弧过渡误差阈值δ_p推导出位置过渡参数r_i,1的上界

另外为了防止两个相邻的过渡轨迹交叉，要求过渡参数r_i,1,r_i,2的取值不大于

综合以上，得出位置过渡参数r_i,1的上界：

步骤1.3、遍历除首尾端点外的轨迹点P_i,i＝2,3,…N-1，根据姿态点的过渡误差阈值δ_o，计算每个姿态点q_i(q_i,1,q_i,2,q_i,3,q_i,4)的过渡四元数B样条的最大姿态过渡参数：

首先计算姿态点q_i-1旋转到姿态点q_i的旋转轴U_i-1及旋转角θ_i-1,并计算姿态点q_i旋转到姿态点q_i+1的旋转轴U_i及旋转角θ_i；若旋转角θ_i-1或旋转角θ_i接近0度，则该轨迹段的姿态不需平顺，不需平顺段将在步骤1.4进行处理，以下介绍需平顺段的过渡方法；

其次设姿态过渡四元数B样条具有5个控制点f_i,0～f_i,4，其中第一和第二控制点f_i,o,f_i,1位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，第三控制点f_i,2与姿态点q_i相同，第四和第五控制点f_i,3,f_i,4位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，五个控制点可通过下式进行计算：

其中s_i,1和s_i,2为两个姿态过渡参数，c为与位姿同步相关的比例系数，首先采用对称的姿态轨迹建立两个位置过渡参数之间的关系：s_i,1θ_i-1＝s_i,2θ_i；

同理可证明过渡轨迹在参数中点误差最大，根据姿态过渡误差阈值δ_o可反推出姿态过渡参数s_i,1的上界：

类似的，相邻两条姿态过渡轨迹不能相交，最终得到姿态过渡参数s_i,1的上界：

步骤1.4、计算满足轨迹点误差且位姿同步的过渡参数t_i,1,t_i,2和比例系数c:

首先根据位置轨迹和姿态轨迹的同步条件推导比例系数c，依据的条件是位置轨迹和姿态轨迹在过渡起点和终点满足同参数的G¹连续性，以起点(E_i,0,f_i,0)的连续性为例：如图1所示，L_i(u)为P_i-1E_i,0之间的线性轨迹，C_i(u)为位置过渡轨迹；如图2所示，S_i(u)为q_{i- 1}f_i,0之间的球面线性轨迹，q_i(u)为姿态过渡轨迹。根据控制点的设置可证明位置和姿态分别G¹连续，若位置和姿态具有同参数的G¹连续性，则满足

根据该条件可推导出比例系数

该比例系数对于过渡轨迹的终点同样适用。

然后根据位置轨迹和姿态轨迹是否可平顺的标记确定位姿同步过渡参数的取值：

若位置轨迹和姿态轨迹均标记为可平顺，则取位置过渡参数和姿态过渡参数的最小值作为位姿同步的过渡参数，即：

若位置轨迹和姿态轨迹均不可平顺，则不需对位置轨迹和姿态轨迹进行平顺，即不需进行步骤2；问题同上述一样

若位置轨迹或姿态轨迹之一可平顺，则可单独采用步骤1.2或步骤1.3计算的过渡参数进行平顺。

步骤2、遍历除首尾端点外的轨迹点P_i,i＝2,3,…N-1，针对可平顺段进行平顺，具体包括如下步骤：

步骤2.1、对于位置可平顺的轨迹段，根据过渡参数可构造位置轨迹的过渡圆弧，如下所示：

步骤2.2、针对姿态可平顺的轨迹段，根据过渡参数和比例系数构造姿态轨迹的过渡曲线，其中控制点如步骤1.3中定义计算，节点向量为U＝[0,0,0,0,0.5,1,1,1,1]，四元数B样条曲线表示为：

其中

为四元数B样条的基函数，在两个节点区间的定义如下：

Claims (5)

1.一种位姿同步的六轴工业机器人轨迹平顺方法，其特征在于：包括如下步骤：

步骤1、根据过渡误差约束、连续性条件和位姿同步性确定每个位置点和姿态点的过渡参数，其中过渡参数用于确定过渡轨迹的起点和终点；

步骤2、根据过渡参数构造位置和姿态的过渡轨迹；

其中，所述步骤1具体包括如下步骤：

步骤1.1、将六轴工业机器人的线性轨迹的位置点用三维点P_i(x_i，y_i，z_i)表示，姿态点用四元数q_i(q_i，1，q_i，2，q_i，3，q_i，4)表示，其中序号i＝1，2，...N，N为总的轨迹点个数(N≥3)；

步骤1.2、遍历除首尾端点外的内部轨迹点P_i(x_i，y_i，z_i)，i＝2，3，...N-1，根据位置点的过渡误差阈值δ_p，计算每个位置点P_i(x_i，y_i，z_i)的过渡圆弧的最大位置过渡参数；

步骤1.3、遍历除首尾端点外的轨迹点P_i，i＝2，3，...N-1，根据姿态点的过渡误差阈值δ_o，计算每个姿态点q_i(q_i，1，q_i，2，q_i，3，q_i，4)的姿态过渡四元数B样条的最大姿态过渡参数；所述姿态过渡四元数B样条具有5个控制点，其中第一和第二控制点f_i，0，f_i，1位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，第三控制点f_i，2与姿态点q_i相同，第四和第五控制点f_i，3，f_i，4位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上；

步骤1.4、计算满足轨迹点误差且位姿同步的过渡参数t_i，1，t_i，2和比例系数c；

所述步骤2具体包括如下步骤：遍历除首尾端点外的轨迹点P_i，i＝2，3，...N-1，所述步骤2针对可平顺段进行平顺，具体包括如下步骤：

步骤2.1、根据过渡参数构造位置轨迹的过渡圆弧，圆弧轨迹表示为：

其中，

为线段P_i-1P_i和线段P_iP_i+1的夹角；

步骤2.2、根据过渡参数和比例系数构造姿态轨迹的过渡曲线，其中控制点如步骤1.3中定义，节点向量为U＝[0，0，0，0，0.5，1，1，1，1]，

为四元数B样条的累加和基函数，四元数B样条曲线表示为：

2.根据权利要求1所述的位姿同步的六轴工业机器人轨迹平顺方法，其特征在于：所述步骤1.2的具体算法为：首先，计算线段P_i-1P_i的长度L_i-1和线段P_iP_i+1的长度L_i，若长度L_i-1或长度L_i太短(小于距离阈值)，则该轨迹段不需平顺；否则计算线段P_i-1P_i和线段P_iP_i+1的夹角

若夹角

接近0度或180度，则该轨迹段也不需平顺，将位置轨迹的不需平顺段做标记，以下先来介绍需平顺段的过渡方法；

然后，设r_i，1和r_i，2为两个位置过渡参数，过渡圆弧的起点可表示为E_i，0＝P_i+r_i，1(P_i-1-P_i)，终点可表示为E_i，1＝P_i+r_i，2(P_i+1-P_i)，根据过渡的对称性条件建立两个位置过渡参数之间的关系：r_i，1L_i-1＝r_i，2L_i；

最后，根据圆弧过渡误差阈值δ_p以及相邻过渡轨迹不相交的条件计算位置过渡参数r_i，1的上界：

3.根据权利要求1或2所述的位姿同步的六轴工业机器人轨迹平顺方法，其特征在于：所述步骤1.3的具体算法为：首先，计算姿态点q_i-1旋转到姿态点q_i的旋转轴U_i-1及旋转角θ_i-1，并计算姿态点q_i旋转到姿态点q_i-1的旋转轴U_i及旋转角θ_i；若旋转角θ_i-1或旋转角θ_i接近0度，则该轨迹段也不需平顺，将姿态轨迹的不需平顺段做标记，以下先来介绍需平顺段的过渡方法；

然后，设姿态过渡四元数B样条具有5个控制点，其中控制点f_i，0，f_i，1位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，控制点f_i，2与姿态点q_i相同，控制点f_i，3，f_i，4位于姿态点q_i-1与姿态点q_i的球面线性插值轨迹上，即：

以上控制点的分布能够保证过渡轨迹与原线性轨迹的G²连续性，其中s_i，1和s_i，2为两个姿态过渡参数，c为与位姿同步相关的比例系数；根据姿态过渡的对称性条件建立两个位置过渡参数之间的关系：

s_i，1θ_i-1＝s_i，2θ_i；

最后，根据姿态过渡误差阈值δ_o、姿态过渡轨迹对称、相邻过渡轨迹不相交的条件计算姿态过渡参数s_i，1的上界：

4.根据权利要求1所述的位姿同步的六轴工业机器人轨迹平顺方法，其特征在于：所述步骤1.4的具体算法为：首先，根据位置轨迹和姿态轨迹同步条件计算比例系数

然后，根据位置和姿态轨迹是否可平顺的标记确定位姿同步过渡参数的取值：若位置轨迹和姿态轨迹均标记为可平顺，则取位置和姿态的过渡参数最小值作为位姿同步的过渡参数，即：t_i，1＝min(r_i，1，s_i，1)，

若位置轨迹和姿态轨迹均不可平顺，则不需对位置轨迹和姿态轨迹进行平顺；若位置轨迹或姿态轨迹之一可平顺，则单独采用步骤1.2或步骤1.3的方法平顺；

其中，r_i，1为位置过渡参数；长度L_i-1为线段P_i-1P_i的长度，长度L_i为线段P_iP_i-1的长度；s_i，1为姿态过渡参数。

5.根据权利要求2所述的位姿同步的六轴工业机器人轨迹平顺方法，其特征在于：所述步骤1.4的具体算法为：首先，根据位置轨迹和姿态轨迹同步条件计算比例系数

然后，根据位置和姿态轨迹是否可平顺的标记确定位姿同步过渡参数的取值：若位置轨迹和姿态轨迹均标记为可平顺，则取位置和姿态的过渡参数最小值作为位姿同步的过渡参数，即：t_i，1＝min(r_i，1，s_i，1)，

若位置轨迹和姿态轨迹均不可平顺，则不需对位置轨迹和姿态轨迹进行平顺；若位置轨迹或姿态轨迹之一可平顺，则单独采用步骤1.2或步骤1.3的方法平顺；

其中，s_i，1为姿态过渡参数。

Images