CN115808904A - 一种工业机器人圆弧过辅助点轨迹规划方法 - Google Patents

Abstract

本发明公开了一种工业机器人圆弧过辅助点轨迹规划方法，涉及工业机器人轨迹规划领域。解决了机器人圆弧运动轨迹经过辅助点姿态的不够平滑的问题，本发明中使用四元数描述方法规划圆弧运动轨迹的姿态，并设计四元数多项式对姿态进行轨迹规划，最终有效解决了机器人圆弧运动轨迹无法平滑通过辅助点姿态问题。按以下步骤进行：获取圆弧运动轨迹规划相关的运动参数；位置几何路径规划；姿态几何路径规划；圆弧速度插补。设计四元数多项式对姿态进行轨迹规划，避免了常规的欧拉角规划的大量三角函数运算和万向节死锁问题。保证了机器人末端轨迹的平滑运行。保证了圆弧运动轨迹位姿的同步性。

Description

一种工业机器人圆弧过辅助点轨迹规划方法

技术领域

本发明涉及工业机器人轨迹规划领域，具体涉及一种工业机器人圆弧过辅助点轨迹规划方法。

背景技术

在工业机器人的实际应用过程中，机器人末端需要沿着圆弧轨迹运动完成焊接、切割以及码垛等工艺，在运动过程中需顺序经过圆弧的起点、辅助点与终点。目前大部分的圆弧轨迹运动，机器人在经过圆弧辅助点时是位置经过，姿态是不经过的，对应某些现场应用可能会引发工艺上的问题，如在焊接应用中，焊缝呈现圆弧形状，在焊接过程中若焊枪姿态不经过辅助点姿态可能会与被焊接工件发生干涉，造成碰撞，损坏被焊接工件以及焊枪。因此，为扩大工业机器人的应用范围，需提出一种圆弧过辅助点姿态的轨迹规划方法，保证轨迹的位置、姿态的速度、加速度的连续性以及位置和姿态的同步性。

公开号为：CN 105353725 A，名称为：一种工业机器人应用的过辅助点姿态空间圆弧插补方法的中国发明专利申请中提出了过辅助点姿态空间圆弧的插补方法，但是专利中描述姿态的方法为欧拉角，转动时会出现万向节死锁问题，并且只保证了姿态变化率的连续性，没有提及姿态二阶的连续性。名为：一类C2连续的单位四元数插值样条曲线的论文中提出一种快速生成单位四元数插值样条曲线，通过选择适当的四次多项式调配函数进行插值，但插值曲线的光顺性有待提升。名为：机械手空间圆弧位姿轨迹规划算法的实现的论文中提出一种圆弧位姿轨迹规划方法，但计算复杂，不能调节圆弧姿态几何路径的平滑度。因此，如何对工业机器人轨迹规划方法进行改进，如何解决机器人圆弧运动过辅助点姿态的平滑规划问题就成为了本领域技术人员亟待解决的技术问题。

发明内容

本发明针对以上问题，提出了一种工业机器人圆弧过辅助点轨迹规划方法，解决了机器人圆弧运动轨迹经过辅助点姿态的不够平滑的问题，以及使用欧拉角插补带来的万向节死锁问题，本发明中使用四元数描述方法规划圆弧运动轨迹的姿态，并设计四元数多项式对姿态进行轨迹规划，最终有效解决了机器人圆弧运动轨迹无法平滑通过辅助点姿态问题。

本发明的技术方案为：按以下步骤进行：

步骤1、获取圆弧运动轨迹规划相关的运动参数；

示教获取圆弧运动轨迹起点空间位置和姿态、辅助点空间位置和姿态、终点空间位置和姿态；

步骤2、位置几何路径规划；根据获取到的圆弧运动起点空间位置、辅助点空间位置、终点空间位置，以三点共面以及三点到空间圆心坐标的距离相等为约束条件联立方程解得圆心坐标，求得半径、辅助点到终点的圆心角、起点到终点的圆心角以及总弧长；

步骤3、姿态几何路径规划；

将示教的三点姿态由欧拉角转换为四元数描述；根据起点、辅助点、终点的姿态，在圆弧起点姿态与辅助点姿态间、辅助点点姿态与圆弧终点姿态间各设计一个姿态辅助点，借助三维空间样条曲线插值思想，构造满足C2连续的四元数姿态轨迹曲线；

步骤4、圆弧速度插补；

步骤4.1、圆弧位置速度规划；

