2.2 运动控制器的基本原理

2.2.1 运动控制器的构成

运动控制器由轨迹生成器、插补器、控制回路和步序发生器四部分构成，如图2-3所示。其基本原理为：运动控制器根据任务的需要，首先由轨迹生成器计算出任务希望的理想轨迹，插补器根据位置或者速度反馈单元的实际状态，按照轨迹生成器的要求，计算出驱动单元下一步将要执行的命令，然后交由控制回路进行精确控制。如果是步进电机，则还有一部分就是步序发生器，步序发生器根据控制回路控制指令进一步生成控制相序和脉冲，达到控制运动对象的目的。

2.2.2 轨迹生成器

轨迹生成器的主要作用就是根据运动任务要求，为系统生成运动轨迹，作为运动系统控制器插补器的输入设定。轨迹生成器的职责是把运动控制器的输入转化为系统希望得到的理想轨迹。运动轨迹是运动系统必须要完成的运动任务。就运动控制器而言，轨迹生成器是运动控制器三个核心部件之一，轨迹生成器性能好与坏对运动控制器起着至关重要的作用。我们这里所说的轨迹是广义上的轨迹，广义上的轨迹可以是路径或者轮廓轨迹，也可以是速度轨迹，还可以是加速度轨迹。狭义上的轨迹则特指路径或者轮廓轨迹。

下面，通过图2-4说明轨迹问题。图2-4中有三个子图，分别表示三种停止操作过程。图中所示的速度变化曲线就是一条速度运行轨迹。由于电机的转速与供电电源频率成正比，因此尽管纵坐标表示的是频率，实质上还是电机的转速。其中，FL表示电机最低转速值，FH表示电机最高转速值，横坐标t表示时间，t0表示立即停止命令发出的时刻，即运动控制器在t0时刻向电机驱动单元发出停止信号。很显然，从图2-4（a）看出，系统恒速度运行的特征是最低速度与最高速度相同，即FL=FH，当t≥t0时，频率FL（FH）=0，表示电机转速从某一个常数值立刻下降到0。图2-4（b）所示的是一条典型的速度梯形曲线，电机最低速度是FL，然后经过t′时间达到最高速度FH，这个时段加速度是常数；在t0时刻到来之前，系统按照FH运行；当t=t0时，又经过t′时段，电机转速将由FH降低到FL，这个时段减加速度也是一个常数。图2-4（c）所示的是一条典型的速度S曲线，系统从t=0时刻开始，初始速度是FL，经过t′时间段，系统稳态速度变为FH，速度的变化率是变化的，不是一个定值；当t′≤t＜t0时，系统速度为FH，当t≥t0时，系统速度由FH变化为FL。速度变化遵循S曲线。图2-4所示的三种操作模式可以采用编码方式预先内置到控制器中。例如，恒速操作的编码为001，变速（线性）操作的编码为002，变速（S曲线）操作的编码为003。控制编程时，可以通过提前往控制器之中预设编码值来选择停止操作的方式。

1.点对点运动轨迹

虽然有很多种运动轨迹可以选择，但是最简单、最直接的运动曲线是点对点运动轨迹。

下面举一个简单的例子，图2-5所示的就是一个点对点的运动轨迹。由图可知，运动的起点从20°开始，运动的终点到100°结束。通常情况下，就位置或者速度系统而言，运动控制器更常用的方法是用编码器脉冲数来取代角度。点对点运动的应用范围十分宽广，包括各种大型医疗自动化诊疗设备，如CT机、MR核磁共振等；机械制造业使用的各类加工中心、数控机床等；科学研究领域的自动检测与定位仪器；军事领域中的自动定位跟踪、自动瞄准系统等；民用自动设施，如电梯等（需要注意的是，自动扶梯不是点对点运动方式）。点对点运动方式的特点是运动需求描述简单，控制容易。对于点对点运动方式，负载速度从零开始，加速到设定速度，稳定运行，然后减速至停止，此时被控对象到达目标位置——终点。在一个典型的点对点运动中，速度从零开始到零结束，并启动加速和停止减速都是平滑的。

点对点运动有两种最常用的速度轨迹曲线，分别是S曲线和梯形曲线。点对点运动的主要研究方法是特征值描述法，其主要研究内容是运动速度轨迹特征曲线和运动加速度轨迹特征曲线。如图2-6（a）所示，S曲线由7个不同的运动时序段构成，不论是速度曲线还是加速度曲线均有7个时序段。对于这7个时序段，时序段Ⅰ中运动对象从静止开始运动，速度平滑增加，而加速度则线性增加，直到最大加速度；时序段Ⅱ中运动对象的速度是线性增加，而加速度则维持在最大加速度，直到时序段Ⅱ结束；时序段Ⅲ中速度继续圆滑上升至最高速度，加速度则呈线性下降，直至加速度为零；时序段Ⅳ中速度保持恒定，直至时序段Ⅳ结束，此时速度达到最大值，加速度则为零。时序段Ⅴ、Ⅵ、Ⅶ是减速方式，它们与时序段Ⅰ、Ⅱ、Ⅲ的加速方式完全是对称的。

