CN115122329A - 一种便捷的工业机器人关节空间轨迹规划方法 - Google Patents

Abstract

本发明提供了一种便捷的工业机器人关节空间轨迹规划方法，包括如下步骤：步骤1、统一参数化关节空间轨迹：采用统一参数u表示线性段轨迹D_i‑1，i(u)和过渡轨迹C_i(u)；步骤2、对步骤1的参数化关节空间轨迹进行平顺过渡，包括过渡段轨迹的基函数的选择计算，以及过渡段轨迹平顺系数的计算；步骤3、对参数u进行速度规划，得到经过速度规划后的机器人关节空间轨迹。基于本发明提出的关节空间轨迹规划方法，通过采用统一的参数化形式表示关节空间轨迹，将多轴的运动统一为一个参数，从而将关节空间轨迹的速度规划转换为对参数的速度规划，无需多次进行轨迹规划，只需对参数速度进行规划即可实现所有关节的同步运动，计算简单效率高。

Description

一种便捷的工业机器人关节空间轨迹规划方法

技术领域

本发明属于工业机器人轨迹规划领域，具体涉及一种便捷的工业机器人关节空间轨迹规划方法。

背景技术

工业机器人加工是当代机械制造的基础工艺之一。机器人轨迹规划是机器人在关节空间或笛卡尔空间移动中按照某种轨迹规划方法使得机器人的运行时间尽可能短，或者能量尽可能小。机器人轨迹规划作为机器人插补算法中的关键技术，对机器人是否能够完成高速、高精度的加工起着决定性作用。

机器人常用的加工轨迹有两种：一种是笛卡尔空间中的直线和圆弧的运动轨迹；另外一种是关节空间中点到点的运动轨迹。本发明研究对象为关节空间中的运动轨迹。

轨迹规划一般包括轨迹平顺和速度规划。轨迹平顺常用的过渡曲线主要有：圆弧、样条等曲线，常用的速度规划方法如梯形加减速、S型加减速、多项式、指数函数和正弦型加减速等方法。

传统的关节空间轨迹平顺常采用Beizer样条曲线和Bspline样条曲线，由于Bézier控制多边形具有凸包性、几何不变性、变差缩减等优良的几何性质成为多项式曲线曲面的几何计算高效算法的基础常用于轨迹平顺。然而该类曲线的偏置曲线不是关于参数的有理多项式，传统的方法采用数值正交方法做近似处理，但是这样会产生较大的加工误差。其弧长只能近似地计算(Taylor级数或Runge-Kutta)。因此即使是恒速插补的情况，其弧长通过近似计算不能确保在插补周期内保持不变。

传统的关节空间轨迹速度规划存在轴关节同步性问题。如果轴关节运动异步，各个关节无法同时到达目标点位置，会导致经过运动学正解后的笛卡尔空间机器人末端轨迹与理论轨迹偏差较大，造成轨迹精度低，最终影响加工质量。

为了保证各个关节同时到达目标点位置，保证各关节轴的运动时间严格一致，解决同步性问题主要采用的技术方法：先将每个关节轴的运动都按照其对应的运动参数物理量进行时间最短的轨迹规划，得到每个关节轴的运动时间，比较这些时间从中选取最大时间，然后再以该最大时间为基准时间，分别对每个轴进行满足固定时间(基准时间)的轨迹规划。但这种技术方法需要经过多次轨迹规划，运算量非常大，计算效率低下。

传统的速度规划方法存在频繁启停，导致速度衔接处存在速度波动，会形成比较大的轮廓误差，难以实现高精度的加工。如果轨迹规划遇到高曲率点或急转弯，为了保证轨迹精度，就必须将速度减少到允许范围内，但是由于控制器性能的限制，不可能将速度瞬间减小到允许范围内，需要经过一定的时间过程。速度前瞻控制是为了在保证加工精度的同时实现高速加工，是一种预先分析后续路径提前发现轨迹突变(发现高曲率点和尖锐拐角)，找出减速点，并对进给速度进行有效控制的方法，在保证速度最大化的同时实现速度的平滑过渡。

速度前瞻规划根据路径形式的不同可分为两类：一是直线或圆弧等简单路径的速度前瞻规划；二是NURBS曲线、Bezier曲线等复杂曲线的速度前瞻规划。目前的速度前瞻需要预读一定数量的轨迹段，计算前瞻段数，找出速度为零的转折点，适用于所有轨迹采用统一的速度规划方法的情况，前瞻算法相对复杂，且受轨迹段顺序的影响计算效率低。

发明内容

针对现有技术存在的问题，本发明对传统的关节空间轨迹规划方法进行改进，提出了一种便捷的工业机器人关节空间轨迹规划方法，旨在快速完成关节空间轨迹的的平顺和速度规划，既可以作为工业机器简单控制器的主规划，又可以作为复杂控制器的预规划，为下一步的轨迹优化提供预备条件，进而提升整个轨迹规划的计算效率。此外，还可以扩展应用到三轴机床的轨迹规划中。

针对Beizer等传统的多项式曲线的弧长和偏置曲线计算存在的问题，本发明的关节空间轨迹平顺选择PH(Pythagorean-hodograph)曲线。它是一类特殊的具有Bezier形式的多项式参数曲线，该曲线的弧长和偏置曲线能够通过某种算法精确地计算，在实际应用中可以减小控制器在插补过程中产生的速度波动。

针对关节空间轨迹的速度规划存在的轴关节同步性运算量大等问题，本发明通过提出采用统一参数化形式表达关节空间轨迹，可以高效解决轴关节的同步性问题，即将多轴的运动统一为一个参数，从而将关节空间轨迹的速度规划转换为对参数的速度规划，只需对参数速度进行规划即可实现所有关节的同步运动。

针对关节空间轨迹的速度规划存在的速度前瞻问题，本发明提出双向混合的速度规划方法，在轨迹段数已知的情况下，采用双向的速度前瞻算法，无需计算前瞻段数，可适用于混合的速度规划方法，针对不同轨迹类型(线性段轨迹、过渡段轨迹)采用最合适的速度规划方法(梯形速度、三角函数、多项式等速度规划方法)，速度规划方法计算简单且效率高。提出的双向混合速度规划方法嵌入在非实时插补计算中使得速度规划不受轨迹段顺序的影响允许分段(多轨迹段)速度规划的并行计算。

