一种误差可控的四轴工业机器人圆弧过渡式平顺轨迹生成方法与流程

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

背景技术：

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

四轴工业机器人现有运动指令包括线性指令、圆弧指令和轴关节运动指令。大多数复杂轨迹由直线运动指令以及少量圆弧运动指令组成。机器人作业过程中为了精确到达轨迹点必须降速到零，再执行下一条指令时机器人速度从零开始加速，整个作业过程中机器人频繁加减速导致作业效率降低，同时机器人振动导致作业质量下降。

平滑过渡指令可使机器人tcp点平滑快速逼近目标点，但不能经过目标点。虽然平滑指令提高了轨迹的连续性，但造成了精度丢失进而影响作业质量。另外一些过渡指令使用插值曲线保证机器人tcp点平滑且经过轨迹点(如kuka的样条指令)，但不能控制两个轨迹点之间的误差，进而影响作业质量。

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

已受理专利申请文献201811468150.x提出了一种误差可控的三维轨迹点轨迹平顺方法，该方法能够生成三维轨迹的连续、保形和满足精度的平顺轨迹。但推广到四轴工业机器人不仅需要考虑轨迹点的姿态平顺，还需要考虑位置和姿态的同步平顺，从而得到位姿同步平顺的四轴平顺轨迹。现有四轴工业机器人的轨迹(位置和姿态，简称位姿)表达中，不具有同时满足高连续性(位姿同步连续)和高精度(满足轨迹点误差和轨迹点之间的位置弦高误差)的平顺轨迹表达。

技术实现要素：

本发明要解决的技术问题是，针对现有四轴工业机器人的轨迹表达存在的上述不足，提供一种误差可控的四轴工业机器人圆弧过渡式平顺轨迹生成方法，该生成方法计算简单，同时具有位姿同步的g1连续性(位置和姿态同步平顺)，能够满足轨迹点误差和轨迹点之间的位置弦高误差，且生成的轨迹能够在不改变四轴工业机器人现有指令格式下直接使用。

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

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

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

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

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

步骤1.1、根据分段阈值计算分段索引集合，输入连续两个轨迹点以上的轨迹段，输出分段索引；设输入的四轴工业机器人的轨迹点集合为轨迹点个数n≥2，其中每个轨迹点pi(xi，yi，zi，θi)为位置(xi，yi，zi)和姿态即绕z轴旋转的角度θi组成的四维向量，位置分段条件以位置距离阈值δd和位置夹角阈值δa作为指标；

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

首先计算轨迹段pi-1pi和pipi+1两段的位置距离di-1和di，若di-1或di小于位置距离阈值δd，则认为索引i满足位置分段条件；否则，计算轨迹段pi-1pi和pipi+1的位置夹角ai，若sinai小于位置夹角阈值δa，则认为索引i满足位置分段条件；否则认为索引i不满足位置分段条件；将满足位置分段条件的索引i加入分段索引集合，默认将首尾索引0，n加入分段索引集合的首尾；

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

步骤1.3、姿态预处理，根据两个轨迹之间劣弧优先的原则遍历轨迹点索引i＝1，2，...n，若相邻两个轨迹点pi-1，pi的姿态的夹角距离oi-1大于180°，oi-1＝|θi-θi-1|，则修改pi的第四维姿态角：若θi＞0，则修改为pi(xi，yi，zi，θi-360°)；否则修改为pi(xi，yi，zi，θi+360°)。

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

步骤2.1、初始迭代参数设定，设当前线性轨迹段的四维轨迹点为记为原始轨迹点，设平顺后的轨迹需满足的位置距离误差阈值为εmax，位置弦高误差阈值记为δmax，姿态点夹角误差阈值omax，迭代次数阈值kmax，设置当前迭代次数为k＝0，迭代轨迹点记为

步骤2.2、遍历索引i＝1，2，...n-1，根据位置点弦高误差约束、g1连续性约束和保形约束分别生成迭代轨迹点处的过渡圆弧轨迹

首先分别计算和的位置距离di-1和di，以及位置点夹角βi，并计算di-1和di的较小值dmmin＝min(di-1，di)；

然后根据位置点弦高误差约束和保形约束计算前后位置点的过渡比例ri-1和ri，首先计算前后位置点的过渡长度为：然后计算两个过渡比例：其中0＜α＜1为保形参数，代表两段过渡圆弧之间的线性轨迹段占整个轨迹段的距离比例；

