CN102722141B - 一种基于pc-fpga的nurbs曲线实时插补方法 - Google Patents

Abstract

本发明公开了一种基于PC-FPGA的NURBS曲线实时插补方法，包括IPC、FPGA运动控制器、伺服驱动器、工业机器人，三维零件Solidworks模型在IPC上生成离散的数据点后，然后通过以太网通讯模块将数据传送到FPGA运动控制器进行实时NURBS曲线插补计算，输出脉冲串到伺服驱动器，以力矩驱动工业机器人进行协调运动，实现对复杂曲面零件的NURBS曲线插补运动。本发明最小采样时间可以由1ms降低到0.01ms,从而能够实现工业机器人高速高精度的运动控制要求，可应用于复杂曲面零件的加工。

Description

一种基于PC-FPGA的NURBS曲线实时插补方法

技术领域

本发明属于工业机器人运动控制系统领域，尤其涉及一种基于PC-FPGA的NURBS曲线实时插补方法。

背景技术

通常，一台喷涂工业机器人的运动控制系统由轨迹插补和轨迹跟踪两大功能模块构成。由于传统的机器人运动控制系统只提供直线和圆弧两种方式的插补，所有的几何轮廓外形都用直线段或者圆弧段来近似拟合。随着零件精度要求的提高，传统方法存在以下问题：越高精度要求，则生成的线段越多，要传送更多的数据，导致大量的数据传输无法满足高速的加工过程；在线段接合处产生进给跳动和速度不连续性，最终影响加工质量。

为了获得高速高精度的性能要求，现代运动控制系统中一般采用B-样条曲线和NURBS曲线进行插补。随着机器人离线编程系统的发展，NURBS为机器人运动路径提供统一的数学描述，更加合理的将机器人CAD/CAM系统和实际机器人的运动控制结合起来。

为了减小进给跳动和改善加工质量，学者提出了一些NURBS的插补算法。Shipitalni提出了一种基于截断泰勒展开式的一阶插补算法，Yang和Kong提出了二阶插补算法，Yeh和Hsu提出了带补偿参数的速度控制插补算子，Forouki提出了一种三阶插补的紧凑递归算法。他们的工作都是在提供一种精确的NURBS插补算法，却很少关注实时的插补实现问题。同时，现有的NURBS插补算法应用在不同的场合下，往往难以满足不同的速度和精度要求。

目前的工业机器人一般采用工业级计算机IPC+DSP的串行计算架构，其中PC完成人机交互的功能，DSP用来处理运动控制器的计算量和一些高速I/O信号，如发生PWM信号和反馈控制。NURBS曲线插补计算需要调用递归函数式，使得计算量很大，在通常的PC+DSP的架构中，当NURBS插补器计算复杂时候，在较短的采样时间内（如1ms），很难完成NURBS插补和伺服控制的计算。Tsai等人实现的NURBS曲面插补和伺服控制总共的计算时间为4ms，其中NURBS插补使用了3.8ms，占了总的计算时间的95%。目前实际的解决办法大多是采取多片DSP的系统架构，这无疑增加系统成本，该种硬件架构设计难度也大。

发明内容

本发明的目的在于克服现有技术的缺点和不足，提供一种基于PC-FPGA的NURBS曲线实时插补方法，通过FPGA的并行计算能力以解决现有技术中，NURBS插补实时性不强的问题，同时有限的降低系统的成本，充分发挥在PC上灵活进行零件建模和FPGA的实时数据处理的能力。

本发明通过下述技术方案实现：

一种基于PC-FPGA的NURBS曲线实时插补方法，

包括IPC、FPGA运动控制器、伺服驱动器、工业机器人机械本体，三维零件Solidworks模型在IPC上生成离散的数据点后，然后通过以太网通讯模块将数据传送到FPGA运动控制器进行实时NURBS曲线插补计算，输出脉冲串到伺服驱动器，以力矩驱动工业机器人进行协调运动，实现对复杂曲面零件的NURBS曲线插补运动。

