[整理]Delta并联机器人的插补研究

第三章Delta并联机器人的插补研究
（一）引言
数控系统从一条已知起点和终点的曲线上自动进行“数据点密化”的工作，将连续的轨迹细化为微小的直线段，这就是插补。

根据加工程序中有限的坐标点，运用相应的算法，在已知的坐标点之间产生一系列的坐标点数据，再将数据点转化为相应的脉冲，从而在加工时对各坐标轴进行脉冲分配，使机床加工出符合要求的轮廓曲线。

（二）插补原理分析
目前普遍应用的插补算法可分为两大类：基准脉冲插补和数据采样插补。

基准脉冲插补又称脉冲增量插补或行程标量插补。

该插补算法的特点是每进行一次插补运算就会产生一个坐标增量，最后再以脉冲数的方式输出给驱动电机。

基准脉冲插补在插补计算过程中不断向各个驱动轴发送进给脉冲，从而实现预定轨迹的插补。

基准脉冲插补方法有：逐点比较法、数字积分法、比较积分法、矢量判断法、最小偏差法、数字脉冲乘法器法等[5]。

由于只有加法和移位，因此基准脉冲插补的实现相对比较简单。

数据采样插补又称为时间标量插补或数字增量插补。

该插补算法的特点是数控装置产生的不是单个脉冲，而是标准的二进制数字。

其采用的是时间分割的方法，根据程序的进给速度，将轮廓曲线分割为采样周期性的进给步长，即用弦线或割线逼近轮廓轨迹。

通常有以下几种方法：直线函数法、扩展数字积分法、二阶递归扩展数字积分插补法、双数字积分插补法[5]。

逐点比较法的原理是：计算机在加工轨迹过程中，采用逐点计算进给步长和判别加工偏差以控制坐标进给方向，从而按规定的轨迹加工出合格的零件。

这种插补方法的特点在于机床每走一步都要进行四个步骤，即：
第一步，偏差判别：判别加工点对轨迹的偏差位置，然后下一步进给的方向；
第二步，进给：控制工作台进给一步，加工的轨迹向规定的轨迹靠拢，缩小偏差；
第三步，偏差计算：计算下一个加工点相对轨迹的偏差，为一下步的方向判别做准备；
第四步，终点判断：判断是否到达程序规定的加工终点。

如到达终点，则停止插补，否则再回到第一步。

如此不断的重复，就能加工出的轮廓。

（三）直线插补 1.平面直线插补
第一象限逐点比较法直线插补：
图3.1 逐点比较法直线插补
Fig3.1 Point by point comparison method linear interpolation
插补规则：当F ≥0时，则沿+X 方向进给一步；当F ≤0时，则沿+Y 方向进给一步。

偏差判别函数：
当M 在OA 上，即F=0时，有：
j e
i e
Y Y X X =
，e j i e F X Y X Y 0=-= 当M 在OA 上方，即F>0时，有j
e
i e
Y Y X X >，e j i e F X Y X Y 0=->
当M 在OA 下方，即F<0时，有
j e
i e
Y Y X X <，j e i e F X Y X Y 0=-< 偏差判别函数的递推形式：设当前点M (X ,Y )i j 的偏差为：,0i j e j i e F F X Y X Y ==->，则根据偏差公式有：
当i,j F 0³时，下一点的坐标为：11X X 1,Y Y i i j j ++=+=，新偏差为：1,,(1)i j e j i j i j e F X Y X Y F Y +=-+=- 当i,j F 0<时，下一点的坐标为：11X X ,Y Y 1i i j j ++==+，新偏差为：,1,(1)+X i j e j i e i j e F X Y X Y F +=+-=
终点判别方法： 判别式（X
Y
,;e
e
X Y 邋
e X ;max(,)
e
e
e
Y X Y 邋或或）进给一步减1，直到减到
0为止。

根据第一象限的插补方法，可以类推出其他三个象限的插补方法，如图3.2。

图3.2 四个象限插补示意图
Fig 3.2 The four quadrant interpolation schemes
2.空间直线插补
空间直线插补逐点比较法的基本原理：
（1）将空间直线向XY 平面和XZ 平面投影，得到两条平面直线； （2）将两条平面直线分别用平面直线逐点比较法进行插补运算；
（3）用X F 和Z F 分别表示两条平面直线f (,)x y 和g(,)z y 的偏差函数，当X F 、Z F 的符号都是使X 方向进给时，才向X 方向进给脉冲。