梯形曲线由三个时序段构成，如图2-6（b）所示。很显然，梯形曲线是S曲线的一个子集，与S曲线相比，它仅具有时序段Ⅱ（恒加速）、时序段Ⅳ（恒速）和时序段Ⅵ（恒减速）三个时序段。

S曲线与梯形曲线进行对比，可以看出S曲线的速度曲线的平滑性远远优于梯形曲线的速度曲线；同时，也发现S曲线的加速度曲线是连续的，而梯形曲线的加速度曲线是阶跃函数，存在突变点，这表明前者的平稳性大大优于后者。因此，对于需要高的运动平稳性的场合，可以优先选用S曲线。

图2-7所示的是梯形曲线的特征点，其中ωmax为最大速度，αmax为最大加速度，tacc为加速度时间，tdec为减速度时间，tmax为最大速度时间，ttotal为整个运动时间，则有如下公式。

注意，如果由式（2-4）计算出的时间是负值，那么说明实际速度不可能达到最大速度，而且速度曲线不是一个梯形，而是一个三角形。式（2-1）～式（2-4）是梯形速度曲线特征点的基本关系式。

【例2-1】 对于如图2-5所示的点对点运动，启动点是20°，结束点是100°，加速时间和减速时间各为0.5s，最大速度时间为7.5s。因此，已知θstart=20°，θend=100°，ωmax=10°/s，αmax=20°/s2.求梯形曲线特征点参数。

解题思路：按照式（2-1）～式（2-4），可以计算出梯形曲线特征点。

解：由式（2-1），可知

tacc=tdec=ω/α=10（°/s）/20（°/s2）=0.5s

tθ=θend−θstart=100°−20°=80°

由式（2-4），求出

tmax=θ/ωmax−tacc/2−tdec/2

=80°/10（°/s）−0.5s/2−0.5s/2=7.5s

由式（2-2），有

ttotal=tacc+tmax+tdec=0.5s+7.5s+0.5s=8.5

【例2-2】 已知θstart=20°，θend=22°，ωmax=10°/s，α=20°/s2，求梯形曲线的特征点参数。max

解题思路：按照式（2-1）～式（2-4），可以计算出梯形曲线特征点。

解：

tacc=tdec=ωmax/αmax=210（°/s）/20（°/s）=0.5s

θ=θend−θstart=22°-20°=2°

tmax=θ/ωmax-tacc/2-tdec/2

=2°/10（°/s）-0.5s/2-0.5s/2=-0.3s

由于时间绝不可能是负值，因此改变的是加速和减速时间。

所以，得出

tmax=−0.116s

其结论是速度不可能达到最大速度。由于梯形曲线特征点参数设置不合理，故导致梯形曲线变化成三角形曲线，显然这种状况属于异常情况。

根据已知参数计算运动曲线，设置曲线的特征点可以根据下述方程确定。

假设初速度=0，当时间条件为0≤t＜tacc时，则

当tacc≤t＜tacc+tmaxtacc时，则

当tacc+tmax≤t＜tacc+tmax+tdec时，则

当tacc+tmax+tdec≤t时，则

θ（t）=θend （2-8）

式（2-5）～式（2-8）在进行曲线特征点设置时使用，可实现程序化控制。图2-8所示的是计算梯形曲线特征点C语言子程序。

梯形曲线的缺陷是如果加速度和减速度是定值，那么在加速度段和减速度段，负载会发生抖动。在某些场合，这种抖动希望能减小到最小，故多项式曲线就起到这样的作用。图2-9所示的是一种典型的多项式曲线。由图可见，在加速度段和减加速度段，速度由多项式ω（t）=At2+Bt+C表示。这种多项式曲线的最大特色为速度平滑，稳定性好，不存在负载抖动的问题。

对于图2-9所示的多项式曲线，已知四个参数值θstart、θend、ωmax、αmax，即起点位置、结束位置、最大角速度与最大角加速度，并且如图2-9所示的速度多项式的约束条件是

因此，按照上述约束条件，可以计算出多项式的系数A、B和C。

0=A⋅02+B⋅0+C⇒C=0

0=2A⋅0+B⇒B=0

故

则

那么，可以求得图2-9中所描述的第一段曲线的方程为

同理，第二段曲线的方程为

第一段曲线（加速段）的路程为

第二段曲线（最大速度）的时间为

2.路径轨迹