在IPC上实现的步骤为：

（1）、IPC根据待喷涂零件CAD/CAM图形特征，将数据坐标系附于喷涂零件的底线中点来生成STL文件；

（2）、把STL文件中每一个三角形的单位法矢量附加到其三个顶点上，删除法矢量为负的点；

（3）、从STL文件中依次取出三角形，按照X坐标升序排序，把相同的X坐标的点组成一个局部点组，再把每组的点，按照Y坐标的升序局部排列；

（4）、在每个点组里面，把有相同Y坐标的多余点去除；通过比较三角形顶点的坐标值，删除重复的点，从而建立一个无重复点的点表；

（5）、将需要插补的离散控制点集，通过IPC上的工业以太网接口和集成在FPGA硬件电路上的以太网接口，把这些离散控制点集数据传送到FPGA运动控制器的FIFO区；

FPGA运动控制器进行以下步骤：

（1）、通过以太网接口从IPC上下载离散数据点到FIFO进行缓冲存储；

（2）、NURBS实时插补器根据FIFO区离散控制点进行NURBS曲线插补计算，然后把计算所得以脉冲量形式经IIR数字滤波输出；

（3）、编码器模块对工业机器人各轴实际运动位置光电编码器反馈信号进行解码；

（4）、IIR数字滤波器结合编码器模块利用PID控制原理，对工业机器人末端进行位置跟踪；

在FPGA运动控制器的NURBS实时插补器中，根据确定曲线参数u逐次迭代的增量，在相同的采样时间T_s内运动的距离是恒定的，将NURBS曲线等长分割，通过沿着曲线的运动速度定义和截取泰勒一阶展开式，给出一种迭代求解曲线参数的方法：

从而实现NURBS曲线插补的速度反馈控制，以适应不同的插补速度场合；

根据T形速度规划，工业机器人的电机以一较低的速度Vmin起动，在加速度a下加速到所需要的速度，接近终点时候在减速度a下减速到Vmin，终点时刻最后突然停止，以获得工业机器人的机械手末端运动轨迹和运动效率；

通过计算NURBS曲线的长度，然后根据给定工业机器人的电机起动速度V_min、工业机器人末端运动速度V_max和加速度，计算加速以及减速时间与总的运动时间，对任意采样时间区间求出相应的速度V_t；

将V_t代入u_k+1迭代公式，得到NURBS曲线相应插补点的坐标(x(u_k+1),y(u_k+1),z(u_k+1))，以脉冲串形式通过IIR数字滤波器进行输出；

伺服驱动器接收来自FPGA运动控制器的脉冲串，采用三环控制方式，从里到外分别为电流环、速度环、位置环，以力矩控制模式控制工业机器人的机械本体进行协调运动，最终实现针对具复杂曲面的零件的NURBS曲线轨迹插补运动。

本发明相对于现有技术，具有如下的优点及效果：

(1)采用基于FPGA运动控制器来进行NURBS插补计算的设计优势主要是速度快、应用灵活。速度快的优势来源于FPGA运动控制器的硬件逻辑，特别是FPGA运动控制器的并行执行和流水线操作。FPGA运动控制器的逻辑功能全部用硬件电路实现，因此所有的延迟只来源于门电路，而一般门电路的延迟都在纳秒级别。与传统DSP相比，系统更加小型化，集成度更高，运行速度更快，同时减少物理连接，因此采用基于FPGA运动控制器实现NURBS插补，可以弥补上面所提到到的缺憾，很好地达到NURBS曲线实时插补和降低系统成本设计维护难度的双重目的。

(2)本发明基于FPGA运动控制器的NURBS实时插补器，采样时间可以从传统方法的1ms减少到0.01ms，伺服控制环可通过改善采样周期，位置跟踪能力可以得到增强。

(3)本发明中使用速度的反馈控制原理，使得每一个采样时间内运动的距离是恒定的，该插补方法可以应用在不同速度的场合下。

