Delta并聯(lián)機器人的插補研究

1、第三章 Delta并聯(lián)機器人的插補研究（一）引言數(shù)控系統(tǒng)從一條已知起點和終點的曲線上自動進行“數(shù)據(jù)點密化”的工作，將連續(xù)的軌跡細(xì)化為微小的直線段，這就是插補。根據(jù)加工程序中有限的坐標(biāo)點，運用相應(yīng)的算法，在已知的坐標(biāo)點之間產(chǎn)生一系列的坐標(biāo)點數(shù)據(jù)，再將數(shù)據(jù)點轉(zhuǎn)化為相應(yīng)的脈沖，從而在加工時對各坐標(biāo)軸進行脈沖分配，使機床加工出符合要求的輪廓曲線。（二）插補原理分析目前普遍應(yīng)用的插補算法可分為兩大類：基準(zhǔn)脈沖插補和數(shù)據(jù)采樣插補。基準(zhǔn)脈沖插補又稱脈沖增量插補或行程標(biāo)量插補。該插補算法的特點是每進行一次插補運算就會產(chǎn)生一個坐標(biāo)增量，最后再以脈沖數(shù)的方式輸出給驅(qū)動電機?；鶞?zhǔn)脈沖插補在插補計算過程中不斷向各個驅(qū)

2、動軸發(fā)送進給脈沖，從而實現(xiàn)預(yù)定軌跡的插補?；鶞?zhǔn)脈沖插補方法有：逐點比較法、數(shù)字積分法、比較積分法、矢量判斷法、最小偏差法、數(shù)字脈沖乘法器法等5。由于只有加法和移位，因此基準(zhǔn)脈沖插補的實現(xiàn)相對比較簡單。數(shù)據(jù)采樣插補又稱為時間標(biāo)量插補或數(shù)字增量插補。該插補算法的特點是數(shù)控裝置產(chǎn)生的不是單個脈沖，而是標(biāo)準(zhǔn)的二進制數(shù)字。其采用的是時間分割的方法，根據(jù)程序的進給速度，將輪廓曲線分割為采樣周期性的進給步長，即用弦線或割線逼近輪廓軌跡。通常有以下幾種方法：直線函數(shù)法、擴展數(shù)字積分法、二階遞歸擴展數(shù)字積分插補法、雙數(shù)字積分插補法5。逐點比較法的原理是：計算機在加工軌跡過程中，采用逐點計算進給步長和判別加工偏差

3、以控制坐標(biāo)進給方向，從而按規(guī)定的軌跡加工出合格的零件。這種插補方法的特點在于機床每走一步都要進行四個步驟，即：第一步，偏差判別：判別加工點對軌跡的偏差位置，然后下一步進給的方向；第二步，進給：控制工作臺進給一步，加工的軌跡向規(guī)定的軌跡靠攏，縮小偏差；第三步，偏差計算：計算下一個加工點相對軌跡的偏差，為一下步的方向判別做準(zhǔn)備；第四步，終點判斷：判斷是否到達(dá)程序規(guī)定的加工終點。如到達(dá)終點，則停止插補，否則再回到第一步。如此不斷的重復(fù)，就能加工出的輪廓。（三）直線插補1.平面直線插補第一象限逐點比較法直線插補：圖3.1 逐點比較法直線插補Fig3.1 Point by point compariso

4、n method linear interpolation插補規(guī)則：當(dāng)F0時，則沿+X方向進給一步；當(dāng)F0時，則沿+Y方向進給一步。偏差判別函數(shù)：當(dāng)M在OA上，即F=0時，有：，當(dāng)M在OA上方，即F0時，有，當(dāng)M在OA下方，即F,且,均為正數(shù)。以X軸為基準(zhǔn)軸，建立兩個平面坐標(biāo)系，如圖3.3所示。直線為OA在XY坐標(biāo)平面內(nèi)的投影，直線為OA在XZ坐標(biāo)平面內(nèi)的投影。在XY平面上，偏差計算公式為： 同理，在XZ坐標(biāo)平面上，偏差計算公式為： 終點判斷方法：有了偏差計算公式，現(xiàn)在關(guān)鍵是如何判別和計算下一步進給的方向。我們規(guī)定：（1）若，沿X軸正方向發(fā)一進給脈沖，由于使加工點+1，故必須在XOY坐標(biāo)平面和

5、XOZ坐標(biāo)平面都必須進行偏差計算，即和。 （2）只要，沿Y軸正方向發(fā)一進給脈沖，由于是使加工點+1，故只需在XOY坐標(biāo)平面內(nèi)進行偏差計算，即。 （3）若，由于使加工點+1，故只需在XOZ坐標(biāo)平面內(nèi)進行偏差計算，即。同樣，在每次計算偏差和發(fā)送進給脈沖的同時，還要進行一次終點判斷，以確定是否達(dá)到終點。如果達(dá)到了終點，則停止插補。 在上述空間直線插補時，首先假設(shè)了該段空間直線的X方向進給步數(shù)為最大,就是說在以長軸為基軸建立的兩平面坐標(biāo)系內(nèi)進行的插補運算，才是正確的。因此每次進行插補運算之前，都必須對三個坐標(biāo)的增量值進行比較和轉(zhuǎn)換，將增量值較大的進行X方向的插補，并設(shè)置相應(yīng)的標(biāo)志位進行變換。其他象限的

