一種直線(xiàn)插補(bǔ)算法及其在機(jī)器人中的應(yīng)用研究*

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

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

0 引 言

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

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

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

1 直線(xiàn)插補(bǔ)算法

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

1．1 插補(bǔ)原理

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

1．2 插補(bǔ)遞推公式

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

基于此可以求出直線(xiàn)軌跡長(zhǎng)度:

(1)插補(bǔ)直線(xiàn)的長(zhǎng)度L:

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

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

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

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

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

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

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

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

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

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

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

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

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

逆運(yùn)動(dòng)學(xué)方程為:

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

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

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

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

圖2 PLC 上的直線(xiàn)插補(bǔ)程序流程圖

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

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

上述插補(bǔ)程序約800 步，PLC 平均掃描時(shí)間為25 ms。

2．2 誤差的定性分析

2．2．1 插補(bǔ)程序造成的誤差

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

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

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

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

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

3 實(shí)驗(yàn)及結(jié)果分析

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

圖5 插補(bǔ)直線(xiàn)的實(shí)際軌跡

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

圖6 插補(bǔ)直線(xiàn)與理論直線(xiàn)的空間重合度

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

圖7 插補(bǔ)直線(xiàn)對(duì)理論直線(xiàn)的跟蹤精度

4 結(jié)束語(yǔ)

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

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

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

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

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

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

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

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

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

［8］金建新．機(jī)床CNC 系統(tǒng)中任意空間曲線(xiàn)的可控步長(zhǎng)插補(bǔ)方法［J］．機(jī)械工程學(xué)報(bào)，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)逐點(diǎn)比較法直線(xiàn)插補(bǔ)方法的改進(jìn)［J］．山東理工大學(xué)學(xué)報(bào):自然科學(xué)版，2012，26(1):67-69．

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