附图说明

图1是基于PC-FPGA系统架构的NURBS轨迹插补实现流程；

图2是NURBS轨迹插补实现数据流；

图3a、3b是STL文件中三角形面片的单位法矢量扩展，其中图3a原来的STL画面；图3b是法向矢量添加到每个顶点；

图4是去除重复冗余点的算法流程图；

图5是NURBS实时轨迹插补器原理；

图6是速度规划分布；

图7是IIR数字滤波；

具体实施方式

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

实施例

如图1、图2所示，基于PC-FPGA的NURBS曲线实时插补方法，包括IPC、FPGA运动控制器、伺服驱动器、工业机器人，三维零件Solidworks模型在IPC上生成离散的数据点后，然后通过以太网通讯模块将数据传送到FPGA运动控制器进行实时NURBS曲线插补计算，输出脉冲串到伺服驱动器，以力矩驱动工业机器人进行协调运动，实现对复杂曲面零件的NURBS曲线插补运动。

在IPC上实现的步骤为：

（1）、IPC根据待喷涂零件CAD/CAM图形特征，将数据坐标系附于喷涂零件的底线中点来生成STL文件；

（2）、把STL文件中每一个三角形的单位法矢量附加到其三个顶点上，删除法矢量为负的点；

（3）、从STL文件中依次取出三角形，按照X坐标升序排序，把相同的X坐标的点组成一个局部点组，再把每组的点，按照Y坐标的升序局部排列；

（4）、在每个点组里面，把有相同Y坐标的多余点去除；通过比较三角形顶点的坐标值，删除重复的点，从而建立一个无重复点的点表；

（5）、将需要插补的离散控制点集，通过IPC上的工业以太网接口和集成在FPGA硬件电路上的以太网接口，把这些离散控制点集数据传送到FPGA运动控制器的FIFO区；

FPGA运动控制器进行以下步骤：

（1）、通过以太网接口从IPC上下载离散数据点到FIFO进行缓冲存储；

（2）、NURBS实时插补器根据FIFO区离散控制点进行NURBS曲线插补计算，然后把计算所得以脉冲量形式经IIR数字滤波输出；

（3）、编码器模块对工业机器人各轴实际运动位置光电编码器反馈信号进行解码；

（4）、IIR数字滤波器结合编码器模块利用PID控制原理，对工业机器人末端进行位置跟踪；

在FPGA运动控制器的NURBS实时插补器中，根据确定曲线参数u逐次迭代的增量，在相同的采样时间T_s内运动的距离是恒定的，将NURBS曲线等长分割，通过沿着曲线的运动速度定义和截取泰勒一阶展开式，给出一种迭代求解曲线参数的方法：

从而实现NURBS曲线插补的速度反馈控制，以适应不同的插补速度场合；

根据T形速度规划，工业机器人的电机以一较低的速度Vmin起动，在加速度a下加速到所需要的速度，接近终点时候在减速度a下减速到Vmin，终点时刻最后突然停止，以获得工业机器人的机械手末端运动轨迹和运动效率；

通过计算NURBS曲线的长度，然后根据给定工业机器人的电机起动速度V_min、工业机器人末端运动速度V_max和加速度，计算加速以及减速时间与总的运动时间，对任意采样时间区间求出相应的速度V_t；

将V_t代入u_k+1迭代公式，得到NURBS曲线相应插补点的坐标(x(u_k+1),y(u_k+1),z(u_k+1))，以脉冲串形式通过IIR数字滤波器进行输出；

伺服驱动器接收来自FPGA运动控制器的脉冲串，采用三环控制方式，从里到外分别为电流环、速度环、位置环，以力矩控制模式控制工业机器人的机械本体进行协调运动，最终实现针对具复杂曲面的零件的NURBS曲线轨迹插补运动。

为更好理解本发明，下面具体予以描述：

