Delta并联机器人的插补研究

1、第三章 Delta并联机器人的插补研究（一）引言数控系统从一条已知起点和终点的曲线上自动进行“数据点密化”的工作，将连续的轨迹细化为微小的直线段，这就是插补。根据加工程序中有限的坐标点，运用相应的算法，在已知的坐标点之间产生一系列的坐标点数据，再将数据点转化为相应的脉冲，从而在加工时对各坐标轴进行脉冲分配，使机床加工出符合要求的轮廓曲线。（二）插补原理分析目前普遍应用的插补算法可分为两大类：基准脉冲插补和数据采样插补。基准脉冲插补又称脉冲增量插补或行程标量插补。该插补算法的特点是每进行一次插补运算就会产生一个坐标增量，最后再以脉冲数的方式输出给驱动电机。基准脉冲插补在插补计算过程中不断向各个驱

2、动轴发送进给脉冲，从而实现预定轨迹的插补。基准脉冲插补方法有：逐点比较法、数字积分法、比较积分法、矢量判断法、最小偏差法、数字脉冲乘法器法等5。由于只有加法和移位，因此基准脉冲插补的实现相对比较简单。数据采样插补又称为时间标量插补或数字增量插补。该插补算法的特点是数控装置产生的不是单个脉冲，而是标准的二进制数字。其采用的是时间分割的方法，根据程序的进给速度，将轮廓曲线分割为采样周期性的进给步长，即用弦线或割线逼近轮廓轨迹。通常有以下几种方法：直线函数法、扩展数字积分法、二阶递归扩展数字积分插补法、双数字积分插补法5。逐点比较法的原理是：计算机在加工轨迹过程中，采用逐点计算进给步长和判别加工偏差

3、以控制坐标进给方向，从而按规定的轨迹加工出合格的零件。这种插补方法的特点在于机床每走一步都要进行四个步骤，即：第一步，偏差判别：判别加工点对轨迹的偏差位置，然后下一步进给的方向；第二步，进给：控制工作台进给一步，加工的轨迹向规定的轨迹靠拢，缩小偏差；第三步，偏差计算：计算下一个加工点相对轨迹的偏差，为一下步的方向判别做准备；第四步，终点判断：判断是否到达程序规定的加工终点。如到达终点，则停止插补，否则再回到第一步。如此不断的重复，就能加工出的轮廓。（三）直线插补1.平面直线插补第一象限逐点比较法直线插补：图3.1 逐点比较法直线插补Fig3.1 Point by point compariso

4、n method linear interpolation插补规则：当F0时，则沿+X方向进给一步；当F0时，则沿+Y方向进给一步。偏差判别函数：当M在OA上，即F=0时，有：，当M在OA上方，即F0时，有，当M在OA下方，即F,且,均为正数。以X轴为基准轴，建立两个平面坐标系，如图3.3所示。直线为OA在XY坐标平面内的投影，直线为OA在XZ坐标平面内的投影。在XY平面上，偏差计算公式为： 同理，在XZ坐标平面上，偏差计算公式为： 终点判断方法：有了偏差计算公式，现在关键是如何判别和计算下一步进给的方向。我们规定：（1）若，沿X轴正方向发一进给脉冲，由于使加工点+1，故必须在XOY坐标平面和

5、XOZ坐标平面都必须进行偏差计算，即和。 （2）只要，沿Y轴正方向发一进给脉冲，由于是使加工点+1，故只需在XOY坐标平面内进行偏差计算，即。 （3）若，由于使加工点+1，故只需在XOZ坐标平面内进行偏差计算，即。同样，在每次计算偏差和发送进给脉冲的同时，还要进行一次终点判断，以确定是否达到终点。如果达到了终点，则停止插补。 在上述空间直线插补时，首先假设了该段空间直线的X方向进给步数为最大,就是说在以长轴为基轴建立的两平面坐标系内进行的插补运算，才是正确的。因此每次进行插补运算之前，都必须对三个坐标的增量值进行比较和转换，将增量值较大的进行X方向的插补，并设置相应的标志位进行变换。其他象限的

6、插补运算也可以用类似的方法进行变换。即在进入插补程序前，三个坐标增量都以绝对值方式寄存在相应的寄存器中，并根据增量值的正负设定各驱动轴的方向标志位。（四）平面圆弧插补逐点比较法第一象限逆时针圆弧插补基本原理如下：在加工圆弧过程中，使用动点到圆心的距离与该圆弧的半径进行比较来反映加工偏差，以确定下一步插补的方向。假设被加工零件的轮廓为第一象限逆时针走向圆弧SE，圆心在O（0，0），半径为R，起点为S，终点为E，圆弧上任意加工动点为N。通过比较动点到圆心的距离与圆弧半径R的大小，可获得末端参考点与圆弧轮廓之间的位置偏差，从而可以确定下一步的插补方向。当动点N正好落在圆弧上时，则有下式成立当动点N落

7、在圆弧外侧时，则有下式成立当动点N落在圆弧内侧时，则有下式成立由此可见，取逐点比较法圆弧插补的偏差函数表达式为 当动点落在圆外时，为了是加工的轨迹逼近要求的轮廓，减小加工误差，应是动点向圆内进给一步，即向X轴负方向前进一步；当动点落在圆内时，应使动点向圆外进给，即向Y轴正方向前进一步。当动点正好落在圆弧上且未到达终点时，为了使加工继续下去，理论上向Y轴正或X轴负方向进给都可以，但一般情况下约定向X轴负方向进给。 综上所述，现将逐点比较法第一象限逆时针圆插补规则概括如下： 当时，即，动点落在圆外，则向X轴负方向进给一步； 当时，即，动点正好落在圆上，约定向X轴负方向进给一步； 当时，即，动点落在

