CN104007705B - 小线段路径压缩平滑的前瞻插补系统 - Google Patents

Abstract

本发明提供了一种小线段路径压缩平滑的前瞻插补系统，包括：路径光顺模块、曲线扫描分段模块、双向加速模块、速度规划模块和插补模块。其中，路径光顺模块，通过读取数控G代码G01段的信息，提取代码中的小线段的坐标信息，根据这些坐标信息计算，对小线段进行路径压缩平滑光顺。曲线扫描分段模块、双向加速模块、速度规划模块完成跃度有限的S型速度规划的任务。插补模块，则根据计算得到的规划速度生成离散的插补点。这些插补点会被存储用于位置闭环控制中。本发明全部过程计算效率高，计算量小，编程实现简单，可进一步用于高速高精的数控机床。

Description

小线段路径压缩平滑的前瞻插补系统

技术领域

本发明涉及一种数控加工技术领域的离线系统，尤其是可以根据小线段(G01代码)生成B样条，接着进行跃度有限的速度规划，进而生成插补点的系统，具体涉及小线段路径压缩平滑的前瞻插补系统。

背景技术

数控加工中，通常CAM(Computer Aided Manufacturing，计算机辅助制造)软件的后置处理器会按加工精度的要求将复杂路径分解为一系列的微小路径段，再由数控系统中的插补器对每段路径进行插补运算，进而进行闭环控制。而这一方面，需要较大的存储空间存放数据；另一方，由于速度规划的不连贯性，势必导致表面质量和加工效率的降低。

用参数曲线将微小路径线段进行拟合，然后再进行速度规划，进而进行插补被证明是一种非常有效的方法。为了满足高速高精度的加工要求，根据微小路径生成的参数曲线应该具有以下特征：拟合精度满足精度要求，利于进行速度规划，利于表示各种类型的曲线，抑制刀路振荡，C²连续等。而规划的速度曲线应该满足加速度连续、跃度有限的要求，如此可减少对机械结构的冲击。

对现有的技术检索发现，也有用Bezier曲线进行小线段转接的(CN 102147600)，然而由于采用转接的方式，需要存储的数据量依然很大，且也不利于速度规划的连贯性。另外，Bezier曲线由于不具有局部支撑性，在设计、修改上也不方便。而在速度规划方面，一般的方法都采用了速度连续，也即加速度有限的方式进行，因此可能会有较大跃度，这仍然会对机械结构造成较大冲击。

发明内容

针对现有技术以上不足，本发明的目的在于采用对小线段拟合，而非转接的方式将小线段光顺为B样条曲线，同时采用跃度有限的方式进行速度规划，而插补则采用了多种不同的方法。

根据本发明提供的一种小线段路径压缩平滑的前瞻插补系统，包括：路径光顺模块、曲线扫描分段模块、双向加速模块、速度规划模块、插补模块；

路径光顺模块，用于通过读取数控G代码G01段的信息，提取代码中的小线段的坐标信息，根据这些坐标信息计算，对小线段进行路径压缩平滑光顺；

曲线扫描分段模块、双向加速模块、速度规划模块，用于完成跃度有限的S型速度规划的任务，得到规划速度；

插补模块，用于根据计算得到的规划速度生成离散的插补点，这些插补点会被存储用于位置闭环控制中。

优选地，所述路径光顺模块，从所有小线段端点根据曲率、弓高误差、曲率噪声这些信息选取合适的点作为拟合所用特征点；为了更进一步提高算法的快速收敛性，及以后的实时控制，通过添加前瞻窗口的方式，将找到的特征点分批进行拟合，生成B样条曲线；在根据曲率选取拟合特征点时，加入了曲率的滤波环节；在拟合过程中，分别采用向心参数化方法和平均化方法求取控制点对应的曲线参数及节点矢量，根据PDM方法进行最小二乘拟合求取初始控制点，通过距离函数得到的优化问题来更新控制点信息，通过弓高误差来增加特征点，最后使用Hausdorff距离来控制拟合精度；

所述曲线扫描分段模块，找出B样条曲线曲率的极大值点，再根据弓高误差、最大进给速度、最大加速度、最大跃度的约束找出B样条上的特征点，根据这些特征点将曲线分为若干段，计算每段曲线的首末的速度极值及长度；

所述双向加速模块，通过双向扫描的方式，使机床能顺利停机，且进行的速度规划曲线确实可行；