图2是NURBS轨迹插补实现数据流；图3a、3b是STL文件中三角形面片的单位法矢量扩展，其中图3a是原来的STL画面；图3b是法向矢量添加到每个顶点；图4是去除重复冗余点的流程图；图5是NURBS实时轨迹插补器原理；图6是速度规划分布；图7是IIR数字滤波；

图2所示，IPC根据待喷涂零件CAD/CAM图形特征，进行数据处理得到喷涂零件需要插补的离散控制点集，利用工业以太网通讯模块把数据传送到基于FPGA的运动控制器下；在Altera公司EP3C40芯片上集成软核、以太网、NURBS实时插补器、编码器解码、FIR数字滤波、FIFO、系统中断等硬件电路，构成基于FPGA的运动控制系统，实现NURBS曲线的实时插补算法，输出脉冲串到各轴运动控制伺服驱动器，伺服驱动器以力矩控制模式驱动工业机器人各轴相应的伺服电机，控制工业机器人各轴进行协调运动，最终实现NURBS曲线轨迹插补。

在IPC上，通过CAD/CAM软件将数据坐标系附于喷涂零件的底线中点来生成STL文件，参阅图3a、3b所示，把STL文件中每一个三角形的单位法矢量附加到其三个顶点上，删除法矢量为负的点。

由于STL文件是由一系列的三角形面片无序排列组合在一起的，没有反映三角形面片之间的拓扑关系，而且每一个小三角形面片必须与相邻的三角形面片共用2个顶点，所以每个顶点通常被重用4次左右。为了删除重复的点，从而建立一个无重复点的点表，参阅图3所示，从STL文件中依次取出三角形，按照X坐标升序排序，把相同的X坐标的点组成一个局部点组，再把每组的点，按照Y坐标的升序局部排列。只要在每个点组里面，把有相同Y坐标的多余点去除即可。通过比较三角形顶点的坐标值，可删除重复的点。利用IPC上的工业以太网接口和集成在FPGA运动控制器硬件电路上的以太网接口，把这些离散点集数据传送到到基于FPGA的运动控制器的FIFO区；

利用Altera公司EP3C40芯片，在基于Avalon总线上，集成软核、以太网、NURBS实时插补器、编码器解码、FIR数字滤波、FIFO、系统中断等硬件电路在SOPC（可编程片上系统），构成基于FPGA的运动控制系统。软核模块负责系统模块任务的调度；以太网通信模块负责与IPC进行通讯，从IPC下载待喷涂工件的离散控制点、工作使能和停止信号，和上传运动控制状态位置信息；FIFO主要对从IPC上下载的数据进行缓冲保存；NURBS实时插补器根据FIFO区离散控制点进行NURBS曲线插补计算；编码器模块输入来自工业机器人各轴电机编码器信号，主要功能是对工业机器人各轴运动位置信号进行解码，参阅图1中所示，编码器模块将当前轴的实际运动位置进行反馈以实现精确控制；IIR数字滤波利用PID控制原理，结编码器模块对工业机器人末端进行位置跟踪。

参阅图5所示，NURBS实时插补器。具体计算处理如下：

一条三维空间曲线的表达式为：

C(u)=x(u)i+y(u)j+z(u)k    (1)

其中u为曲线参数，基本图形转换规范(IGES)规定一条P阶NURBS曲线定义为：

$C\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}\left(u\right)P_i=\frac{\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{N}_{i,p}\left(u\right)w_i{P}_i}{\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{N}_{i,p}\left(u\right)w_i}---\left(2\right)$

其中{P_i}为控制点，{w_i}为控制点权值，{N_i，p(u)}为p阶B样条曲线基函数。

NURBS曲线的三维坐标下：

$\left\{\begin{array}{c}x\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}\left(u\right)P_{\mathrm{xi}}\\ y\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}\left(u\right)P_{\mathrm{yi}}\\ z\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}\left(u\right)P_{\mathrm{zi}}\end{array}\right.---\left(4\right)$

NURBS曲线m阶导数如下：

