李進(jìn)文，何素梅，吳海彬

(福州大學(xué)機械工程與自動化學(xué)院，福建福州350108)

0 引 言

工業(yè)機器人在搬運、碼垛、焊接、上下料等場合的應(yīng)用越來越普遍，技術(shù)越來越成熟。很多經(jīng)濟型工業(yè)機器人普遍選用通用的運動控制型PLC 作為機器人控制器。運動控制型PLC 有專門的高速脈沖輸出通道，可以通過對電機驅(qū)動器輸出脈沖實現(xiàn)單軸的運動控制。通常情況下，當(dāng)機器人需要按照規(guī)劃的直線或曲線軌跡進(jìn)行運動時，機器人控制器需要具備插補功能。在軌跡跟蹤精度要求不是很高的前提下，可以用直線插補替代圓弧插補實現(xiàn)曲線軌跡的運動，因此，直線插補是機器人實現(xiàn)插補運動最基本最重要的方法。有些運動控制型PLC 只提供單軸或多軸高速脈沖輸出功能，無插補指令;少數(shù)PLC 提供直線插補指令，但PLC 自帶的插補指令，其插補軸數(shù)一般為兩軸，并且這兩軸均為直線運動關(guān)節(jié)，即兩軸間插補的脈沖比例固定不變。而對于直線運動關(guān)節(jié)和轉(zhuǎn)動關(guān)節(jié)、或轉(zhuǎn)動關(guān)節(jié)與轉(zhuǎn)動關(guān)節(jié)之間的插補，這樣的插補指令并不能使用。

針對以上問題，有研究者開發(fā)了一套直線插補或者圓弧插補程序［1-4］，但所用方法并沒有充分利用PLC內(nèi)置的脈沖輸出指令，插補脈沖頻率低、插補實時性差。通過采用專用的定位模塊也可以實現(xiàn)直線插補［5］，雖然提高了插補脈沖頻率，但大大增加了系統(tǒng)成本。有相關(guān)文獻(xiàn)提出利用PLC 本身內(nèi)置的脈沖輸出指令［6-7］，開發(fā)高速直線插補程序，但是其只適用于插補軸為兩軸、插補的脈沖比例固定不變的場合。

針對以上問題，本研究提出了一種基于等時間間隔直線插補算法，該算法可以在任意結(jié)構(gòu)形式的工業(yè)機器人上實現(xiàn)任意軸數(shù)的空間直線插補，適用于任意低成本的機器人控制器。

1 直線插補算法

基于等時間間隔的空間直線插補，就是在一個確定的時間間隔(也稱為插補周期)ΔT 內(nèi)完成一次插補計算，為各坐標(biāo)方向的運動提供一組數(shù)據(jù)。機器人控制器按照插補計算得到的數(shù)據(jù)，控制機器人各關(guān)節(jié)同時完成一次微小運動，從而使機器人執(zhí)行器末端經(jīng)過規(guī)劃的插補點［8-10］。

1．1 插補原理

機器人末端執(zhí)行器在笛卡爾坐標(biāo)系中的描述，包括位置描述與姿態(tài)描述，因此其插補算法中也包括位置插補與姿態(tài)插補。姿態(tài)插補一般采取線性方式，即把末端執(zhí)行器在曲線上的終點和起點的方位差均勻地分配到插補的每一步，算法簡單，本研究不作討論。位置插補，就是求出在一個插補周期內(nèi)，機器人末端執(zhí)行器從當(dāng)前位置(Xi，Yi，Zi)沿插補直線方向截取線段長ΔL(ΔL 取決于插補速度V、加減速時間Ta)后，所到達(dá)的下一個插補點位置(Xi+1，Yi+1，Zi+1)。

1．2 插補遞推公式

已知空間插補直線的起點坐標(biāo)P1(x1，y1，z1)和終點坐標(biāo)P2(x2，y2，z2)，其中P1，P2為相對于基座坐標(biāo)系的坐標(biāo)點，可以通過示教得到。設(shè)V 為沿插補直線方向的運動速度，ΔT 為插補周期，Ta為插補運動的加減速時間。