由步骤2生成圆弧的运动路径后，即可对路径的位置空间作速度规划，由速度规划方法以及最大速度、最大加速度的边界条件规划出圆弧位置空间的速度、加速度、圆弧位置起点到辅助点的运动时间和圆弧位置的总运动时间；

步骤4.2、圆弧姿态速度规划；

基于步骤3中得到的参数化的姿态几何路径以及步骤4.1得到的圆弧位置起点到辅助点的运动时间和圆弧位置的总运动时间，由姿态速度规划模块规划姿态速度；

最终，通过以上步骤4.1、步骤4.2中的实时插补得到圆弧运动轨迹各个时刻的机器人末端位置和姿态矢量。

进一步的，步骤3具体为：

将示教的三点姿态由欧拉角(a，b，c)转换为四元数q描述，各轴姿态转换为四元数描述的公式为：

圆弧起点姿态为q_s(w_s，x_s，y_s，z_s)，圆弧辅助点姿态为q_m(w_m，x_m，y_m，z_m)，圆弧终点姿态为q_e(w_e，x_e，y_e，z_e)。

根据起点、辅助点、终点的姿态，在圆弧起点姿态与辅助点姿态间、辅助点点姿态与圆弧终点姿态间各设计一个姿态辅助点分别为q_aux，q_aux2；借助三维空间样条曲线插值思想，构造满足C2连续的四元数姿态轨迹曲线，其中q_s ^-1、q_m ^-1、q_e ^-1分别为q_s、q_m、q_e的逆，q_aux1 ^-1、q_aux2 ^-1分别为q_aux1、q_aux2的逆，θ表示四元数角度，

表示q_s ^-1q_m和q_m ^-1q_e角度的最小值；

四元数姿态插值公式为：

γ(u_i)＝-u_i ⁶+3u_i ⁵-3u_i ⁴+u_i ³i＝1,2

α(u_i)＝u_i ⁴-2u_i ³+2u_ii＝1,2

β(u_i)＝-u_i ⁴+2u_i ³i＝1,2

上式中α(u_i)，β(u_i)，γ(u_i)为满足条件的多项式，k为柔顺参数，取值范围为(0，1)，值越大，姿态曲线的曲率变化量越小，平滑度越好，圆弧起点到辅助点间i为1，圆弧辅助点到终点间i为2，Q_i(u_i)为两段期间通过一系列插值的期望姿态路径函数，u_i为描述如何沿着此路径运动的运动率，其为关于时间的函数。

进一步的，步骤4.2中姿态速度规划模块的工作方法为：

设计运动率为u_i(t)，圆弧起点到辅助点间i为1，圆弧辅助点到终点间i为2，通过五次多项式曲线拟合，求得相应的系数c。

