CN109664303B - 一种误差可控的四轴工业机器人b样条过渡式平顺轨迹生成方法 - Google Patents

Abstract

一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成方法，包括：步骤1、四轴工业机器人轨迹预处理：将轨迹分为需平顺轨迹段和不需平顺轨迹段，并对四轴姿态进行预处理，保证两个轨迹点之间走劣弧轨迹；步骤2、四轴轨迹B样条平顺：遍历步骤1生成的需平顺轨迹段，对每一条轨迹段按照轨迹点误差阈值、位置点弦高误差阈值和连续性要求采用几何迭代法生成B样条过渡式平顺轨迹。本发明B样条过渡式平顺轨迹由线性轨迹和B样条轨迹组成，整条轨迹具有位置和姿态同步的G1或G2连续性，满足轨迹点误差和轨迹点之间的位置弦高误差，且采用样条轨迹能够实现四轴工业机器人复杂应用的轨迹平顺，进而提高四轴工业机器人的作业效率和质量。

Description

一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成 方法

技术领域

本发明属于工业机器人轨迹优化领域，具体涉及一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成方法。

背景技术

四轴工业机器人，又称平面关节(SCARA)机器人，具有三个旋转轴和一个平动轴，广泛应用于搬运、装配和涂胶等作业中。

四轴工业机器人的运动指令大多与六轴工业机器人一致，包括线性指令、圆弧指令和轴关节运动指令。同时每条指令可以指定精确到点或是平滑过渡。精确到点指令能够满足轨迹精度要求，但到达轨迹点必须降速为零，导致作业效率降低；平滑可以在轨迹点处不降速为零的情况下平滑过渡，但导致了轨迹点精度的降低。

机器人复杂应用的轨迹大都采用线性指令或圆弧指令描述，个别机器人厂家为了在提高连续性的同时提高精度，增加了自由曲线或样条运动指令。如安川机器人的MOVS指令，采用自由曲线插补，轨迹为通过三个点的抛物线。KUKA机器人采用样条组的概念建立连续的直线、圆弧或样条段。与常规运动相比，更易于保持编程设定的速度；但两个轨迹点之间的轨迹误差无法由用户控制，可能出现轨迹偏离原始轨迹过大的情况出现，从而导致作业质量的降低。

已受理专利申请文献201710097192.6提出了一种可同时控制位置点误差和弦高误差的工业机器人平顺运动轨迹生成方法，其中的过渡曲线提供了三次B样条和四次B样条两种曲线，但双轨平顺在直线段和样条段的连接点处姿态不一定连续。已受理专利申请文献201811468150.X提出了一种误差可控的三维轨迹点轨迹平顺方法，该方法只适用于三维位置点的连续、保形和满足精度的轨迹平顺。

现有四轴工业机器人的轨迹(位置和姿态，简称位姿)表达中，不具有同时满足高连续性(位姿同步连续)和高精度(满足轨迹点误差和轨迹点之间的位置弦高误差)的平顺轨迹表达。

发明内容

本发明要解决的技术问题是，针对现有四轴工业机器人的轨迹表达存在的上述不足，提供一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成方法，提供了G1连续样条和G2连续样条两种平顺方法，平顺样条的生成计算简单；能够达到整条轨迹位姿同步的G1连续和G2连续；能够满足四轴工业机器人的轨迹点(位置和姿态)的误差，以及轨迹点之间的位置点弦高误差；且能够在机器人控制器内部不改变机器人现有轨迹点定义的基础上增加样条指令。

本发明为解决上述技术问题所采用的技术方案是：

一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成方法，包括如下步骤：

步骤1、四轴工业机器人轨迹预处理：将线性轨迹按照位置距离和夹角分成需平顺轨迹段和不需平顺轨迹段；并对姿态进行预处理；

步骤2、需平顺轨迹段平顺：遍历步骤1生成的需平顺轨迹段，对每一段需平顺轨迹段按照轨迹点误差阈值、位置点弦高误差阈值和连续性要求采用几何迭代法生成G1或G2连续的B样条过渡式平顺轨迹。

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

步骤1.1、根据分段阈值计算分段索引集合，输入连续两个轨迹点以上的轨迹段，输出分段索引；设输入的四轴工业机器人的轨迹点集合为

