CN114237161A - 一种基于数字滤波的工业机器人nurbs曲线插补方法 - Google Patents

Abstract

本发明公开了一种基于数字滤波的工业机器人NURBS曲线插补方法，包括如下步骤：S1、读取NURBS曲线数据和运动约束；S2、根据曲率变化情况、轨迹弦高误差约束和最大进给速度约束确定速度极低点的位置和速度，并在该点处将曲线分割成块；S3、利用自适应辛普森算法计算各块曲线的长度，计算过程中各分块曲线将被细分为若干段并生成反长度函数；S4、根据运动约束及各分块曲线的长度和始末速度，计算各滤波器的最短时间长度，然后据此配置配置滤波器和双阶跃输入信号参数，通过数字滤波得到位移规划；S5、位移规划结果结合步骤S3中相应段的反长度函数快速计算得到插值点参数，进而求得期望点的坐标。

Description

一种基于数字滤波的工业机器人NURBS曲线插补方法

技术领域

本发明属于运动控制领域，具体涉及一种基于数字滤波的工业机器人NURBS曲线插补方法。

背景技术

目前主流的工业机器人只支持直线插补和圆弧插补，在加工复杂曲线和曲面时，往往需要需要将其离散成大量的小线段，然而该方法主要存在两个方面的不足：一是小线段一阶不连续导致进给速度存在大小和方向的突变，影响加工精度且会导致系统振动；二是庞大的工程文件会增加内存消耗并对数据通信造成负担，不能满足系统的实时性要求。与传统的小线段方法相比，使用参数插值法具有内存消耗小，速度平滑度高的特点，其中非均匀有理B样条(Non-Uniform Rational B-Spline，NURBS)为自由曲线曲面和解析形状提供了统一的数学表达形式，由于其具有直观性、局部性、收敛性和逼近性等优点，因此STEP-NC采用NURBS作为计算机辅助设计与制造(CAD/CAM)系统中数据交换的标准接口，并得到广泛的应用。应用NURBS曲线进行复杂曲线曲面加工可以大幅提高工件的加工精度和效率，同时NURBS曲线参数插值也已经成为工业机器人加工中的重要工具。进给速度规划是NURBS曲线插补中的最重要组成部分，Shpitalni(“Realtime curve interpolators,”Computer-Aided Design)提出以恒定的进给速率进行NURBS曲线插补，但此方法未考虑到曲线大曲率对轨迹精度的负面影响；Syh-Shiuh("Adaptive-feedrate interpolation forparametric curves with a confined chord error,"Computer-Aided Design)提出在插补过程中根据曲线曲率和弦高误差约束自适应调节进给速度，但此方法未考虑到速度和加速的的平滑性，调整过程中的速度突变会导致运动控制系统振动；Jahanpour("A novelacc-jerk-limited NURBS interpolation enhanced with an optimized S-shapedquintic feedrate scheduling scheme,"International Journal of AdvancedManufacturing Technology)对每个曲线转角使用五次多项式规划进给速度，但需要额外的优化算法且只能离线插补，无法满足实时性要求；此外，目前的轨迹规划通常以时间最优、能量最优及平滑性作为规划目标，这类规划方法主要考虑的是单数控设备作业的情况，无法灵活调节运动控制器的运行时间，而在实际应用场景中，工业机器人在进行焊接或分拣操作时，通常需要与其他设备协同完，另外在机器人进行工件切削或打磨操作时，需要根据工件的形状调节进给速度，因此，为了满足与其他运动设备的配合要求和轨迹精度要求，曲线插补器需要灵活地调整轨迹中特定点的进给速度和到达时间。

发明内容

针对现有技术存在的上述问题，本发明的目的是提出一种基于数字滤波的工业机器人NURBS曲线插补方法，该方法可获得良好的轨迹精度，速度平滑性和较高的插补实时性，且能灵活调节轨迹中关键点的进给速度和到达时间。

本发明至少通过如下技术方案之一实现。

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

S1、获取NURBS曲线数据和运动约束；

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

