一种工业机器人门型轨迹插补方法与流程

发明涉及工业机器人应用领域，尤其涉及一种高速高精度的工业机器人门型轨迹插补方法。

背景技术：

门型轨迹是工业机器人应用领域中使用的比较广的固定运动轨迹，通常应用于3c装配行业，对机器人运动的速度和精度要求比较高。在工业机器人高速插补时，如果不对门型轨迹进行过渡轨迹处理，容易造成机器人频繁启停，影响机器人控制的速度和精度。同时，相比于s型和梯形速度规划方法，采用五次样条曲线对各段轨迹进行插值，其加速度变化分布在插补的各个区间，且变化平滑，这样可以把机器人各关节的驱动力矩，分配到机器人插补的各个区间，可以防止高速插补时起始或结束段各关节的驱动力矩过大，造成电机报警，导致插补无法完成。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种计算简单方便，满足高速高精度控制的工业机器人门型轨迹插补方法。

上述目的是通过以下技术方案实现的：

一种工业机器人门型轨迹插补方法，包括步骤：

(1)根据目标点和用户要求，确定门型轨迹的过渡方案和对整段轨迹进行分段处理；

(2)使用五次样条曲线，根据各段边界条件，对各段轨迹进行插值，确定各段轨迹的参数方程，最后根据五次样条曲线，分别对各段轨迹进行插补。

进一步地，所述步骤(1)具体包括步骤：

(11)根据工业机器人的常用的门型运动轨迹，机器人运动的路径为p3→p1→p2→p4，然后从p4→p2→p1→p3，这里以p3→p1→p2→p4的轨迹进行插补分析；

(12)在工业机器人门型运动轨迹的插补时，为了保证高速插补的要求，将p3→p1段及p2→p4段和平面段的p1→p2段进行速度合成，得到合成后的插补路径；

(13)设定过渡系数为ratio，整段运动轨迹的期望运动时间为ttotal，根据ratio参数的值将整段轨迹分为五段：上升段、上过渡段、平面段、下过渡段、下降段，其中上过渡段轨迹由上升过渡段插补和平面过渡段插补合成而成，下过渡段轨迹由平面过渡段和下降过渡段插补合成而成，各段的插补时间为：

从p3→p1→p2→p4的插补总时间为：

上升段和上升过渡段的插补总时间为：

下降段和下降过渡段的插补总时间为：

平面段的插补时间为：

上升段和下降段的插补时间为：

上升过渡段和下降过渡段的插补时间为：

上升过渡段和直线过渡段插补同时进行，并进行速度合成，合成轨迹称为上过渡段；下降过渡段和直线过渡段插补同时进行，并进行速度合成，合成轨迹称为下过渡段。

这样，根据目标点和用户要求，确定了门型轨迹的过渡方案和对整段轨迹进行分段处理。

进一步地，所述步骤(2)具体包括步骤：

(21)将速度规划和插补结合起来，建立以下插补表达式：

pm＝ps+u(pe-ps)；

式中，ps为插补路径起点位姿，pe为插补路径终点位姿，u为插补归一化参数，pm为当前计算插补点。

(22)将插补时间t换算到0-1之间，设定当段路径的总插补时间为tm，则插补序列n处对应的插补时间为：

t＝n/tm；

(23)插补归一化参数u和插补时间t的函数关系为：

u＝a1t⁵+a2t⁴+a3t³+a4t²+a5t+a6

a1、a2、a3、a4、a5、a6为速度规划系数；

(24)根据速度规划和插补的特点，可以确定五次样条曲线的六个边界条件：

通过边界条件，即可求得各速度规划系数：

代入插补表达式，即可完成各段轨迹的插补过程；

则u和t的函数关系为：

u＝6t⁵-15t⁴+10t³

在t∈[0，1]时，有

u′＝30t⁴-60t³+30t²＝30t²(t-1)²≥0。

这表明，使用这一函数关系进行速度规划和插补时，可以保证插补归一化参数u的变化是单调的，不会出现插补速度和位移的抖动，满足速度规划和插补的要求。