所述速度规划模块，根据机床最大进给速度、加速度、跃度及允许的曲线弓高误差以及每段曲线首末速度、弧长信息，采用五段式加减速速度规划，使生成的速度规划曲线跃度有限；首先根据给定的约束条件，求取一个阈值曲率，然后根据阈值曲率确定曲线上进行速度规划的关键点，然后求取关键点的弧长，然后再进行速度规划；

所述插补模块，根据规划速度，进行插补，将插补所得位置信息离线存储，用于进行位置的实时控制；

根据前瞻窗口，将选取的特征点分批进行拟合，提高算法效率；同时，在拟合生成的多段B样条的连接处，应用离散速度约束的条件，使速度规划在连接点处不必降为0，保证了速度规划的连贯性；

通过增加前瞻窗口，使曲线拟合、速度规划在较短时间内完成，同时使插补与位置控制实时完成。

优选地，选取合适的点作为拟合所用特征点，具体如下：

首先将整段曲线的端点作为特征点；

离散曲率的计算公式为

${\mathrm{\κ}}_i=\mathrm{sgn}\left({\mathrm{\Δq}}_{i-1}{q}_i{q}_{i+1}\right)\frac{2{\mathrm{sin\α}}_i}{\left|\right|q_{i+1}-q_{i-1}|{|}^2},i=1,2,...,n-1$

式中，κ_i表示离散曲率，q_i-1，q_i，q_i+1分别表示相邻的三个点，α_i表示q_i-1q_i与q_iq_i+1的夹角，n表示总点数,Δ与sgn(·)的定义见下：

其中

${\mathrm{\&alpha;}}_i=\arccos \frac{<q_{i+1}-q_i,q_i-q_{i-1}>}{\left|\right|q_{i+1}-q_i\left|\right|\left|\right|q_i-q_{i-1}\left|\right|}$

而为有序的离散点列，该点列即为各小线段的始末点；其中，<·>表示内积，||·||表示2-范数，表示3维欧式空间；

小线段中端点处离散曲率最大的点将作为特征点；考虑到小线段中的曲噪声，加入一定的滤波机制；

根据曲率引入的特征点，应满足在该特征点处|κ_i|-|κ_f|≥δ_f且|κ_i|-|κ_l|≥δ_f，其中κ_f与κ_l分别是第i点前后的曲率绝对值的极小值，而δ_f为滤波参数；

接着根据弓高误差选择特征点；q_i、q_i+1、q_i+2、q_i+3、q_i+4是所有离散点中的连续5个点，分别计算弓高距离d_i+1、d_i+2、d_i+3，其中，d_i+1为q_i+1到q_iq_i+4的弓高距离，d_i+2为q_i+2到q_iq_i+4的弓高距离，d_i+3为q_i+3到q_iq_i+4的弓高距离，若其中有大于给定弓高误差δ的弓高距离，再判断该点处曲率是否大于给定的阈值κ_c；若大于，则将该点添加为特征点；这里假定q_i+2为特征点，接着判断q_i与q_i+2之间点到q_iq_i+2及q_i+2与q_i+4之间点到q_i+2q_i+4的弓高误差，若其中有弓高误差大于给定弓高误差δ的，再判断该点处曲率是否大于给定的阈值κ_c，若大于给定曲率，则将该点作为特征点。

优选地，最小二乘拟合求取初始控制点，具体如下：

(a)求取控制点参数

采用向心法求取控制点曲线参数；

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{u}}_0=0,{\stackrel{\&OverBar;}{u}}_n=1\\ {\stackrel{\&OverBar;}{u}}_k={\stackrel{\&OverBar;}{u}}_{k-1}+\frac{\sqrt{\left|\right|q_k-q_{k-1}\left|\right|}}{\underset{i=0}{\overset{n-1}{\&Sigma;}}\sqrt{\left|\right|q_{i+1}-q_i\left|\right|}},k=1,2,...,n-1\end{array}\right.$

其中，表示起始控制点对应的曲线参数，表示结束控制点对应的曲线参数， 表示中间控制点对应的曲线参数，q_k、q_k-1表示选取的特征点；

接着利用平均化法配置节点矢量；