否则只在一个平面内向Y 或Z 方向发进给脉冲，而另一个平面不发脉冲。

图3.3 空间直线插补
Fig3.3 Space in a straight line interpolation
加工偏差公式：如图3.3所示。

设要加工空间直线OA ，以坐标原点O 为起点，以e (,,)e e X Y Z 为终
点的空间直线。

设e X >e Y ,e X >e Z ,且e X ,e Y ,e Z 均为正数。

以X 轴为基准轴，建立两个平面坐标系，如图3.3所示。

直线'a 为OA 在XY 坐标平面内的投影，直线''a 为OA 在XZ 坐标平面内的投影。

在XY 平面上，偏差计算公式为：1,,i j i j e F F Y +=- ,+1,+X i j i j e F F =
同理，在XZ 坐标平面上，偏差计算公式为：1,k ,P P i i k e Z +=- ,k 1,P P i i k e X +=+
终点判断方法：
有了偏差计算公式，现在关键是如何判别和计算下一步进给的方向。

我们规定：
（1）若0F ³，0P ³，沿X 轴正方向发一进给脉冲，由于使加工点i X +1，故必须在XOY 坐标平面和XOZ 坐标平面都必须进行偏差计算，即1,,i j i j e F F Y +=-和1,k ,P P i i k e Z +=-。

（2）只要0F <，沿Y 轴正方向发一进给脉冲，由于是使加工点j Y +1，故只需在XOY 坐标平面内进行偏差计算，即,+1,+X i j i j e F F =。

（3）若0F ³，0P <，由于使加工点Z k +1，故只需在XOZ 坐标平面内进行偏差计算，即
,k 1,P P i i k e X +=+。

同样，在每次计算偏差和发送进给脉冲的同时，还要进行一次终点判断，以确定是否达到终点。

如果达到了终点，则停止插补。

在上述空间直线插补时，首先假设了该段空间直线的X 方向进给步数为最大,就是说在以长轴为基轴建立的两平面坐标系内进行的插补运算，才是正确的。

因此每次进行插补运算之前，都必须对三个坐标的增量值进行比较和转换，将增量值较大的进行X 方向的插补，并设置相应的标志位进行变换。

其他象限的插补运算也可以用类似的方法进行变换。

即在进入插补程序前，三个坐标增量都以绝对值方式寄存在相应的寄存器中，并根据增量值的正负设定各驱动轴的方向标志位。

（四）平面圆弧插补
逐点比较法第一象限逆时针圆弧插补基本原理如下：
在加工圆弧过程中，使用动点到圆心的距离与该圆弧的半径进行比较来反映加工偏差，以确定下一步插补的方向。

假设被加工零件的轮廓为第一象限逆时针走向圆弧SE ，圆心在O （0，0），半径为R ，起点为S (,)s s X Y ，终点为E (,)e e X Y ，圆弧上任意加工动点为N (,)i i X Y 。

通过比较动点到圆心的距离与圆弧半径R 的大小，可获得末端参考点与圆弧轮廓之间的位置偏差，从而可以确定下一步的插补方向。

当动点N (,)i i X Y 正好落在圆弧上时，则有下式成立
当动点N (,)i i X Y 落在圆弧外侧时，则有下式成立
当动点N (,)i i X Y 落在圆弧内侧时，则有下式成立
由此可见，取逐点比较法圆弧插补的偏差函数表达式为
当动点落在圆外时，为了是加工的轨迹逼近要求的轮廓，减小加工误差，应是动点向圆内进给一
步，即向X 轴负方向前进一步；当动点落在圆内时，应使动点向圆外进给，即向Y 轴正方向前进一步。

当动点正好落在圆弧上且未到达终点时，为了使加工继续下去，理论上向Y 轴正或X 轴负方向进给都可以，但一般情况下约定向X 轴负方向进给。