6、插補運算也可以用類似的方法進行變換。即在進入插補程序前，三個坐標(biāo)增量都以絕對值方式寄存在相應(yīng)的寄存器中，并根據(jù)增量值的正負(fù)設(shè)定各驅(qū)動軸的方向標(biāo)志位。（四）平面圓弧插補逐點比較法第一象限逆時針圓弧插補基本原理如下：在加工圓弧過程中，使用動點到圓心的距離與該圓弧的半徑進行比較來反映加工偏差，以確定下一步插補的方向。假設(shè)被加工零件的輪廓為第一象限逆時針走向圓弧SE，圓心在O（0，0），半徑為R，起點為S，終點為E，圓弧上任意加工動點為N。通過比較動點到圓心的距離與圓弧半徑R的大小，可獲得末端參考點與圓弧輪廓之間的位置偏差，從而可以確定下一步的插補方向。當(dāng)動點N正好落在圓弧上時，則有下式成立當(dāng)動點N落

7、在圓弧外側(cè)時，則有下式成立當(dāng)動點N落在圓弧內(nèi)側(cè)時，則有下式成立由此可見，取逐點比較法圓弧插補的偏差函數(shù)表達(dá)式為 當(dāng)動點落在圓外時，為了是加工的軌跡逼近要求的輪廓，減小加工誤差，應(yīng)是動點向圓內(nèi)進給一步，即向X軸負(fù)方向前進一步；當(dāng)動點落在圓內(nèi)時，應(yīng)使動點向圓外進給，即向Y軸正方向前進一步。當(dāng)動點正好落在圓弧上且未到達(dá)終點時，為了使加工繼續(xù)下去，理論上向Y軸正或X軸負(fù)方向進給都可以，但一般情況下約定向X軸負(fù)方向進給。 綜上所述，現(xiàn)將逐點比較法第一象限逆時針圓插補規(guī)則概括如下： 當(dāng)時，即，動點落在圓外，則向X軸負(fù)方向進給一步； 當(dāng)時，即，動點正好落在圓上，約定向X軸負(fù)方向進給一步； 當(dāng)時，即，動點落在

8、圓內(nèi)，則向軸正方向進給一步。 由偏差函數(shù)表達(dá)式可知，要想計算偏差F值，就必須對動點坐標(biāo)、圓弧半徑進行平方運算。為了簡化運算過程，按逐點比較法直線插補的思路，可以推導(dǎo)出逐點比較法圓弧插補過程中偏差函數(shù)計算的遞推公式。假設(shè)第i次插補后，動點坐標(biāo)為N，其對應(yīng)偏差函數(shù)為當(dāng)，向X軸負(fù)方向進給一步，則新的動點坐標(biāo)值為， 因此，新的偏差函數(shù)為 即： 同理，當(dāng)，則向Y軸正方向進給一步，則新的動點坐標(biāo)值為，因此，可求得新的偏差函數(shù)為即： 將上式進行比較，可以看出兩點不同：第一，遞推形式的偏差計算公式中僅有加、減法以及乘法運算，而乘以2可等效為該二進制數(shù)左移一位，這顯然比平方運算的運算量小得多；第二，進給后新的偏

9、差函數(shù)值與前一點的偏差值以及動點坐標(biāo)N之間可以建立關(guān)系式。由于動點坐標(biāo)值隨著插補運算的進行而不斷變化，因此，每插補一次，動點坐標(biāo)就必須根據(jù)偏差值修正一次，為下一步的偏差計算做準(zhǔn)備。至此，將第一象限逆圓弧插補的規(guī)則和計算公式匯總，見表3.1。偏差函數(shù)動點位置進給方向新偏差計算動點坐標(biāo)修正在圓上或圓外-X，在圓內(nèi)Y，表3.1Table 3.1和直線插補一樣，圓弧插補過程也需要進行終點判別。當(dāng)圓弧輪廓僅在一個象限區(qū)域內(nèi)，其終點判別可用直線終點判別的方法進行判別，只是計算公式略有不同。 ， 式中 、 被插補圓弧輪廓的起點坐標(biāo)；、 被插補圓弧輪廓的終點坐標(biāo)?？筛鶕?jù)上述方法推導(dǎo)出其他象限不同走向圓弧的插補