$\left\{\begin{array}{c}{u}_0=...=u_p=0,u_{m-p}=...u_m=1\\ u_{j+p}=\frac{1}{p}\underset{i=j}{\overset{j+p-1}{\&Sigma;}}{\stackrel{\&OverBar;}{u}}_i,j=1,2,...,n-p\end{array}\right.$

其中，u₀…u_p表示起始重复的p个曲线参数，u_m-p…u_m表示结束重复的p个曲线参数，u_j+p表示中间的曲线参数，p表示曲线阶数，表示控制点对应的曲线参数；

(b)最小二乘拟合确定控制点

根据PDM方法进行最小二乘拟合，求解拟合曲线，获得初始的控制点矢量w⁰；接着使用距离函数求取以下优化问题

其中，w表示控制点，表示距离函数，w^k表示第k个控制点，表示pq的单位化向量，p_i表示参考位置点，表示参数曲线，w_n表示控制点；

求取控制点的增量Δw；按w^k+1＝w^k+Δw更新权值，其中，w^k表示在第k次迭代时的控制点；在拟合精度不满足要求时，在两两特征点之间查找误差最大的离散点作为特征点，然后重新进行拟合求取新的控制点。

优选地，双向扫描及速度规划，具体如下：

采用跃度有限的五段式S型速度规划，根据拟合得到的B样条曲线，找出B样条曲线上的进行速度规划的关键点，关键点包括曲率大于阈值κ_cr处的极值点和间断点，确定关键点处的速度极值，然后再对关键点确定的每段进行双向扫描，确定关键点处的速度极值及规划的速度类型(ACC+CF+DEC、ACC+DEC、ACC+CF、CF+DEC、ACC、DEC、CF；其中，ACC表示加速段，DEC表示减速段，CF表示恒速段)；曲率的阈值计算为

${\mathrm{\&kappa;}}_{cr}=min(\frac{8\mathrm{\&delta;}}{{\left(V_{\max }{T}_s\right)}^2+4{\mathrm{\&delta;}}^2},\frac{A_n}{V_{\max }^2},\sqrt{\frac{J_{max}}{V_{max}^3}})$

其中，δ表示给定的弓高误差，V_max表示最大的进给速度，T_s表示插补时间，A_n表示最大的法向加速度，J_max表示最大的跃度；

而关键点处的速度极值按下式确定

$V_i=min(\frac{2}{T_s}\sqrt{\frac{1}{{{\mathrm{\&kappa;}}_i}^2}-{(\frac{1}{{\mathrm{\&kappa;}}_i}-\mathrm{\&delta;})}^2},\sqrt{\frac{A_{max}}{{\mathrm{\&kappa;}}_i}},\sqrt[3]{\frac{J_{\max }}{{{\mathrm{\&kappa;}}_i}^2}})$

其中，V_i表示关键点处的最大可取速度，A_max表示给定的最大加速度；

另外，为了提高运算的效率及收敛性，采用对特征点分批拟合的方式；其中，根据断点的速度约束方式，给出连接点处的速度约束如下

$V=min\{\frac{A_{\max }{T}_s}{2\sin (\mathrm{\&theta;}/2)},F\}$

其中，θ为连接点处两小线段之间的夹角，V表示间断点处的最大可取速度，F表示给定的最大进给速度。

优选地，插补按照一、二阶泰勒展开法进行，具体如下：

一、二阶泰勒展开法按下式计算

$u_{k+1}=u_k+\frac{V\left(u_k\right)T_s+({T_s}^2/2)A\left(u_k\right)}{\left|\right|C^{\&prime;}\left(u_k\right)\left|\right|}-\frac{{\left(V\right(u_k\left)T_s\right)}^2{C}^{\&prime;}\left(u_k\right)\&CenterDot;C^{\&prime;\&prime;}\left(u_k\right)}{2\left|\right|C^{\&prime;}\left(u_k\right)|{|}^4}$

其中，A(u_k)为当前步长的加速度，V(u_k)为当前步长的速度，T_s为插补周期，C′(u_k)、C″(u_k)分别为曲线的一阶、二阶导数；一阶展开只取右端前两项；u_k+1表示后一个插补点对应的曲线参数，u_k表示当前插补点对应的曲线参数；

优选地，插补按照龙格-库塔方法进行，具体如下：

迭代过程如下

$\left\{\begin{array}{c}{u}_{i+1}=u_i+\frac{T_s}{6}(K_1+2K_2+2K_3+K_4)\\ K_1=f(k{T}_s,u_k)\\ K_2=f(k{T}_s+\frac{T_s}{2},u_k+\frac{T_s}{2}{K}_1)\\ K_3=f(k{T}_s+\frac{T_s}{2},u_k+\frac{T_s}{2}{K}_2)\\ K_4=f(k{T}_s+T_s,u_k+T_s{K}_3)\end{array}\right.$

其中，u_i+1、u_i分别表示后一个插补点及当前插补点对应的曲线参数，T_s表示插补周期，K₁、K₂、K₃、K₄表示龙格-库塔方法的中间值，k表示插补周期数。

$f(t,u_k)=\frac{V\left(t\right)}{\sqrt{x^{\&prime;}{\left(u_k\right)}^2+y^{\&prime;}{\left(u_k\right)}^2+z^{\&prime;}{\left(u_k\right)}^2}}$

其中，V(t)表示当前插补时刻的规划速度，x′(u_k)、y′(u_k)、z′(u_k)分别表示x、y、z轴位置对曲线参数u的导数。

优选地，插补按照亚当姆斯方法进行，具体如下：

亚当姆斯显式公式方法利用历史插值点来计算当前插值参数，亚当姆斯显式公式计算如下

$u_{k+1}=u_k+\frac{T_s}{24}(55f_k-59f_{k-1}+37f_{k-2}-9f_{k-3})$

其中，u_i+1、u_i分别表示后一个插补点及当前插补点对应的曲线参数，T_s表示插补周期，f_k、f_k-1、f_k-2、f_k-3表示亚当姆斯方法的中间结果。

$f_k=\frac{V\left(u_k\right)}{\sqrt{x^{\&prime;}{\left(u_k\right)}^2+y^{\&prime;}{\left(u_k\right)}^2+z^{\&prime;}{\left(u_k\right)}^2}}$

其中，V(t)表示当前插补时刻的规划速度，x′(u_k)、y′(u_k)、z′(u_k)分别表示x、y、z轴位置对曲线参数u的导数。

优选地，插补按照速度校正多项式方法，，具体为：建立(u_i,s_i)之间的拟合多项式u(s)，然后利用该多项式在已知弧长的情况下直接求出曲线参数。

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

1、特征点的选择更为合理，可以有效减少控制点数量及迭代计算次数；

2、由于使用了点-曲线距离函数微分特性，因此本发明不仅可应用于二维曲线，也可以应用于三维曲线的拟合。

3、全部过程计算效率高，计算量小，编程实现简单，可进一步用于高速高精的数控机床。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明流程图。

图2为小线段拟合流程图。

图3为小线段曲线的离散曲率计算。

图4为曲率噪声。

图5为所提出的特征点选择机制。

图6为跃度有限的速度规划。

图7为弓高点选择的原因。

图8为弓高选择机制及细分判断。

图9为两个前瞻窗口的拟合曲线衔接。

图10为三维鸽子拟合曲线及其上的关键点。

图11为特征点在曲率曲线上的分布。

图12为控制点分布。

图13为拟合结果。

图14为速度极值曲线。

图15为速度规划曲线。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

以下结合附图对本发明的实施详细说明，但本发明的保护范围不限于下述的实施例。

针对小线段的B样条拟合，提出了一种基于曲率与弓高的特征点选择方法，由此减少了拟合曲线的控制点个数以及拟合过程中的迭代次数，从而提高拟合算法的实时性能。相对于现有的方法。

如图1所示，本实施例的具体流程包括：从数控文件中读取G01代码段作为代处理的小线段，根据得到的小线段找出其中的特征点；在拟合过程中，通过添加前瞻窗口，选取一定数量的特征点，对特征点分批拟合光顺；求取B样条上的关键点，计算关键点间的弧长及始末速度；通过双向扫描修正每段曲线始末的速度极值，接着采用修正后的始末速度、曲线弧长进行5段式跃度有限的S型曲线速度规划；存储划分后B样条各段的速度规划及曲线几何信息，这样做是为了方便以后的实时控制；根据速度规划信息和几何信息，计算每个插补周期的进给速度，根据各种数值方法由弧长计算曲线参数进而计算每个插补周期的各轴的位置。

拟合光顺的具体流程如下。

1)特征点的选择

首先整段曲线的端点一定被作为特征点。

如图3所示，离散曲率的计算公式为

${\mathrm{\&kappa;}}_i=\mathrm{sgn}\left({\mathrm{\&Delta;q}}_{i-1}{q}_i{q}_{i+1}\right)\frac{2{\mathrm{sin\&alpha;}}_i}{\left|\right|q_{i+1}-q_{i-1}|{|}^2},i=1,2,...,n-1$

其中

${\mathrm{\&alpha;}}_i=\arccos \frac{<q_{i+1}-q_i,q_i-q_{i-1}>}{\left|\right|q_{i+1}-q_i\left|\right|\left|\right|q_i-q_{i-1}\left|\right|}$

而为有序的离散点列，该点列即为各小线段的始末点。

小线段中端点处离散曲率最大的点将作为特征点。考虑到小线段中的曲噪声，如图4所示，如果考虑所有的曲率极大值点，会引入很多无用的点，而这些点会影响之后拟合算法的收敛性，因此，有必要加入一定的滤波机制。

本发明中根据曲率引入的特征点，应满足在该点处|κ_i|-|κ_f|≥δ_f且|κ_i|-|κ_l|≥δ_f，其中κ_f与κ_l分别是第i点前后的曲率绝对值的极小值，而δ_f为滤波参数。

接着根据弓高误差选择特征点。选择弓高点是为了避免损失小线段曲率变化不大部分的几何信息，如图7所示。具体的选择方法，如图8所示，q_i到q_i+4是所有离散点中的连续5个点，分别计算q_i+1到q_i+3到q_iq_i+4的弓高距离d_i+1到d_i+4，若其中有大于给定弓高误差δ的，假设d_i+2＞δ，再判断该点处曲率是否大于给定的阈值κ_c。若大于，则将该点添加为特征点，接着在q_i到q_i+2与q_i+2到q_i+4之间分别重复上面过程。

2)点-曲线距离函数及最小二乘拟合