沿着一条路径的所有位置点来描述的轨迹曲线就是路径轨迹曲线。路径轨迹曲线还有其他说法，如位置轨迹曲线和轮廓轨迹曲线，都是表述行走的路径，但是路径与轮廓还是有所差别，路径更强调的是行驶线路，而轮廓则更加强调走过的具体位置及其精度。那么，什么样的场合适合于采用路径轨迹曲线呢？通常，当运动需求没有速度或者加速度要求、而对路径或者形状有很严格的要求时，采用这种方法。路径轨迹曲线的运动表述法利用的是参数函数“p（u）”。当参数值u的值域是0～1时，函数值的变化范围也是0～1。无论如何，函数的参数选择条件是，运动开始与停止时的速度都是0，这样就使得最终多项式方程简化为公式（2-11）。随后这个方程可以用在式（2-9）中，使控制器能够在起点和终点之间产生一条光滑的运动路径。

式中，θstart为运动的起点位置，θend为运动的结束位置，tstart为运动的开始时间，tend为运动的结束时间。

p（u）=Au3+Bu2+Cu+D （2-14）

式中，多项式的系数是

然后，按照下列计算式计算求出，即

故 D=0，C=0，B=3，A=−2

简化求出

p（u）=−2u3+3u2 （2-15）

【例2-3】 本例题给出的是三角函数多项式，利用给出的多项式生成位置曲线。已知，p（u）=Asin（Bt+C）+D，按照图2-9所示的曲线求出相关特征点。

解题思路：结合式（2-9）、式（2-10）、式（2-11），即可求出多项式系数A、B、C、D。

解：p（u）=Asin（Bt+C）+D

故

故

故

因此，就可求得

图2-10所示的是按照p（u）函数更新路径轨迹曲线特征点的C语言子程序。函数table_init（ ）只在程序开始设置全局时间和表值的时候调用一次。当一个新目标位置由调用函数table_generate（ ）指定时，就会生成设置表。每次中断扫描检查设置点时，函数table_update（ ）就被调用，并且更新全局设置点变量。函数point_current（ ）用于安排时间，在这个函数中还包含一个简单的时钟记忆系统。

3.多轴运动轨迹

除了单轴运动之外，很多机器的运动轨迹都是多轴运动复合的产物。例如，一个机器人完成平面运动，从一个位置点到达另一个新的位置点，必须完成两点之间的复合运动。

在机械加工领域，工作装置头的运动往往是一种多轴运动。对于平面二维复合运动，有办公室里广泛使用的打印机、绘图仪，机械加工领域中的车床、排钻等；对于空间三维点位运动，有机械加工中的数控钻、数控坐标镗、数控坐标铣、加工中心与柔性制造系统，大型物流中心中的自动运搬机、行车等。

4.往复运动轨迹

除了点对点运动之外，还有一个范围广泛的应用就是往复运动。往复运动的运动轨迹是周期的，其特点是由一个主定时器或编码器做运动索引。电子凸轮也属于运动索引范畴，它还有一个功能就是变速器，其变速功能是通过电子齿轮实现的。

电子凸轮（也称电子轴）的设定方式有两种：①输入特征参数，②使用可下载表格（这种方式基本上是通过串口或者数据总线完成的）。表格的数据用来为主编码器或者主时钟提供基本数据，每一项表格数据对应编码器的一个位置，每一个编码器位置确定对应的一个运动目标点。可以提前把各类运动轨迹编制成数据表，用户根据实际需要下载一个梯形曲线或一个S形曲线。更常见的是，还可以自定义一个配置文件，模拟一个专门的机械凸轮的功能。

有很多方法可以指定主时钟和控制轴之间的关系。最常见的方法是使用编码器计数的数值换算对应主编码器旋转的角度。例如，用主编码器旋转360°对应编码器计数数值变化，并允许在表的每个位置定义一个或多个输出点，当执行表时，在阅读表的最后一个位置的数据之后，运动控制器将对表格数据进行覆盖，并重新从起点开始，因此开始和结束位置的目标必须是相同的，这样才能避免产生跳跃运动。

2.2.3 插补器

1.插补器定义

插补器是运动控制器中的算法。所谓“插补”，就是在一条已知起点和终点的曲线上进行数据点的密化。通常，可以采用的形式有简单的一次插补、二次插补或者更为复杂一些的三次样条函数插补。插补结果将送入控制回路。

由插补的定义可以看出，在轮廓控制系统中，插补是最重要的功能，是轮廓控制系统的本质特征。插补算法的稳定性和精度将直接影响到CNC系统的性能指标，所以为使高级数控系统能发挥其功能，不论是在国外还是国内，精度高、速度快的插补算法一直是科研人员希望能够突破的难点，也是各数控公司保密的核心技术。例如，西门子、Fanuc公司的数控系统，其许多功能都是对用户开放的，但其插补算法却从不对用户开放。

2.插补的种类

插补的种类很多。按插补工作是由硬件电路还是由软件程序完成的，可将其分为硬件插补和软件插补。图2-11所示的就是直线插补和圆弧插补，以及二者的组合。

