[发明专利]一种基于数字滤波的工业机器人NURBS曲线插补方法

权利要求书

1.一种基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，包括步骤如下：

S1、获取NURBS曲线数据和运动约束，其中曲线数据包括阶数、节点向量、控制点坐标及其权重因子，运动约束包括弦高误差约束、最大进给速度约束、最大进给加速度约束、最大进给加加速度约束和插补周期约束；

S2、根据曲率变化情况、弦高误差约束和最大进给速度约束确定速度极低点的位置及进给速度，然后在该点处将曲线分割成块；

S3、利用自适应辛普森算法计算各块曲线的长度，计算过程中各分块曲线将被细分为若干段并生成五次多项式形式的反长度函数，并利用区间起点、中间点和末点处的曲线长度与一阶导数构建边界条件以确定多项式系数；

反长度函数表示为：

u(l)＝c₀+c₁(l-l_k)+c₂(l-l_k)²+c₃(l-l_k)³+c₄(l-l_k)⁴+c₅(l-l_k)⁵

式中，c₀～c₅为多项式系数，l_k为曲线上的点C(u_k)相对于该分段所在曲线块起点的曲线长度，点C(u_k)也是该曲线分段的起点，其中多项式系数通过下列边界等式联立计算得到：

其中u_k和u_k+1是参数区间起点和末点，而u_k+0.5是参数区间中点，即u_k+0.5＝(u_k+u_k+1)/2，l_k+0.5和l_k+1分别是曲线上的点C(u_k+0.5)和C(u_k+1)相对于该分段所在曲线块起点的曲线长度，l′(u_k)、l′(u_k+1)、l′(u_k+0.5)分别是参数区间起点、末点和中点处曲线长度关于曲线参数的导数，其表达式为x′(u)、y′(u)、z′(u)分别为曲线在x轴、y轴、z轴方向上的导数；

S4根据弦高误差约束、最大进给速度约束、最大加速度约束和最大加加速度约束四类运动约束，及各分块曲线的初速度、末速度和长度这三种特征参数，配置各滤波器的最短时间长度参数、双阶跃的输入信号的幅值参数以及斜坡输出信号的斜率参数，之后串联配置好的有限脉冲响应滤波器对双阶跃输入信号进行数字滤波，并将结果叠加斜坡信号以获得满足S形加减速和始末速度约束的位移规划；

根据运动约束和各分块曲线的特征参数设置三个滤波器的时间长度参数和双阶跃输入信号参数，其中运动约束包括弦高误差约束、最大进给速度约束、最大加速度约束、最大加加速度约束，分块曲线的特征参数包括始末速度和曲线长度，配置过程具体如下：

为了使用有限脉冲响应滤波器实现S形加减速规划，三个滤波器的时间长度参数需满足如下相互条件：

其次，S形加减速位移规划与有限脉冲响应滤波器的时间长度参数T₁、T₂、T₃有如下关系：

T₂＝(V-v₁)/A

T₃＝A/J

其中，v₁＝min(v_o,v_s)，v₂＝max(v_o,v_s)，v_o,v_s分别为分块曲线的初速度和末速度，L为轨迹长度，V、A、J分别为速度最大值、加速度绝对值最大值、加加速度绝对值最大值；

为了实现满足运动约束的最短运动时间规划，令V＝v_t,A＝A_t,J＝J_t，其中v_t,A_t,J_t分别为最大进给速度约束、最大加速度约束、最大加加速度约束，将其代入上式并得到各滤波器的最短时间长度但结果可能不满足时间长度的相互条件，需分情况进行校正：

(1)当时，令此时

(2)当时，令此时

(3)当且时，令V′＝v₁+root²/J，A′＝root，其中root是函数式x³+0.5J(3v₁+v₂)x-0.5LJ²＝0的实数解，此时

使用修正的最大速度V′，最大加速度绝对值A′重新计算滤波器的时间长度参数，并重新验证是否满足相互条件，若不满足，则继续根据不满足条件继续修正，直至满足；最终得到的结果即为满足S形加减速的最短运行时间规划，实际配置滤波器的时间长度参数时需大于该结果并满足时间长度的相互条件，即：

实际配置滤波器的时间长度参数时需满足最小时间长度条件和各时间长度的相互条件，即：在进行最短运行时间规划，故配置各滤波器为其最小时间长度，即

根据期望轨迹的始末速度和分块曲线长度，结合滤波器的时间长度参数来设置输入双阶跃输入信号的幅值参数，具体如下：

在正确配置滤波器时间长度参数T₁、T₂、T₃的前提下，双阶跃输入信号的幅值参数h₁、h₂根据目标分块初速度v_o、末速度v_s和曲线长度L进行配置，即：

其中T_s为插补周期；

S5、基于位移规划结果，结合相应段的反长度函数快速计算得到插值点参数，进而求得期望点的坐标。