(1)求取控制点参数

采用向心法求取控制点曲线参数。

$\left\{\begin{array}{c}{\stackrel{\&OverBar;}{u}}_0=0,{\stackrel{\&OverBar;}{u}}_n=1\\ {\stackrel{\&OverBar;}{u}}_k={\stackrel{\&OverBar;}{u}}_{k-1}+\frac{\sqrt{\left|\right|q_k-q_{k-1}\left|\right|}}{\underset{i=1}{\overset{n-1}{\&Sigma;}}\sqrt{\left|\right|q_{i+1}-q_k\left|\right|}},k=1,2,...,n-1\end{array}\right.$

接着利用平均化法配置节点矢量.

$\left\{\begin{array}{c}{u}_0=...=u_p=0,u_{m-p}=...u_m=1\\ u_{j+p}=\frac{1}{p}\underset{i=j}{\overset{j+p-1}{\&Sigma;}}{\stackrel{\&OverBar;}{u}}_i,j=1,2,...,n-p\end{array}\right.$

(2)最小二乘拟合确定控制点

根据PDM方法进行最小二乘拟合，求解拟合曲线，获得初始的控制点矢量w⁰。接着使用距离函数求取以下优化问题

求取控制点的增量Δw。按w^k+1＝w^k+Δw更新权值。在拟合精度不满足要求时，在两两特征点之间查找误差最大的离散点作为特征点，然后重新进行拟合求取新的控制点。具体流程见图2。

3)双向扫描及速度规划。

采用跃度有限的五段式S型速度规划，规划的速度曲线，见图6。根据拟合得到的B样条，找出B样条上的进行速度规划的关键点——包括曲率大于阈值κ_cr处的极值点和间断点，确定关键点处的速度极值，然后再对关键点确定的每段进行双向扫描，确定关键点处的速度极值及规划的速度类型(ACC+CF+DEC、ACC+DEC、ACC+CF、CF+DEC、ACC、DEC、CF；其中，ACC表示加速段，DEC表示减速段，CF表示恒速段)。曲率的阈值可计算为

${\mathrm{\&kappa;}}_{cr}=min(\frac{8\mathrm{\&delta;}}{{\left(V_{\max }{T}_s\right)}^2+4{\mathrm{\&delta;}}^2},\frac{A_n}{V_{\max }^2},\sqrt{\frac{J_{max}}{V_{\max }^3}})$

而关键点处的速度极值可以按下式确定

$V_i=min(\frac{2}{T_s}\sqrt{\frac{1}{{{\mathrm{\&kappa;}}_i}^2}-{(\frac{1}{{\mathrm{\&kappa;}}_i}-\mathrm{\&delta;})}^2},\sqrt{\frac{A_{max}}{{\mathrm{\&kappa;}}_i}},\sqrt[3]{\frac{J_{\max }}{{{\mathrm{\&kappa;}}_i}^2}})$

另外，为了提高运算的效率及收敛性，采用了对特征点分批拟合的方式。于是可能得到多段B样条曲线，在两端B样条曲线的连接处，曲线可能不是G¹的，因此有必要根据断点的速度约束方式，给出连接点处的速度约束如下

$V=min\{\frac{A_{\max }{T}_s}{2\sin (\mathrm{\&theta;}/2)},F\}$

其中，θ为连接点处两小线段之间的夹角。

4)插补