轨迹点个数N≥2，其中每个轨迹点P_i(x_i,y_i,z_i,θ_i)为位置(x_i,y_i,z_i)和姿态即绕Z轴旋转的角度θ_i组成的四维向量，位置分段条件以位置距离阈值δ_d和位置夹角阈值δ_a作为指标；

遍历轨迹点索引i＝1,2,…N-1，分别判断索引i是否满足位置分段条件：

首先计算轨迹段P_i-1P_i和P_iP_i+1两段的位置距离d_i-1和d_i，若d_i-1或d_i小于位置距离阈值δ_d，则认为索引i满足位置分段条件；否则，计算轨迹段P_i-1P_i和P_iP_i+1的位置夹角a_i，若sin a_i小于位置夹角阈值δ_a，则认为索引i满足位置分段条件；否则认为索引i不满足位置分段条件；将满足位置分段条件的索引i加入分段索引集合，默认将首尾索引0,N加入分段索引集合的首尾；

步骤1.2、根据分段索引集合分段，将一整段轨迹段根据分段索引集合分成若干轨迹段，其中轨迹段中轨迹点个数大于2的轨迹段记为需平顺轨迹段，用于下个步骤的轨迹平顺；否则记为不需平顺轨迹段，按照线性轨迹输出到平顺后的轨迹中；

步骤1.3、姿态预处理，根据两个轨迹之间劣弧优先的原则遍历轨迹点索引i＝1,2,…N，若相邻两个轨迹点P_i-1,P_i的姿态的夹角距离o_i-1大于180°，o_i-1＝|θ_i-θ_i-1|，则修改P_i的第四维姿态角：若θ_i>0，则修改为P_i(x_i,y_i,z_i,θ_i-360°)；否则修改为P_i(x_i,y_i,z_i,θ_i+360°)。

按上述方案，所述步骤2对每一段需平顺轨迹段生成B样条过渡式平顺轨迹的方法具体如下：

步骤2.1、初始迭代参数设定，设当前线性轨迹段的四维轨迹点为

记为原始轨迹点，设平顺后的轨迹需满足的位置距离误差阈值为ε_max，位置弦高误差阈值记为δ_max，姿态点夹角误差阈值o_max，迭代次数阈值k_max，设置当前迭代次数为k＝0，迭代轨迹点记为

步骤2.2、遍历索引i＝1,2,…N-1，根据位置点弦高误差约束、G1或G2连续性约束和保形约束分别生成迭代轨迹点

处的过渡B样条轨迹

首先分别计算

和

的位置距离d_i-1和d_i，以及位置点夹角β_i，并计算d_i-1和d_i的较小值d_min＝min(d_i-1,d_i)；

然后根据位置点弦高误差约束和保形约束计算

前后位置点的过渡比例r_i-1和r_i，首先计算前后位置点的过渡长度，对于G1过渡B样条，过渡长度为：

对于G2过渡B样条，过渡长度为

然后计算两个过渡比例：

其中0<α<1为保形参数，代表两段过渡B样条之间的线性轨迹段占整个轨迹段的距离比例；c>1为决定控制点的比例系数；对于G1连续样条，当

时，过渡样条的前三维组成了平面PH样条；对于G2连续样条，取c＝1.5；

最后根据G1或G2连续条件和两个过渡比例计算四维样条的控制点，满足G1连续条件的过渡样条具有四个控制点；满足G2连续条件的过渡样条具有五个控制点，各个控制点的计算方法如下：

G1样条：

G2样条：

步骤2.3、遍历索引i＝1,2,…N-1，计算第i条过渡B样条

与原始轨迹点Q_i的轨迹点误差，首先根据步骤2.2中的控制点构造四维B样条

采用三次均匀节点的B样条曲线：

其中t∈[0,1]；对于G1样条，M＝3；对于G2样条，M＝4。B_j,3(t)为B样条基函数，根据节点向量计算得到；

然后计算四维B样条轨迹的参数中点作为轨迹点误差最大点：

最后计算过渡B样条

与原始轨迹点Q_i的轨迹点误差，轨迹点误差包括位置距离误差和姿态点夹角误差，根据