按照数学模型，可分为一次（直线插补）、二次（圆弧插补、抛物线插补、椭圆、双曲线、二次样条曲线）和高次（样条线插补）。

目前，应用最多的插补分为两类：基准脉冲插补和数据采样插补。下面对插补做一下详细的分析。

1）直线插补

（1）直线插补原理

若坐标点（x0，y0）和（x1，y1）已知，则直线插补就是这两点之间的直线。对于x∈01（x0，x1），符合直线规则的y值可以由下式求出：

如图2-12所示，A（x0，y0）和B（x1，y1）是已知点，直线AB上点（x，y）xy是两个点间的直线插值，直线插补依据x，就能求得y，即

式（2-17）还可以被看成加权平均数，加权值大小反比于终点与未知点间的距离。距离越近，影响越大。权值是和。

在这个区间之外，计算采用的是与式（2-17）形式相同的线性归纳法。例如，已知（xk−1，yk−1）和（xk，yk），对于xk−1＜x*＜xk，y*值由下式求得：

在计算机图形学中会经常应用到直线插补。两个值间的直线插补是基本的操作，常用单词“lerp”作为计算机图形学领域中的术语，“lerp”的确切含义就是直线插补。

“lerp”操作被固化在所有现代计算机图形处理器中。它们通常用来作为基石，完成更复杂的操作。因为这种操作实现方便，光滑（平滑）函数不需要太多地输入表项，它可以利用快速查询能力实现精准的查询操作。

（2）直线插补步骤

直线插补一般有三个步骤：①偏差函数的构造，②偏差函数递推计算，③终点判别。

① 偏差函数的构造

如图2-13所示，Pi（x，y）是直线OA上任意一点，点O（0，0）是起始点，Ae（xe，ye）是终止点，对于OA直线上任何一点Pi，应满足。若实际点坐标为F′i（xi，yi），用Fi表示x/y，那么Fi=0，表明Fi在直线上；若Fi＞0，则表明实际点位于直线OA上方；若Fi＜0，表明实际点位于直线OA下方。

② 偏差函数递推计算

偏差函数的计算采用的是递推方式，即由前一个点的坐标计算后一个点的位置，即

Fi=yixe−xiye （2-19）

若Fi≥0，规定向+x方向走一步，即

xi+1=xi （2-20）

Fi+1=xeyi−ye（xi+1）=Fi−ye

若Fi＜0，规定向+y方向走一步，即

yi+1=yi+1 （2-21）

Fi+1=xe（yi+1）−ye（xi）=Fi+xe

③ 终点判别

直线插补的终点判别方法有三种：判别插补总步数；分别判别各子坐标插补步数；仅判别插补步数多的那一个坐标轴。建立终点判别插补总步数Σ，并把Σ值放入终点判别计数器，每插补一次，计数器执行减一操作，直至Σ=0才停止插补。

【例2-4】 直线OA的终点坐标Ae（6，4），求递推关系。

解：对于直线OA，终点坐标为xe=6，ye=4，插补从直线起点0开始，故F0=0。终点判别插补总步数Σ=6+4=10。由于Σ=10，所以插补总步数就是10步。具体过程如图2-14所示，计算过程见表2-1。

2）圆弧插补

（1）圆弧插补原理

圆弧插补算法针对的是多轴运动，其思想就是用弦进给代替弧进给。圆弧插补分为顺时针圆弧插补和逆时针圆弧插补两种。对于机械加工而言，当加工圆弧时，可以将加工点到圆心的距离与该圆的半径相比较，以此来反映加工时的偏差。以第一象限逆圆弧为例，可推导出圆弧插补的偏差计算公式。

要加工如图2-15所示的圆弧AE。圆弧AE位于第一象限，逆时针走向，半径为R，以原点为圆心，起点坐标为A（x0，y0），对于圆弧上任一加工点的坐标设为P（xi，yi）。点P与圆心的距离的平方为。

运用上述法则，使用偏差公式，便可加工出图2-15所示折线的近似圆弧。

（2）圆弧插补步骤

圆弧插补的具体步骤与直线插补完全相同，也是三个步骤：①偏差函数的构造，②偏差函数递推计算，③终点判别。

① 偏差函数的构造

若成立，则P（xi，yi）刚好落在圆弧上。

若，则P（xi，yi）在圆弧外侧，即Rp＞R，F＞0。

若，则P（xi，yi）在圆弧内侧，即Rp＞R，F＜0。

故偏差判别式为

② 偏差函数递推计算

对于逆圆弧，若Fi≥0，则规定向−x方向插补一步，即

若Fi＜0，则规定向+y方向插补一步，即

对于顺圆弧插补，若Fi≥0，则规定向−y方向插补一步，即

若Fi＜0，规定向+x方向插补一步，即

③ 终点判别

圆弧插补的终点判别方法有两种。判别插补的总步数，即

Σ=|xA−xB|+|yA−yB| （2-27）