最后根据g1连续条件和两个过渡比例计算四维圆弧的起点终点和圆心

步骤2.3、遍历索引i＝1，2，...n-1，计算第i条过渡圆弧与原始轨迹点qi的轨迹点误差：

首先根据步骤2.2中的起点、终点和圆心构造四维圆弧其中前三维为欧式空间的圆弧曲线，第四维为旋转角随圆弧同步变化生成的曲线；

其中t∈[0，1]，αi＝180°-βi，

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

最后计算过渡圆弧与原始轨迹点qi的轨迹点误差，轨迹点误差包括位置距离误差和姿态点夹角误差，分别计算的三维位置距离和姿态夹角距离作为位置距离误差和姿态点夹角误差；

步骤2.4、计算所有过渡圆弧与原始轨迹点的最大位置距离误差和最大姿态点夹角误差若小于位置距离误差阈值εmax，且小于姿态点夹角误差阈值omax，或者当前迭代次数k大于迭代次数阈值kmax，终止迭代并输出圆弧平顺轨迹转步骤2.6；否则，转步骤2.5；

步骤2.5、遍历i＝1，...n-1，根据原始轨迹点qi和轨迹点误差最大点计算偏移向量并更新迭代轨迹点：令k＝k+1，转步骤2.2；

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

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

1、能够生成一种高连续性、保形和高精度的四轴工业机器人圆弧过渡式平顺轨迹，生成的轨迹能够在不改变机器人现有轨迹指令结构的条件下直接执行；

2、相对于四轴工业机器人现有线性轨迹，本发明生成的圆弧过渡式平顺轨迹具有更高的连续性，从而提高了四轴工业机器人的作业效率，减少机器磨损；

3、相对于四轴工业机器人现有平滑轨迹，本发明生成的轨迹除具有连续性外还具有更高的执行精度，能够到达预设轨迹的位置和姿态，且保证满足轨迹点之间的位置精度，进一步提高作业精度；

4、相对于现有四轴工业机器人轨迹的姿态不连续问题，本发明生成的四轴工业机器人的连续轨迹能够同时满足位置和姿态的连续性，且位置和姿态同步连续，为最终生成高精度高效率的四轴工业机器人执行轨迹提供充分条件。

附图说明

图1为本发明实施例四轴工业机器人圆弧过渡式平顺轨迹生成流程图；

图2为本发明实施例四轴工业机器人轨迹预处理流程图；

图3为本发明实施例姿态角度预处理示意图；

图4为本发明实施例轨迹平顺流程图；

图5为本发明实施例圆弧过渡示意图。

具体实施方式

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

本发明一种误差可控的四轴工业机器人圆弧过渡式平顺轨迹生成方法，其主要流程图如附图1所示，主要包括预处理和轨迹平顺两个主要步骤：

步骤1、四轴工业机器人轨迹预处理：将线性轨迹按照位置距离和夹角分成需平顺轨迹段和不需平顺轨迹段；对整条轨迹的所有轨迹点进行遍历，根据轨迹的位置距离和夹角进行分段，将整条轨迹划分为若干段轨迹段集合；并对四轴姿态进行预处理，保证两个轨迹点之间走劣弧轨迹，预处理步骤的流程图如附图2所示，主要包括计算分段索引和根据分段索引集合分段两个主要步骤：

步骤1.1、根据分段阈值计算分段索引集合，输入连续两个轨迹点以上的轨迹段，输出分段索引，设输入的四轴工业机器人的轨迹点集合为(轨迹点个数n≥2)，其中两个轨迹点pi-1，pi之间的位置距离计算方法为设相邻三个轨迹点pi-1，pi，pi+1的前三维组成的位置点分别为pi-1，pi，pi+1。若di-1di≠0，三个位置点的夹角为

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

步骤1.2、根据分段索引集合分段，实施例中设步骤1.1得到的分段索引集合为{0，1，14,16}，则将轨迹段分成三段：{p0～p1}，{p1～p14}，{p14～p16}，其中轨迹段中轨迹点个数大于2的轨迹段记为需平顺轨迹段，如上例中第二段和第三段，用于下个步骤的轨迹平顺；否则记为不需平顺轨迹段，如上例中第一段，按照线性轨迹输出到平顺后的轨迹中。