$\left\{\begin{array}{c}{x}^{\′}\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}^{\′}\left(u\right)P_{\mathrm{xi}}\\ y^{\′}\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}^{\′}\left(u\right)P_{\mathrm{yi}}\\ z^{\′}\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{R}_{i,p}^{\′}\left(u\right)P_{\mathrm{zi}}\end{array}\right.---\left(5\right)$

为了实现速度的反馈控制，采用一种等距的插补方法，即在每一个采样时间区间，运动的距离是恒定的，沿曲线的运动速度定义为：

$V\left(u\right)=\frac{\mathrm{ds}}{\mathrm{dt}}=\left(\frac{\mathrm{ds}}{\mathrm{du}}\right)\left(\frac{\mathrm{du}}{\mathrm{dt}}\right)---\left(6\right)$

即：

$\frac{\mathrm{du}}{\mathrm{dt}}=\frac{V}{\mathrm{ds}/\mathrm{du}}---\left(7\right)$

式中, $\frac{\mathrm{ds}}{\mathrm{du}}={(x^{\′2}+y^{\′2}+z^{\′2})}^{\frac{1}{2}},$ $x^{\′}=\frac{\mathrm{dx}}{\mathrm{du}},$ $y^{\′}=\frac{\mathrm{dy}}{\mathrm{du}},$ $z^{\′}=\frac{\mathrm{dz}}{\mathrm{du}}..$

为求取u_k+1，在采样时间T_s附近的泰勒展开式：

$u_{k+1}=u_k+T{\stackrel{\·}{u}}_k+(T^2/2){\stackrel{\·\·}{u}}_k+...\left(8\right)$

当T足够小且曲线曲率半径不是很小，那么可以用一阶近似：

$u_{k+1}=u_k+T_s{\stackrel{\·}{u}}_k---\left(9\right)$

将(9)代入(7)得

$u_{k+1}=u_k+\frac{{\mathrm{VT}}_s}{{(x^{\′2}+y^{\′2}+z^{\′2})}^{\frac{1}{2}}}---\left(10\right)$

以上是NURBS曲线实时插补计算方法，根据式(5)和式(10)即可进行u的迭代求解，计算u_k+1之后利用式(4)式即可计算下一个命令点坐标x_k+1,y_k+1,z_k+1.

在工业机器人运动定位的过程中，为了获得末端机械手较为平滑的运动，常需要经过加速-匀速-减速-停止的过程。参阅图6所示，一开始的时候，电机以一个较低的速度V_min起动。在0→t₁时间电机以恒定加速度a加速到V_max；在t₁→t₂时间一直以V_max恒速运动；当快要接近终点时，在t₂→t₃时间以减速度a作匀减速，在t₃时刻前速度为V_min，到达t₃时刻时候电机停止，其中加速时间和减速时间相等t_a=t_d=t₁。对于不同的电机，起转速度V_min不一样。若V_min太大，可能导致电机无法起转。若V_min太小，加速过程会较长，从而降低了运动的效率。从V-t图中可以看出，NURBS曲线长度为梯形的总面积：

S=(V_max+V_min)·t₁+V_max(t₂-t₁)               (11)

运动速度如下分段函数：