由于B样条的弧长与曲线的非解析对应关系，从曲线弧长到曲线参数有不同的估算方法，对应于此，有不同的插补算法。

(1)一、二阶泰勒展开法

一、二阶泰勒展开法可按下式计算

$u_{k+1}=u_k+\frac{V\left(u_k\right)T_s+({T_s}^2/2)A\left(u_k\right)}{\left|\right|C^{\&prime;}\left(u_k\right)\left|\right|}-\frac{{\left(V\right(u_k\left)T_s\right)}^2{C}^{\&prime;}\left(u_k\right)\&CenterDot;C^{\&prime;\&prime;}\left(u_k\right)}{2\left|\right|C^{\&prime;}\left(u_k\right)|{|}^4}$

其中，A(u_k)为当前步长的加速度，V(u_k)为当前步长的速度，T_s为插补周期，C′(u_k)、C″(u_k)分别为曲线的一阶、二阶导数。一阶展开只取右端前两项。一般情况下采用一阶方法进行插补。

(2)龙格-库塔方法

采用龙格-库塔方法是一种间接使用泰勒级数法的技术，以四阶龙格-库塔法为例，其迭代过程如下

$\left\{\begin{array}{c}{u}_{i+1}=u_i+\frac{T_s}{6}(K_1+2K_2+2K_3+K_4)\\ K_1=f(k{T}_s,u_k)\\ K_2=f(k{T}_s+\frac{T_s}{2},u_k+\frac{T_s}{2}{K}_1)\\ K_3=f(k{T}_s+\frac{T_s}{2},u_k+\frac{T_s}{2}{K}_2)\\ K_4=f(k{T}_s+T_s,u_k+T_s{K}_3)\end{array}\right.$

其中，

$f(t,u_k)=\frac{V\left(t\right)}{\sqrt{x^{\&prime;}{\left(u_k\right)}^2+y^{\&prime;}{\left(u_k\right)}^2+z^{\&prime;}{\left(u_k\right)}^2}}$

这种方法表面上不用计算曲线的二阶导数，然而由于需要中间型值点，反而增加了计算量，因此只是在某些不便于计算二阶导数的情况下可用。

(3)亚当姆斯方法

亚当姆斯显式公式方法利用历史插值点来计算当前插值参数，以三步预测为例，亚当姆斯显式公式可计算如下

$u_{k+1}=u_k+\frac{T_s}{24}(55f_k-59f_{k-1}+37f_{k-2}-9f_{k-3})$

其中，这种方法理论上可到达五阶精度，然而可能引起精度波动。

(4)速度校正多项式方法

速度校正多项式方法核心在于建立(u_i,s_i)之间的拟合多项式u(s)，然后利用该多项式在已知弧长的情况下直接求出曲线参数。

5)实验结果