本发明有如下有益效果：相比于s型和梯形速度规划方法，采用五次样条曲线对各段轨迹进行插值，其加速度变化分布在插补的各个区间，且变化平滑，这样可以把机器人各关节的驱动力矩，分配到机器人插补的各个区间，可以防止高速插补时起始或结束段各关节的驱动力矩过大，造成电机报警，导致插补无法完成。

附图说明

图1是工业机器人的门型轨迹示意图。

图2是门型轨迹过渡和分段后的示意图。

图3是五次样条曲线的速度和加速度变化示意图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的说明。

一种工业机器人门型轨迹插补方法，包括步骤：

(1)根据目标点和用户要求，确定门型轨迹的过渡方案和对整段轨迹进行分段处理；

(2)使用五次样条曲线，根据各段边界条件，对各段轨迹进行插值，确定各段轨迹的参数方程，最后根据五次样条曲线，分别对各段轨迹进行插补。

具体而言，所述步骤(1)具体包括步骤：

(11)根据工业机器人的常用的门型运动轨迹(见图1)，机器人运动的路径为p3→p1→p2→p4，然后从p4→p2→p1→p3，这里以p3→p1→p2→p4的轨迹进行插补分析；

(12)在工业机器人门型运动轨迹的插补时，为了保证高速插补的要求，将p3→p1段及p2→p4段和平面段的p1→p2段进行速度合成，得到合成后的插补路径(见图2)；

(13)设定过渡系数为ratio，整段运动轨迹的期望运动时间为ttotal，根据ratio参数的值将整段轨迹分为五段：上升段、上过渡段、平面段、下过渡段、下降段，其中上过渡段轨迹由上升过渡段插补和平面过渡段插补合成而成，下过渡段轨迹由平面过渡段和下降过渡段插补合成而成，各段的插补时间为：

从p3→p1→p2→p4的插补总时间为：

上升段和上升过渡段的插补总时间为：

下降段和下降过渡段的插补总时间为：

平面段的插补时间为：

上升段和下降段的插补时间为：

上升过渡段和下降过渡段的插补时间为：

上升过渡段和直线过渡段插补同时进行，并进行速度合成，合成轨迹称为上过渡段；下降过渡段和直线过渡段插补同时进行，并进行速度合成，合成轨迹称为下过渡段。

这样，根据目标点和用户要求，确定了门型轨迹的过渡方案和对整段轨迹进行分段处理。

具体而言，所述步骤(2)具体包括步骤：

(21)将速度规划和插补结合起来，建立以下插补表达式：

pm＝ps+u(pe-ps)

式中，ps为插补路径起点位姿，pe为插补路径终点位姿，u为插补归一化参数，pm为当前计算插补点。

(22)将插补时间t换算到0-1之间，设定当段路径的总插补时间为tm，则插补序列n处对应的插补时间为：

t＝n/tm；

(23)插补归一化参数u和插补时间t的函数关系为：

u＝a1t⁵+a2t⁴+a3t³+a4t²+a5t+a6

a1、a2、a3、a4、a5、a6为速度规划系数；

(24)根据速度规划和插补的特点，可以确定五次样条曲线的六个边界条件：

通过边界条件，即可求得各速度规划系数：

代入插补表达式，即可完成各段轨迹的插补过程；

则u和t的函数关系为：

u＝6t⁵-15t⁴+10t³

在t∈[0，1]时，有

u′＝30t⁴-60t³+30t²＝30t²(t-1)²≥0

u′和u″即为机器人的速度和加速度归一化表达式，两者在t∈[0，1]时的变化曲线如下附图3所示。这表明，使用这一函数关系进行速度规划和插补时，可以保证插补归一化参数u的变化是单调的，不会出现插补速度和位移的抖动，满足速度规划和插补的要求。同时，在图3可以看出，相比于s型和梯形速度规划方法，采用五次样条曲线对各段轨迹进行插值，其加速度变化分布在插补的各个区间，且变化平滑，这样可以把机器人各关节的驱动力矩，分配到机器人插补的各个区间，可以防止高速插补时起始或结束段各关节的驱动力矩过大，造成电机报警，导致插补无法完成。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。