10、運算。如圖3.4所示：圖3.4 四象限圓弧插補進給方向Fig 3.4 Four quadrant circular arc interpolation direction of feed四象限圓弧插補計算如下表：線型偏差計算坐標(biāo)進給偏差計算坐標(biāo)進給 表3.2Table 3.2從表3.2可以看出，當(dāng)按第一象限逆時針圓進行插補運算時，若將X軸方向進給反向，則可以走出第二象限順時針圓來；或者將Y軸方向進給反向，則可以走出來；或者將X軸和Y軸的進給均反向，則可以走出來；并且這四種線型（、）使用的偏差計算公式都相同，無須改變。 還可以看出，當(dāng)按第一象限逆時針圓線型插補時，若將計算公式坐標(biāo)X與Y對調(diào)，即用

11、Y方式對X坐標(biāo)進行插補，用X方式對Y坐標(biāo)進行插補，那么就可得到的走向。類似地通過改變插補進給方向，利用的計算公式就可得到其他三種線型、的插補。（五）本章小結(jié)本章主要論述了平面直線、空間直線和平面圓弧的逐點比較法插補。通過對逐點比較法的偏差函數(shù)、終點判別等的研究，得到了平面直線、空間直線和平面圓弧逐點比較法插補方法，為后續(xù)的編程及運動控制打下了基礎(chǔ)。第四章 Delta并聯(lián)機器人插補算法編程及驗證（一）引言并聯(lián)機器人的控制系統(tǒng)是并聯(lián)機器人設(shè)計的基本內(nèi)容，也是并聯(lián)機器人發(fā)展研究的重點。本章將利用Visual Basic軟件與運動控制卡相結(jié)合，編寫一個簡單的控制系統(tǒng)，實現(xiàn)對并聯(lián)機器人的直線、平面圓弧運

12、動的控制。運動控制卡采用固高科技公司的GTS-400-PV-PCI-G，該運動控制卡提供了Visual Basic等語言的函數(shù)庫和Windows動態(tài)鏈接庫，可實現(xiàn)復(fù)雜的控制功能6。（二）控制軟件系統(tǒng)設(shè)計驗證軟件系統(tǒng)設(shè)計：界面上包括空間直線、平面圓弧插補以及插補軌跡的平面顯示；程序上包括正解、反解模塊、初始化運動控制卡及PT運動模式。設(shè)計該系統(tǒng)的主要目的是驗證第三章的插補算法。插補步長為0.01mm。使用Visual Basic編寫的控制軟件，界面如圖4.1所示。圖4.1控制軟件界面 Control system main interface1.系統(tǒng)的反解模塊反解模塊是Delta控制系統(tǒng)必不可少

13、的一個部分，也是核心部分。反解模塊的作用是在主動臂的轉(zhuǎn)角與末端機構(gòu)的坐標(biāo)值之間建立一種轉(zhuǎn)化的關(guān)系，即在已知末端機構(gòu)坐標(biāo)值的情況下求主動臂的轉(zhuǎn)角。通過反解模塊可以實現(xiàn)通過伺服電機的脈沖對末端機構(gòu)的運動進行控制。反解公式在第二章中已經(jīng)求解出。其具體的Visual Basic代碼如下：Public Function fj_x(ByVal x As Double, ByVal y As Double, ByVal z As Double) As Double x軸反解函數(shù)，用于求出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) 將角度轉(zhuǎn)化為脈沖End FunctionPublic Function fj_y(ByVal x As Double, ByVal y As Double, ByVal z As Double) As Double y軸反解函數(shù)，用于求出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) 將角度轉(zhuǎn)化為脈沖End FunctionPublic Function fj_z(ByVal x As Double, ByVal y As Double, ByVal z As Double) As Double z軸反解函數(shù)，用于求出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) 將角度轉(zhuǎn)化為脈沖End Function2.系統(tǒng)的正解模塊正解模塊是反解模塊的逆運算，即通過主動臂的轉(zhuǎn)角去求解出末端機構(gòu)的坐標(biāo)。在控制系統(tǒng)中，正解模塊可以用于對系統(tǒng)工作空間范圍的判斷及運動軌跡的顯示。在執(zhí)行運動前，判斷終點是否超出規(guī)定的空間范圍，從而建立相應(yīng)的報警機制，實現(xiàn)對整個系統(tǒng)的保護作用。正解的求解方法在第二章中已經(jīng)給出。具體的Visual Basic程序代碼如下：Public Function zj_x(ByVal j1 As Double, ByVal j2 As Double, ByVa

20、l j3 As Double) As Double X軸正解函數(shù) 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 正解結(jié)果End FunctionPublic Function zj_y(ByVal j1 As Double, ByVal j2 As Double, ByVal j3 As Double) As Double Y軸正解函數(shù) 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 正解結(jié)果End FunctionPublic Function zj_z(ByVal j1 As Double, ByVal j2 As Double, ByVal j3 As Double) As Double Z軸正解函數(shù) 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)