S3、利用自适应辛普森算法计算各块曲线的长度，计算过程中各分块曲线将被细分为若干段并生成反长度函数；

S4根据运动约束及各分块曲线的的特征参数，计算各滤波器的最短时间长度，配置滤波器和双阶跃的输入信号参数，通过数字滤波得到位移规划；

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

优选地，获取的运动约束包括：最大弦高误差

最大进给速度v_t、最大进给加速度A_t、最大进给加速度J_t、插补周期T_s；获取的NURBS曲线数据包括：阶数p、控制点P_i、节点向量U、控制点权重因子w_i，则具有n+1个控制点的p次NURBS曲线定义如下：

其中{N_i，p(u)}是定义在非周期且非均匀节点向量U上的p次B样条基函数，节点向量U是m+1个非递减数的集合：

采用如下递推方式定义基函数：

控制点数n+1，节点数m+1和曲线阶数p+1满足关系m＝n+p+1。

优选地，根据曲率变化情况和运动约束找到速度极低点，具体为：首先扫描整条曲线的曲率，若κ(P_i)＞κ(P_i-1)，κ(P_i)＞(P_i+1)，则控制点P_i为曲率极大值点，并将其作为速度极低点候选点，然后根据弦高误差约束、最大进给速度约束和插补周期筛选候选点筛选速度极低点。

优选地，在速度极低点处将曲线分割为块，并根据曲率、弦高误差约束和插补周期设置速度极低点处的进给速度：

其中T_s为插补周期，κ_i为速度极低点曲率，

为弦高误差约束，相邻速度极低点的速度是该分块曲线的初速度和末速度。

优选地，利用自适应辛普森算法计算各分块曲线的长度，包括以下步骤：

(1)使用积分表示NURBS曲线在分块区间[a，b]的长度，根据辛普森公式，得曲线区间[a，b]的近似长度为；

(2)根据复合辛普森积分公式原理，两相等子区间的辛普森积分结果之和近似于对完整区间的积分结果；

(3)比较步骤(1)、步骤(2)得到的近似结果，判断是否满足误差条件，若满足误差条件，则该区间的曲线长度为步骤(2)得到的结果，否则，将区间[a，b]细分，相应的分块曲线会被细分为段，然后分别对得到的两个曲线分段重复上述步骤(1)～步骤(3)，直至所有子区间满足误差条件，此时区间[a，b]的曲线长度为所有子区间曲线长度之和，即分块曲线的长度为所有分段曲线长度之和。

优选地，五次多项式拟合曲线长度l和曲线参数u的关系为各分块曲线内的各分段曲线生成反长度函数。

优选地，对于每个曲线分块，使用串联的三个有限脉冲响应滤波器对双阶跃输入信号进行数字滤波，并将结果叠加斜坡信号以获得满足S形加减速和始末速度约束的位移规划。优选地，根据运动约束和各分块曲线的特征参数设置三个滤波器的时间长度参数和双阶跃输入信号参数，其中运动约束包括弦高误差约束、最大进给速度约束、最大加速度约束、最大加加速度约束，分块曲线的特征参数包括始末速度和曲线长度。

优选地，双阶跃的输入信号的幅值参数h₁、h₂是根据目标分块初速度v_o、末速度v_s和曲线长度L进行配置。

优选地，步骤S5中，位移规划结果为各插补周期时刻的期望插补点相对于所在曲线分块起点的曲线长度，为了获得期望插补点的曲线参数，应先定位其所在曲线分段，当位移规划结果l_i对应的插补点位在第k个曲线分段时，其满足如下条件：

l_k≤l_i＜l_k+(Δl)_k

其中(Δl)_k为第k个曲线分段的曲线长度，在使用自适应辛普森算法计算分块曲线长度时已求出，l_k为曲线分段起点相对于曲线分块起点的曲线长度。

与现有的技术相比，本发明的有益效果为：