分别判别各坐标轴的插补步数，即

Σx=|xA−xB|，Σy=|yA−yB| （2-28）

【例2-5】已知第一象限起点A（4，0），终点B（0，4），计算插补过程。

解题思路：根据图2-16所示，起点A（4，0），终点B（0，4），显然此插补属于圆弧插补类别的逆圆弧插补，按照式（2-23），式（2-24）计算，流程如图2-16和表2-2所示。

解：首先根据图2-16所示的起点和终点坐标，判断出本题需要采用逆圆弧插补的方法，因此需要按照式（2-23）和式（2-24）计算，从而求出表2-2。

3）抛物线插补

抛物线插补算法是一种新型插补算法。抛物线函数是一种简单函数，既可以利用硬件，也可以利用软件来实现。

抛物线插补算法以抛物线顶点的坐标值作为原点，抛物线的起点和终点都换算成以抛物线顶点为原点的相对坐标。抛物线的开口方向不同，它的数学表达式也不一样。为简明起见，以一种情况为例说明抛物线插补计算的方法和过程。设抛物线方程为

x2=2py （2-29）

抛物线采用8个特征点A、B、C、D、E、F、G和H。抛物线插补在相关特征点之间进行，如图2-17所示的抛物线特征点图。根据高等数学有关曲率的计算公式，采用三次样条函数。

式（2-30）中的y′和y′分别是y对x的一阶和二阶导数。

式（2-31）中的只有在抛物线顶点时等于1，其他情况都是大于1，因此抛物线曲率圆半径R可用式（2-30）表示；根据式（2-30），计算抛物线周期插补绝对值Δi时，可以把抛物线看成半径R=p+2y的圆弧。利用增量式圆弧插补计算法，计算插补增量绝对Δi=max（Δx，Δy），并以此作为抛物线插补增量Δi。如图2-18所示，抛物线方程为

x2=2py

（x+Δx）2=2p（y+Δy） （2-32）

可得

（2x+Δx）Δx=2p（y+Δy） （2-33）

为了便于计算机软件处理，令

Fx=|x|，Fy=|y|

Δl=max（|Δx|，|Δy|）

Δs=min（|Δx|，|Δy|） （2-34）

把Δs标志为二进制，即

如果p≥|x|，则插补增量绝对值|Δx|≥|Δy|，那么

Δl=|Δx|，Δs=|Δy|

式（2-33）可以写成绝对值表达式（2-36），即

式中，是算子。

如果p＜|x|，则插补增量绝对值|Δx|＜|Δy|，那么Δl=|Δy|，Δs=|Δx|。式（2-33）可以写成

按照式（2-36）和式（2-37），可计算圆弧插补增量绝对值Δs。

式（2-36）和式（2-37）是用绝对值表示的，当从抛物线上任一点向顶点方向插补时，插补点（x，y）的绝对值是逐渐缩小的，故这种情况下W=−1；反之，当插补点从抛物线开口小的方向向开口大的方向插补时，插补点（x，y）坐标的绝对值是逐渐增大的，W=1。

图2-19所示的是插补增量图，图中a（x0，y0）是抛物线起点，b（xe，ye）是抛物线终点。I是抛物线起点坐标中的二次项符号，即

J是抛物线b（xe，ye）终点坐标中的二次项符号，即

为了便于逻辑或运算，令

K=I+J （2-38）

如果K=1，则说明是跨象限插补，式（2-36）和式（2-37）中的W=−1。

如果K=0，则抛物线插补不跨象限。

还可以比较起点和终点坐标的|x0|和|xe|：如果|x0|＜|xe|，则W=1；如果|x0|＞|xe|，则W=−1。

4）样条插补

为了满足快速成形制造及模具加工领域中高速度和高精度的要求，利用B样条曲线良好的局部控制性和计算机CPU处理速度快的优点，开发出基于三次非均匀B样条曲线的插补算法。

（1）样条插补原理

快速成形制造系统读入STL文件后，首先利用拟合算法将这些离散的数据点拟合为分段的三次非均匀无理B样条；然后构造一链表用来保存每段B样条的系数和长度；插补时读入此链表，根据分段标志，以当前插补点为控制目标，求出下一插补点的坐标值，完成变速度自适应插补。B样条的参数表达式为

式中，a0i、ai1、ai2、ai3、bi0、bi1、bi2和bi3为各小段系数。

式（2-39）中，如果三次项系数为零，则表示抛物线；三次项和二次项系数均为零，则表示直线，因此可以把三次非均匀无理B样条、抛物线和直线用同一个公式统一起来。

（2）样条插补步骤