的三维位置距离和姿态夹角距离得到；

步骤2.4、计算所有过渡B样条与原始轨迹点的最大位置距离误差

和最大姿态点夹角误差

若

小于位置距离误差阈值ε_max，且

小于姿态点夹角误差阈值o_max，或者当前迭代次数k大于迭代次数阈值k_max，终止迭代并输出B样条平顺轨迹

转步骤2.6；否则，转步骤2.5；

步骤2.5、遍历i＝1,…N-1,根据原始轨迹点Q_i和轨迹点误差最大点

计算偏移向量

并更新迭代轨迹点：

令k＝k+1，转步骤2.2；

步骤2.6、对步骤2.4输出的平顺轨迹进行整理并输出，平顺后的轨迹由N条四维线性轨迹和(N-1)条四维B样条平顺轨迹组合而成，按顺序依次是：线性轨迹

平顺B样条轨迹

线性轨迹

平顺B样条轨迹

平顺B样条轨迹

线性轨迹

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

1、根据四轴工业机器人的圆弧过渡式平顺轨迹生成方法，能够生成位置和姿态同步的G1连续平顺轨迹，将该思路推广到四轴工业机器人的B样条轨迹生成，能够生成一种高连续性(G1或G2连续)、保形和高精度的四轴工业机器人B样条过渡式平顺轨迹，平顺轨迹由线性轨迹和B样条轨迹组成，能够满足轨迹点精度和轨迹点之间的位置精度，达到位置和姿态同步的G1或G2连续平顺轨迹；

2、相对于四轴工业机器人现有线性轨迹、平滑功能及样条或自由曲线指令，本发明生成的B样条过渡式平顺轨迹既具有更高的连续性，又能在保证连续性的同时保持更高的执行精度，能够到达预设轨迹的位置和姿态，且保证满足轨迹点之间的位置精度，从而在提高四轴工业机器人作业效率的同时提高作业精度；

3、不仅能够生成位姿同步的G1连续平顺轨迹，也能够生成位姿同步的G2连续轨迹，且B样条轨迹相对于圆弧轨迹能够生成曲率更小的平顺轨迹，相对于圆弧过渡式平顺轨迹本发明B样条轨迹具有更好的灵活性和连续性。

附图说明

图1为本发明实施例四轴工业机器人B样条一次迭代平顺流程图；

图2为本发明实施例构造G1连续样条示意图；

图3为本发明实施例构造G2连续样条示意图；

图4为本发明实施例位置和姿态同步连续示意图。

具体实施方式

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

本发明一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成方法，包括如下步骤：

步骤1、四轴工业机器人轨迹预处理：将线性轨迹按照位置距离和夹角分成需平顺轨迹段和不需平顺轨迹段；对整条轨迹的所有轨迹点进行遍历，根据轨迹的位置距离和夹角进行分段，将整条轨迹划分为若干段轨迹段集合；并对四轴姿态进行预处理：

步骤1.1、根据分段阈值计算分段索引集合，输入连续两个轨迹点以上的轨迹段，输出分段索引，设输入的四轴工业机器人的轨迹点集合为

(轨迹点个数N≥2)，其中两个轨迹点P_i-1,P_i之间的位置距离计算方法为

设相邻三个轨迹点P_i-1,P_i,P_i+1的前三维组成的位置点分别为p_i-1,p_i,p_i+1；若d_i-1d_i≠0，三个位置点的夹角为

位置分段条件以位置距离阈值δ_d和相邻位置夹角阈值δ_a作为指标，根据位置分段与已受理专利申请(201811468150.X)的三维点分段方法一致，遍历轨迹点索引i＝1,2,…N-1，分别判断索引i是否满足位置分段条件；若d_i-1<δ_d或d_i<δ_d，则认为索引i满足位置分段条件，否则若sina_i<δ_a,则认为索引i满足位置分段条件；将满足位置分段条件的索引i加入分段索引集合，默认将首尾索引0,N加入分段索引集合的首尾；