1、轨迹精度高。根据曲率变化情况，弦高误差约束，最大速度约束确定速度极低点的位置和进给速度，并在速度极低点处分割曲线获得若干分块曲线，然后分别对各分块曲线进行速度规划和轨迹插补，保证在轨迹大曲率处也能获得良好的轨迹精度，此外，还使用反长度函数取代了传统的泰勒展开法求取目标插补点参数，利用边界条件避免了逼近拟合计算中的截断误差，进步提高了轨迹精度。

2、速度平滑性高。使用了有限脉冲响应滤波器算法，生成满足S形加减速的位移规划，保证了各曲线分块间，各分块曲线内速度、加速度的连续性和平滑性，有效降低了插补中的速度波动，减少运动设备的振动，此外，正确配置输入信号的幅值参数和滤波器的时间长度参数，可保证轨迹规划满足最大速度约束，最大加速度约束和最大加加速度约束。

3、插补实时性高。使用了自适应的辛普森算法将曲线分块细分为若干段并计算曲线长度，细分操作根据设定的长度误差约束自适应执行，有效减小了计算冗余；再者，使用反长度函数取代了传统的一阶泰勒展开求取目标插补点参数，减少了复杂耗时的NURBS曲线导数计算；此外，使用数字滤波获得满足S形加减速的位移规划，相比于传统的基于分段5次多项式的速度规划方法，不需要额外了分支判断且极大地减少了计算量。

4、灵活性高。将曲线分割为若干块并使用有限脉冲响应滤波器对各曲线分块进行轨迹规划，只需调整滤波器的时间长度参数和输入信号的幅值参数，就可快速调节分割点处的进给速度和到达时间。

附图说明

图1为本发明所述的基于数字滤波的工业机器人NURBS曲线插补方法的流程图。；

图2为本发明实例中的机器人D-H图；

图3为本发明实例中的“∞”形NURBS曲线；

图4为∞形NURBS曲线的曲率图；

图5为本发明方法得到的速度曲线图；

图6为本发明方法得到的加速度曲线图；

图7为本发明方法得到的加加速度曲线图；

图8为本发明方法得到的弦高误差曲线图。

具体实施

下面结合附图和具体实施实例对本发明作进一步详细的描述。

如图1所示，本实施例提供一种基于数字滤波的工业机器人NURBS曲线插补方法，其包括预处理模块和实时插补模块两部分，具体如下：

预处理模块包括：读取NURBS曲线数据和运动参数；根据曲率变化情况、轨迹弦高误差约束和最大进给速度约束确定速度极低点的位置及进给速度；在速度极低点处将曲线分割成块；对各分块曲线，使用自适应辛普森算法将曲线细分为段并计算分区曲线的长度；为各分段曲线生产反长度函数；预估各分块曲线满足运动约束条件下的最短运行时间，即滤波器的最小时间长度。

实时插补模块包括：为配置滤波器时间长度参数使其满足相互条件和最短运行时间条件，并进一步根据分块曲线的长度、始末速度配置双阶跃输入信号的幅值参数；构建输入信号的离散表达式，再使用数字滤波处理输入信号，获得满足S形加减速及运动约束的位移规划；位移规划结果结合响应曲线分段的反长度函数计算得到目标插值点的参数；根据参数和读取NURBS曲线参数计算插值点坐标。

一种基于数字滤波的工业机器人NURBS曲线插补方法，包括以下步骤：

1、预处理阶段，包括以下步骤：

1.1、读取NURBS曲线数据和运动参数

获取的运动参数包括：最大弦高误差

最大进给速度v_t、最大进给加速度A_t、最大进给加加速度J_t、插补周期T_s；获取的NURBS曲线数据包括：阶数p、控制点{P_i}、节点向量U、控制点权重因子{w_i}。则具有n+1个控制点的p次NURBS曲线可定义如下：

其中{N_i，p(u)}是定义在非周期且非均匀节点向量U上的p次B样条基函数，节点向量U是m+1个非递减数的集合：

可采用如下递推方式定义基函数：

控制点数n+1，节点数m+1和曲线阶数p+1满足关系m＝n+p+1。

1.2、根据曲率变化情况和运动约束找到速度极低点