样条插补算法采用参数化数据采样原理来实现插补过程，其基本思想是按照给定的采样周期将时间轴分成非均匀间隔的小区间，插补过程中根据进给速度、加减速要求和允许误差，在各采样周期产生空间小直线段ΔL1、ΔL2、ΔL3、…、ΔLm，…去逼近被插补曲线，逐步求得伺服控制所需的各插补直线段端点P1、P2、P3、…、Pm、…的坐标值，即以当前插补点的速度为控制目标，以插补周期T内的插补步长等于曲线上的弧长为依据，最终求出下一插补点的坐标。在给出当前点所在的曲线表达式

之后，就可求得当前插补点的曲率半径为

当前插补点误差要求的最大插补直线段长度ΔLmax如图2-20所示。当前样条段弧长s根据各段的弧长s1、s2、…、sm、…，进而求得样条总弧长s=∑si。根据其他一系列条件求出当前插补点的插补速度V，从而完成插补运算。具体实现如下：

ΔLmax=[4δ（2R−δ）]1/2 （2-42）

样条每小段弧长为

（3）样条插补计算

因为B样条曲线是一分段曲线，为便于计算，引入一新的参变量ω，取值域为[0，m]，m为样条曲线的总段数，并用ω1表示ω的整数部分，ω2表示ω的小数部分，即ω=ω1ω2。这样，可将式（2-39）写为

插补计算不是一种静态的几何计算，它不仅要使当前插补点与前一插补点间的距离满足进给速度及加减速等要求，而且还要保证这两点间的插补直线段与被插补曲线间的误差在给定的允许范围内。为满足上述要求，本插补算法采取以瞬时加工速度为控制目标，以允许误差等为约束条件的实时插补算法，其基本过程如下。

如图2-21所示，已知插补前一点P′、速度V′（初始值为0）、参数ω′、半径R′，由速度V′和插补周期T求得插补直线段：ΔL=V′T，由此（使弧P′P的弧长等于ΔL）得到下一个插补点P，从而求得点P参数ω，ω1和ω2，再根据式（2-41），计算求出曲率半径R，最终得到如下几个约束条件。

① Vmin≤V≤Vmax

② VT≤ΔTmax⇒V≤ΔLmax/T=[4δ（2R−δ）]1/2

③ V2/R≤AR⇒V≤（ARR）1/2

④ V≤V1+ATT

⑤ V2≤2ADD⇒V（2ADB）1/2

式中，Vmin、Vmax分别是最低和最高运动速度，T是插补周期，δ是插补误差范围，AR是向心加速度，AT、AD分别是切向加、减速度，D1是当前插补点与终点之间距离，是实时判断是否到达终点的标志。

如图2-22所示，用A、B、C、D、E、F、G表示，六个小段分别用s1、s2、s3、s4、s5和s6表示。假设O是当前插补点，距离终点G的距离是D1，从点G开始向后探索，逐步比较D1与各段弧长，即可迅速确定当前插补点位置是介于CD之间的，即弧OD=D1-（s4+s5+s6），则弧CO=s3-OD，ω1=2。

设置ω2=CO/s3，代入弧长公式求得临时弧长O1C，比较O1C与OC的大小，如图2-23所示，采用迭代法即可精确求出O1的位置，也就是求出ω2值；代入式（2-44），就可以求出下一个插补点坐标，然后循环就可以求出全部插补点。

5）基准脉冲插补

基准脉冲插补又称为行程标量插补或脉冲增量插补。这种插补算法的特点是每次插补结束，运动控制装置向每个运动坐标输出基准脉冲序列，每个脉冲插补的实现方法比较简单，只有加法和移位，可以利用硬件实现。目前，随着计算机技术的迅猛发展，多采用软件完成这类算法。脉冲的累积值代表运动轴的位置，脉冲产生的速度与运动轴的速度成比例。

基准脉冲插补方法方法有很多，常用的有以下几种：①数字脉冲乘法器插补法；②逐点比较法；③数字积分法；④矢量判别法；⑤比较积分法；⑥最小偏差法；⑦目标点跟踪法；⑧直接函数法；⑨单步跟踪法；⑩加密判别和双判别插补法等。

早期常用的基准脉冲插补算法有逐点比较法、单步跟踪法等，插补精度常为一个脉冲当量。20世纪80年代后期，插补算法改进为直接函数法、最小偏差法等，使插补精度提高到半个脉冲当量，但执行速度不很理想，因此在插补精度和运动速度要求均高的CNC系统中，应用受限。近年来的插补算法有改进的最小偏差法、映射法等，兼有插补精度高和插补速度快的特点。

总的说来，最小偏差法插补精度较高，并且有利于电机的连续运动。

6）数据采样插补

数据采样插补又称为时间标量插补或数字增量插补。这类插补算法的特点是数控装置产生的不是单个脉冲，而是标准二进制字。插补运算分两步完成。第一步为粗插补，它是在给定起点和终点的曲线之间插入若干个点，即用若干条微小直线段来逼近给定曲线，每一微小直线段的长度ΔL都相等。粗插补在每个插补运算周期中计算一次，因此，每一微小直线段的长度ΔL与进给速度F和插补周期T有关，即ΔL=FT。第二步为精插补，它是在粗插补算出的每一微小直线段的基础上再进行“数据点的密化”工作。这一步相当于直线的基准脉冲插补。