步骤1.2、根据分段索引集合分段，将一整段轨迹段根据分段索引集合分成若干轨迹段，其中轨迹段中轨迹点个数大于2的轨迹段记为需平顺轨迹段，用于下个步骤的轨迹平顺；否则记为不需平顺轨迹段，按照线性轨迹输出到平顺后的轨迹中；

步骤1.3、根据两个轨迹之间劣弧优先的原则对姿态预处理，遍历轨迹点索引i＝1,2,…N，相邻两个轨迹点P_i-1,P_i的姿态夹角距离计算方法为o_i-1＝|θ_i-θ_i-1|，若o_i-1大于180°，则修改P_i的第四维姿态角：若θ_i>0，则修改为P_i(x_i,y_i,z_i,θ_i-360°)；否则修改为P_i(x_i,y_i,z_i,θ_i+360°)。

步骤2、遍历步骤1生成的需平顺轨迹段，对每一段需平顺轨迹段按照轨迹点误差阈值、位置点弦高误差阈值和连续性要求采用几何迭代法生成B样条过渡式平顺轨迹，生成平顺轨迹的思路与圆弧平顺思路一致，主要区别在于满足精度和连续性的B样条曲线的构造，以下以一段需平顺轨迹段介绍几何迭代法构造B样条过渡式平顺轨迹的步骤：

步骤2.1、初始迭代参数设定，设当前线性轨迹段的四维轨迹点为

记为原始轨迹点，设平顺后的轨迹需满足的位置距离误差阈值为ε_max，位置弦高误差阈值记为δ_max，姿态点夹角误差阈值o_max，迭代次数阈值k_max，设置当前迭代次数为k＝0，迭代轨迹点记为

当k_max＝1时，将生成如现有平滑功能类似的过渡样条，如图1所示，虚线为线性轨迹的位置点连线，实线为最大迭代次数为1的过渡曲线，该过渡曲线能够达到连续的效果，但无法保证位置点的精度满足高精度的需求，因此采用接下来的步骤生成既满足精度又满足连续性的位姿同步的B样条过渡轨迹；

步骤2.2、遍历索引i＝1,2,…N-1，根据位置点弦高误差约束、G1或G2连续性约束和保形约束分别生成迭代轨迹点

处的过渡B样条轨迹

首先分别计算

和

的位置距离d_i-1和d_i，以及位置点夹角β_i，位置距离和位置点夹角的计算方法需要先取出四维向量的前三维，计算三维距离和三维位置点夹角，并计算d_i-1和d_i的较小值d_min＝min(d_i-1,d_i)；

然后根据位置点弦高误差约束和保形约束计算

前后位置点的过渡比例r_i-1和r_i，首先计算前后位置点的过渡长度，根据B样条的性质、连续性、精度要求和保形要求推导出，对于G1过渡B样条，过渡长度为：

对于G2过渡B样条，过渡长度为：

该长度指的是三维位置点的过渡长度，过渡长度的第一部分表示满足弦高误差阈值的过渡长度，第二部分表示满足保形要求的过渡长度；

然后计算两个过渡比例：

其中0<α<1为保形参数，代表两段过渡B样条之间的线性轨迹段占整个轨迹段的距离比例；c>1为决定控制点的比例系数；对于G1连续样条，当

时，过渡样条的前三维组成了具有良好弧长计算性质的平面PH样条，该样条能够通过解析方法很简单的计算出B样条的弧长，为速度规划提供便利。对于G2连续样条，不失一般性，可取c＝1.5.

最后根据G1或G2连续条件和两个过渡比例计算四维样条的控制点，如图2所示，满足G1连续条件的过渡样条具有四个控制点(白色圆点)，左右对称；如图3所示，满足G2连续条件的过渡样条具有五个控制点(白色圆点)，前两个和后两个左右对称，中间点与过渡点重合，各个控制点的计算方法如下(四维向量的运算规则与三维相同)：

G1样条：

G2样条：

步骤2.3、遍历索引i＝1,2,…N-1，计算第i条过渡B样条

与原始轨迹点Q_i的轨迹点误差，首先根据步骤2.2中的控制点构造四维B样条

采用三次均匀节点的B样条曲线：