8、圆内，则向轴正方向进给一步。 由偏差函数表达式可知，要想计算偏差F值，就必须对动点坐标、圆弧半径进行平方运算。为了简化运算过程，按逐点比较法直线插补的思路，可以推导出逐点比较法圆弧插补过程中偏差函数计算的递推公式。假设第i次插补后，动点坐标为N，其对应偏差函数为当，向X轴负方向进给一步，则新的动点坐标值为， 因此，新的偏差函数为 即： 同理，当，则向Y轴正方向进给一步，则新的动点坐标值为，因此，可求得新的偏差函数为即： 将上式进行比较，可以看出两点不同：第一，递推形式的偏差计算公式中仅有加、减法以及乘法运算，而乘以2可等效为该二进制数左移一位，这显然比平方运算的运算量小得多；第二，进给后新的偏

9、差函数值与前一点的偏差值以及动点坐标N之间可以建立关系式。由于动点坐标值随着插补运算的进行而不断变化，因此，每插补一次，动点坐标就必须根据偏差值修正一次，为下一步的偏差计算做准备。至此，将第一象限逆圆弧插补的规则和计算公式汇总，见表3.1。偏差函数动点位置进给方向新偏差计算动点坐标修正在圆上或圆外-X，在圆内Y，表3.1Table 3.1和直线插补一样，圆弧插补过程也需要进行终点判别。当圆弧轮廓仅在一个象限区域内，其终点判别可用直线终点判别的方法进行判别，只是计算公式略有不同。 ， 式中 、 被插补圆弧轮廓的起点坐标；、 被插补圆弧轮廓的终点坐标。可根据上述方法推导出其他象限不同走向圆弧的插补

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

11、Y方式对X坐标进行插补，用X方式对Y坐标进行插补，那么就可得到的走向。类似地通过改变插补进给方向，利用的计算公式就可得到其他三种线型、的插补。（五）本章小结本章主要论述了平面直线、空间直线和平面圆弧的逐点比较法插补。通过对逐点比较法的偏差函数、终点判别等的研究，得到了平面直线、空间直线和平面圆弧逐点比较法插补方法，为后续的编程及运动控制打下了基础。第四章 Delta并联机器人插补算法编程及验证（一）引言并联机器人的控制系统是并联机器人设计的基本内容，也是并联机器人发展研究的重点。本章将利用Visual Basic软件与运动控制卡相结合，编写一个简单的控制系统，实现对并联机器人的直线、平面圆弧运

12、动的控制。运动控制卡采用固高科技公司的GTS-400-PV-PCI-G，该运动控制卡提供了Visual Basic等语言的函数库和Windows动态链接库，可实现复杂的控制功能6。（二）控制软件系统设计验证软件系统设计：界面上包括空间直线、平面圆弧插补以及插补轨迹的平面显示；程序上包括正解、反解模块、初始化运动控制卡及PT运动模式。设计该系统的主要目的是验证第三章的插补算法。插补步长为0.01mm。使用Visual Basic编写的控制软件，界面如图4.1所示。图4.1控制软件界面 Control system main interface1.系统的反解模块反解模块是Delta控制系统必不可少

13、的一个部分，也是核心部分。反解模块的作用是在主动臂的转角与末端机构的坐标值之间建立一种转化的关系，即在已知末端机构坐标值的情况下求主动臂的转角。通过反解模块可以实现通过伺服电机的脉冲对末端机构的运动进行控制。反解公式在第二章中已经求解出。其具体的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 -

14、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 -

15、 4 * U1 * W1) / (2 * U1) a1 = 2 * Atn(t1) * 180 / 3.14 - 50.007 相对于零点的角度 fj_x = CDbl(Format(a1 * 277.778, 0.000) 将角度转化为脉冲End FunctionPublic 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.37

16、9 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 * U

17、2 * W2) / (2 * U2) a2 = 2 * Atn(t2) * 180 / 3.14 - 50.007 fj_y = CDbl(Format(a2 * 277.778, 0.000) 将角度转化为脉冲End FunctionPublic 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 =

18、 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 -

19、 50.007 fj_z = CDbl(Format(a3 * 277.778, 0.000) 将角度转化为脉冲End Function2.系统的正解模块正解模块是反解模块的逆运算，即通过主动臂的转角去求解出末端机构的坐标。在控制系统中，正解模块可以用于对系统工作空间范围的判断及运动轨迹的显示。在执行运动前，判断终点是否超出规定的空间范围，从而建立相应的报警机制，实现对整个系统的保护作用。正解的求解方法在第二章中已经给出。具体的Visual Basic程序代码如下：Public Function zj_x(ByVal j1 As Double, ByVal j2 As Double, ByVa

20、l 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) * (

21、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 *

22、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 =

23、m1 + n1 * g zj_x = a11 - e 正解结果End FunctionPublic 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 Doubl

24、e 计算过渡值 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:

25、 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) - Sq

26、r(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 FunctionPublic 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

27、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)