$V_t=\left\{\begin{array}{cc}{V}_{\min }+\mathrm{at}& t<t_1\\ V_{\max }& t_1\&le;t<t_2\\ V_{\max }-\mathrm{at}& t_2\&le;t<t_3\\ 0& t=t_3\end{array}\right.---\left(12\right)$

把式(12)带入式(10)可得

$u_{k+1}=u_k+\frac{V_t{T}_s}{{(x^{\&prime;2}+y^{\&prime;2}+z^{\&prime;2})}^{\frac{1}{2}}}---\left(13\right)$

通过计算NURBS曲线的长度，然后根据给定的电机起动速度V_min、机器人末端运动速度V_max和加速度a，计算加速（减速）时间和总的运动时间，对任意采样时间区间求出相应的速度Vt，通过式(13)即可确定u_k+1。

PID控制在目前控制领域仍颇为常用，由于FPGA运动控制器是在数字逻辑门上实现，因此PID控制要转换成IIR数字滤波形式。参阅图7所示IIR数字滤波采用两环控制方式，从里到外分别是速度环、位置环，如表一所示设置参数：

表一：

参见图1所示，伺服驱动器模块接收来自FPGA运动控制器的脉冲串，根据伺服驱动器手册设置相关参数：采用三环控制方式，从里到外分别为电流环、速度环、位置环，以力矩模式控制工业机器人的机械本体进行协调运动，最终实现针对具复杂曲面的零件的NURBS曲线插补运动。

如上所述，便可较好地实现本发明。

Claims (1)

1.一种基于PC-FPGA的NURBS曲线实时插补方法，其特征在于：

包括IPC、FPGA运动控制器、伺服驱动器、工业机器人，三维零件Solidworks模型在IPC上生成离散的数据点后，然后通过以太网通讯模块将数据传送到FPGA运动控制器进行实时NURBS曲线插补计算，输出脉冲串到伺服驱动器，以力矩驱动工业机器人进行协调运动，实现对复杂曲面零件的NURBS曲线插补运动；

在IPC上实现的步骤为：

（1）、 IPC根据待喷涂零件CAD/CAM图形特征，将数据坐标系附于喷涂零件的底线中点来生成STL文件；

（2）、把STL文件中每一个三角形的单位法矢量附加到其三个顶点上，删除法矢量为负的点；

（3）、从STL 文件中依次取出三角形，按照X坐标升序排序，把相同的X 坐标的点组成一个局部点组，再把每组的点，按照Y坐标的升序局部排列；

（4）、在每个点组里面，把有相同Y 坐标的多余点去除；通过比较三角形顶点的坐标值，删除重复的点，从而建立一个无重复点的点表； 

（5）、将需要插补的离散控制点集，通过IPC上的工业以太网接口和集成在FPGA硬件电路上的以太网接口，把这些离散控制点集数据传送到FPGA运动控制器的FIFO区； 

FPGA运动控制器进行以下步骤：

（1）、通过以太网接口从IPC上下载离散数据点到FIFO进行缓冲存储；

（2）、 NURBS实时插补器根据FIFO区离散控制点进行NURBS曲线插补计算，然后把计算所得以脉冲量形式经IIR数字滤波器输出； 

（3）、编码器模块对工业机器人各轴实际运动位置光电编码器反馈信号进行解码； 

（4）、 IIR数字滤波器结合编码器模块利用PID控制原理，对工业机器人末端进行位置跟踪； 

在FPGA运动控制器的NURBS实时插补器中， 根据确定曲线参数                                                

逐次迭代的增量，在相同的采样时间内运动的距离是恒定的，将NURBS曲线等长分割，通过沿着曲线的运动速度定义和截取泰勒一阶展开式，给出一种迭代求解曲线参数的方法：

，从而实现NURBS曲线插补的速度反馈控制，以适应不同的插补速度场合；

根据T形速度规划，工业机器人的电机以一较低的速度Vmin起动，在加速度a下加速到所需要的速度，接近终点时候在减速度a下减速到Vmin，终点时刻最后突然停止，以获得工业机器人的机械手末端运动轨迹和运动效率；

通过计算NURBS曲线的长度，然后根据给定工业机器人的电机起动速度

、工业机器人末端运动速度

和加速度，计算加速以及减速时间与总的运动时间，对任意采样时间区间求出相应的速度

； 

将

代入

迭代公式，得到NURBS曲线相应插补点的坐标

，以脉冲串形式通过IIR数字滤波器进行输出；

伺服驱动器接收来自FPGA运动控制器的脉冲串，采用三环控制方式，从里到外分别为电流环、速度环、位置环，以力矩控制模式控制工业机器人的机械本体进行协调运动，最终实现针对具复杂曲面的零件的NURBS曲线轨迹插补运动。

Images