在数控系统中，采样周期的选取对于实际加工的精度影响很大，如果采样周期选取得太大，加工精度就不能得到保证，但是采样周期选取得太小，又会影响加工速度，所以在实际选取时要尽量二者兼顾。

数据采样插补适用于闭环、半闭环以直流和交流伺服电机为驱动装置的位置采样控制系统。粗插补在每个插补周期内计算出坐标实际位置增量值，而精插补则在每个采样周期内采样闭环或半闭环反馈位置增量值及插补输出的指令位置增量值。然后算出各坐标轴相应的插补指令位置和实际反馈位置，并将二者相比较，求得跟随误差。根据所求得跟随误差算出相应轴的精速度，并输给驱动装置。一般粗插补运算用软件实现，而精插补既可以用软件，也可以用硬件实现。

数据采样插补方法很多，常用的方法有：①直接函数法；②扩展数字积分法；③二阶递归扩展数字积分圆弧插补法；④圆弧双数字积分插补法；⑤角度逼近圆弧插补法；⑥ITM（Improved Tustin Method）法。近年来，众多学者又研究了更多的插补类型及改进方法，有改进DDA圆弧插补算法、空间圆弧的插补时间分割法、抛物线的时间分割插补方法、椭圆弧插补法、B样条等参数曲线的插补方法、任意空间参数曲线的插补方法。

上述方法均为基于时间分割的思想，根据编程的进给速度，将轮廓曲线分割为插补周期的进给段（轮廓步长），即用弦线或割线等逼近轮廓轨迹，然后在此基础上，应用上述不同的方法求解各坐标轴分量。不同的求解方法有不同的逼近精度和不同的计算速度。

随着STEP标准的颁布，NURBS曲线、曲面插补方法的应用越来越广泛。由于NURBS法囊括了圆弧等二次曲线及自由曲线和曲面的表达式，故使得未来的CNC系统的线型代码指令可以“瘦身”为直线和NURBS两大类。

如果脱离速度控制谈插补算法，那么插补只能应用于计算机图形学中，所以只有将加减速控制与插补算法有机结合起来，才能构成完整的CNC系统运动控制模块。在基准脉冲插补算法中，可以靠改变插补周期来控制进给速度，而在数据采样算法中，进给速度与插补周期没有直接联系。数据采样算法的加减速控制分为插补前加减速控制和插补后加减速控制。由于插补后加减速控制是以各个轴分别考虑的，不但损失加工精度，而且可能导致终点判别错误，所以在高精度加工中均采用插补前加减速控制。但是对于任意曲线、曲面加工来说，插补前加减来控制的减速点预测是非常困难的。

加减速控制的方法有直线、梯形、指数、抛物线和复合曲线加减速法等。直线加减速方法计算简单，但是存在冲击；指数加减速方法没有冲击，但速度慢于直线加减速方法，而且计算复杂；复合曲线加减速方法不存在冲击，速度适中，但计算复杂。所以，根据所需不同的控制精度、控制速度选择合适的加减速控制方法是很重要的。

3.运动进程表

随着定位进程表技术的发展，运动曲线轮廓可以由定位点表确定。定位点表将依据时钟确定定位点的修改。图2-24所示的是一个定位点表的流程。在这个系统中，总运动时间看做系统时间，定位点表是一个中断驱动子程序。随着时间的流逝，程序执行到定位点表的下一个值，中断频率时间间隔必须小于或者等于用于计算定位点的时间步长。另外，伺服驱动模块还需要增加一个算法，如PID算法。

按照定位点表的输出修正每一步时间的步序，引导输出始终跟随目标值，这样产生的误差就很小，如图2-25所示。图2-25纵坐标是速度，横坐标是时间，轨迹步序曲线是由轨迹表时间节拍构成的，从图可以看出轨迹的组成是由节拍的末端点组成的折线。

图2-26所示的是理论期望的位置与实际得到的位置误差关系图，其中实线是期望的位置，虚线是实际得到的位置。

2.2.4 控制回路

在运动控制器中固化有控制算法，由算法依据期望的位置或速度与实际的位置或速度的差值信号进行计算。运动控制器通常使用增强型PID（比例、积分、微分）算法，使得控制功能更加强大。PID的增益设定值决定控制回路的控制能力。下面结合前馈控制研究实际运动控制系统控制回路的基本架构，如图2-27所示。图中包括两个组成部分：一是实时控制回路，二是物理系统。这里所说的物理系统就是执行器，具体在本系统中是电机。图2-27中θd是位置输入设定；θa是位置实际输出，实时控制回路的作用是为物理系统提供控制指令；ωd是前馈控制输入设定值，通过ωd的变更，就可以实现对算法的修正。