步骤1.3、根据两个轨迹之间劣弧优先的原则对姿态预处理，遍历轨迹点索引i＝1，2，...n，相邻两个轨迹点pi-1，pi的姿态夹角距离计算方法为oi-1＝|θi-θi-1|，若oi-1大于180°，则修改pi的第四维姿态角：若θi＞0，则修改为pi(xi，yi，zi，θi-360°)；否则修改为pi(xi，yi，zi，θi+360°)，如图3所示，θi-1＝-170°，θi＝30°，若从θi-1到θi线性插值，将走优弧，于是将θi修改为-330°，即将pi点修改为(xi，yi，zi，-330°)。

步骤2、遍历步骤1生成的需平顺轨迹段，对每一段按照轨迹点误差阈值、位置点弦高误差阈值和连续性要求采用几何迭代法生成圆弧过渡式平顺轨迹，其中整个轨迹平顺的流程如图4所示，以下以一段需平顺轨迹段介绍圆弧过渡式平顺轨迹生成方法：

步骤2.1、初始迭代参数设定，设当前线性轨迹段的四维轨迹点为记为原始轨迹点，设平顺后的轨迹需满足的位置距离误差阈值为εmax，位置弦高误差阈值记为δmax，姿态点夹角误差阈值omax，迭代次数阈值kmax，三个误差阈值根据实际应用需求设置，要求εmax≤δmax，kmax≥1，当kmax＝1时，得到的圆弧过渡式平顺轨迹与现有圆弧平滑轨迹一致，设置当前迭代次数为k＝0，迭代轨迹点记为

步骤2.2、遍历索引i＝1，2，...n-1，根据位置点弦高误差约束、g1连续性约束和保形约束分别生成迭代轨迹点处的过渡圆弧轨迹

如图5所示，首先采用步骤1.1的方法计算位置距离di-1和di，以及位置点夹角βi，并计算di-1和di的较小值dmmin＝min(di-1，di)；然后根据位置点弦高误差约束和保形约束计算前后位置点的过渡长度：进而计算两个过渡比例ri-1和ri：最后根据过渡比例计算四维圆弧的起点、终点和圆心能够保证位置和姿态的同步连续。

其中0＜α＜1为保形参数，代表两段过渡圆弧之间的线性轨迹段占整个轨迹段的距离比例，若取α＝0.2，代表两个圆弧之间的直线部分占整个直线部分距离的20％。

最后根据g1连续条件和两个过渡比例计算四维圆弧的起点终点和圆心

以上四维向量的加减和数乘运算规则同三维向量的运算规则一致，根据起点、终点和圆心即可构造四维圆弧，其中前三维组成了欧式空间的圆弧曲线，第四维为姿态的随圆弧同步变化生成的曲线。

步骤2.3、遍历索引i＝1，2，...n-1，计算第i条过渡圆弧与原始轨迹点qi的轨迹点误差：

首先根据步骤2.2中的起点、终点和圆心构造四维圆弧

其中t∈[0，1]，αi＝180°-βi，

然后计算四维圆弧轨迹的参数中点作为轨迹点误差最大点：采用参数节点中点作为轨迹点误差最大点是因为三维位置轨迹为三维空间圆弧，根据对称性在中点的位置点误差最大；

最后计算过渡圆弧与原始轨迹点qi的轨迹点误差，轨迹点误差包括位置距离误差和姿态点夹角误差，分别计算的三维位置距离和姿态夹角距离作为位置距离误差和姿态点夹角误差；

步骤2.4、计算所有过渡圆弧与原始轨迹点的最大位置距离误差和最大姿态点夹角误差遍历索引i＝1，2，...n-1，计算的三维位置距离和姿态夹角最大位置距离为的最大值；最大姿态夹角距离为的最大值；

若小于位置距离误差阈值εmax，且小于姿态点夹角误差阈值omax，或者当前迭代次数k大于迭代次数阈值kmax，终止迭代并输出圆弧平顺轨迹转步骤2.6；否则，转步骤2.5；

步骤2.5、遍历i＝1，...n-1，根据原始轨迹点qi和轨迹点误差最大点计算偏移向量并更新迭代轨迹点：令k＝k+1，转步骤2.2；

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

以上得到的平顺轨迹能够保证位置和姿态的同步连续，即不仅保证平顺轨迹上任意轨迹点的位置pi(xi，yi，zi)的g1连续，而且保证任意点qi＝pi+rz(θi)v的g1连续性，其中v为任意三维向量，rz(θi)为绕z轴的旋转矩阵。