采用空间离散的鸽子曲线(包含很多小线段)进行实验。拟合后的控制点的分布，拟合的结果见图11、图12、图13。同时可以看到在拟合过程中选取的特征点，见图10。最后给出速度极值曲线和规划的速度曲线，见图14、图15。从图中可以看出，采用了本发明后，进行拟合使用的特征点数量较少，而拟合效果也极好，速度规划使用的时间要远远少于直接对小线段进行速度规划的结果。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (5)

1.一种小线段路径压缩平滑的前瞻插补系统，其特征在于，包括：路径光顺模块、曲线扫描分段模块、双向加速模块、速度规划模块和插补模块；

路径光顺模块，用于通过读取数控G代码G01段的信息，提取代码中的小线段的坐标信息，根据这些坐标信息计算，对小线段进行路径压缩平滑光顺；

曲线扫描分段模块、双向加速模块和速度规划模块，用于完成跃度有限的S型速度规划的任务，得到规划速度；

插补模块，用于根据计算得到的规划速度生成离散的插补点，这些插补点会被存储用于位置闭环控制中；

所述路径光顺模块，从所有小线段端点根据曲率、弓高误差、曲率噪声这些信息选取合适的点作为拟合所用特征点；为了更进一步提高算法的快速收敛性，及以后的实时控制，通过添加前瞻窗口的方式，将找到的特征点分批进行拟合，生成B样条曲线；在根据曲率选取拟合特征点时，加入了曲率的滤波环节；在拟合过程中，分别采用向心参数化方法和平均化方法求取控制点对应的曲线参数及节点矢量，根据PDM方法进行最小二乘拟合求取初始控制点，通过距离函数得到的优化问题来更新控制点信息，通过弓高误差来增加特征点，最后使用Hausdorff距离来控制拟合精度；