基于此可以求出直線軌跡長度:

(1)插補直線的長度L:

(2)插補總步數(shù)N:不考慮加減速的情況，可得理論插補步數(shù)N0:

實際上，考慮到機械臂的慣性以及高速脈沖輸出通道對初始脈沖頻率的限制，應(yīng)該在插補直線的首尾分別設(shè)置加、減速段，因此實際的插補總步數(shù)應(yīng)大于理論插補步數(shù)。由插補運動的加減速時間Ta可求得笛卡爾坐標(biāo)系下的加減速度a:

加減速段的插補步數(shù)Na:

從而，得到實際插補總步數(shù)N:

(3)確定插補點與插補起點的距離Si:根據(jù)加減速特性的相關(guān)參數(shù)可計算出任意插補點Ni(i=1～N)與插補起點P1的距離Si:

(4)確定插補點Ni的坐標(biāo):插補起點P1(x1，y1，z1)與Si在笛卡爾坐標(biāo)系三坐標(biāo)軸方向上的分量疊加，即可得到各插補點在笛卡爾坐標(biāo)系下的坐標(biāo)(Xi，Yi，Zi):

式中:ΔX=x2－x1;ΔY=y2－y1;ΔZ=z2－z1。

由插補遞推過程知，插補點總在插補直線上，算法沒有累積誤差。求得各插補點的坐標(biāo)后，再通過機器人運動學(xué)逆解，即可求得各插補點對應(yīng)的關(guān)節(jié)變量。

對于不同的控制器，特別是PLC，由于存儲空間有限，其所能存儲的插補點數(shù)據(jù)是有限的。上述算法中插補周期ΔT 可以根據(jù)控制器的存儲空間以及插補直線的數(shù)量進(jìn)行選擇，從而可以降低對控制器的性能要求。

2 插補算法的應(yīng)用

以實驗室自行研發(fā)的四關(guān)節(jié)沖床上下料機器人為研究平臺，探討并驗證上述算法。該機器人結(jié)構(gòu)示意圖和實物圖如圖1 所示。

圖1 四關(guān)節(jié)沖床上下料機器人

利用D-H 法建立機器人坐標(biāo)系，可得到機器人末端執(zhí)行器的正運動學(xué)方程為:

逆運動學(xué)方程為:

由于關(guān)節(jié)4 的運動只對機器人末端執(zhí)行器的姿態(tài)產(chǎn)生影響，這里不予討論。

2．1 插補算法在PLC 上的實現(xiàn)

機器人控制器選用松下FP-X C60T 運動控制型PLC，其內(nèi)置兩個頻率范圍1．5 Hz～100 kHz 和兩個頻率范圍1．5 Hz～20 kHz 的脈沖輸出通道，具有強大的浮點和三角函數(shù)運算功能，非常適合算法編程［11］。PLC 上的直線插補程序流程圖如圖2 所示。以下通過流程圖2 重點說明插補算法在PLC 上的實現(xiàn)。

插補程序采用絕對值式脈沖輸出。插補運動開始前先建立數(shù)據(jù)表，數(shù)據(jù)表結(jié)構(gòu)如圖3 所示。數(shù)據(jù)表由連續(xù)的雙字組成，分別是控制代碼、頻率、目標(biāo)脈沖值及脈沖輸出停止標(biāo)志組成。其中控制代碼以位為單位使用，包括占空比、頻率范圍選擇、脈沖輸出模式、脈沖輸出方式等。

圖2 PLC 上的直線插補程序流程圖

圖3 PLC 的數(shù)據(jù)表結(jié)構(gòu)

