CN101493687B - 实时前瞻全程加减速控制的nurbs曲线自适应分段插补方法 - Google Patents

Abstract

本发明公开了一种实时前瞻全程加减速控制的NURBS曲线自适应分段插补方法，采用矩阵形式表示NURBS曲线，并进行预处理；对所述NURBS曲线采用自适应速度调整算法，获得满足插补精度要求的进给速度等参数；在曲线曲率各极大值处将曲线分段；采用S曲线加减速控制方法对各分段曲线进行速度规划，得到速度曲线各加减速阶段变化时间；根据插补周期和各加减速阶段变化时间，得到各插补周期进给速度和对应插补点坐标；根据插补点坐标和进给速度，生成控制信号给定值。采用本发明方法，可实现NURBS曲线插补全程前瞻处理，使实时插补过程快速高效；整个插补运动过程平滑稳定，适用范围广泛，对高速高精数控系统的发展有重要意义。

Description

实时前瞻全程加减速控制的NURBS曲线自适应分段插补方法

技术领域

该发明涉及数控系统领域，特别是数控机床或关节型机器人运动控制技术中一种具有前瞻功能的实时全程加减速控制的自适应分段NURBS曲线插补方法。

背景技术

非均匀有理B样条(Non-Uniform Rational B-Spline，简称NURBS曲线)因其设计灵活、算法稳定等优点成为计算机几何信息表达、设计和数据交换等方面的工业标准，在CAD/CAM系统中得到广泛应用，航空、宇航、汽车、模具等行业中大量自由型面采用NURBS曲线来表示。NURBS曲线插补在高速加工中的优势主要体现在：曲线信息完整、程序代码大为简化、加工精度更高、进给速度更快、进给运动过程平稳；因此研究NURBS曲线插补技术，对于提高计算机数字控制系统的性能具有十分重要意义。

目前市场上具有NURBS曲线插补功能的数控系统产品不多，且多集中在FANUC等国外公司。现行公开的NURBS曲线插补方法还存在很多缺点，如：不能全面控制插补全过程中速度、加速度和加加速度的变化，算法复杂，实时性不高，难以实现减速点位置的准确预测等，对实现现代数控加工的高速高精及关节型机器人高速平滑的运动目标带来困难。

发明内容

本发明为了克服现有技术中的不足，提供一种实时前瞻全程S曲线加减速控制的非均匀有理B样条(NURBS)曲线自适应分段插补方法。该方法采用前瞻预处理与实时插补分离的方式，将大部分数值计算集中在前瞻预处理部分，减少实时插补过程的计算量，从而提高插补实时性。以机器常数(最大加速度、最大加加速度)和允许最大弦高误差为限制条件，采用S曲线加减速控制方法对NURBS曲线全程插补进给速度进行规划，使整个插补运动过程平滑稳定，且满足插补精度要求，同时解决减速点预测难的问题，是一种适用范围广泛的数控插补控制方法。

为实现上述目标，本发明的技术方案如下：

一种实时前瞻全程加减速控制的NURBS曲线自适应分段插补方法，包括以下步骤：

前瞻预处理部分：

(1)采用矩阵形式表示NURBS曲线，并进行预处理；

(2)对所述NURBS曲线采用自适应速度调整算法，获得满足插补精度要求的进给速度参数；

(3)在速度自适应调整范围内的曲线曲率各极大值处将曲线分段；

(4)采用S曲线加减速控制方法对各分段曲线进行速度规划，得到速度曲线各加减速阶段变化时间；

实时插补部分：

(5)根据插补周期和各加减速阶段变化时间，得到各插补周期进给速度和对应插补点坐标；

(6)根据插补点坐标和进给速度，生成控制信号给定值，输出到系统动力部分。

本发明方法所述三次NURBS曲线矩阵形式表示为：

$P_i\left(t\right)=\frac{\left[\begin{array}{cccc}1& t& t^2& t^3\end{array}\right]A_i\left[\begin{array}{c}{\mathrm{\ω}}_{i-3}{d}_{i-3}\\ {\mathrm{\ω}}_{i-2}{d}_{i-2}\\ {\mathrm{\ω}}_{i-1}{d}_{i-1}\\ {\mathrm{\ω}}_i{d}_i\end{array}\right]}{\left[\begin{array}{cccc}1& t& t^2& t^3\end{array}\right]A_i\left[\begin{array}{c}{\mathrm{\ω}}_{i-3}\\ {\mathrm{\ω}}_{i-2}\\ {\mathrm{\ω}}_{i-1}\\ {\mathrm{\ω}}_i\end{array}\right]}$

其中：P_i(t)为位置矢量，ω_i为权因子，与控制顶点d_i相对应，