综上所述，现将逐点比较法第一象限逆时针圆插补规则概括如下：
当0F >时，即2220i i F X Y R =+->，动点落在圆外，则向X 轴负方向进给一步； 当0F =时，即2220i i F X Y R =+-=，动点正好落在圆上，约定向X 轴负方向进给一步； 当0F <时，即2220i i F X Y R =+-<，动点落在圆内，则向Ｙ轴正方向进给一步。

由偏差函数表达式可知，要想计算偏差F 值，就必须对动点坐标、圆弧半径进行平方运算。

为了
简化运算过程，按逐点比较法直线插补的思路，可以推导出逐点比较法圆弧插补过程中偏差函数计算的递推公式。

假设第i 次插补后，动点坐标为N (,)i i X Y ，其对应偏差函数为
当0i F ³，向X 轴负方向进给一步，则新的动点坐标值为
ON 22222R Y X Y X e e i i =+=+22222R Y X Y X e e i i =+>+22222R Y X Y X e e i i =+<+222R Y X F i i -+=222
R Y X F i i i -+=
11i i X X +=-， 1i i Y Y +=
因此，新的偏差函数为
222222
111(1)i i i i i F X Y R X Y R +++=+-=-+-
即： 121i i i F F X +=-+
同理，当0i F <，则向Y 轴正方向进给一步，则新的动点坐标值为
1i i X X +=，11i i Y Y +=+
因此，可求得新的偏差函数为
即：121i i i F F Y +=++
将上式进行比较，可以看出两点不同：第一，递推形式的偏差计算公式中仅有加、减法以及乘法
运算，而乘以2可等效为该二进制数左移一位，这显然比平方运算的运算量小得多；第二，进给后新的偏差函数值与前一点的偏差值以及动点坐标N (,)i i X Y 之间可以建立关系式。

由于动点坐标值随着插补运算的进行而不断变化，因此，每插补一次，动点坐标就必须根据偏差值修正一次，为下一步的偏差计算做准备。

至此，将第一象限逆圆弧插补的规则和计算公式汇总，见表3.1。

表3.1
Table 3.1
和直线插补一样，圆弧插补过程也需要进行终点判别。

当圆弧轮廓仅在一个象限区域内，其终点判别可用直线终点判别的方法进行判别，只是计算公式略有不同。

e s e s X X Y Y =-+-å max{,}e s e s X X Y Y =--å
1
e s X X =-å
，2
e s Y Y =-å
式中 s X 、s Y —— 被插补圆弧轮廓的起点坐标；
e X 、e Y —— 被插补圆弧轮廓的终点坐标。

()22
22212111R Y X R Y X F i i i i i -++=-+=+++
可根据上述方法推导出其他象限不同走向圆弧的插补运算。

如图3.4所示：
图3.4 四象限圆弧插补进给方向
Fig 3.4 Four quadrant circular arc interpolation direction of feed
四象限圆弧插补计算如下表：
Table 3.2
从表3.2可以看出，当按第一象限逆时针圆1NR 进行插补运算时，若将X 轴方向进给反向，则可以走出第二象限顺时针圆2SR 来；或者将Y 轴方向进给反向，则可以走出4SR 来；或者将X 轴和Y 轴的进给均反向，则可以走出3NR 来；并且这四种线型（1NR 、2SR 、3NR 、4SR ）使用的偏差计算公式都相同，无须改变。

还可以看出，当按第一象限逆时针圆1NR 线型插补时，若将计算公式坐标X 与Y 对调，即用Y 方
式对X 坐标进行插补，用X 方式对Y 坐标进行插补，那么就可得到1SR 的走向。

类似地通过改变插补进给方向，利用1SR 的计算公式就可得到其他三种线型2NR 、3NR 、4NR 的插补。

（五）本章小结
本章主要论述了平面直线、空间直线和平面圆弧的逐点比较法插补。

通过对逐点比较法的偏差函数、终点判别等的研究，得到了平面直线、空间直线和平面圆弧逐点比较法插补方法，为后续的编程及运动控制打下了基础。

第四章 Delta 并联机器人插补算法编程及验证
（一）引言
并联机器人的控制系统是并联机器人设计的基本内容，也是并联机器人发展研究的重点。

本章将
利用Visual Basic软件与运动控制卡相结合，编写一个简单的控制系统，实现对并联机器人的直线、平面圆弧运动的控制。