本研究在建立數(shù)據(jù)表前，先求出只需計算1 次的初始數(shù)據(jù)，為計算插補點做準(zhǔn)備。之后，根據(jù)插補步數(shù)選擇加速段—勻速段—減速段距離，獲得插補點坐標(biāo)。通過機器人運動學(xué)逆解，即可求得各插補點對應(yīng)的關(guān)節(jié)變量，進(jìn)而求得插補點及每一步對應(yīng)的各關(guān)節(jié)目標(biāo)脈沖和脈沖頻率。控制數(shù)據(jù)表每一組頻率及目標(biāo)脈沖值對應(yīng)一個插補點。插補計算結(jié)束時，建立運動控制數(shù)據(jù)表，并同時啟動各通道脈沖輸出，直到所插補直線脈沖輸出完成，結(jié)束插補程序。

上述插補程序約800 步，PLC 平均掃描時間為25 ms。

2．2 誤差的定性分析

2．2．1 插補程序造成的誤差

由于機器人結(jié)構(gòu)的關(guān)系，求得各插補點坐標(biāo)進(jìn)而求取各插補點及每一步對應(yīng)的各關(guān)節(jié)目標(biāo)脈沖和脈沖頻率時，可能得到某兩點之間的目標(biāo)脈沖值相等的情況(即［S+2(N+1)］=［S+2(N +3)］)。此時，若不對目標(biāo)脈沖值進(jìn)行修正，會造成實際輸出脈沖數(shù)量與指定脈沖數(shù)值不同。本研究所采取的修正方法為:當(dāng)出現(xiàn)上一插補點的目標(biāo)脈沖值與當(dāng)前插補點的目標(biāo)脈沖值相等時，將當(dāng)前插補點的目標(biāo)脈沖值加1，使其與上一插補點的目標(biāo)脈沖值不一致。設(shè)當(dāng)前插補點目標(biāo)脈沖值修正后對應(yīng)的坐標(biāo)為N'i(X'i，Y'i，Z'i)，修正后各關(guān)節(jié)變量與修正前各關(guān)節(jié)變量差值Δθ1、Δd2、Δd3(即Δθ1=δ1，Δd2=δ2，Δd3=δ3)。此時，插補點定位誤差δi:

筆者所研究的機器人，各關(guān)節(jié)脈沖當(dāng)量分別為δ1=0．002 5 rad、δ2=0．025 mm、δ3=0．013 mm，將上述數(shù)據(jù)帶代入式(10)，得到修正后插補點定位誤差δi≈6．25 ×10－4mm，可以滿足實際使用。

2．2．2 最低脈沖頻率造成的誤差

由于PLC 硬件上最低脈沖頻率的限制，求得的兩插補點間對應(yīng)的各關(guān)節(jié)脈沖頻率可能低于允許最低脈沖頻率，此時PLC 會以最低脈沖頻率進(jìn)行脈沖輸出，這樣，在某個插補點的運動上一個關(guān)節(jié)先于另一個關(guān)節(jié)完成脈沖輸出，造成插補誤差。最低脈沖頻率造成的誤差如圖4 所示。粗實線是理想的插補直線，虛線是為滿足最低脈沖頻率限制要求得到的實際插補直線。

圖4 最低脈沖頻率造成的誤差

3 實驗及結(jié)果分析

為驗證插補算法在自行研發(fā)的四關(guān)節(jié)沖床上下料機器人的應(yīng)用效果，本研究設(shè)計實驗如下:機器人以起點P1(－926． 57，535． 03，50)，終點P2(－1 144．43，659．60，186．60)做直線插補，其中:V =240 mm/s，ΔT =50 ms、Ta=200 ms，實際插補軌跡如圖5 所示。由于機器人在運動過程中、特別是在加減速階段存在一定的抖動情況，以及傳動系統(tǒng)少許的間隙，致使機器人在實際運動過程中末端操作器的軌跡偏差還會更大一些。

圖5 插補直線的實際軌跡