首先，定义关节空间多维轨迹点的基本运算，包括多维距离、多维加法、多维减法和多维数乘：

关节空间多维距离是指两个多维轨迹点之间在关节空间的抽象距离，设P_i-1,P_i∈R^m,定义多维距离运算Dis_m(P_i-1,P_i)：

其中m为总的关节轴数，θ_i,k表示轨迹点P_i第k个关节的位置值，θ_i-1,k表示轨迹点P_i-1第k个关节的位置值,k＝1,…,m，R在数学上表示实数集，i表示轨迹点编号；

关节空间多维加法是指两个多维向量之间在关节空间的位置加法，设P_i-1,P_i∈R^m,定义多维加法运算Add_m(P_i-1,P_i)：

Add_m(P_i-1,P_i)＝((θ_i,1+θ_i-1,1),…,(θ_i,k+θ_i-1,k),…,(θ_i,m+θ_i-1,m)) (Eq.2)

关节空间多维减法是指两个多维轨迹点之间在关节空间的位置减法，设P_i-1,P_i∈R^m，定义多维减法运算Sub_m(P_i-1,P_i)：

Sub_m(P_i-1,P_i)＝((θ_i,1-θ_i-1,1),…,(θ_i,k-θ_i-1,k),…,(θ_i,m-θ_i-1,m)) (Eq.3)

关节空间多维数乘是指一个多维向量和一个常数σ的数乘运算，设P_i∈R^m,σ∈R,定义多维数乘运算Sca_m(P_i,σ)：

Sca_m(P_i,σ)＝((σθ_i,1),…,(σθ_i,k),…,(σθ_i,m)) (Eq.4)

一种便捷的工业机器人关节空间轨迹规划方法，包括如下步骤：

步骤1、统一参数化关节空间轨迹；

所述关节空间轨迹主要包括线性段轨迹D_i-1,i(u)和过渡轨迹C_i(u)；

采用统一参数u表示关节空间线性段轨迹D_i-1,i(u)：

D_i-1,i(u)＝(1-u)P_i-1+uP_i(i＝1,…,n) (Eq.5)

保存成如下结构数组：

D_i-1,i(P_i-1,P_i,u) (Eq.6)

其中，P_i-1为线性段轨迹起点，P_i为线性段轨迹终点，u为参数且u∈[0,1],i表示轨迹点编号，n表示总轨迹点段数；

采用统一参数u表示关节空间过渡轨迹C_i(u)：

采用凸组合(控制点加基函数组合的方式)表示关节空间过渡轨迹，选择适当的基函数作为过渡曲线，采用统一参数u表示关节空间过渡段轨迹C_i(u)：

C_i(u)＝F_i(u)P_i-1+J_i(u)P_i+K_i(u)P_i+1 (Eq.7)

保存成如下结构数组：

C_i(P_i-1,P_i,P_i+1,u) (Eq.8)

其中F_i(u),J_i(u),K_i(u)表示基函数，满足F_i(u)+J_i(u)+K_i(u)＝1，P_i-1、P_i、P_i+1为凸组合的控制顶点。

该方法可针对不同的情况扩展、客制化，实现不同类型机器人及不同连续性要求的机器人轨迹过渡，平顺过渡可选择多项式、样条等曲线作为过渡曲线，不同过渡类型的区别仅在于基函数的不同；

步骤2、关节空间轨迹的平顺过渡；

获取关节空间指令轨迹点

获取机器人参数(包括运动参数、关节速度阈值、关节加速度阈值等参数)；i表示轨迹点编号，n表示总轨迹点段数；

关节空间轨迹的平顺过渡主要包括：过渡段轨迹的基函数的选择计算，以及过渡段轨迹平顺系数的计算；

基于步骤2输入：关节空间轨迹点

初始关节空间参数轨迹

关节加速度阈值A_max；

最终输出：平顺过渡后的关节空间轨迹S(u)(由线性段轨迹和过渡段轨迹组成)；

针对Beizer等传统的多项式曲线的弧长和偏置曲线计算存在的问题，本发明的关节空间轨迹平顺选择PH(Pythagorean-hodograph)曲线。它是一类特殊的具有Bezier形式的多项式参数曲线，该曲线的弧长和偏置曲线能够通过某种算法精确地计算。

为了简化计算，本发明选择三阶Beizer PH曲线作为过渡曲线，其过渡曲线函数可表示如下：

其中E_i,0、E_i,1、E_i,2、E_i,3为控制点，

参数u∈[0,1]；

控制点可表示为：

其中，r_i,1，r_i,2,β_i,1，β_i,2∈[0,1]为平顺系数。

步骤2.1、过渡段轨迹的基函数选择确定；

本发明选择三阶Beizer PH过渡曲线为例进行详细说明，但本发明不仅限于三阶Beizer PH过渡曲线。

将公式(Eq.10)中的控制点带入公式(Eq.9)中并简化表达成公式(Eq.7)计算基函数的表达式有：

在(Eq.11)中，基函数F_i(u)、J_i(u)、K_i(u)的计算与平顺系数r_i,1，r_i,2,β_i,1，β_i,2有关，下面对平顺系数的计算进行详细说明；

步骤2.2计算过渡段轨迹的平顺系数r_i,1，r_i,2,β_i,1，β_i,2；

所有轴关节的过渡段轨迹的平顺系数一致：

步骤2.2.1基于PH曲线控制点的几何特性计算平顺系数β_i,1,β_i,2，由PH曲线控制点的几何特性和多维距离运算公式可得：

令Dis_m(P_i-1,P_i)＝λ_i·Dis_m(P_i+1,P_i)，假设r_i,2＝α_i·r_i,1,比例系数α_i作为输入，可通过输入设定，或者通过距离之比计算

其中m为总关节轴数，j为关节轴编号，i表示轨迹点编号，由此可得：

由式(Eq.12)进一步可得：

β_i,1 ²·{(Dis_m(Sub_m(P_i-1,P_i),Sca_m(Sub_m(P_i+1,P_i),λ_i)))²-λ_i ²·(Dis_m(P_i+1,P_i))²}+β_i,1·((λ_i+α_i)λ_i·(Dis_m(P_i+1,P_i))²)-λ_i·α_i·(Dis_m(P_i+1,P_i))²＝0 (Eq.14)