根据曲率变化情况和运动约束找到速度极低点，具体为：首先扫描整条曲线的曲率，若κ(P_i)＞κ(P_i-1)，κ(P_i)＞(P_i+1)，则P_i为曲率极大值点，并将其作为速度极低点候选点，然后根据弦高误差约束、最大进给速度约束和插补周期筛选候选点获得速度极低点，筛选条件如下：

其中κ_i为候选点曲率，

为弦高误差约束，v_t为最大进给速度约束，T_s为插补周期，

为最大弦高误差。在本实例曲线中共刷选得到4个速度极低点A、B、C、D。

1.3、分割曲线并设置分割点处进给速度

在速度极低点出将曲线分割为块，并根据曲率、弦高误差约束和插补周期设置速度极低点处的进给速度：

其中T_s为插补周期，κ_i为速度极低点曲率，

为弦高误差约束，相邻速度极低点的速度是该分块曲线的初速度和末速度。本实例曲线中，各速度极低点的进给速度被设置为v_A＝v_D＝139.0mm/s，v_B＝v_C＝103.3mm/s。

1.4、计算各分块曲线长度

使用自适应辛普森算法计算各分块曲线的长度，具体为：

首先NURBS曲线在分块区间[a，b]的长度用积分表示为：

其中x′(u)、y′(u)、z′(u)分别为曲线在x轴、y轴、z轴方向上的导数；积分式用辛普森公式近似，辛普森公式如下：

其中，f(u₁)、f((u₁+u₂)/2)、f(u₂)分别为被积函数在积分区间左端点、中间点和右端点的值，则令

u为NURBS曲线参数；利用上述辛普森公式及其复合形式求取NRUBS曲线长度，包括以下步骤：

(1)根据辛普森公式，得曲线区间[a，b]的近似长度为：

l_ab≈S(a，b)

其中S(a，b)表示将区间左右端点代入上述辛普森公式所获得的结果，即令u₁＝a，u₂＝b。

(2)根据复合辛普森积分公式原理，两相等子区间的辛普森积分结果之和近似于对完整区间的积分结果，由此可得：

其中S(a，(a+b)/2)和S((a+b)/2，b)为将两子区间左右端点代入前述辛普森公式所获结果。

(3)比较上述两次得到的近似结果，判断是否满足误差条件：

其中φ为允许的长度误差。若满足误差条件，则该区间的曲线长度为l_ab＝S(a，(a+b)/2)+S((a+b)/2，b)，否则，将区间[a，b]细分为[a，(a+b)/2]和[(a+b)/2，b]，相应的分块曲线会被细分为段，然后分别对得到的两个曲线分段重复上述步骤(1)～步骤(3)，直至所有子区间满足误差条件，此时区间[a，b]的曲线长度为所有子区间曲线长度之和，即分块曲线的长度为所有分段曲线长度之和。

在本实例中，各分块曲线的长度为：L_OA＝L_DO＝67.3mm，L_AB＝L_CD＝90.4mm，L_BC＝143.9mm。

1.5为曲线中的各分段曲线生成反长度函数

为各分块曲线内的各分段曲线生成反长度函数，以通过五次多项式拟合曲线长度l和曲线参数u的关系，设某块曲线内的第k段曲线的参数区间为[u_k，u_k+1]，则其反长度函数可表示为：