其中t∈[0,1]；对于G1样条，M＝3；对于G2样条，M＝4；B_j,3(t)为B样条基函数，根据节点向量计算得到；其中G1连续样条的节点向量为[0,0,0,0,1,1,1,1]；G2连续样条的节点向量为[0,0,0,0,0.5,1,1,1,1]；

然后计算四维B样条轨迹的参数中点作为轨迹点误差最大点：

如图2和图3中的星点所示；

最后计算过渡B样条

与原始轨迹点Q_i的轨迹点误差，轨迹点误差包括位置距离误差和姿态点夹角误差，根据

的三维位置距离和姿态夹角距离得到；

步骤2.4、计算所有过渡B样条与原始轨迹点的最大位置距离误差

和最大姿态点夹角误差

最大位置距离为所有过渡B样条与迭代轨迹点之间的三维位置距离中的最大值，最大姿态点夹角误差为所有过渡B样条与迭代轨迹点之间的姿态夹角的最大值，将最大位置距离和最大姿态点夹角与阈值比较，若

小于位置距离误差阈值ε_max，且

小于姿态点夹角误差阈值o_max，或者当前迭代次数k大于迭代次数阈值k_max，终止迭代并输出B样条过渡式平顺轨迹

转步骤2.6；否则，转步骤2.5；

步骤2.5、遍历i＝1,…N-1,根据原始轨迹点Q_i和轨迹点误差最大点

计算偏移向量

并更新迭代轨迹点：

令k＝k+1，转步骤2.2；

步骤2.6、对步骤2.4输出的平顺轨迹进行整理并输出，平顺后的轨迹由N条四维线性轨迹和(N-1)条四维B样条平顺轨迹组合而成，按顺序依次是：线性轨迹

平顺B样条轨迹

线性轨迹

平顺B样条轨迹

平顺B样条轨迹

线性轨迹

以上得到的平顺轨迹能够保证位置和姿态的同步连续，即不仅保证平顺轨迹上任意轨迹点的位置p_i(x_i,y_i,z_i)的G1或G2连续,而且保证任意点q_i＝p_i+R_Z(θ_i)v的G1或G2连续性。其中v为任意三维向量，R_Z(θ_i)为绕z轴的旋转矩阵。如图4所示，图中实线为平顺前工具中心点(TCP)的线性位置轨迹；虚线为平顺后的直线与样条组成的位置点平顺轨迹(如上式位置点p_i的轨迹)，图中加粗虚线可认为工具坐标系的X轴上某点的位置轨迹(如上式位置点q_i的轨迹)，可看出平顺后的轨迹能够达到位置和姿态的同步连续。

Claims (1)

1.一种误差可控的四轴工业机器人B样条过渡式平顺轨迹生成方法，其特征在于，包括如下步骤：

步骤1、四轴工业机器人轨迹预处理：将线性轨迹按照位置距离和夹角分成需平顺轨迹段和不需平顺轨迹段；并对姿态进行预处理，具体包括如下步骤：

步骤1.1、根据分段阈值计算分段索引集合，输入连续两个轨迹点以上的轨迹段，输出分段索引；设输入的四轴工业机器人的轨迹点集合为

轨迹点个数N≥2，其中每个轨迹点P_i(x_i,y_i,z_i,θ_i)为位置(x_i,y_i,z_i)和姿态即绕Z轴旋转的角度θ_i组成的四维向量，位置分段条件以位置距离阈值δ_d和位置夹角阈值δ_a作为指标；

遍历轨迹点索引i＝1,2,…N-1，分别判断索引i是否满足位置分段条件：

首先计算轨迹段P_i-1P_i和P_iP_i+1两段的位置距离d_i-1和d_i，若d_i-1或d_i小于位置距离阈值δ_d，则认为索引i满足位置分段条件；否则，计算轨迹段P_i-1P_i和P_iP_i+1的位置夹角a_i，若sin a_i小于位置夹角阈值δ_a，则认为索引i满足位置分段条件；否则认为索引i不满足位置分段条件；将满足位置分段条件的索引i加入分段索引集合，默认将首尾索引0,N加入分段索引集合的首尾；