$A_i=\left[\begin{array}{cccc}{a}_{\mathrm{1,1}}& a_{\mathrm{1,2}}& a_{\mathrm{1,3}}& a_{\mathrm{1,4}}\\ a_{\mathrm{2,1}}& a_{\mathrm{2,2}}& a_{\mathrm{2,3}}& a_{\mathrm{2,4}}\\ a_{\mathrm{3,1}}& a_{\mathrm{3,2}}& a_{\mathrm{3,3}}& a_{\mathrm{3,4}}\\ a_{\mathrm{4,1}}& a_{\mathrm{4,2}}& a_{\mathrm{4,3}}& a_{\mathrm{4,4}}\end{array}\right]=\left[\begin{array}{cccc}\frac{{\left({\▿}_i\right)}^2}{{\▿}_{i-1}^2{\▿}_{i-2}^3}& 1-a_{\mathrm{1,1}}-a_{\mathrm{1,3}}& \frac{{\left({\▿}_{i-1}\right)}^2}{{\▿}_{i-1}^2{\▿}_{i-1}^3}& 0\\ -3a_{\mathrm{1,1}}& 3(a_{\mathrm{1,1}}-a_{\mathrm{2,3}})& \frac{3{\▿}_i{\▿}_{i-1}}{{\▿}_{i-1}^2{\▿}_{i-1}^3}& 0\\ 3a_{\mathrm{1,1}}& -3(a_{\mathrm{1,1}}-a_{\mathrm{3,3}})& \frac{3{\left({\▿}_i\right)}^2}{{\▿}_{i-1}^2{\▿}_{i-1}^3}& 0\\ -a_{\mathrm{1,1}}& a_{\mathrm{1,1}}-a_{\mathrm{4,3}}-a_{\mathrm{4,4}}& -(\frac{a_{\mathrm{3,3}}}{3}+a_{\mathrm{4,4}}+\frac{{\left({\▿}_i\right)}^2}{{\▿}_i^2{\▿}_{i-1}^3})& \frac{{\left({\▿}_i\right)}^2}{{\▿}_i^2{\▿}_i^3}\end{array}\right]$

特别地

对于节点矢量U＝[u₀，u₁，…，u_n+k+1]，可生成

其中k为曲线阶次，n为控制顶点个数，

参数u∈[u_i，u_i+1)，(0≤t ≤1，i＝3，4，...，n)。

展开矩阵表达式A_i，整理并令：

Y₀＝a_1，1ω_i-3d_i-3+a_1，2ω_i-2d_i-2+a_1，3ω_i-1d_i-1+a_1，4ω_id_i

Y₁＝a_2，1ω_i-3d_i-3+a_2，2ω_i-2d_i-2+a_2，3ω_i-1d_i-1+a_2，4ω_id_i

Y₂＝a_3，1ω_i-3d_i-3+a_3，2ω_i-2d_i-2+a_3，3ω_i-1d_i-1+a_3，4ω_id_i

Y₃＝a_4，1ω_i-3d_i-3+a_4，2ω_i-2d_i-2+a_4，3ω_i-1d_i-1+a_4，4ω_id_i

Y₀′＝a_1，1ω_i-3+a_1，2ω_i-2+a_1，3ω_i-1+a_1，4ω_i

Y₁′＝a_2，1ω_i-3+a_2，2ω_i-2+a_2，3ω_i-1+a_2，4ω_i

Y₂′＝a_3，1ω_i-3+a_3，2ω_i-2+a_3，3ω_i-1+a_3，4ω_i

Y₃′＝a_4，1ω_i-3+a_4，2ω_i-2+a_4，3ω_i-1+a_4，4ω_i

则： $P_i\left(t\right)=\frac{Y_0+Y_{1}t+Y_2{t}^2+Y_3{t}^3}{Y_0^{\′}+Y_1^{\′}t+Y_2^{\′}{t}^2+Y_3^{\′}{t}^3}$ 0≤t≤1，i＝3，4，...，n

由于控制顶点d_i与权因子ω_i都是已知，而且A_i仅与节点矢量U有关，这样可以在插补前预先求出系数Y₀，Y₁，Y₂，Y₃，Y₀′，Y₁′，Y₂′，Y₃′，插补计算时只需直接调用各项系数，从而大大加快计算速度。

本发明方法所述NURBS曲线采用自适应速度调整算法获取满足插补精度要求的进给速度参数，包括以下步骤：

(2.1)采用二阶泰勒展开式计算插补点对应参数u值：

令：x＝P_x(u)，y＝P_y(u)，z＝P_z(u)

$x^{\′}=\frac{{\mathrm{dP}}_x\left(u\right)}{\mathrm{du}},$ $y^{\′}=\frac{{\mathrm{dP}}_y\left(u\right)}{\mathrm{du}},$ $z^{\′}=\frac{{\mathrm{dP}}_z\left(u\right)}{\mathrm{du}}$