u_i(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴+c₅t⁵i＝1,2

通过u_i(t)实时地插补输出圆弧运动轨迹各个时刻的机器人末端姿态矢量，将插补得到的四元数转换为欧拉角，完成圆弧段上姿态的轨迹规划。四元数转换为欧拉角公式为：

最终，通过以上步骤4.1、步骤4.2中的实时插补得到圆弧运动轨迹各个时刻的机器人末端位置和姿态矢量。

本发明的有益效果为：

一、本发明提出的圆弧过辅助点姿态轨迹规划方法，用四元数描述姿态进行规划，避免了常规的欧拉角规划的大量三角函数运算和万向节死锁问题。

二、本发明通过四元数多项式规划圆弧起点、辅助点、终点姿态路径，在整个圆弧轨迹中机器人姿态速度和加速度不会发生突变，保证了机器人末端轨迹的平滑运行。

三、本发明可以通过调节柔顺参数，改变姿态插值曲线的平滑度。

四、本发明基于圆弧位置路径的速度规划时间同步规划姿态速度，保证了圆弧运动轨迹位姿的同步性。

附图说明

图1是本发明的工作流程图；

图2是本发明中姿态插值曲线示意图。

具体实施方式

为能清楚说明本专利的技术特点，下面通过具体实施方式，并结合其附图，对本专利进行详细阐述。

本发明设计的四元数多项式姿态曲线可以平滑地通过圆弧辅助点姿态，并且通过改变柔顺系数，调节姿态插值曲线的平滑度，解决了圆弧运动轨迹无法平滑通过辅助点姿态问题。

一方面，本发明提供的圆弧过辅助点姿态方法规划了机器人圆弧段的运动轨迹，保证了在整个圆弧运动轨迹规划中机器人的姿态速度、姿态加速度不会发生突变，从而保证了机器人末端轨迹的平滑运行，可以通过改变柔顺参数，调节姿态的平滑度。另一方面，本发明基于圆弧位置速度规划方法同步规划了机器人圆弧姿态速度，保证了机器人圆弧位姿的同步性。

主要方法和步骤如下：

步骤1、获取圆弧运动轨迹规划相关的运动参数；

示教获取圆弧运动轨迹起点空间位置P_s(x_s，y_s，z_s)和姿态R_s(a_s，b_s，c_s)、辅助点空间位置P_m(x_m，y_m，z_m)和姿态R_m(a_m，b_m，c_m)、终点空间位置P_e(x_e，y_e，z_e)和姿态R_e(a_e，b_e，c_e)。

步骤2、位置几何路径规划；

根据获取到的圆弧运动起点空间位置P_s(x_s，y_s，z_s)、辅助点空间位置P_m(x_m，y_m，z_m)、终点空间位置P_e(x_e，y_e，z_e)，以三点共面以及三点到空间圆心坐标的距离相等为约束条件联立方程解得圆心坐标，进一步求得半径、辅助点到终点的圆心角、起点到终点的圆心角以及总弧长。

步骤3、姿态几何路径规划；

将示教的三点姿态由欧拉角(a，b，c)转换为四元数q描述，各轴姿态转换为四元数描述的公式为：

圆弧起点姿态为q_s(w_s，x_s，y_s，z_s)，圆弧辅助点姿态为q_m(w_m，x_m，y_m，z_m)，圆弧终点姿态为q_e(w_e，x_e，y_e，z_e)。

根据起点、辅助点、终点的姿态，在圆弧起点姿态与辅助点姿态间、辅助点点姿态与圆弧终点姿态间各设计一个姿态辅助点分别为q_aux1，q_aux2；借助三维空间样条曲线插值思想，构造满足C2连续的四元数姿态轨迹曲线，C2代表了函数曲线的可微性，组合参数曲线在连接点处具有二阶的连续导矢，这类光顺性称之为具有C2连续；其中q_s ^-1、q_m ^-1、q_e ^-1分别为q_s、q_m、q_e的逆，q_aux1 ^-1、q_aux2 ^-1分别为q_aux1、q_aux2的逆，θ表示四元数角度，

表示q_s ^-1q_m和q_m ^-1q_e角度的最小值。

四元数姿态插值公式为：

γ(u_i)＝-u_i ⁶+3u_i ⁵-3u_i ⁴+u_i ³i＝1,2

α(u_i)＝u_i ⁴-2u_i ³+2u_ii＝1，2

β(u_i)＝-u_i ⁴+2u_i ³i＝1,2

上式中α(u_i)，β(u_i)，γ(u_i)为满足条件的多项式，k为柔顺参数，取值范围为(0，1)，值越大，姿态曲线的曲率变化量越小，平滑度越好，圆弧起点到辅助点间i为1，圆弧辅助点到终点间i为2，Q_i(u_i)为两段期间通过一系列插值的期望姿态路径函数，u_i为描述如何沿着此路径运动的运动率，其为关于时间的函数。

步骤4、圆弧速度插补；

步骤4.1、圆弧位置速度规划；

由步骤2生成圆弧的运动路径后，即可对路径的位置空间作速度规划，可以选择梯形加减速控制规划方法或S型曲线加减速控制规划方法或其它规划方法，由相应的速度规划方法以及最大速度v_max、最大加速度a_max的边界条件规划出圆弧位置空间的速度、加速度、圆弧位置起点到辅助点的运动时间t_m和圆弧位置的总运动时间t_e，其中最大速度由指令给出，最大加速度为机器人***参数或由指令给出。本步骤中圆弧位置速度模块F(t)根据时间t实时的插补输出圆弧运动轨迹各个时刻的机器人末端位置矢量。圆弧位置轨迹的边界条件如下：

步骤4.2、圆弧姿态速度规划；

基于步骤3中得到的参数化的姿态几何路径以及步骤4.1得到的圆弧位置起点到辅助点的运动时间t_m和圆弧位置的总运动时间t_e，由姿态速度规划模块规划姿态速度，设计运动率为u_i(t)，圆弧起点到辅助点间i为1，圆弧辅助点到终点间i为2，通过五次多项式曲线拟合，求得相应的系数c。

u_i(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴+c₅t⁵i＝1,2

通过u_i(t)实时地插补输出圆弧运动轨迹各个时刻的机器人末端姿态矢量，将插补得到的四元数转换为欧拉角，完成圆弧段上姿态的轨迹规划。四元数转换为欧拉角公式为：

最终，通过以上步骤4.1、步骤4.2中的实时插补得到圆弧运动轨迹各个时刻的机器人末端位置和姿态矢量。

本发明具体实施途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (3)

1.一种工业机器人圆弧过辅助点轨迹规划方法，其特征在于，按以下步骤进行：

步骤1、获取圆弧运动轨迹规划相关的运动参数；

示教获取圆弧运动轨迹起点空间位置和姿态、辅助点空间位置和姿态、终点空间位置和姿态；

步骤2、位置几何路径规划；根据获取到的圆弧运动起点空间位置、辅助点空间位置、终点空间位置，以三点共面以及三点到空间圆心坐标的距离相等为约束条件联立方程解得圆心坐标，求得半径、辅助点到终点的圆心角、起点到终点的圆心角以及总弧长；

步骤3、姿态几何路径规划；

将示教的三点姿态由欧拉角转换为四元数描述；根据起点、辅助点、终点的姿态，在圆弧起点姿态与辅助点姿态间、辅助点点姿态与圆弧终点姿态间各设计一个姿态辅助点，借助三维空间样条曲线插值思想，构造满足C2连续的四元数姿态轨迹曲线；

步骤4、圆弧速度插补；

步骤4.1、圆弧位置速度规划；

由步骤2生成圆弧的运动路径后，即可对路径的位置空间作速度规划，由速度规划方法以及最大速度、最大加速度的边界条件规划出圆弧位置空间的速度、加速度、圆弧位置起点到辅助点的运动时间和圆弧位置的总运动时间；

步骤4.2、圆弧姿态速度规划；

基于步骤3中得到的参数化的姿态几何路径以及步骤4.1得到的圆弧位置起点到辅助点的运动时间和圆弧位置的总运动时间，由姿态速度规划模块规划姿态速度；

最终，通过以上步骤4.1、步骤4.2中的实时插补得到圆弧运动轨迹各个时刻的机器人末端位置和姿态矢量。

2.根据权利要求1所述的一种工业机器人圆弧过辅助点轨迹规划方法，其特征在于，步骤3具体为：

将示教的三点姿态由欧拉角(a,b,c)转换为四元数q描述，各轴姿态转换为四元数描述的公式为：

圆弧起点姿态为q_s(w_s,x_s,y_s,z_s)，圆弧辅助点姿态为q_m(w_m,x_m,y_m,z_m)，圆弧终点姿态为q_e(w_e，x_e，y_e，z_e)。

根据起点、辅助点、终点的姿态，在圆弧起点姿态与辅助点姿态间、辅助点点姿态与圆弧终点姿态间各设计一个姿态辅助点分别为q_aux，q_aux2；借助三维空间样条曲线插值思想，构造满足C2连续的四元数姿态轨迹曲线，其中q_s ^-1、q_m ^-1、q_e ^-1分别为q_s、q_m、q_e的逆，q_aux ^-1、q_aux ^-1分别为q_aux1、q_aux2的逆，θ表示四元数角度，

表示q_s ^-1q_m和q_m ^-1q_e角度的最小值；

四元数姿态插值公式为：

γ(u_i)＝-u_i ⁶+3u_i ⁵-3u_i ⁴+u_i ³ i＝1,2

α(u_i)＝u_i ⁴-2u_i ³+2u_i i＝1,2

β(u_i)＝-u_i ⁴+2u_i ³ i＝1,2

上式中α(u_i)，β(u_i)，γ(u_i)为满足条件的多项式，k为柔顺参数，取值范围为(0，1)，值越大，姿态曲线的曲率变化量越小，平滑度越好，圆弧起点到辅助点间i为1，圆弧辅助点到终点间i为2，Q_i(u_i)为两段期间通过一系列插值的期望姿态路径函数，u_i为描述如何沿着此路径运动的运动率，其为关于时间的函数。

3.根据权利要求1所述的一种工业机器人圆弧过辅助点轨迹规划方法，其特征在于，步骤4.2中姿态速度规划模块的工作方法为：

设计运动率为u_i(t)，圆弧起点到辅助点间i为1，圆弧辅助点到终点间i为2，通过五次多项式曲线拟合，求得相应的系数c。

u_i(t)＝c₀+c₁t+c₂t²+c₃t³+c₄t⁴+c₅t⁵ i＝1，2

通过u_i(t)实时地插补输出圆弧运动轨迹各个时刻的机器人末端姿态矢量，将插补得到的四元数转换为欧拉角，完成圆弧段上姿态的轨迹规划。四元数转换为欧拉角公式为：

最终，通过以上步骤4.1、步骤4.2中的实时插补得到圆弧运动轨迹各个时刻的机器人末端位置和姿态矢量。

Images