运动控制卡采用固高科技公司的GTS-400-PV-PCI-G，该运动控制卡提供了Visual Basic等语言的函数库和Windows动态链接库，可实现复杂的控制功能[6]。

（二）控制软件系统设计
验证软件系统设计：界面上包括空间直线、平面圆弧插补以及插补轨迹的平面显示；程序上包括正解、反解模块、初始化运动控制卡及PT运动模式。

设计该系统的主要目的是验证第三章的插补算法。

插补步长为0.01mm。

使用Visual Basic编写的控制软件，界面如图4.1所示。

图4.1控制软件界面
Fig.4.1 Control system main interface
1.系统的反解模块
反解模块是Delta控制系统必不可少的一个部分，也是核心部分。

反解模块的作用是在主动臂的转角与末端机构的坐标值之间建立一种转化的关系，即在已知末端机构坐标值的情况下求主动臂的转角。

通过反解模块可以实现通过伺服电机的脉冲对末端机构的运动进行控制。

反解公式在第二章中已经求解出。

其具体的Visual Basic代码如下：
Public Function fj_x(ByVal x As Double, ByVal y As Double, ByVal z As Double) As Double 'x轴反解函数，用于求出x的脉冲
Dim R1#, R2#, l1#, l2#, U1#, V1#, W1#, t1#, z1#
z1 = z - 646.379
R1 = 200
R2 = 50
l1 = 200
l2 = 600
U1 = (l2 ^ 2 - l1 ^ 2 - x ^ 2 - y ^ 2 - z1 ^ 2 - (R1 - R2) ^ 2 + (R1 - R2) * (Sqr(3) * x + y)) / l1 + 2 * z1 V1 = -2 * (2 * (R1 - R2) - Sqr(3) * x - y)
W1 = (l2 ^ 2 - l1 ^ 2 - x ^ 2 - y ^ 2 - z1 ^ 2 - (R1 - R2) ^ 2 + (R1 - R2) * (Sqr(3) * x + y)) / l1 - 2 * z1 t1 = (-V1 - Sqr(V1 ^ 2 - 4 * U1 * W1)) / (2 * U1)
a1 = 2 * Atn(t1) * 180 / 3.14 - 50.007 '相对于零点的角度
fj_x = CDbl(Format(a1 * 277.778, "0.000")) '将角度转化为脉冲
End Function
Public Function fj_y(ByVal x As Double, ByVal y As Double, ByVal z As Double) As Double 'y轴反解函数，用于求出y的脉冲
Dim R1#, R2#, l1#, l2#, U2#, V2#, W2#, t2#, z2#
z2 = z - 646.379
R1 = 200
R2 = 50
l1 = 200
l2 = 600
U2 = (l2 ^ 2 - l1 ^ 2 - x ^ 2 - y ^ 2 - z2 ^ 2 - (R1 - R2) ^ 2 - (R1 - R2) * (Sqr(3) * x - y)) / l1 + 2 * z2 V2 = -2 * (2 * (R1 - R2) + Sqr(3) * x - y)
W2 = (l2 ^ 2 - l1 ^ 2 - x ^ 2 - y ^ 2 - z2 ^ 2 - (R1 - R2) ^ 2 - (R1 - R2) * (Sqr(3) * x - y)) / l1 - 2 * z2 t2 = (-V2 - Sqr(V2 ^ 2 - 4 * U2 * W2)) / (2 * U2)
a2 = 2 * Atn(t2) * 180 / 3.14 - 50.007
fj_y = CDbl(Format(a2 * 277.778, "0.000")) '将角度转化为脉冲
End Function
Public Function fj_z(ByVal x As Double, ByVal y As Double, ByVal z As Double) As Double 'z轴反解函数，用于求出z的脉冲
Dim R1#, R2#, l1#, l2#, U3#, V3#, W3#, t3#, z3#
z3 = z - 646.379
R1 = 200
R2 = 50
l1 = 200
l2 = 600
U3 = (l2 ^ 2 - l1 ^ 2 - x ^ 2 - y ^ 2 - z3 ^ 2 - (R1 - R2) ^ 2 - (R1 - R2) * 2 * y) / (2 * l1) + z3
V3 = -2 * (R1 - R2 + y)
W3 = (l2 ^ 2 - l1 ^ 2 - x ^ 2 - y ^ 2 - z3 ^ 2 - (R1 - R2) ^ 2 - (R1 - R2) * 2 * y) / (2 * l1) - z3
t3 = (-V3 - Sqr(V3 ^ 2 - 4 * U3 * W3)) / (2 * U3)
a3 = 2 * Atn(t3) * 180 / 3.14 - 50.007
fj_z = CDbl(Format(a3 * 277.778, "0.000")) '将角度转化为脉冲
End Function
2.系统的正解模块
正解模块是反解模块的逆运算，即通过主动臂的转角去求解出末端机构的坐标。