所述曲线扫描分段模块，找出B样条曲线曲率的极大值点，再根据弓高误差、最大进给速度、最大加速度和最大跃度的约束找出B样条上的特征点，根据这些特征点将曲线分为若干段，计算每段曲线的首末的速度极值及长度；

所述双向加速模块，通过双向扫描的方式，使机床能顺利停机，且进行的速度规划曲线确实可行；

所述速度规划模块，根据机床最大进给速度、加速度、跃度及允许的曲线弓高误差以及每段曲线首末速度、弧长信息，采用五段式加减速速度规划，使生成的速度规划曲线跃度有限；首先根据给定的约束条件，求取一个阈值曲率，然后根据阈值曲率确定曲线上进行速度规划的关键点，然后求取关键点的弧长，然后再进行速度规划；

所述插补模块，根据规划速度，进行插补，将插补所得位置信息离线存储，用于进行位置的实时控制；

根据前瞻窗口，将选取的特征点分批进行拟合，提高算法效率；同时，在拟合生成的多段B样条的连接处，应用离散速度约束的条件，使速度规划在连接点处不必降为0，保证了速度规划的连贯性；

通过增加前瞻窗口，使曲线拟合和速度规划在较短时间内完成，同时使插补与位置控制实时完成。

2.根据权利要求1所述的小线段路径压缩平滑的前瞻插补系统，其特征在于，选取合适的点作为拟合所用特征点，具体如下：

首先将整段曲线的端点作为特征点；

离散曲率的计算公式为

${\mathrm{\&kappa;}}_i=\mathrm{sgn}\left({\mathrm{\&Delta;q}}_{i-1}{q}_i{q}_{i+1}\right)\frac{2{\mathrm{sin\&alpha;}}_i}{\left|\right|q_{i+1}-q_{i-1}|{|}^2},i=1,2,...,n-1$

式中，κ_i表示离散曲率，q_i-1，q_i，q_i+1分别表示相邻的三个点，α_i表示q_i-1q_i与q_iq_i+1的夹角，n表示总点数,Δ与sgn(·)的定义见下：

其中

${\mathrm{\&alpha;}}_i=\arccos \frac{<q_{i+1}-q_i,q_i-q_{i-1}>}{\left|\right|q_{i+1}-q_i\left|\right|\left|\right|q_i-q_{i-1}\left|\right|}$

而为有序的离散点列，该点列即为各小线段的始末点；其中，<·>表示内积，||·||表示2-范数，表示3维欧式空间；

小线段中端点处离散曲率最大的点将作为特征点；考虑到小线段中的曲率噪声，加入一定的滤波机制；

根据曲率引入的特征点，应满足在该特征点处|κ_i|-|κ_f|≥δ_f且|κ_i|-|κ_l|≥δ_f，其中κ_f与κ_l分别是第i点前后的曲率绝对值的极小值，而δ_f为滤波参数；

接着根据弓高误差选择特征点；q_i、q_i+1、q_i+2、q_i+3、q_i+4是所有离散点中的连续5个点，分别计算弓高距离d_i+1、d_i+2、d_i+3，其中，d_i+1为q_i+1到q_iq_i+4的弓高距离，d_i+2为q_i+2到q_iq_i+4的弓高距离，d_i+3为q_i+3到q_iq_i+4的弓高距离，若其中有大于给定弓高误差δ的弓高距离，再判断该点处曲率是否大于给定的阈值κ_c；若大于，则将该点添加为特征点；这里假定q_i+2为特征点，接着判断q_i与q_i+2之间点到q_iq_i+2及q_i+2与q_i+4之间点到q_i+2q_i+4的弓高误差，若其中有弓高误差大于给定弓高误差δ的，再判断该点处曲率是否大于给定的阈值κ_c，若大于给定曲率，则将该点作为特征点。

3.根据权利要求1所述的小线段路径压缩平滑的前瞻插补系统，其特征在于，双向扫描及速度规划，具体如下：