为了简化表达式，令：

即将式(Eq.14)化为：

β_i,1 ²·μ₁+β_i,1·μ₂+μ₃＝0 (Eq.16)

判断方程(Eq.16)是否有解：

Δ＝μ₂ ²-4μ₁μ₃

＝(λ_i-α_i)²λ_i ²(Dis_m(P_i+1，P_i))⁴+4α_iλ_i·(Dis_m(P_i+1，P_i))²·(Dis_m(Sub_m(P_i-1，P_i)，Sca_m(Sub_m(P_i+1，P_i)，λ_i)))²≥0 (Eq.17)

方程(Eq.16)至少有一个实根，即有：

式(Eq.18)满足β_i，1∈[0，1]，最终求得β_i，1的值，带入式(Eq.13)最终求得β_i，2的值；

步骤2.2.2、基于加速度阈值计算平顺系数r_i，1，r_i，2；

过渡段轨迹的加速度为：

过渡段轨迹的参数速度保持匀速，即

由此可得：

故过渡段轨迹的各关节的加速度与平顺系数r_i，1，r_i，2、参数速度H_i相关；

由下式(Eq.21)可知，过渡段轨迹的加速度是关于时间t的一次多项式，故在端点处u＝0/1时，满足加速度阈值A_max即可：

由于轨迹平顺前没有速度规划，所以参数速度H_i取前后线性轨迹的最大参数速度V_umax，i-1，V_umax，i+1中的较小者：

H_i＝min(V_umax，i-1，V_umax，i+1) (Eq.22)

令r_i，2＝α_i·r_i，1，结合以上公式即求得平顺系数r_i，1，r_i，2。

基于上述求得的系数，构造平顺过渡后的轨迹曲线S(u)：

保存为如下结构数组：

步骤3、对参数u进行速度规划，得到经过速度规划后的机器人关节空间轨迹；

由步骤2得到了平顺过渡后的关节空间轨迹S(u)，对参数u进行速度规划，针对不同轨迹类型(线性段轨迹、过渡段轨迹)采用最合适的速度规划方法(梯形速度、三角函数、多项式等速度规划方法)，充分考虑速度前瞻采用双向的速度前瞻算法。

首先根据轨迹类型进行合适的速度规划方法可行性研究，对于线性段轨迹可以选择二次多项式、三次多项式、梯形加减速或三角函数的速度规划方法，对于过渡段轨迹(PH曲线)可以选择一次多项式、二次多项式、梯形加减速或匀速的速度规划方法。选择的速度规划方法多项式次数越高计算越复杂。不同的速度规划方法可根据轨迹类型的不同进行组合。

为了简化计算，本发明对过渡段参数轨迹采用匀速速度规划方法，线性段参数轨迹采用梯形加减速(或三角函数)的速度规划方法，充分考虑到速度前瞻性，采用正逆向计算过渡段轨迹的参数速度规划，最后对线性段轨迹进行参数速度规划。

基于步骤3输入：关节空间参数轨迹S(u)、关节速度阈值V_max、关节加速度阈值A_max；

最终输出：参数u位移u(t)、关节空间关节位置S(t)即经过速度规划后的轨迹结构式。

步骤3.1、轨迹段参数速度阈值V_umax、参数加速度阈值A_umax计算；

基于步骤3.1输入：关节空间参数轨迹S(u)、关节速度阈值V_max、关节加速度阈值A_max；

输出：参数速度阈值V_umax、参数加速度阈值A_umax；

对于线性段轨迹D(u):

根据关节速度阈值、加速度阈值计算线性段轨迹D_i-1,i的最大参数速度V_umax,i-1,i，最大参数加速度A_umax,i-1,i：

其中A_u,i-1,i为线性段轨迹D_i-1,i的参数加速度，V_u,i-1,i为线性段轨迹D_i-1,i的参数速度；

计算得到每个关节的参数速度、参数加速度约束求交集，并取最大值作为V_umax,i-1,i、A_umax,i-1,i；

最终得到线性段轨迹的参数速度阈值

参数加速度阈值

对于过渡段轨迹C(u):

根据关节速度阈值、加速度阈值计算过渡段轨迹C_i的最大参数速度V_umax，i，最大参数加速度A_umax，i：

过渡段参数速度采用匀速的速度规划方法，即u(t)＝H_it，过渡段轨迹的速度函数为：

根据关节速度阈值条件可得：

关节空间轨迹关节之间相互独立，针对每个关节关于系数H_i的约束可以独立求解：

对第k个关节有：

-V_max，k≤(-3θ_i，0，k+9θ_i，1，k-9θ_i，2，k+3θ_i，3，k)H_i，k ³t²+(6θ_i，0，k-12θ_i，1，k+6θ_i，2，k)H_i，k ²t+(3θ_i，1，k-3θ_i，0，k)H_i，k≤V_max，k (Eq.28)

其中V_max，k为第k个关节的最大速度，θ_i，0，k为E_i，0的第k个关节角，H_i，k为第k个关节对应的第i段过渡段参数速度；为简化表示：令

则：

由式(Eq.29)满足两端的速度约束(t＝0和

处)可得：

令

即有

当H_i，kt∈[0，1]时将其带入式(Eq.29)得到：

过渡段的加速度函数为：

根据关节加速度阈值条件，对第k个关节有：

由式(Eq.33)可得：

根据式(Eq.30)、(Eq.31)、(Eq.34)得到每个关节处系数H_i，k的不等式约束，求交集得参数速度的约束V_umax，i：

最终得到过渡段轨迹的参数速度

参数加速度

保存为：

步骤3.2、过渡段逆向速度规划；

逆向速度规划是指从整体轨迹的末段逆向向起始段开始进行速度规划，基于终点速度连续性计算过渡段参数终点速度H_i，end约束，根据前后轨迹段速度的连续性，即C_i的终点速度等于D_i，i+1的起点速度：

(3E_i，3-3E_i，2)·H_i，end＝(E_i+1，0-E_i，3)·V_{ubegin，i，i+1} (Eq.38)

其中V_end，i表示过渡轨迹C_i的终点速度，V_{ubegin，i，i+1}为线性轨迹D_i，i+1的起点参数速度，H_i，end表示参数终点速度；