在控制系统中，正解模块可以用于对系统工作空间范围的判断及运动轨迹的显示。

在执行运动前，判断终点是否超出规定的空间范围，从而建立相应的报警机制，实现对整个系统的保护作用。

正解的求解方法在第二章中已经给出。

具体的Visual Basic程序代码如下：
Public Function zj_x(ByVal j1 As Double, ByVal j2 As Double, ByVal j3 As Double) As Double '''X轴正解函数
Dim a11, a22, a33, b1, b2, b3, c1, c2, c3 As Double
Dim e, f, g As Double
Dim k1, k2, l1, l2, p1, p2 As Double
Dim d2, d3 As Double
Dim m1, m2, n1, n2 As Double
'计算过渡值
a11 = Sqr(3) * (150 + 200 * Sin(j1)) / 2
b1 = 75 + 100 * Sin(j1)
c1 = 200 * Cos(j1)
a22 = -Sqr(3) * (150 + 200 * Sin(j2)) / 2
b2 = 75 + 100 * Sin(j2)
c2 = 200 * Cos(j2)
a33 = 0
b3 = 150 + 200 * Sin(j3)
c3 = 200 * Cos(j3)
k1 = a11 - a22: l1 = b1 - b2: p1 = c1 - c2
k2 = a11 - a33: l2 = -b1 - b3: p2 = c1 - c3
d2 = (k1 ^ 2 + l1 ^ 2 + p1 ^ 2) / 2: d3 = (k2 ^ 2 + l2 ^ 2 + p2 ^ 2) / 2
m1 = (d2 * l2 + l1 * d3) / (k1 * l2 + k2 * l1): n1 = -(p1 * l2 + l1 * p2) / (k1 * l2 + k2 * l1)
m2 = (d2 * k2 - d3 * k1) / (k1 * l2 + k2 * l1): n2 = (k1 * p2 - k2 * p1) / (k1 * l2 + k2 * l1)
g = (-2 * (m1 * n1 + m2 * n2) - Sqr((2 * m1 * n1 + 2 * m2 * n2) ^ 2 - 4 * (n1 ^ 2 + n2 ^ 2 + 1) * (m1 ^ 2 + m2 ^ 2 - 600 ^ 2))) / (2 * (n1 ^ 2 + n2 ^ 2 + 1))
e = m1 + n1 * g
zj_x = a11 - e '正解结果
End Function
Public Function zj_y(ByVal j1 As Double, ByVal j2 As Double, ByVal j3 As Double) As Double '''Y轴正解函数
Dim a11, a22, a33, b1, b2, b3, c1, c2, c3 As Double
Dim e, f, g As Double
Dim k1, k2, l1, l2, p1, p2 As Double
Dim d2, d3 As Double
Dim m1, m2, n1, n2 As Double
'''计算过渡值
a11 = Sqr(3) * (150 + 200 * Sin(j1)) / 2
b1 = 75 + 100 * Sin(j1)
c1 = 200 * Cos(j1)
a22 = -Sqr(3) * (150 + 200 * Sin(j2)) / 2
b2 = 75 + 100 * Sin(j2)
c2 = 200 * Cos(j2)
a33 = 0
b3 = 150 + 200 * Sin(j3)
c3 = 200 * Cos(j3)
k1 = a11 - a22: l1 = b1 - b2: p1 = c1 - c2
k2 = a11 - a33: l2 = -b1 - b3: p2 = c1 - c3
d2 = (k1 ^ 2 + l1 ^ 2 + p1 ^ 2) / 2: d3 = (k2 ^ 2 + l2 ^ 2 + p2 ^ 2) / 2
m1 = (d2 * l2 + l1 * d3) / (k1 * l2 + k2 * l1): n1 = -(p1 * l2 + l1 * p2) / (k1 * l2 + k2 * l1)
m2 = (d2 * k2 - d3 * k1) / (k1 * l2 + k2 * l1): n2 = (k1 * p2 - k2 * p1) / (k1 * l2 + k2 * l1)
g = (-2 * (m1 * n1 + m2 * n2) - Sqr((2 * m1 * n1 + 2 * m2 * n2) ^ 2 - 4 * (n1 ^ 2 + n2 ^ 2 + 1) * (m1 ^ 2 + m2 ^ 2 - 600 ^ 2))) / (2 * (n1 ^ 2 + n2 ^ 2 + 1))
f = m2 + n2 * g
zj_y = b1 - f '正解结果
End Function
Public Function zj_z(ByVal j1 As Double, ByVal j2 As Double, ByVal j3 As Double) As Double '''Z轴正解函数
Dim a11, a22, a33, b1, b2, b3, c1, c2, c3 As Double
Dim e, f, g As Double
Dim k1, k2, l1, l2, p1, p2 As Double
Dim d2, d3 As Double
Dim m1, m2, n1, n2 As Double
'''计算过渡值
a11 = Sqr(3) * (150 + 200 * Sin(j1)) / 2
b1 = 75 + 100 * Sin(j1)
c1 = 200 * Cos(j1)
a22 = -Sqr(3) * (150 + 200 * Sin(j2)) / 2
b2 = 75 + 100 * Sin(j2)
c2 = 200 * Cos(j2)
a33 = 0
b3 = 150 + 200 * Sin(j3)
c3 = 200 * Cos(j3)
k1 = a11 - a22: l1 = b1 - b2: p1 = c1 - c2
k2 = a11 - a33: l2 = -b1 - b3: p2 = c1 - c3
d2 = (k1 ^ 2 + l1 ^ 2 + p1 ^ 2) / 2: d3 = (k2 ^ 2 + l2 ^ 2 + p2 ^ 2) / 2
m1 = (d2 * l2 + l1 * d3) / (k1 * l2 + k2 * l1): n1 = -(p1 * l2 + l1 * p2) / (k1 * l2 + k2 * l1)
m2 = (d2 * k2 - d3 * k1) / (k1 * l2 + k2 * l1): n2 = (k1 * p2 - k2 * p1) / (k1 * l2 + k2 * l1)
g = (-2 * (m1 * n1 + m2 * n2) - Sqr((2 * m1 * n1 + 2 * m2 * n2) ^ 2 - 4 * (n1 ^ 2 + n2 ^ 2 + 1) * (m1 ^ 2 + m2 ^ 2 - 600 ^ 2))) / (2 * (n1 ^ 2 + n2 ^ 2 + 1))
zj_z = g - c1 + 646.379 '正解结果
End Function
3.PT 运动模式
运动控制卡的PT 运动模式：PT 模式使用一系列“位置、时间”数据点描述速度规划，能够实现任意速度规划。

PT 运动速度曲线如下图。

图4.2 PT 运动速度曲线
Fig 4.2 PT Movement speed curve
整个速度曲线被分割成5段，第1段起点速度为0，经过时间T1运动位移P1，因此第1段的终点速度为121v 1P T =
；第2段起点速度为1v ，经过时间T2运动位移P2，因此第2段的终点速度为2122v 1
P v T =-；第3、4、5段依次类推[6]。

只需要给出每段所需时间和位移，运动控制器会计算段内各点的速度和位置，生成一条连续的速
度曲线。

为了得到光滑的速度曲线，可以增加分割段数。

在描述一次完整的PT运动时，第1段的起点位置和时间被假定为0，各段的终点位置和时间都是相对于第1段起点的绝对值[6]。

位置的单位为脉冲，时间单位为毫秒。

(三)系统运行调试
调试的程序为： G01，X0，Y0，Z50
G01，X100，Y0，Z50
G02，X100，Y0，Z50，I50，J0，K0
1.插补前轨迹显示
图4.3插补前轨迹图
Fig4.3 before interpolating
2.插补效果图
图4.4插补效果图
Fig4.4 after interpolating。