图2-28所示的是多回路运动控制器的组合控制回路。该系统具有灵活的组合性，可以实现单回路（位置、速度、力矩）控制，也可以实现双回路控制，还可以实现三回路控制。其基本控制程序也是三种调节器的控制算法（位置、速度、力矩），已经固化在只读存储器中。用户与运动控制器的通信可以通过CAN、RS232、RS485来实现。系统中还有八路I/O控制接口，其中六路输入、两路输出，具体如图2-28所示。

随着智能控制技术的发展，新型运动控制系统控制回路开始采用模糊PID算法。下面举一个实例。

图2-29所示的是一个实验装置，任务就是实现点对点的运动。它由一套伺服电机驱动器、伺服电机、编码器和PC组成。控制器是PC，PC上运行的控制软件是利用MATLAB的实验程序。

1.数学模型

忽略系统的非线性，按照牛顿学有关定律，这套旋转位置系统实验装置的动态数学模型为

图2-30所示的是一个模糊PID调节器的系统框图，其基础算法是PID算法，但P、I、D参数（KP、KI、KD）是依据输入与输出的误差及误差的变化率来决定的。

模糊PID调节器按照下列步骤进行设计。

（1）根据被控制对象的线性模型和所期望的性能指标设定PID调节器增益的标称值。

（2）KP、KI、KD是在PID调节器增益的标称值基础上设计出的模糊调谐值。

2.PID标称值设计

图2-30所示的PID调节器的控制算法可以用式（2-33）表示。KP、KI、KD分别为调节器增益的比例、积分和微分。

利用Simulink Response Optimization toolbox构建控制对象的线性模型，依据所构建的线性模型确定PID调节器增益的标称值。由toolbox设定期望的响应，使系统达到稳态时的误差尽可能小。为了实现期望响应，设定下述参数值。

Rise time=0.1s

Settling time=0.4s

Overshoot=5%

%settling=0.001

最后，可以得出PID控制器的增益标称值为

KP=19.21，KD=0.3，KI=0.1

3.模糊调节器设计

1）模糊调节器比例增益系数KP

模糊PID调节器第一个参数是比例增益系数KP。KP的输入是误差和误差变化率。图2-31所示的是KP误差隶属函数。图2-32所示的是KP误差变化率隶属函数。图2-33所示的是KP输出值隶属函数。模糊PID调节器KP的输入与输出规则见表2-3。最后，模糊调节器KP的输出值采用中心面积法对KP去模糊化求得。

注：表中各字母含义与图2-31、图2-32和图2-33中相同。

2）模糊调节器微分增益系数KD

模糊PID调节器第二个参数是微分增益系数KD。KD的输入是误差及误差变化率。图2-34所示的是KD误差隶属函数。图2-35所示的是KD误差变化率隶属函数。图2-36所示的是KD输出值隶属函数。模糊PID调节器KD的输入与输出规则见表2-4。最后，模糊调节器KD的输出值采用中心面积法对KD去模糊化求得。

注：表中各字母含义与图2-34、图2-35和图2-36中相同。

3）模糊调节器积分增益系数KI

模糊PID调节器第三个参数是积分增益系数KI。KI的输入是误差和误差变化率。图2-37所示的是KI误差隶属函数。图2-38所示的是KI误差变化率隶属函数。图2-39所示的是KI输出值隶属函数。模糊PID调节器KI的输入与输出规则见表2-5。最后，模糊调节器KI的输出值采用中心面积法对KI去模糊化得到。

注：表中各字母含义与图2-37、图2-38和图2-39中相同。

4.控制结果与讨论

借助点对点模糊PID实验装置，利用Simulink Optimization Toolbox设定PID参数增益标称值进行实验对比，发现采用模糊PID的方法更加有效。图2-40所示的是本实验装置在输入90°步长时的系统实时响应图，图中实线部分是采用模糊PID控制算法得到的实际结果，虚线部分是采用传统经典PID算法得出的实验结果。有关实验位置系统的定位性能见表2-6。

从表2-6可以看出，模糊PID算法的性能要优于经典PID算法，尤其是系统的稳定时间和位置精度指标大幅度优于经典PID算法。本实验位置测量传感器是一种光纤编码器，把光纤编码器安装在负载轴上，测量负载轴的角度位置，90°时编码器的输出是4096个计数单位，也可以进行相对角度的测量。

此外，利用本实验装置还对模糊PID和经典PID的鲁棒性进行了性能对照。为了评测系统的鲁棒性，需要给旋转位置单元增加惯量。图2-41所示的是增加旋转位置装置惯量后采用模糊PID与经典PID的实验结果曲线，相应的实验数据见表2-6。

从表2-6中数据可以得出，随着惯量的增加，模糊PID性能没有改变，因此可以得出的结论：模糊PID对惯量改变的鲁棒性要优于经典PID。