当线性轨迹D_i，i+1的参数是梯形速度规划，其起点参数速度V_{ubegin，i，i+1}受到最大参数速度V_{umax，i，i+1}，最大参数加速度A_{umax，i，i+1}、终点速度的约束：

求上式起点参数速度的约束交集并取最大值作为V_{ubegin，i，i+1}带入(Eq.38)，求第k个关节下参数终点速度H_i，end的约束范围：

(3θ_i，3，k-3θ_i，2，k)·H_i，k≤(θ_i+1，0，k-θ_i，3，k)·V_{ubegin，i，i+1} (Eq.40)

求交集得：

0＜H_i，end≤H_i，2 (Eq.41)

对参数终点速度H_i，end约束求交集并取值：

H_i，end＝Min{V_umax，i，H_i，2} (Eq.42)

其中Min表示取小值运算操作。

步骤3.3、过渡段正向速度规划；

正向速度规划是指从整体轨迹的起始段正向向末段开始进行速度规划，基于起点速度连续性计算过渡段参数起点速度H_i，begin约束，根据前后轨迹段速度的连续性，即C_i的起点速度等于D_i-1，i的终点速度：

(3E_i，1-3E_i，0)·H_i，begin＝(E_i，0-P_i-1)·V_{uend，i-1，i} (Eq.44)

其中V_begin，i表示过渡轨迹C_i的起点速度，V_{uend，i-1，i}为线性轨迹D_i-1，i的终点参数速度，H_i，end表示参数起点速度；

线性轨迹D_i-1，i的参数是梯形速度规划，其终点参数速度V_{uend，i-1，i}受到最大参数速度V_{umax，i-1，i}，最大参数加速度A_{umax，i-1，i}、起点速度V_{ubegin，i-1，i}的约束：

求上式终点参数速度的约束交集并取最大值作为V_{uend，i-1，i}带入(Eq.44)，求第k个关节下参数起点速度H_i，begin的约束范围：

(3θ_i，1，k-3θ_i，0，k)·H_i，k≤(θ_i，0，k-θ_i-1，k)·V_{uend，i-1，i} (Eq.46)

并求交集可得：

0＜H_i，begin≤H_i，3 (Eq.47)

对参数起点速度H_i，begin约束求交集并取值：

H_i，begin＝Min{V_umax，i，H_i，3} (Eq.48)

过渡段的参数速度规划：过渡段轨迹的参数运动模型为u_i(t)＝H_it，通过已有的约束条件对参数速度H_i进行约束，得到H_i值，计算运动总时间T_all，i，即可完成运动规划：

H_i＝Min{H_i，begin，H_i，end，V_umax，i}，(i＝1，...，n-1) (Eq.49)

将u_i(t)＝H_it带入C_i(u)中计算得到：

C_i(t)＝[(3β_i，1r_i，1-r_i，1)(P_i-1-P_i)+(r_i，2-3β_i，2r_i，2)(P_i+1-P_i)]H_i ³t³+[(3r_i，1-6r_i，1β_i，1)(P_i-1-P_i)+3β_i，2r_i，2(P_i+1-θ₁)]H_i ²t²+(3β_i，1r_i，1-3r_i，1)(P_i-1-P_i)H_it+r_i，1(P_i-1-P_i)+P_i

(Eq.51)

即有：

步骤3.4、线性段速度规划：

基于已有的过渡段轨迹的速度函数

以梯形速度规划进行线性段轨迹的速度规划；

步骤3.4.1、根据速度连续性，计算线性段轨迹的起终点参数速度；

根据起点速度连续性，即D_i，i+1的起点速度等于C_i的终点速度：

(E_i+1，0-E_i，3)·V_{ubegin，i，i+1}＝(3E_i，3-3E_i，2)·H_i (Eq.55)

计算得到线性段轨迹D_i，i+1的起点参数速度V_{ubegin，i，i+1}；

根据终点速度连续性，即D_i，i+1的终点速度等于C_i+1的起点速度：

(E_i+1，0-E_i，3)·V_{uend，i，i+1}＝(3E_i+1，1-3E_i+1，0)·H_i+1 (Eq.57)

计算得到线性段轨迹D_i，i+1的终点参数速度V_{uend，i，i+1}；

步骤3.4.2、线性段参数速度规划：已知起终点参数速度采用梯形速度规划方法对参数进行速度规划；

梯形速度规划方法通过输入：起点参数速度V_{ubegin，i，i+1}，终点参数速度V_{uend，i，i+1}，最大参数速度V_{umax，i，i+1}，最大参数加速度A_{umax，i，i+1}；输出：运行总时间T_{all，i，i+1}，加速时间T_up，i，i+1，减速时间T_{down，i，i+1}；具体步骤如下：

Step1：计算加速所需路程s_1，i，i+1和减速所需路程s_2，i，i+1；

计算V_{ubegin，i，i+1}加速到V_{umax，i，i+1}的路程：

计算V_{uend，i，i+1}减速到V_{umax，i，i+1}的路程：

Step2：计算加速时间T_up，i，i+1，减速时间，T_{down，i，i+1}，和运动总时间T_{all，i，i+1}；

1)如果(s_1，i，i+1+s_2，i，i+1＞1)则说明不存在匀速段，需计算能够达到的最大速度V

D_i，i+1(t)＝P_i+u_i，i+1(t)·(P_i+1-P_i) (Eq.63)

2)如果(s_1，i，i+1+s_2，i，i+1＜1)则说明存在匀速段

D_i，i+1(t)＝P_i+u_i，i+1(t)·(P_i+1-P_i) (Eq.67)

线性段轨迹经过速度规划后保存为如下结构数组：

整个机器人关节空间轨迹经过速度规划后保存为如下结构数组：

本发明具有如下优点：

1、本发明提出的便捷的关节空间轨迹规划方法中的轴关节同步方法简单高效。采用统一的参数化形式表示关节空间轨迹，即将多轴的运动统一为一个参数，从而将关节空间轨迹的速度规划转换为对参数的速度规划，无需多次进行轨迹规划，只需对参数速度进行规划即可实现所有关节的同步运动；