采用跃度有限的五段式S型速度规划，根据拟合得到的B样条曲线，找出B样条曲线上的进行速度规划的关键点，关键点包括曲率大于阈值κ_cr处的极值点和间断点，确定关键点处的速度极值，然后再对关键点确定的每段进行双向扫描，确定关键点处的速度极值及规划的速度类型ACC+CF+DEC、ACC+DEC、ACC+CF、CF+DEC、ACC、DEC、CF；其中，ACC表示加速段，DEC表示减速段，CF表示恒速段；曲率的阈值计算为

${\mathrm{\&kappa;}}_{cr}=min(\frac{8\mathrm{\&delta;}}{{\left(V_{\max }{T}_s\right)}^2+4{\mathrm{\&delta;}}^2},\frac{A_n}{V_{\max }^2},\sqrt{\frac{J_{\max }}{V_{\max }^3}})$

其中，δ表示给定的弓高误差，V_max表示最大的进给速度，T_s表示插补时间，A_n表示最大的法向加速度，J_max表示最大的跃度；

而关键点处的速度极值按下式确定

$V_i=min(\frac{2}{T_s}\sqrt{\frac{1}{{{\mathrm{\&kappa;}}_i}^2}-{(\frac{1}{{\mathrm{\&kappa;}}_i}-\mathrm{\&delta;})}^2},\sqrt{\frac{A_{max}}{{\mathrm{\&kappa;}}_i}},\sqrt[3]{\frac{J_{\max }}{{{\mathrm{\&kappa;}}_i}^2}})$

其中，V_i表示关键点处的最大可取速度，A_max表示给定的最大加速度；κ_i表示离散曲率；

另外，为了提高运算的效率及收敛性，采用对特征点分批拟合的方式；其中，根据断点的速度约束方式，给出连接点处的速度约束如下

$V=min\{\frac{A_{max}{T}_s}{2\sin (\mathrm{\&theta;}/2)},F\}$

其中，θ为连接点处两小线段之间的夹角，V表示间断点处的最大可取速度，F表示给定的最大进给速度。

4.根据权利要求1所述的小线段路径压缩平滑的前瞻插补系统，其特征在于，插补按照龙格-库塔方法进行，具体如下：

迭代过程如下

$\left\{\begin{array}{c}{u}_{i+1}=u_i+\frac{T_s}{6}(K_1+2K_2+2K_3+K_4)\\ K_1=f(k{T}_s,u_k)\\ K_2=f(k{T}_s+\frac{T_s}{2},u_k+\frac{T_s}{2}{K}_1)\\ K_3=f(k{T}_s+\frac{T_s}{2},u_k+\frac{T_s}{2}{K}_2)\\ K_4=f(k{T}_s+T_s,u_k+T_s{K}_3)\end{array}\right.$

其中，u_i+1、u_i分别表示后一个插补点及当前插补点对应的曲线参数，T_s表示插补周期，K₁、K₂、K₃、K₄表示龙格-库塔方法的中间值，k表示插补周期数；

$f(t,u_k)=\frac{V\left(t\right)}{\sqrt{x^{\&prime;}{\left(u_k\right)}^2+y^{\&prime;}{\left(u_k\right)}^2+z^{\&prime;}{\left(u_k\right)}^2}}$

其中，V(t)表示当前插补时刻的规划速度，x′(u_k)、y′(u_k)、z′(u_k)分别表示x、y、z轴位置对曲线参数u的导数。

5.根据权利要求1所述的小线段路径压缩平滑的前瞻插补系统，其特征在于，插补按照亚当姆斯方法进行，具体如下：

亚当姆斯显式公式方法利用历史插值点来计算当前插值参数，亚当姆斯显式公式计算如下

$u_{k+1}=u_k+\frac{T_s}{24}(55f_k-59f_{k-1}+37f_{k-2}-9f_{k-3})$

其中，u_k+1、u_k分别表示后一个插补点及当前插补点对应的曲线参数，T_s表示插补周期，f_k、f_k-1、f_k-2、f_k-3表示亚当姆斯方法的中间结果；

$f_k=\frac{V\left(u_k\right)}{\sqrt{x^{\&prime;}{\left(u_k\right)}^2+y^{\&prime;}{\left(u_k\right)}^2+z^{\&prime;}{\left(u_k\right)}^2}}$

其中，V(t)表示当前插补时刻的规划速度，x′(u_k)、y′(u_k)、z′(u_k)分别表示x、y、z轴位置对曲线参数u的导数。