u(l)＝c₀+c₁(l-l_k)+c₂(l-l_k)²+c₃(l-l_k)³+c₄(l-l_k)⁴+c₅(1-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轴方向上的导数。

1.6、计算各滤波器的最小时间长度参数

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

为了使用有限脉冲响应滤波器实现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形加减速的最短运行时间规划，实际配置滤波器的时间长度参数时需大于该结果并满足时间长度的相互条件，即：

2.1、配置滤波器时间长度参数

实际配置滤波器的时间长度参数时需满足最小时间长度条件和各时间长度的相互条件，即：

在本实例中进行最短运行时间规划，故配置各滤波器为其最小时间长度，即

2.2、配置双阶跃输入信号参数

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

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

2.3、构建离散输入信号并使用串联的三个滤波器对其进行数字滤波

对于每个曲线分块，使用串联的三个有限脉冲响应滤波器对双阶跃输入信号进行数字滤波，并将结果叠加斜坡信号以获得满足S形加减速和始末速度约束的位移规划，具体如下：

首先，有限脉冲响应滤波器的传递函数如下：

其中T_i为滤波器的时间长度参数。根据数字滤波的原理，第i个滤波器在第k个插补周期的输出结果

可表示为：

其中N_i＝ceil(T_i/T_s)表示该滤波器的作用步数，T_i为该滤波器的时间长度参数，并规定

为双阶跃输入信号的离散表示，由于双阶跃输入信号的时间连续表达式如下：

r(t)＝h₁ε(t)+h₂ε(t-T₁)

其中h₁，h₂为常数，t为时间，ε(t)是阶跃函数：

在时间区间[0，T₁+T₂+T₃]对输入信号进行采样，T₁、T₂、T₃表示有限脉冲响应滤波器的时间长度参数，采样频率为插补周期T_s，得到输入信号的离散表示

即：

其中N₁、N₂、N₃为有限脉冲响应滤波器的作用步数。至此，根据

和

的表达式，可获得各个滤波器的输出结果，为了满足各分块曲线的初速度约束，输出结果需要叠加斜坡信号，即：

其中，

为第三个滤波器在第k个插补周期输出位移规划结果，v_o为曲线分块的初速度，

即为目标分块曲线满足S形加减速和初速度约束的位移规划，该位移规划的插补周期为T_s，运动时长为T₁+T₂+T₃。

2.4、计算插值点坐标

根据有限脉冲响应滤波器输出的位移规划，结合反长度函数快速计算插补点，包括以下步骤：

首先位移规划结果为各插补周期时刻的期望插补点相对于所在曲线分块起点的曲线长度，为了获得期望插补点的曲线参数，应先定位其所在曲线分段，当位移规划结果l_i对应的插补点位在第k个曲线分段时，其满足如下条件：

l_k≤l_i＜l_k+(Δl)_k

其中(Δl)_k为第k个曲线分段的曲线长度，在使用自适应辛普森算法计算分块曲线长度时已求出，l_k为曲线分段起点相对于曲线分块起点的曲线长度，可通过累加其前部各曲线分段的曲线长度获得，即：

其中(Δl)_j为第j个曲线分段的曲线长度，接着调用该分段区间的反长度函数计算插补点的曲线参数u，即：

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

所得结果结合NURBS曲线的参数表达式C(u)即可获得目标插补点的位置坐标。所得轨迹的速度、加速度、加加速度和弦高误差如图5、图6、图7、图8所示。

将本发明方法在PC上进行仿真实验，所用的编程软件为Visual Studio 2015，使用C#语言编写程序，测试环境如下：

操作系统：windows10

CPU：Intel(R)Core(TM)i5-5200U

主频：2.20GHz

内存：8G

机器人：SCARA(SSP400-150-3)

建立所使用SCARA机器人的D-H坐标系如图2所示，本实验以空间∞形的NURBS曲线为例，其形状如图3所示，其参数如下：

阶数p：3

控制点{P_i}：(120，300，70)，(180，360，80)，(180，240，60)，(120，300，70)，(60，360，80)，(60，240，60)，(120，300，70)

权重因子{w_i}：[1，10，10，1，10，10，1]

节点向量U：[0，0，0，0，0.354，0.5，0.646，1，1，1，1]

设定曲线的运动参数如下：

弦高误差

0.5μm

最大进给速度v_t：200mm/s

最大加速度A_t：1500mm/s2

最大加加速度J_t：30000mm/s3

插补周期T_s：1ms。

以上所述，仅为本发明创造较佳的具体实施方式，但本发明创造的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明创造披露的技术范围内，根据本发明创造的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明创造的保护范围之内。

Claims (10)

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

S1、获取NURBS曲线数据和运动约束；

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

S3、利用自适应辛普森算法计算各块曲线的长度，计算过程中各分块曲线将被细分为若干段并生成反长度函数；

S4根据运动约束及各分块曲线的的特征参数，计算各滤波器的最短时间长度，配置滤波器和双阶跃的输入信号参数，通过数字滤波得到位移规划；

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

2.根据权利要求1所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，获取的运动约束包括：最大弦高误差

最大进给速度v_t、最大进给加速度A_t、最大进给加速度J_t、插补周期T_s；获取的NURBS曲线数据包括：阶数p、控制点P_i、节点向量U、控制点权重因子w_i，则具有n+1个控制点的p次NURBS曲线定义如下：

其中{N_i,p(u)}是定义在非周期且非均匀节点向量U上的p次B样条基函数，节点向量U是m+1个非递减数的集合：

采用如下递推方式定义基函数：

控制点数n+1，节点数m+1和曲线阶数p+1满足关系m＝n+p+1。

3.根据权利要求1所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，根据曲率变化情况和运动约束找到速度极低点，具体为：首先扫描整条曲线的曲率，若κ(P_i)＞κ(P_i-1),κ(P_i)＞(P_i+1)，则控制点P_i为曲率极大值点，并将其作为速度极低点候选点，然后根据弦高误差约束、最大进给速度约束和插补周期筛选候选点筛选速度极低点。

4.根据权利要求1所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，在速度极低点处将曲线分割为块，并根据曲率、弦高误差约束和插补周期设置速度极低点处的进给速度：

其中T_s为插补周期，κ_i为速度极低点曲率，

为弦高误差约束，相邻速度极低点的速度是该分块曲线的初速度和末速度。

5.根据权利要求1所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，利用自适应辛普森算法计算各分块曲线的长度，包括以下步骤：

(1)使用积分表示NURBS曲线在分块区间[a,b]的长度，根据辛普森公式，得曲线区间[a,b]的近似长度为；

(2)根据复合辛普森积分公式原理，两相等子区间的辛普森积分结果之和近似于对完整区间的积分结果；

(3)比较步骤(1)、步骤(2)得到的近似结果，判断是否满足误差条件，若满足误差条件，则该区间的曲线长度为步骤(2)得到的结果，否则，将区间[a,b]细分，相应的分块曲线会被细分为段，然后分别对得到的两个曲线分段重复上述步骤(1)～步骤(3)，直至所有子区间满足误差条件，此时区间[a,b]的曲线长度为所有子区间曲线长度之和，即分块曲线的长度为所有分段曲线长度之和。

6.根据权利要求1所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，五次多项式拟合曲线长度l和曲线参数_u的关系为各分块曲线内的各分段曲线生成反长度函数。

7.根据权利要求1所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，对于每个曲线分块，使用串联的三个有限脉冲响应滤波器对双阶跃输入信号进行数字滤波，并将结果叠加斜坡信号以获得满足S形加减速和始末速度约束的位移规划。

8.根据权利要求7所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，根据运动约束和各分块曲线的特征参数设置三个滤波器的时间长度参数和双阶跃输入信号参数，其中运动约束包括弦高误差约束、最大进给速度约束、最大加速度约束、最大加加速度约束，分块曲线的特征参数包括始末速度和曲线长度。

9.根据权利要求8所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，双阶跃的输入信号的幅值参数h₁、h₂是根据目标分块初速度v_o、末速度v_s和曲线长度L进行配置。

10.根据权利要求1～9任一项所述的基于数字滤波的工业机器人NURBS曲线插补方法，其特征在于，步骤S5中，位移规划结果为各插补周期时刻的期望插补点相对于所在曲线分块起点的曲线长度，为了获得期望插补点的曲线参数，应先定位其所在曲线分段，当位移规划结果l_i对应的插补点位在第k个曲线分段时，其满足如下条件：

l_k≤l_i＜l_k+(Δl)_k

其中(Δl)_k为第k个曲线分段的曲线长度，在使用自适应辛普森算法计算分块曲线长度时已求出，l_k为曲线分段起点相对于曲线分块起点的曲线长度。

Images