2、本发明提出的便捷的机器人关节空间轨迹平顺方法计算简单效率高。采用弧长可精确计算的PH曲线作为平顺曲线，基于PH曲线控制点的几何特性和加速度阈值约束计算平顺系数计算过程简单效率高。

3、本发明提出的便捷的机器人关节空间速度规划方法可实现并行计算，大大提高计算效率。提出的双向混合速度规划方法嵌入在非实时插补计算中使得速度规划不受轨迹段顺序的影响允许分段(多轨迹段)速度规划的并行计算。

4、本发明提出的便捷的关节空间轨迹规划方法具有多轨迹适用性。可同时适用于SCARA机器人以及六轴机器人的关节空间轨迹的平顺和速度规划。

5、不同的平顺和速度规划方法可以组合和扩展。采用控制点加基函数组合的参数化形式表示关节空间过渡轨迹根据基函数的不同选择表达不同的平顺过渡曲线，速度规划针对不同轨迹类型(线性段轨迹、过渡段轨迹)采用不同的混合速度规划方法。

附图说明

图1为关节空间轨迹平顺过渡示意图；

图2为本发明提出的便捷的关节空间轨迹规划算法流程图；

图3为本发明提出的轨迹平顺算法流程图；

图4为本发明提出的关节空间参数轨迹速度规划流程图；

图5为关节空间过渡段参数轨迹逆向速度规划流程图；

图6为关节空间过渡段参数轨迹正向速度规划流程图。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

一种便捷的工业机器人关节空间轨迹规划方法，该方法可以快速实现对关节空间轨迹的平顺和速度规划，下面以SCARA机器人为例，说明本发明方法的具体实现过程：

步骤1、采用统一参数化形式表示线性轨迹D_i-1，i(u)、过渡轨迹C_i(u)；

SCARA机器人在关节空间上的轨迹点P₀＝[0°，0°，0，0°]^T；P₁＝[30°，45°，25mm，40°]^T；P₂＝[70°，80°，60mm，90°]^T；P₃＝[50°，120°，40mm，120°]^T；SCARA关节速度阈值V_max＝{360，360，800，564.7}、关节加速度阈值A_max＝{3600，3600，8000，5647}；

D_0，1(u)＝(1-u)P₀+uP₁，D_1，2(u)＝(1-u)P₁+uP₂，D_2，3(u)＝(1-u)P₂+uP₃

C₁(u)＝F₁(u)P₀+J₁(u)P₁+K₁(u)P₂，C₂(u)＝F₂(u)P₁+J₂(u)P₂+K₂(u)P₃。

步骤2、关节空间轨迹的平顺过渡；

采用三阶PH曲线作为平顺过渡曲线确定基函数和平顺系数，本发明案例中初始关节空间轨迹为三段线性段轨迹D_0，1(u)、D_1，2(u)、D_2，3(u)，经过平顺过渡后变成由三段线性段轨迹D_0，1(u)、D_1，2(u)、D_2，3(u)和两段过渡段轨迹C₁(u)、C₂(u)组成的关节空间轨迹：

平顺过渡后的轨迹曲线S(u)：

具体算法为PH轨迹平顺过渡算法：

输入：关节空间轨迹点

初始关节空间参数轨迹

关节加速度阈值A_max；

输出：平顺过渡后的关节空间轨迹S(u)：

2.1、确定过渡段轨迹C(u)的基函数：

其中E_i，0、E_i，1、E_i，2、E_i，3为控制点，

本实施案例中i＝1，2；

2.2、计算平顺系数

计算

根据公式(Eq.15)计算μ₁，μ₂，μ₃；根据公式(Eq.16)计算β_i，1，根据公式(Eq.13)计算β_i，2；

计算H：

根据公式(Eq.21)计算r_i，1，r_i，2；根据(Eq.10)计算控制点E_i，0、E_i，1、E_i，2、E_i，3；

2.3、构造平顺后的参数空间轨迹：

D_0，1(u)＝(1-u)P₀+u(P₁+r_1，1(P₀-P₁))，D_1，2(u)＝(1-u)(P₁+r_1，2(P₂-P₁))+u(P₂+r_2，1(P₁-P₂))，D_2，3(u)＝(1-u)(P₂+r_2，2(P₃-P₂))+uP₃

步骤3、参数u速度规划；

采用双向混合速度规划对参数u进行速度规划，具体过程为：

输入：关节空间参数轨迹S(u)、关节速度阈值V_max、关节加速度阈值A_max；

输出：参数u位移u(t)、关节空间关节位置S(t)；

3.1、计算参数速度阈值V_umax、参数加速度阈值A_umax；

计算线性段轨迹D_i-1，i(u)的最大参数速度V_{umax，i-1，i}，最大参数加速度A_{umax，i-1，i}，其中i＝1，2，3：

求出每个关节对应的参数速度，参数加速度取交集得V_{umax，i-1，i}，A_{umax，i-1，i}；

计算过渡段轨迹C_i(u)的最大参数速度V_umax，i，最大参数加速度A_umax，i，其中i＝1，2；

求出每个关节对应的参数速度取交集得V_umax，i。

3.2、过渡段轨迹C_i(u)参数u速度规划；

3.2.1、逆向计算过渡段参数终点速度H_i，end：

计算线性段轨迹D_i，i+1的起点参数速度V_{ubegin，i，i+1}：

计算过渡段参数终点速度H_i，end：

约束求交集得数终点速度H_i，end。

3.2.2、正向计算过渡段参数起点速度H_i，begin：

计算线性段轨迹D_i-1，i的终点参数速度V_{u，end，i-1，i}：

计算过渡段参数起点速度H_i，begin：

约束求交集得参数起点速度H_i，begin。

3.2.3、计算过渡段参数速度H_i：

H_i＝Min{H_i，begin，H_i，end，V_umax，i}，i＝1，2

计算过渡段参数u_i(t)＝H_i·t，根据公式(Eq.51)、(Eq.52)计算C_i(t)，V_i(t)。

3.3、线性段轨迹D_i，i+1(u)参数u速度规划：

3.3.1、计算D_i，i+1(u)的起点参数速度V_{ubegin，i，i+1}：

3.3.2、计算D_i，i+1的终点参数速度V_{uend，i，i+1}：