插補過程中，每10 ms 采樣一次PLC 各脈沖通道輸出的絕對脈沖值，結(jié)合機器人結(jié)構(gòu)，得到插補直線采樣點的空間坐標(biāo)如圖6 所示。從圖6 可以看出，空間上插補直線可以比較好的跟蹤理論直線軌跡。

圖6 插補直線與理論直線的空間重合度

為進(jìn)一步定量分析插補直線對理論直線的跟蹤精度，以插補直線為橫坐標(biāo)，插補直線與理論直線對應(yīng)采樣點的距離為縱坐標(biāo)，作圖得到插補直線對理論直線的跟蹤精度如圖7 所示。跟蹤誤差小于0．3 mm，滿足機器人直線插補的精度要求。雖然插補直線不能與理論直線完全重合，但其基本與理論直線平行，如圖6 中插補直線與理論直線的局部放大圖所示。如果需要提高直線插補跟蹤精度，則需要減小插補周期，增加插補點數(shù)，但這反而會提高對控制器的性能要求。

圖7 插補直線對理論直線的跟蹤精度

4 結(jié)束語

本研究針對工業(yè)機器人在笛卡爾坐標(biāo)系中的空間直線軌跡規(guī)劃問題，提出了一種基于等時間間隔的直線插補算法。該算法可以在任意結(jié)構(gòu)形式的工業(yè)機器人上實現(xiàn)任意軸數(shù)的空間直線插補，對于有直線插補要求、但跟蹤精度要求不高的應(yīng)用場合，可以通過適當(dāng)增加插補周期、減少插補點數(shù)的方法降低控制器的硬件配置，從而降低機器人系統(tǒng)成本。所述算法已成功應(yīng)用在自行研發(fā)的四關(guān)節(jié)沖床上下料機器人上，實際應(yīng)用表明了該直線插補算法的有效性，具有一定的應(yīng)用價值。

今后將通過其他機器人平臺來進(jìn)一步驗證提出的直線粗插補算法的有效性。同時，考慮到圓弧插補算法在機器人曲線軌跡規(guī)劃中應(yīng)用的普遍性，今后將進(jìn)一步探討圓弧插補算法及其在低配置控制器上的實現(xiàn)。

［1］張鐵異，黃柄瓊，盧福寧，等．基于松下電工FPl 型PLC 的直線插補程序設(shè)計［J］．裝備制造技術(shù)，2007(10):68-70．

［2］黃菊生，龔慶壽，夏 平，等． 數(shù)控平臺PLC 控制系統(tǒng)的開發(fā)［J］．機床與液壓，2005(4):56-58．

［3］孫建仁．步進(jìn)電機的PIE 控制系統(tǒng)設(shè)計［J］．機械研究與應(yīng)用，2001(4):23-26．

［4］趙燕偉，朱雙勝，盧 東，等．基于直線插補的字體邊緣識別技術(shù)［J］．機電工程，2009，26(5):52-54．

［5］譚懷亮，劉 曉，賀再紅．基于三菱PLC 的刨床數(shù)控系統(tǒng)［J］．機械與電子，1999(6):51-53．

［6］任善巖． PLC 直線插補方法的研究［J］． 機床與液壓，2009，37(9):56-58．．

［7］葉伯生．計算機數(shù)控系統(tǒng)原理、編程與操作［M］．武漢:華中理工大學(xué)出版社，1999．

［8］金建新．機床CNC 系統(tǒng)中任意空間曲線的可控步長插補方法［J］．機械工程學(xué)報，2002(4):95-97．

［9］ADACHI H． Line interpolation apparatus and line interpolation method:U．S． Patent 6，636，267［P］．2003-10-21．

［10］席港港，趙慶志，王 軍，等．傳統(tǒng)逐點比較法直線插補方法的改進(jìn)［J］．山東理工大學(xué)學(xué)報:自然科學(xué)版，2012，26(1):67-69．

［11］Matsushita Electric Works，Ltd．． FP Series Programming Manual［Z］．Matsushita Electric Works，Ltd．，2012．