$x^{\′\′}=\frac{d^2{P}_x\left(u\right)}{{\mathrm{du}}^2},$ $y^{\′\′}=\frac{d^2{P}_y\left(u\right)}{{\mathrm{du}}^2},$ $z^{\′\′}=\frac{d^2{P}_z\left(u\right)}{{\mathrm{du}}^2}$

则： $u_{i+1}=u_i+\frac{V\left(u_i\right)T+(T^2/2)(\mathrm{dV}\left(u_i\right)/\mathrm{dt})}{\sqrt{{\left(x^{\′}\right)}^2+{\left(y^{\′}\right)}^2+{\left(z^{\′}\right)}^2}}-\frac{{\left(V\left(u_i\right)T\right)}^2(x^{\′}{x}^{\′\′}+y^{\′}{y}^{\′\′}+z^{\′}{z}^{\′\′})}{2{({\left(x^{\′}\right)}^2+{\left(y^{\′}\right)}^2+{\left(z^{\′}\right)}^2)}^2}$

其中，V(u_i)为当前插补点进给速度，T为插补周期；

(2.2)采用圆弧近似方法确定弦高误差和进给速度及曲率半径之间的关系：

如附图1所示，在区间u∈[u_i，u_i+1)内，用一段圆弧拟合NURBS曲线，ρ_i是u＝u_i处的曲率半径。ρ_i＝1/β_i，其中β_i是NURBS曲线上任意一点的曲率，可通过下式计算：

${\mathrm{\β}}_i=\frac{{[{\left|\begin{array}{cc}{y}^{\′}& z^{\′}\\ y^{\′\′}& z^{\′\′}\end{array}\right|}^2+{\left|\begin{array}{cc}{z}^{\′}& x^{\′}\\ z^{\′\′}& x^{\′\′}\end{array}\right|}^2+{\left|\begin{array}{cc}{x}^{\′}& y^{\′}\\ x^{\′\′}& y^{\′\′}\end{array}\right|}^2]}^{1/2}}{{(x^{\′2}+y^{\′2}+z^{\′2})}^{3/2}}$

C(u_i)和C(u_i+1)分别是近似圆上u＝u_i和u＝u_i+1处的插补点，而P(u_i)和P(u_i+1)分别是NURBS曲线上u＝u_i和u＝u_i+1处的插补点。因为C(u_i)＝P(u_i)，并令L＝||C(u_i+1)-C(u_i)||，则进给速度V(u_i)可以近似地表示为：

$V\left(u_i\right)=\frac{L}{T}$

弦高误差δ为：

$\mathrm{\δ}={\mathrm{\ρ}}_i-\sqrt{{\mathrm{\ρ}}_i^2-{\left(\frac{L}{2}\right)}^2}$

如果限定弦高误差δ的大小，则相应的进给速度为：

$V\left(u_i\right)=\frac{2}{T}\sqrt{{\mathrm{\ρ}}_i^2-{({\mathrm{\ρ}}_i-\mathrm{\δ})}^2}$

上式表明进给速度V(u_i)应随δ和ρ_i的变化自适应地调整，调整规则如下：