3.3.3、计算V_{ubegin，i，i+1}加速到V_{umax，i，i+1}的路程：

3.3.4、计算V_{uend，i，i+1}减速到V_{umax，i，i+1}的路程：

3.3.5、如果(s_1，i，i+1+s_2，i，i+1＞1)；则

T_{all，i，i+1}＝T_up，i，i+1+T_{down，i，i+1}；

根据(Eq.62)计算u_i，i+1(t)，根据(Eq.63)计算D_i，i+1(t)，根据公式(Eq.64)计算V_i，i+1(t)；

如果s_1，i，i+1+s_2，i，i+1＜1；则

根据式(Eq.66)(Eq.66)计算u_i，i+1(t)，根据式(Eq.67)计算D_i，i+1(t)，根据公式式(Eq.68)计算V_i，i+1(t)；

然后得到线性段轨迹经过速度规划后的结构数组：

和整个轨迹经过速度规划后的结构数组：

本发明的保护范围并不限于上述的实施例，显然，本领域的技术人员可以对本发明进行各种改动和变形而不脱离本发明的范围和精神。倘若这些改动和变形属于本发明权利要求及其等同技术的范围内，则本发明的意图也包含这些改动和变形在内。

Claims (7)

1.一种便捷的工业机器人关节空间轨迹规划方法，其特征在于，包括如下步骤：

步骤1、统一参数化关节空间轨迹；所述关节空间轨迹包括线性段轨迹D_i-1,i(u)和过渡轨迹C_i(u)；

采用统一参数u表示关节空间线性段轨迹D_i-1,i(u)：

D_i-1,i(u)＝(1-u)P_i-1+uP_i(i＝1,…,n) (Eq.5)

保存成如下结构数组：

D_i-1,i(P_i-1,P_i,u) (Eq.6)

其中，P_i-1为线性段轨迹起点，P_i为线性段轨迹终点，u为参数且u∈[0,1],i表示轨迹点编号，n表示总轨迹点段数；

采用控制点加基函数组合的凸组合方式表示关节空间过渡轨迹，采用统一参数u表示关节空间过渡段轨迹C_i(u)：

C_i(u)＝F_i(u)P_i-1+J_i(u)P_i+K_i(u)P_i+1 (Eq.7)

保存成如下结构数组：

C_i(P_i-1,P_i,P_i+1,u) (Eq.8)

其中F_i(u),J_i(u),K_i(u)表示基函数，满足F_i(u)+J_i(u)+K_i(u)＝1，P_i-1、P_i、P_i+1为凸组合的控制顶点；

步骤2、对步骤1的参数化关节空间轨迹进行平顺过渡，包括过渡段轨迹的基函数的选择计算，以及过渡段轨迹平顺系数的计算；

获取关节空间指令轨迹点

和机器人参数，包括运动参数、关节速度阈值、关节加速度阈值，i表示轨迹点编号，n表示总轨迹点段数，P_i表示第i个关节空间轨迹点；

基于关节空间轨迹点

初始关节空间参数轨迹

和关节加速度阈值A_max，得到平顺过渡后的关节空间轨迹S(u)；

步骤3、对参数u进行速度规划，得到经过速度规划后的机器人关节空间轨迹；

基于步骤2得到的平顺过渡后的关节空间轨迹S(u)，对参数u进行速度规划，针对不同轨迹类型，采用合适的速度规划方法，对于线性段轨迹选择二次多项式、三次多项式、梯形加减速或三角函数的速度规划方法，对于过渡段轨迹选择一次多项式、二次多项式、梯形加减速或匀速的速度规划方法；

为简化计算，步骤3中对过渡段参数轨迹采用匀速速度规划方法，线性段参数轨迹采用梯形加减速的速度规划方法，考虑速度前瞻性，采用正逆向计算过渡段轨迹的参数速度规划，最后对线性段轨迹进行参数速度规划；

基于步骤3输入关节空间参数轨迹S(u)、关节速度阈值V_max、关节加速度阈值A_max；最终输出得到参数u位移u(t)和关节空间关节位置S(t)，即经过速度规划后的关节空间轨迹结构式。

2.如权利要求1所述的一种便捷的工业机器人关节空间轨迹规划方法，其特征在于：定义关节空间多维轨迹点的基本运算，包括多维距离、多维加法、多维减法和多维数乘：

关节空间多维距离是指两个多维轨迹点之间在关节空间的抽象距离，设P_i-1,P_i∈R^m,定义多维距离运算Dis_m(P_i-1,P_i)：

其中m为总的关节轴数，θ_i,k表示轨迹点P_i第k个关节的位置值，θ_i-1,k表示轨迹点P_i-1第k个关节的位置值,k＝1,…,m，R在数学上表示实数集，i表示轨迹点编号；

关节空间多维加法是指两个多维向量之间在关节空间的位置加法，设P_i-1,P_i∈R^m,定义多维加法运算Add_m(P_i-1,P_i)：

Add_m(P_i-1,P_i)＝((θ_i,1+θ_i-1,1),…,(θ_i,k+θ_i-1,k),…,(θ_i,m+θ_i-1,m)) (Eq.2)

关节空间多维减法是指两个多维轨迹点之间在关节空间的位置减法，设P_i-1,P_i∈R^m，定义多维减法运算Sub_m(P_i-1,P_i)：

Sub_m(P_i-1,P_i)＝((θ_i,1-θ_i-1,1),…,(θ_i,k-θ_i-1,k),…,(θ_i,m-θ_i-1,m)) (Eq.3)

关节空间多维数乘是指一个多维向量和一个常数σ的数乘运算，设P_i∈R^m,σ∈R,定义多维数乘运算Sca_m(P_i,σ)：

Sca_m(P_i，σ)＝((σθ_i，1)，...，(σθ_，k)，...，(σθ_i，m)) (Eq.4)。

3.如权利要求2所述的一种便捷的工业机器人关节空间轨迹规划方法，其特征在于：所述步骤2对关节空间轨迹的平顺过渡中，选择三阶Beizer PH曲线作为过渡曲线，其过渡曲线函数表示如下：

其中E_i,0、E_i,1、E_i,2、E_i,3为控制点，

参数u∈[0,1]；

控制点表示为：

其中，r_i,1，r_i,2,β_i,1，β_i,2∈[0,1]为平顺系数；