步骤1.2、根据分段索引集合分段，将一整段轨迹段根据分段索引集合分成若干轨迹段，其中轨迹段中轨迹点个数大于2的轨迹段记为需平顺轨迹段，用于下个步骤的轨迹平顺；否则记为不需平顺轨迹段，按照线性轨迹输出到平顺后的轨迹中；

步骤1.3、姿态预处理，根据两个轨迹之间劣弧优先的原则遍历轨迹点索引i＝1,2,…N，若相邻两个轨迹点P_i-1,P_i的姿态的夹角距离o_i-1大于180°，o_i-1＝|θ_i-θ_i-1|，则修改P_i的第四维姿态角：若θ_i>0，则修改为P_i(x_i,y_i,z_i,θ_i-360°)；否则修改为P_i(x_i,y_i,z_i,θ_i+360°)；

步骤2、需平顺轨迹段平顺：遍历步骤1生成的需平顺轨迹段，对每一段需平顺轨迹段按照轨迹点误差阈值、位置点弦高误差阈值和连续性要求采用几何迭代法生成G1或G2连续的B样条过渡式平顺轨迹，具体如下：

步骤2.1、初始迭代参数设定，设当前线性轨迹段的四维轨迹点为

记为原始轨迹点，设平顺后的轨迹需满足的位置距离误差阈值为ε_max，位置弦高误差阈值记为δ_max，姿态点夹角误差阈值o_max，迭代次数阈值k_max，设置当前迭代次数为k＝0，迭代轨迹点记为

步骤2.2、遍历索引i＝1,2,…N-1，根据位置点弦高误差约束、G1或G2连续性约束和保形约束分别生成迭代轨迹点

处的过渡B样条轨迹

首先分别计算

和

的位置距离d_i-1和d_i，以及位置点夹角β_i，并计算d_i-1和d_i的较小值d_min＝min(d_i-1,d_i)；

然后根据位置点弦高误差约束和保形约束计算

前后位置点的过渡比例r_i-1和r_i，首先计算前后位置点的过渡长度，对于G1过渡B样条，过渡长度为：

对于G2过渡B样条，过渡长度为

然后计算两个过渡比例：

其中0<α<1为保形参数，代表两段过渡B样条之间的线性轨迹段占整个轨迹段的距离比例；c>1为决定控制点的比例系数；对于G1连续样条，当

时，过渡样条的前三维组成了平面PH样条；对于G2连续样条，取c＝1.5；

最后根据G1或G2连续条件和两个过渡比例计算四维样条的控制点，满足G1连续条件的过渡样条具有四个控制点；满足G2连续条件的过渡样条具有五个控制点，各个控制点的计算方法如下：

G1样条：

G2样条：

步骤2.3、遍历索引i＝1,2,…N-1，计算第i条过渡B样条

与原始轨迹点Q_i的轨迹点误差，首先根据步骤2.2中的控制点构造四维B样条

采用三次均匀节点的B样条曲线：

其中t∈[0,1]；对于G1样条，M＝3；对于G2样条，M＝4；B_j,3(t)为B样条基函数，根据节点向量计算得到；

然后计算四维B样条轨迹的参数中点作为轨迹点误差最大点：

最后计算过渡B样条

与原始轨迹点Q_i的轨迹点误差，轨迹点误差包括位置距离误差和姿态点夹角误差，根据

的三维位置距离和姿态夹角距离得到；

步骤2.4、计算所有过渡B样条与原始轨迹点的最大位置距离误差

和最大姿态点夹角误差

若

小于位置距离误差阈值ε_max，且

小于姿态点夹角误差阈值o_max，或者当前迭代次数k大于迭代次数阈值k_max，终止迭代并输出B样条平顺轨迹

转步骤2.6；否则，转步骤2.5；

步骤2.5、遍历i＝1,…N-1,根据原始轨迹点Q_i和轨迹点误差最大点

计算偏移向量

并更新迭代轨迹点：

令k＝k+1，转步骤2.2；

步骤2.6、对步骤2.4输出的平顺轨迹进行整理并输出，平顺后的轨迹由N条四维线性轨迹和(N-1)条四维B样条平顺轨迹组合而成，按顺序依次是：线性轨迹

平顺B样条轨迹

线性轨迹

平顺B样条轨迹

平顺B样条轨迹

线性轨迹

Images