$V\left(u_i\right)=\left\{\begin{array}{cc}F& \frac{2}{T}\sqrt{{\mathrm{\&rho;}}_i^2-{({\mathrm{\&rho;}}_i-\mathrm{\&delta;})}^2}>F\\ \frac{2}{T}\sqrt{{\mathrm{\&rho;}}_i^2-{({\mathrm{\&rho;}}_i-\mathrm{\&delta;})}^2}& \frac{2}{T}\sqrt{{\mathrm{\&rho;}}_i^2-{({\mathrm{\&rho;}}_i-\mathrm{\&delta;})}^2}<F\end{array}\right.$

其中F是进给速度指令值。如果曲线上当前点的曲率半径足够小，则弦高误差可能超过误差限定值，这时插补算法将进给速度由F减小到以满足限定的弦高误差要求；否则，以给定的进给速度F继续进行插补；

(2.3)将步骤(2.2)中得到的速度V(u_i)代入(2.1)中计算下一插补点对应参数u_i+1；

(2.4)重复以上(2.1)～(2.3)3个步骤，直到完成全部NURBS曲线自适应速度调整。

本发明方法所述弦高误差δ在任意范围都可实现。

本发明方法所述在速度自适应调整范围内的曲线曲率各极大值处将曲线分段，包括以下步骤：

(3.1)根据自适应速度调整公式，找出速度开始发生较大变化时曲线的曲率β_rg：

${\mathrm{\&beta;}}_{\mathrm{rg}}=\frac{2\mathrm{\&delta;}}{F^2{\left(\frac{T}{2}\right)}^2+{\mathrm{\&delta;}}^2}$

其中，δ为允许最大插补精度误差值；

(3.2)根据获得的整条曲线曲率β_i，在β_i≥β_rg范围内找出各曲率极大值点，并以各曲率极大值点对应参数u(记为u_seg)为分段点，将曲线分段；

(3.3)根据步骤(3.2)得到分段曲线各项参数，计算出各曲线段速度最大值V_max、初始速度V_str、结束速度V_end、各段轨迹长度L_seg和分段点对应的参数u_seg值。

本发明方法所述采用S曲线加减速控制方法对各分段曲线进行速度规划，得到速度曲线各加减速阶段变化时间，计算过程如附图3、附图4所示，包括以下步骤：

(4.1)计算满足分段曲线最后一段速度要求的最短轨迹长度L_dec：

$L_{\mathrm{dec}}=(V_{\mathrm{str}}\left(n\right)+V_{\mathrm{end}}\left(n\right))\sqrt{|V_{\mathrm{str}}\left(n\right)-V_{\mathrm{end}}\left(n\right)|/J_{\max }}$

其中，n为分段后曲线段段数。

(4.2)比较分段曲线末段实际长度L_seg(n)与L_dec，当L_seg(n)≥L_dec时进入步骤(4.3)，当L_seg(n)＜L_dec时合并分段曲线最后两段，且L_seg(n-1)＝L_seg(n-1)+L_seg(n)，V_end(n-1)＝V_end(n)，n＝n-1，返回步骤(4.1)。

(4.3)根据步骤(4.1)～(4.2)调整后的分段曲线各参数，结合S曲线加减速控制方法重新规划分段曲线的速度曲线各加减速变化阶段时间t_i(i＝1，2，…，7)，其中S曲线加减速控制方法的位移公式如下：

$s\left(t\right)=\left\{\begin{array}{cc}{v}_{s}t+\frac{1}{6}{J}_{\max }{t}^3& t\&Element;[0,t_1)\\ s_1+v_{1}t+\frac{1}{2}{A}_{\max }{t}^2& t\&Element;{[t}_1,t_1+t_2)s_1=v_1{t}_1+\frac{1}{6}{J}_{\max }{t}_1^3\\ s_2+v_{2}t+\frac{1}{2}{A}_{\max }{t}^2-\frac{1}{6}{J}_{\max }{t}^3& t\&Element;[t_1+t_2,2t_1+t_2),s_2=s_1+v_1{t}_2+\frac{1}{2}{A}_{\max }{t}_2^2\\ s_3+v_{3}t& t\&Element;[2t_1+t_2,2t_1+t_2+t_4),s_3=s_2+v_2{t}_3+\frac{1}{2}{A}_{\max }{t}_3^2-\frac{1}{6}{J}_{\max }{t}_3^3\\ s_4+v_{4}t-\frac{1}{6}{J}_{\max }{t}^3& t\&Element;[2t_1+t_2+t_4,2t_1+t_2+t_4+t_5),s_4=s_3+v_3{t}_4\\ s_5+v_{5}t-\frac{1}{2}{A}_{\max }{t}^2& t\&Element;[2t_1+t_2+t_4+t_5,2t_1+t_2+t_4+t_5+t_6),s_5=s_4+v_4{t}_5-\frac{1}{6}{J}_{\max }{t}_5^3\\ s_6+v_{6}t-\frac{1}{2}{A}_{\max }{t}^2+\frac{1}{6}{J}_{\max }{t}^3& t\&Element;[{2t}_1+t_2+t_4+t_5+t_6,{2t}_1+t_2+t_4+2t_5+t_6),s_6=s_5+v_5{t}_6\frac{1}{2}{A}_{\max }{t}_6^2\end{array}\right.$

A_max为最大加速度，J_max为最大加加速度，t₁，t₂，t₃为各加速阶段时间，t₄为匀速段时间，t₅，t₆，t₇为各减速阶段时间。

加速阶段轨迹长度

减速阶段轨迹长度

本发明方法所述实时插补模块，根据插补周期和由预处理模块得到的分段曲线各加减速阶段变化时间，计算出分段曲线各插补周期内进给速度和对应插补点坐标，包括以下步骤：

(5.1)根据插补周期T和步骤(4.3)获得分段曲线速度曲线中各加减速阶段变化时间，计算各加减速阶段插补步数。

(5.2)根据步骤(5.1)获得的插补步数，结合S曲线加减速位移计算公式，计算得到各分段曲线在每一插补周期内的进给速度。

(5.3)根据步骤(5.2)获得的进给速度代入步骤(2.1)，计算得插补点对应参数u，再代入曲线矩阵表达展开式，可得到各插补点坐标，即可得到插补点位置增量，最后将插补点位置增量转换为脉冲信号，发送给系统动力部分驱动电机运行。

该方法所述动力部分为步进电机或伺服驱动系统。

本发明与现有技术相比具有以下优点和效果：

(1)提高插补全过程执行机构运动平滑性

本算法采用S曲线加减速控制方法来规划插补进给速度，使整个插补过程的速度变化平滑、加速度连续、加加速度恒定，且都保持在允许范围内，因此避免加速度及加加速度过大对系统造成冲击，提高执行机构运动平滑性。

(2)提高减速点位置预测的准确性

本算法通过分析各分段曲线起始速度、终点速度、最大速度及曲线段长度，按照S曲线加减速控制方法自适应调整进给速度在加速、匀速及减速各部分所需要的准确时间，同时确定了减速点位置，且比其他单独预测方法获得的减速点位置更精确。

(3)提高实时性

本算法采用前瞻预处理与实时插补相结合的策略，在前瞻预处理在实时插补前完成，对实时性要求没有影响；同时在前瞻部分中采用NURBS曲线的矩阵表达式预先计算出表达式中各项系数，在实时插补中可直接使用，减少计算量和计算时间，在主频为2.42GHz的Pentium4处理器VC++6.0平台下，前瞻预处理所需时间不超过50μs，实时插补所需计算时间不超过10μs，相对1ms左右的插补周期来说完全满足实时性要求。

(4)使用范围更广泛

本算法中前瞻预处理部分也可用于加工前检测加工参数的合理性，为实时插补过程提供可靠保证。

附图说明

图1是本发明中的圆弧近似法估算下一插补点示意图

图2为本发明的总体结构流程图

图3是本发明的S曲线加减速规划计算流程图

图4是本发明的S曲线加减速规划中重新计算最大速度流程图

具体实施方式

下面结合附图对本发明作进一步描述。

如附图2所示，本发明中前瞻预处理和实时插补单独进行，以保证前瞻预处理不影响实时性要求。

前瞻预处理包括以下步骤：

(1)采用矩阵形式表示三次NURBS曲线如下所示：

$P_i\left(t\right)=\frac{\left[\begin{array}{cccc}1& t& t^2& t^3\end{array}\right]A_i\left[\begin{array}{c}{\mathrm{\&omega;}}_{i-3}{d}_{i-3}\\ {\mathrm{\&omega;}}_{i-2}{d}_{i-2}\\ {\mathrm{\&omega;}}_{i-1}{d}_{i-1}\\ {\mathrm{\&omega;}}_i{d}_i\end{array}\right]}{\left[\begin{array}{cccc}1& t& t^2& t^3\end{array}\right]A_i\left[\begin{array}{c}{\mathrm{\&omega;}}_{i-3}\\ {\mathrm{\&omega;}}_{i-2}\\ {\mathrm{\&omega;}}_{i-1}\\ {\mathrm{\&omega;}}_i\end{array}\right]}$

其中：P_i(t)为位置矢量，ω_i为权因子，与控制顶点d_i相对应，

$A_i=\left[\begin{array}{cccc}{a}_{\mathrm{1,1}}& a_{\mathrm{1,2}}& a_{\mathrm{1,3}}& a_{\mathrm{1,4}}\\ a_{\mathrm{2,1}}& a_{\mathrm{2,2}}& a_{\mathrm{2,3}}& a_{\mathrm{2,4}}\\ a_{\mathrm{3,1}}& a_{\mathrm{3,2}}& a_{\mathrm{3,3}}& a_{\mathrm{3,4}}\\ a_{\mathrm{4,1}}& a_{\mathrm{4,2}}& a_{\mathrm{4,3}}& a_{\mathrm{4,4}}\end{array}\right]=\left[\begin{array}{cccc}\frac{{\left({\&dtri;}_i\right)}^2}{{\&dtri;}_{i-1}^2{\&dtri;}_{i-2}^3}& 1-a_{\mathrm{1,1}}-a_{\mathrm{1,3}}& \frac{{\left({\&dtri;}_{i-1}\right)}^2}{{\&dtri;}_{i-1}^2{\&dtri;}_{i-1}^3}& 0\\ -3a_{\mathrm{1,1}}& 3(a_{\mathrm{1,1}}-a_{\mathrm{2,3}})& \frac{3{\&dtri;}_i{\&dtri;}_{i-1}}{{\&dtri;}_{i-1}^2{\&dtri;}_{i-1}^3}& 0\\ 3a_{\mathrm{1,1}}& -3(a_{\mathrm{1,1}}-a_{\mathrm{3,3}})& \frac{3{\left({\&dtri;}_i\right)}^2}{{\&dtri;}_{i-1}^2{\&dtri;}_{i-1}^3}& 0\\ -a_{\mathrm{1,1}}& a_{\mathrm{1,1}}-a_{\mathrm{4,3}}-a_{\mathrm{4,4}}& -(\frac{a_{\mathrm{3,3}}}{3}+a_{\mathrm{4,4}}+\frac{{\left({\&dtri;}_i\right)}^2}{{\&dtri;}_i^2{\&dtri;}_{i-1}^3})& \frac{{\left({\&dtri;}_i\right)}^2}{{\&dtri;}_i^2{\&dtri;}_i^3}\end{array}\right]$

特别地

对于节点矢量U＝[u₀，u₁，…，u_n+k+1]，可生成

其中k为曲线阶次，n为控制顶点个数， $t=\left({u-u}_i\right)/(u_{i+1}-u_i)=(u-u_i)/{\&dtri;}_i,$ 参数u∈[u_i，u_i+1)(0≤t≤1，i＝3，4，...，n)。

展开矩阵表达式A_i，整理并令：

Y₀＝a_1，1ω_i-3d_i-3+a_1，2ω_i-2d_i-2+a_1，3ω_i-1d_i-1+a_1，4ω_id_i

Y₁＝a_2，1ω_i-3d_i-3+a_2，2ω_i-2d_i-2+a_2，3ω_i-1d_i-1+a_2，4ω_id_i

Y₂＝a_3，1ω_i-3d_i-3+a_3，2ω_i-2d_i-2+a_3，3ω_i-1d_i-1+a_3，4ω_id_i

Y₃＝a_4，1ω_i-3d_i-3+a_4，2ω_i-2d_i-2+a_4，3ω_i-1d_i-1+a_4，4ω_id_i

Y′₀＝a_1，1ω_i-3+a_1，2ω_i-2+a_1，3ω_i-1+a_1，4ω_i

Y′₁＝a_2，1ω_i-3+a_2，2ω_i-2+a_2，3ω_i-1+a_2，4ω_i

Y′₂＝a_3，1ω_i-3+a_3，2ω_i-2+a_3，3ω_i-1+a_3，4ω_i

Y′₃＝a_4，1ω_i-3+a_4，2ω_i-2+a_4，3ω_i-1+a_4，4ω_i

则： $P_i\left(t\right)=\frac{Y_0+Y_{1}t+Y_2{t}^2+Y_3{t}^3}{Y_0^{\&prime;}+Y_1^{\&prime;}t+Y_2^{\&prime;}{t}^2+Y_3^{\&prime;}{t}^3}$ 0≤t≤1，i＝3，4，...，n

根据已知条件权因子ω_i、控制顶点d_i及节点矢量U，计算出系数Y₀，Y₁，Y₂，Y₃，Y′₀，Y′₁，Y′₂，Y′₃。

(2)自适应速度调整模块：以弦高误差为限制条件，自适应调整插补进给速度，包括以下步骤：

(a)将曲线上当前点对应参数u_i代入步骤(1)中NURBS曲线矩阵表达式，计算出曲线上对应点曲率β_i及曲率半径ρ_i；

(b)将曲率半径ρ_i、系统参考速度F、插补周期T及允许最大弦高误差δ代入自适应调整规则式：

$V\left(u_i\right)=\left\{\begin{array}{cc}F& \frac{2}{T}\sqrt{{\mathrm{\&rho;}}_i^2-{({\mathrm{\&rho;}}_i-\mathrm{\&delta;})}^2}>F\\ \frac{2}{T}\sqrt{{\mathrm{\&rho;}}_i^2-{({\mathrm{\&rho;}}_i-\mathrm{\&delta;})}^2}& \frac{2}{T}\sqrt{{\mathrm{\&rho;}}_i^2-{({\mathrm{\&rho;}}_i-\mathrm{\&delta;})}^2}<F\end{array}\right.$

可得到该插补周期内符合误差要求的进给速度V(u_i)；

(c)将自适应调整后的速度V(u_i)，代入二阶泰勒展开式计算下一插补点对应参数u_i+1值：

$u_{i+1}=u_i+\frac{V\left(u_i\right)T+(T^2/2)(\mathrm{dV}\left(u_i\right)/\mathrm{dt})}{\sqrt{{\left(x^{\&prime;}\right)}^2+{\left(y^{\&prime;}\right)}^2+{\left(z^{\&prime;}\right)}^2}}-\frac{{\left(V\left(u_i\right)T\right)}^2(x^{\&prime;}{x}^{\&prime;\&prime;}+y^{\&prime;}{y}^{\&prime;\&prime;}+z^{\&prime;}{z}^{\&prime;\&prime;})}{2{({\left(x^{\&prime;}\right)}^2+{\left(y^{\&prime;}\right)}^2+{\left(z^{\&prime;}\right)}^2)}^2}$

其中：x＝P_x(u)，y＝P_y(u)，z＝P_z(u)

$x^{\&prime;}=\frac{{\mathrm{dP}}_x\left(u\right)}{\mathrm{du}},$ $y^{\&prime;}=\frac{{\mathrm{dP}}_y\left(u\right)}{\mathrm{du}},$ $z^{\&prime;}=\frac{{\mathrm{dP}}_z\left(u\right)}{\mathrm{du}}$

$x^{\&prime;\&prime;}=\frac{d^2{P}_x\left(u\right)}{{\mathrm{du}}^2},$ $y^{\&prime;\&prime;}=\frac{d^2{P}_y\left(u\right)}{d{u}^2},$ $z^{\&prime;\&prime;}=\frac{d^2{P}_z\left(u\right)}{{\mathrm{du}}^2}$

重复以上(a)、(b)、(c)三个步骤，并记录每一插补周期的进给长度L_i、进给速度V(u_i)、对应插补点参数u_i以及对应插补点曲率β_i；直至u_i+1＝1，完成全部NURBS曲线自适应速度调整。

(3)将NURBS曲线分段，包括以下步骤：

(a)计算曲线插补中需要进行自适应速度调整的临界曲率β_rg：

${\mathrm{\&beta;}}_{\mathrm{rg}}=\frac{2\mathrm{\&delta;}}{F^2{\left(\frac{T}{2}\right)}^2+{\mathrm{\&delta;}}^2}$

δ为允许最大弦高误差。

(b)依次将步骤(2)中获得的插补点曲率β_i与β_rg比较，找出每一β_i≥β_rg区域内的曲率极大值，记为β_seg(j)，并找出其对应参数u值，记为u_seg(j)(j＝1，2，…，n-1)，n为分段后曲线段数量。再以u_seg(j)为分段点，将曲线分为n段。

(c)最后根据步骤(2)中获得的每一插补周期的进给长度L_i、进给速度V(u_i)、对应插补点参数u_i，计算出每一分段曲线的速度最大值V_max(j)、初始速度V_str(j)、结束速度V_end(j)以及各段轨迹长度L_seg(j)，j＝1，2，…，n。

(4)完成曲线分段后，进行S曲线加减速规划，计算过程如附图3，4所示，包括以下步骤：

(a)最末曲线段规划，包括以下步骤：

(a1)计算满足最末曲线段速度要求的最短轨迹长度L_dec：

$L_{\mathrm{dec}}=(V_{\mathrm{str}}\left(n\right)+V_{\mathrm{end}}\left(n\right))\sqrt{|V_{\mathrm{str}}\left(n\right)-V_{\mathrm{end}}\left(n\right)|/J_{\max }}$

(a2)比较L_seg(n)与L_dec，当L_seg(n)≥L_dec时完成最末曲线段规划；当L_seg(n)＜L_dec时合并分段曲线最后两段，且L_seg(n-1)＝L_seg(n-1)+L_seg(n)，V_end(n-1)＝V_end(n)，n＝n-1，返回步骤(a1)。

(b)完成最末曲线段规划后，对最终分段曲线依次进行S曲线加减速规划，包括以下步骤：

(b1)读取分段曲线段参数V_max、V_str、V_end及L_seg。判断V_max、V_str、V_end之间大小关系，当V_max＝V_str＝V_end时，由S曲线加减速位移公式可得t₄＝L_seg/V_max，t₁＝t₂＝t₃＝t₅＝t₆＝t₇＝0，t₁，t₂，t₃为加速各阶段时间，t₄为匀速段时间，t₅，t₆，t₇为减速各阶段时间。当V_max≠V_str或V_max≠V_end时，进入步骤(b2)。

(b2)当V_max≠V_str或V_max≠V_end时：

①判断V_max-V_str与A_max ²/J_max大小。当 $V_{\max }-V_{\mathrm{str}}>A_{\max }^2/J_{\max }$ 时：t₁＝t₃＝A_max/J_max，t₂＝(V_max-V_str)/A_max-t₁；当 $V_{\max }-V_{\mathrm{str}}\&le;A_{\max }^2/J_{\max }$ 时： $A_{\max }=\sqrt{(V_{\max }-V_{\mathrm{str}})J_{\max }},$ $t_1=t_3=\sqrt{(V_{\max }-V_{\mathrm{str}})/J_{\max }},$ t₂＝0。使用计算得到的t₁，t₂，t₃计算符合该时间分布的加速轨迹长度La： $\mathrm{La}=V_{\mathrm{str}}(t_1+t_2+t_3)+\frac{1}{2}{J}_{\max }{t}_1^2(t_2+t_3)+\frac{1}{2}{A}_{\max }{(t_2+t_3)}^2.$

②判断V_max-V_end与A_max ²/J_max大小。当 $V_{\max }-V_{\mathrm{end}}>A_{\max }^2/J_{\max }$ 时：t₅＝t₇＝A_max/J_max，t₆＝(V_max-V_end)/A_max-t₅；当 $V_{\max }-V_{\mathrm{end}}\&le;A_{\max }^2/J_{\max }$ 时： $A_{\max }=\sqrt{(V_{\max }-V_{\mathrm{str}})J_{\max }},$ $t_5=t_7=\sqrt{(V_{\max }-V_{\mathrm{end}})/J_{\max }},$ t₆＝0。使用计算得到的t₅，t₆，t₇计算符合该时间分布的减速轨迹长度Ld： $\mathrm{Ld}=V_{\max }(t_5+t_6+t_7)-\frac{1}{2}{J}_{\max }{t}_5^2(t_6+t_7)-\frac{1}{2}{A}_{\max }{(t_6+t_7)}^2.$

(b3)比较L_seg与La+Ld大小：①当L_seg＞La+Ld时，t₄＝(L_seg-La-Ld)/V_max；②当L_seg＝La+Ld时，t₄＝0；③L_seg＜La+Ld时，说明该段曲线实际最大速度达不到自适应调整得到的最大速度，需要进入步骤(b31)重新计算最大速度V_max。当出现情况①或②时，完成该段曲线加减速规划，并将时间t_i(i＝1，2，…，7)存入存储区；重复步骤(b1)～(b3)直至完成所有分段曲线加减速规划。

(b31)重新计算该段曲线最大速度V_max，如附图4所示，包括以下步骤：

(b31-1)假设实际最大速度为 $V_{\max }^{\&prime;}=\max (V_{\mathrm{str}},V_{\mathrm{end}})+A_{\max }^2/J_{\max },$ 代入步骤(b2)，计算符合V_max＝V′_max的加速轨迹长度La和减速轨迹长度Ld。

(b31-2)比较L_seg与La+Ld大小：①当L_seg＞La+Ld时， $V_{\max }=\frac{\sqrt{A_{\max }^4-2J_{\max }[A_{\max }^2(V_{\mathrm{str}}+V_{\mathrm{end}})-J_{\max }(V_{\mathrm{str}}^2+V_{\mathrm{end}}^2)-2A_{\max }{J}_{\max }{L}_{\mathrm{seg}}]}-A_{\max }^2}{2J_{\max }}$ ；②当L_seg＝La+Ld时，V_max＝V′_max；③L_seg＜La+Ld时，说明该段曲线实际最大速度达不到假设的最大速度V′_max，需要进入步骤(b31-3)重新求解。当出现情况①或②时，完成该段曲线最大速度求解，代入步骤(b2)～(b3)完成加减速规划。

(b31-3)假设实际最大速度为 $V_{\max }^{\&prime;\&prime;}=\min (V_{\mathrm{str}},V_{\mathrm{end}})+A_{\max }^2/J_{\max },$ 代入步骤(b2)，计算符合V_max＝V″_max的加速轨迹长度La和减速轨迹长度Ld。比较L_seg与La+Ld大小：当L_seg＝La+Ld时，V_max＝V″_max；当L_seg＞La+Ld及L_seg＜La+Ld时按照附图4所示流程获得满足L_seg＝La+Ld的最大速度V_max，代入步骤(b2)～(b3)完成加减速规划。

实时插补包括以下步骤：

(5)依次读取由前瞻预处理获得的分段曲线加减速时间t_i(i＝1，2，…，7)，根据系统给定的采样周期T，将加减速时间t_i(i＝1，2，…，7)离散化，则每一阶段插补次数为N_i，N_i为t_i/T圆整后数据(i＝1，2，…，7)，将离散后时间点依次代入S曲线加减速控制方法位移公式，计算得每一时间离散点对应进给速度Vt。将Vt代入步骤(2)中(c)，可计算得下一插补点参数u并代入步骤(1)中三次NURBS曲线矩阵表达，计算出对应曲线上点坐标X(u)，Y(u)，Z(u)。

(6)根据步骤(5)获得的插补点速度和位置信息，转换为运动控制信号，发送给系统动力部分驱动电机运行，直至完成该段曲线插补，再重复步骤(5)、(6)进行下一段曲线实时插补。

本发明方法主要采用了数值计算，这会带来时间和空间代价，但大量计算集中在前瞻预处理阶段，不会对实时插补过程造成影响。

[0142]在此说明书中，本发明已参照其特定流程作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。

Claims (2)

1.实时前瞻全程加减速控制的NURBS曲线自适应分段插补方法，

其特征在于，包括以下步骤：

前瞻预处理部分：

(1)采用矩阵形式表示NURBS曲线，并进行预处理；

(2)对所述NURBS曲线采用自适应速度调整算法，获得满足插补精度要求的进给速度参数；

(3)在速度自适应调整范围内的曲线曲率各极大值处将曲线分段；

(4)采用S曲线加减速控制方法对各分段曲线进行速度规划，得到速度曲线各加减速阶段变化时间；

实时插补部分：

(5)根据插补周期和各加减速阶段变化时间，得到各插补周期进给速度和对应插补点坐标；

(6)根据插补点坐标和进给速度，生成控制信号给定值，输出到系统动力部分。

2.根据权利要求1所述实时前瞻全程加减速控制的NURBS曲线自适应分段插补方法，其特征在于，所述步骤(6)，动力部分为步进电机或伺服驱动系统。

Images