步骤2.1、过渡段轨迹的基函数选择确定；

将公式(Eq.10)中的控制点带入公式(Eq.9)中并简化表达成公式(Eq.7)，计算基函数的表达式有：

在(Eq.11)中，基函数F_i(u)、J_i(u)、K_i(u)的计算与平顺系数r_i,1，r_i,2,β_i,1，β_i,2有关；

步骤2.2计算过渡段轨迹的平顺系数r_i,1，r_i,2,β_i,1，β_i,2，所有轴关节的过渡段轨迹的平顺系数一致：

步骤2.2.1基于PH曲线控制点的几何特性计算平顺系数β_i,1,β_i,2，由PH曲线控制点的几何特性和多维距离运算公式可得：

令Dis_m(P_i-1,P_i)＝λ_i·Dis_m(P_i+1,P_i)，假设r_i,2＝α_i·r_i,1,比例系数α_i作为输入，通过输入设定，或者通过距离之比计算

其中m为总关节轴数，j为关节轴编号，i表示轨迹点编号，由此可得

由式(Eq.12))进一步可得：

β_i,1 ²·{(Dis_m(Sub_m(P_i-1,P_i),Sca_m(Sub_m(P_i+1,P_i),λ_i)))²-λ_i ²·(Dis_m(P_i+1,P_i))²}+β_i,1·((λ_i+α_i)λ_i·(Dis_m(P_i+1,P_i))²)-λ_i·α_i·(Dis_m(P_i+1,P_i))²＝0 (Eq.14)

为了简化表达式，令：

即将式(Eq.14)化为：

β_i,1 ²·μ₁+β_i,1·μ₂+μ₃＝0 (Eq.16)

判断方程(Eq.16)是否有解：

Δ＝μ₂ ²-4μ₁μ₃

＝(λ_i-α_i)²λ_i ²(Dis_m(P_i+1,P_i))⁴+4α_iλ_i·(Dis_m(P_i+1,P_i))²·(Dis_m(Sub_m(P_i-1,P_i),Sca_m(Sub_m(P_i+1,P_i),λ_i)))²≥0 (Eq.17)

基于方程(Eq.16)至少有一个实根，即有：

式(Eq.18)满足β_i,1∈[0,1]，求得β_i,1的值，带入式(Eq.13)求得β_i,2的值；

步骤2.2.2、基于加速度阈值计算平顺系数r_i,1，r_i,2；

过渡段轨迹的加速度为：

过渡段轨迹的参数速度保持匀速，即

由此可得：

过渡段轨迹的各关节的加速度与平顺系数r_i,1,r_i,2、参数速度H_i相关；

由下式(Eq.21)可知，过渡段轨迹的加速度是关于时间t的一次多项式，故在端点u＝0/1处，满足加速度阈值A_max即：

由于轨迹平顺前没有速度规划，故参数速度H_i取前后线性轨迹的最大参数速度V_umax,i-1,V_umax,i+1中的较小者：

H_i＝min(V_umax,i-1,V_umax,i+1) (Eq.22)

令r_i,2＝α_i·r_i,1，结合以上公式即求得平顺系数r_i,1,r_i,2；

基于上述求得的系数，构造平顺过渡后的轨迹曲线S(u)：

保存为如下结构数组：

4.如权利要求1所述的一种便捷的工业机器人关节空间轨迹规划方法，其特征在于，进一步的，所述步骤3包括如下具体步骤：

步骤3.1、轨迹段参数速度阈值V_umax、参数加速度阈值A_umax的计算；

基于步骤3.1输入关节空间参数轨迹S(u)、关节速度阈值V_max、关节加速度阈值A_max；输出得到：参数速度阈值V_umax、参数加速度阈值A_umax；

对于线性段轨迹D(u):

根据关节速度阈值、加速度阈值计算线性段轨迹D_i-1,i的最大参数速度V_umax,i-1,i，最大参数加速度A_umax,i-1,i：

其中A_u,i-1,i为线性段轨迹D_i-1,i的参数加速度，V_u,i-1,i为线性段轨迹D_i-1,i的参数速度；

计算得到每个关节的参数速度、参数加速度约束求交集，并取最大值作为V_umax,i-1,i、A_umax,i-1,i；

最终得到线性段轨迹的参数速度阈值

参数加速度阈值

对于过渡段轨迹

根据关节速度阈值、加速度阈值计算过渡段轨迹C_i的最大参数速度V_umax,i，最大参数加速度A_umax,i：

过渡段参数速度采用匀速的速度规划方法，即u(t)＝H_it，过渡段轨迹的速度函数为：

根据关节速度阈值条件可得：

关节空间轨迹关节之间相互独立，针对每个关节关于系数H_i的约束可独立求解：

对第k个关节有：

-V_max,k≤(-3θ_i,0,k+9θ_i,1,k-9θ_i,2,k+3θ_i,3,k)H_i,k ³t²+(6θ_i,0,k-12θ_i,1,k+6θ_i,2,k)H_i,k ²t+(3θ_i,1,k-3θ_i,0,k)H_i,k≤V_max,k (Eq.28)

其中V_max,k为第k个关节的最大速度，θ_i,0,k为E_i,0的第k个关节角，H_i,k为第k个关节对应的第i段过渡段参数速度；为简化表示：令

则：

由式(Eq.29)满足两端的速度约束(t＝0和

处)可得：

令

即有

当H_i,kt∈[0,1]时将其带入式(Eq.29)得到：

过渡段的加速度函数为：

根据关节加速度阈值条件，对第k个关节有：

由式(Eq.33)可得：

根据式(Eq.30)、(Eq.31)、(Eq.34)得到每个关节处系数H_i,k的不等式约束，求交集得参数速度的约束V_umax,i：

最终得到过渡段轨迹的参数速度

参数加速度

保存为：

5.如权利要求4所述的一种便捷的工业机器人关节空间轨迹规划方法，其特征在于，进一步的，所述步骤3包括如下具体步骤：

步骤3.2、过渡段逆向速度规划；

逆向速度规划是指从整体轨迹的末段逆向向起始段开始进行速度规划，基于终点速度连续性计算过渡段参数终点速度H_i,end约束，根据前后轨迹段速度的连续性，即C_i的终点速度等于D_i,i+1的起点速度：

(3E_i,3-3E_i,2)·H_i,end＝(E_i+1,0-E_i,3)·V_ubegin,i,i+1 (Eq.38)

其中V_end,i表示过渡轨迹C_i的终点速度，V_ubegin,i,i+1为线性轨迹D_i,i+1的起点参数速度，H_i,end表示参数终点速度；

当线性轨迹D_i,i+1的参数是梯形速度规划，其起点参数速度V_ubegin,i,i+1受到最大参数速度V_umax,i,i+1，最大参数加速度A_umax,i,i+1、终点速度的约束：

求上式起点参数速度的约束交集并取最大值作为V_ubegin,i,i+1带入(Eq.38)，求第k个关节下参数终点速度H_i,end的约束范围：

(3θ_i,3,k-3θ_i,2,k)·H_i,k≤(θ_i+1,0,k-θ_i,3,k)·V_ubegin,i,i+1 (Eq.40)

求交集得：

0<H_i,end≤H_i,2 (Eq.41)

对参数终点速度H_i,end约束求交集并取值：

H_i,end＝Min{V_umax,i,H_i,2} (Eq.42)

其中Min表示取小值运算操作。

6.如权利要求5所述的一种便捷的工业机器人关节空间轨迹规划方法，其特征在于，进一步的，所述步骤3包括如下具体步骤：

步骤3.3、过渡段正向速度规划；

正向速度规划是指从整体轨迹的起始段正向向末段开始进行速度规划，基于起点速度连续性计算过渡段参数起点速度H_i,begin约束，根据前后轨迹段速度的连续性，即C_i的起点速度等于D_i-1,i的终点速度：

(3E_i,1-3E_i,0)·H_i,begin＝(E_i,0-P_i-1)·V_uend,i-1,i (Eq.44)

其中V_begin,i表示过渡轨迹C_i的起点速度，V_uend,i-1,i为线性轨迹D_i-1,i的终点参数速度，H_i,end表示参数起点速度；

线性轨迹D_i-1,i的参数是梯形速度规划，其终点参数速度V_uend,i-1,i受到最大参数速度V_umax,i-1,i，最大参数加速度A_umax,i-1,i、起点速度V_ubegin,i-1,i的约束：

求上式终点参数速度的约束交集并取最大值作为V_uend,i-1,i带入(Eq.44)，求第k个关节下参数起点速度H_i,begin的约束范围：

(3θ_i,1,k-3θ_i,0,k)·H_i,k≤(θ_i,0,k-θ_i-1,k)·V_uend,i-1,i (Eq.46)

并求交集可得：

0<H_i,begin≤H_i,3 (Eq.47)

对参数起点速度H_i,begin约束求交集并取值：

H_i,begin＝Min{V_umax,i,H_i,3} (Eq.48)

过渡段的参数速度规划：过渡段轨迹的参数运动模型为u_i(t)＝H_it，通过已有的约束条件对参数速度H_i进行约束，得到H_i值，计算运动总时间T_all,i，即可完成运动规划：

H_i＝Min{H_i,begin,H_i,end,V_umax,i},(i＝1,…,n-1) (Eq.49)

将u_i(t)＝H_it带入C_i(u)中计算得到：

C_i(t)＝[(3β_i,1r_i,1-r_i,1)(P_i-1-P_i)+(r_i,2-3β_i,2r_i,2)(P_i+1-P_i)]H_i ³t³+[(3r_i,1-6r_i,1β_i,1)(P_i-1-P_i)+3β_i,2r_i,2(P_i+1-θ₁)]H_i ²t²+(3β_i,1r_i,1-3r_i,1)(P_i-1-P_i)H_it+r_i,1(P_i-1-P_i)+P_i (Eq.51)

即有：

7.如权利要求6所述的一种便捷的工业机器人关节空间轨迹规划方法，其特征在于，进一步的，所述步骤3包括如下具体步骤：

步骤3.4、线性段速度规划：

基于已有的过渡段轨迹的速度函数

以梯形速度规划进行线性段轨迹的速度规划；

步骤3.4.1、根据速度连续性，计算线性段轨迹的起终点参数速度；

根据起点速度连续性，即D_i,i+1的起点速度等于C_i的终点速度：

(E_i+1,0-E_i,3)·V_ubegin,i,i+1＝(3E_i,3-3E_i,2)·H_i (Eq.55)

计算得到线性段轨迹D_i,i+1的起点参数速度V_ubegin,i,i+1；

根据终点速度连续性，即D_i,i+1的终点速度等于C_i+1的起点速度：

(E_i+1,0-E_i,3)·V_uend,i,i+1＝(3E_i+1,1-3E_i+1,0)·H_i+1 (Eq.57)

计算得到线性段轨迹D_i,i+1的终点参数速度V_uend,i,i+1；

步骤3.4.2、线性段参数速度规划：已知起终点参数速度采用梯形速度规划方法对参数进行速度规划；

梯形速度规划方法通过输入：起点参数速度V_ubegin,i,i+1，终点参数速度V_uend,i,i+1，最大参数速度V_umax,i,i+1，最大参数加速度A_umax,i,i+1；输出：运行总时间T_all,i,i+1，加速时间T_up,i,i+1，减速时间T_down,i,i+1；具体步骤如下：

Step1:计算加速所需路程s_1,i,i+1和减速所需路程s_2,i,i+1；

计算V_ubegin,i,i+1加速到V_umax,i,i+1的路程：

计算V_uend,i,i+1减速到V_umax,i,i+1的路程：

Step2:计算加速时间T_up,i,i+1，减速时间,T_down,i,i+1，和运动总时间T_all,i,i+1；

1)如果(s_1,i,i+1+s_2,i,i+1>1)则说明不存在匀速段，需计算能够达到的最大速度V

D_i,i+1(t)＝P_i+u_i,i+1(t)·(P_i+1-P_i) (Eq.63)

2)如果(s_1,i,i+1+s_2,i,i+1<1)则说明存在匀速段

D_i,i+1(t)＝P_i+u_i,i+1(t)·(P_i+1-P_i) (Eq.67)

线性段轨迹经过速度规划后保存为如下结构数组：

整个机器人关节空间轨迹经过速度规划后保存为如下结构数组：

Images