焊接機(jī)器人工具坐標(biāo)系標(biāo)定的研究與實(shí)現(xiàn)

康存鋒，王紅偉，張鵬飛，李舒進(jìn)，陳樹(shù)君

（北京工業(yè)大學(xué)機(jī)械與應(yīng)用電子技術(shù)學(xué)院，北京 100124）

焊接機(jī)器人工具坐標(biāo)系標(biāo)定的研究與實(shí)現(xiàn)

康存鋒，王紅偉，張鵬飛，李舒進(jìn)，陳樹(shù)君

（北京工業(yè)大學(xué)機(jī)械與應(yīng)用電子技術(shù)學(xué)院，北京 100124）

采用六點(diǎn)標(biāo)定法對(duì)工具坐標(biāo)系進(jìn)行標(biāo)定.首先標(biāo)定工具參數(shù)的位置向量，其次標(biāo)定工具坐標(biāo)系的姿態(tài)矩陣，其中工具中心點(diǎn)位置標(biāo)定采用線性最小二乘法的矩陣形式進(jìn)行求解，工具坐標(biāo)系姿態(tài)采用向量和矩陣的基本運(yùn)算進(jìn)行求解.針對(duì)RB_1400六自由度焊接機(jī)器人，基于開(kāi)放式軟件平臺(tái)CODESYS開(kāi)發(fā)了工具坐標(biāo)系標(biāo)定算法模塊和焊槍位姿變換模塊，最后以此為基礎(chǔ)移動(dòng)機(jī)器人對(duì)準(zhǔn)基準(zhǔn)點(diǎn)，測(cè)試分別繞x、y、z軸旋轉(zhuǎn)檢查T(mén)CP點(diǎn)的位置是否符合要求.結(jié)果表明：該方法確實(shí)可行，并且可以適用于其他類(lèi)型機(jī)器人的工具坐標(biāo)系標(biāo)定.

工具坐標(biāo)系；焊接機(jī)器人；標(biāo)定

機(jī)器人工具坐標(biāo)系（tool coordinate frame，TCF）標(biāo)定就是確定工具坐標(biāo)系相對(duì)于機(jī)器人末端連桿坐標(biāo)系的變換矩陣［1］.對(duì)于焊接機(jī)器人［2］，焊槍是完成焊接任務(wù)必不可少的工具，即使是同一批量的焊槍?zhuān)矔?huì)因?yàn)榧庸ふ`差導(dǎo)致其偏移量不完全相同，工具坐標(biāo)系的準(zhǔn)確度又直接影響機(jī)器人的軌跡精度，所以建立一種準(zhǔn)確、快速的標(biāo)定方法對(duì)機(jī)器人的應(yīng)用具有重要意義.目前國(guó)內(nèi)外工業(yè)機(jī)器人工具標(biāo)定主要有外部基準(zhǔn)法和多點(diǎn)標(biāo)定法.外部基準(zhǔn)法如FANUC機(jī)器人利用DynaCal系統(tǒng)進(jìn)行標(biāo)定，ABB弧焊機(jī)器人采用BullEyes系統(tǒng)工具自動(dòng)標(biāo)定方法，但這些依賴(lài)于外部基準(zhǔn).多點(diǎn)標(biāo)定法因其標(biāo)定簡(jiǎn)單被廣泛使用，李亮玉等［3］提出了六點(diǎn)標(biāo)定法，使用了VB6.0開(kāi)發(fā)了FANUC弧焊機(jī)器人離線標(biāo)定系統(tǒng)，但只是對(duì)標(biāo)定過(guò)程進(jìn)行了介紹；趙嬌嬌等［4］利用球心擬合法進(jìn)行TCP位置標(biāo)定.然而，沒(méi)有采用自動(dòng)化軟件工具CODESYS來(lái)實(shí)現(xiàn)完整的機(jī)器人工具標(biāo)定解決方案.

本文采用工具坐標(biāo)系的六點(diǎn)標(biāo)定法，工具中心點(diǎn)（tool center point，TCP）的位置標(biāo)定是使4個(gè)標(biāo)定點(diǎn)（前4點(diǎn)）TCP位置重合，從而計(jì)算出工具中心點(diǎn).TCF姿態(tài)標(biāo)定是使3個(gè)標(biāo)定點(diǎn)（后3點(diǎn)）之間有特殊的方位關(guān)系，從而計(jì)算出工具坐標(biāo)系相對(duì)于機(jī)器人末端坐標(biāo)系的姿態(tài).首次在符合IEC61131-3標(biāo)準(zhǔn)的開(kāi)放式軟件平臺(tái)CODESYS［5］開(kāi)發(fā)工具坐標(biāo)系標(biāo)定算法模塊和焊槍位姿變換模塊，針對(duì)RB_1400焊接機(jī)器人系統(tǒng)進(jìn)行工具參數(shù)的標(biāo)定，然后示教機(jī)器人進(jìn)行工具校驗(yàn).實(shí)驗(yàn)結(jié)果發(fā)現(xiàn)機(jī)器人有較高的定位精度，從而保證在更換焊槍或誤操作碰撞焊槍的情況下，就可以簡(jiǎn)單、準(zhǔn)確地實(shí)現(xiàn)系統(tǒng)標(biāo)定.

1　焊槍工具坐標(biāo)系標(biāo)定

1.1TCP位置標(biāo)定

圖1為RB_1400焊接機(jī)器人示意圖.圖中B為機(jī)器人的基坐標(biāo)系，它是其他坐標(biāo)系的參考坐標(biāo)系；E為機(jī)器人末端坐標(biāo)系，它是描述工具坐標(biāo)系的參考坐標(biāo)系；T為工具坐標(biāo)系.

機(jī)器人末端坐標(biāo)系E相對(duì)于機(jī)器人基坐標(biāo)系B的變換關(guān)系為BET；工具坐標(biāo)系T相對(duì)于末端坐標(biāo)系E的變換關(guān)系為ETT；工具坐標(biāo)系T相對(duì)于基坐標(biāo)B的變換關(guān)系為BTT；三者的轉(zhuǎn)換關(guān)系為

式中：BET由機(jī)器人的正解方程得到，由旋轉(zhuǎn)矩陣BER和位置矢量BpE0組成.

式中矢量n、o、a描述了機(jī)器人末端的姿態(tài).

式中矢量BpE0表示機(jī)器人末端的位置.根據(jù)RB_ 1400機(jī)器人正解運(yùn)動(dòng)方法中計(jì)算θ1、θ2、θ3的方法，采用x-y-z歐拉角［6］法來(lái)描述焊槍的姿態(tài)，θ1為繞著z軸的回轉(zhuǎn)角；θ2為繞著y軸的俯仰角；θ3為繞著x軸的偏轉(zhuǎn)角.

采用四點(diǎn)法標(biāo)定TCP如圖2所示.預(yù)先將焊絲伸出10 mm標(biāo)定TCP的4個(gè)位置，4點(diǎn)之間各差90°且不能在一個(gè)平面上.

對(duì)式（1）以各自的分塊形式展開(kāi)，得到

式中BERi分別為機(jī)器人末端坐標(biāo)系4個(gè)點(diǎn)的旋轉(zhuǎn)矩陣（i=1，2，3，4）；BpEi分別為機(jī)器人末端坐標(biāo)系4個(gè)點(diǎn)的位置矢量；ETR為工具的旋轉(zhuǎn)矩陣；EpT為工具的位置矢量；BTRi分別為工具坐標(biāo)系末端4個(gè)點(diǎn)的旋轉(zhuǎn)矩陣；BpT為工具坐標(biāo)系末端的位置矢量.將式（4）進(jìn)行變形得到計(jì)算工具中心點(diǎn)的通式

因4個(gè)不同位姿下工具坐標(biāo)系在基坐標(biāo)系的位置不變，即BpTx、BpTy和BpTz為定值；ETR、EpT各個(gè)參數(shù)不變也為定值［7］.

根據(jù)以上操作從機(jī)器人控制程序中讀取機(jī)器人末端坐標(biāo)系處位姿的數(shù)據(jù)，如表1所示.

表1　機(jī)器人末端坐標(biāo)系的位姿數(shù)據(jù)（P1～P4）Table 1 Data of robot terminate frame（P1-P4）

結(jié)合式（5），將位姿P2的數(shù)據(jù)減去位姿P1的數(shù)據(jù)有如下關(guān)系：

由式（6）展開(kāi)得

依次類(lèi)推可得

式（8）包含Epx、Epy和Epz3個(gè)未知量，系數(shù)為9×3的矩陣.其為不相容方程組，不可直接用非齊次線性方程組求解的方法或者solve求解.本文采用最小二乘法的矩陣形式，因其系數(shù)矩陣不是方陣，不可直接求逆，因此使用廣義逆.采用高斯消元法得到

1.2TCP姿態(tài)標(biāo)定

1.1節(jié)已經(jīng)計(jì)算出工具坐標(biāo)系的位置，還需要標(biāo)定計(jì)算TCP的姿態(tài).TCP姿態(tài)采用z/x方向標(biāo)定，此過(guò)程保持TCF的姿態(tài)不變［8］.

如圖3所示，將位置標(biāo)定點(diǎn)P4作為第1個(gè)TCF姿態(tài)標(biāo)定點(diǎn)，示教機(jī)器人沿 +x方向至少移動(dòng)250 mm后作為第2個(gè)標(biāo)定點(diǎn)（P5）；然后回到第1個(gè)標(biāo)定點(diǎn)示教機(jī)器人沿+z方向移動(dòng)至少250 mm作為第3個(gè)標(biāo)定點(diǎn)（P6）.得到的機(jī)器人末端坐標(biāo)系的數(shù)據(jù)如表2所示.

表2　機(jī)器人末端坐標(biāo)系的位姿數(shù)據(jù)（P4～P6）Table 2 Data of robot terminate frame（P4-P6）

因?yàn)?個(gè)標(biāo)定點(diǎn)的姿態(tài)保持不變，由式（2）（5）可得BERi都相等，EpT在1.1節(jié)算出且不變.因?yàn)榈?個(gè)姿態(tài)標(biāo)定點(diǎn)與第2個(gè)標(biāo)定點(diǎn)（沿+x方向）之間的向量關(guān)系也就是工具坐標(biāo)系沿+x方向的向量，因此得到工具坐標(biāo)系T的x軸軸向向量

相似地，根據(jù)第1個(gè)姿態(tài)標(biāo)定點(diǎn)與第3個(gè)標(biāo)定點(diǎn)（沿+z方向）之間的向量關(guān)系，由此可以得到工具坐標(biāo)系T的Z軸軸向向量

式中：Bp5E為第2個(gè)標(biāo)定點(diǎn)（P5）相對(duì)于基坐標(biāo)的位置；Bp4E為第1個(gè)標(biāo)定點(diǎn)（P4）相對(duì)于基坐標(biāo)的位置；Bp6E為第3個(gè)標(biāo)定點(diǎn)（P6）相對(duì)于基坐標(biāo)的位置.

同理y軸軸向向量由右手定則可得

再對(duì)Z=X×Y進(jìn)行計(jì)算，以保證坐標(biāo)系矢量的正交性.

得到每個(gè)軸的軸向向量之后，對(duì)其進(jìn)行單位化操作，得到工具坐標(biāo)T相對(duì)于基坐標(biāo)B的姿態(tài)，左乘末端坐標(biāo)系E旋轉(zhuǎn)矩陣的逆，求出工具坐標(biāo)系的旋轉(zhuǎn)矩陣.

2　算法實(shí)現(xiàn)與結(jié)果比較

在CODESYS中編寫(xiě)的工具坐標(biāo)系標(biāo)定算法模塊和焊槍位姿正逆變換模塊，分別如圖4～6所示.

工具坐標(biāo)系標(biāo)定算法模塊的輸入為執(zhí)行此功能塊按鍵（bexecute）和6個(gè)校驗(yàn)點(diǎn)的位姿（pos1～pos6），輸出為焊槍的位姿.

焊槍位姿變換正解模塊輸入為機(jī)器人末端坐標(biāo)系 E的位姿（pi）和焊槍的位姿（dOffsetx～dOffsetθ3），輸出為末端工具坐標(biāo)系 T的位姿（piOut）.

焊槍位姿變換逆解模塊輸入為末端工具坐標(biāo)系T的位姿和焊槍的位姿，輸出為機(jī)器人末端坐標(biāo)系E的位姿.

對(duì)RB_1400六自由度焊接機(jī)器人的焊槍進(jìn)行標(biāo)定，得到焊槍的位姿如圖7所示.圖中tcpmatrix是焊槍3×3的旋轉(zhuǎn)矩陣，aElems［0］～aElems［8］依次為這 9個(gè)數(shù)；經(jīng)過(guò)SMC_Matrix3_ TOYawPitchRoll這個(gè)函數(shù)，求解出dC_rad、dB_rad 和dA_rad這3個(gè)方位角；TCPOffset.dx-TCPOffset. dθ3即為最終焊槍的位姿.

結(jié)合式（3）得到工具坐標(biāo)系相對(duì)于機(jī)器人末端的位姿變換矩陣為

RB_1400六自由度機(jī)器人DH參數(shù)如表3所示.

表3　RB_1400機(jī)器人DH參數(shù)Table 3 DH parameters of RB_1400 robot

在機(jī)器人控制程序中選擇第1個(gè)標(biāo)定點(diǎn)，計(jì)算得到焊槍工具系相對(duì)于基坐標(biāo)系處的位置為

示教機(jī)器人任意姿態(tài)對(duì)準(zhǔn)上述的基準(zhǔn)點(diǎn)，從機(jī)器人控制程序中得到機(jī)器人末端工具坐標(biāo)系的位置為

隨后示教機(jī)器人分別繞 x、y、z軸旋轉(zhuǎn)，校驗(yàn)TCP點(diǎn)的位置發(fā)現(xiàn)機(jī)器人的定位精度達(dá)到了±0.4 mm，完全滿足實(shí)際的加工要求.

3　結(jié)論

1）以RB_1400六自由度焊接機(jī)器人為應(yīng)用對(duì)象，采用基于PC的結(jié)構(gòu)，首次以符合IEC61131-3標(biāo)準(zhǔn)的CODESYS平臺(tái)作為軟運(yùn)動(dòng)控制器的運(yùn)行環(huán)境，在已完成機(jī)器人的正逆算法的基礎(chǔ)上，開(kāi)發(fā)機(jī)器人的工具標(biāo)定算法模塊和焊槍位姿變換模塊，將其添加到機(jī)器人控制程序中.

2）實(shí)驗(yàn)驗(yàn)證了該方法既是一種可靠簡(jiǎn)便、精度高的系統(tǒng)標(biāo)定方法，也是快速標(biāo)定機(jī)器人系統(tǒng)的解決方法.

3）開(kāi)發(fā)的功能模塊符合IEC61131-3標(biāo)準(zhǔn)規(guī)范，具有開(kāi)放性和可擴(kuò)展性，它們?cè)诔晒τ糜诒緦?shí)驗(yàn)室的同時(shí)也適用于其他類(lèi)型機(jī)器人.

［1］熊永倫.機(jī)器人技術(shù)基礎(chǔ)［M］.武漢：華中科技大學(xué)出版社，1995：23-47.

［2］林尚揚(yáng).焊接機(jī)器人及其應(yīng)用［M］.北京：機(jī)械工業(yè)出版社，2000：15-40.

［3］李亮玉，李香，岳建鋒，等.FANUC弧焊機(jī)器人系統(tǒng)標(biāo)定研究與實(shí)現(xiàn)［J］.天津工業(yè)大學(xué)學(xué)報(bào)，2007，26（2）：69-72. LI LY，LIX，YUEJF，etal.Researchand implementation for FANUC robot welding systemcalibration ［J］.Journal of Tianjin Polytechnic University，2007，26 （2）：69-72.（in Chinese）

［4］趙嬌嬌，朱偉.弧焊機(jī)器人系統(tǒng)標(biāo)定［J］.福州大學(xué)學(xué)報(bào)，2008（增刊1）：31-34. ZHAO J J，ZHU W.Arc welding robot calibration［J］. Journal of Fuzhou University，2008（Suppl 1）：31-34.（in Chinese）

［5］CODESYS PLC.編程用戶手冊(cè)［M］.北京：北京工業(yè)大學(xué)3s資格中心，2007：25-40.

［6］CRAIG J J.機(jī)器人學(xué)導(dǎo)論［M］.北京：機(jī)械工業(yè)出版社，2006：29-37.

［7］樸永杰，邱濤，陳善本.弧焊機(jī)器人TCF參數(shù)的標(biāo)定［J］.機(jī)器人，2001，23（2）：109-112. PIAO Y J，QIU T，CHEN S B.Calibration of TCF parameters of arc welding robot［J］.Robot，2001，23 （2）：109-112.（in Chinese）

［8］劉成業(yè)，李文廣，馬世國(guó)，等.一種機(jī)器人工具坐標(biāo)系標(biāo)定方法［J］.山東科學(xué)報(bào)，2012（1）：69-74. LIU C Y，LI W G，MA S G，et al.A robot tool frame calibration method［J］.Shandong Science，2012（1）：69-74.（in Chinese）

（責(zé)任編輯 楊開(kāi)英）

Study and Realization of Tool Coordinate Frame Calibration for Welding Robots

KANG Cunfeng，WANG Hongwei，ZHANG Pengfei，LI Shujin，CHEN Shujun
（College of Mechanical Engineering and Applied Electronics Technology，Beijing University of Technology，Beijing 100124，China）

The tool coordinate system was calibrated according to six-point calibration method.First，it calibrated the position vector of the tool parameters，followed by calibration of orientation matrix of the tool coordinate system，which used the matrix form of the linear least squares to fit tool center point position and the basic operations of the vector and the matrix transformation to calculate tool coordinate frame orientation.For the RB_1400 six-DOF welding robot，the paper developed the tool coordinate system calibration algorithm module and the welding gun pose transformation module based on the automation software CODESYS.Finally，by executing a movement to the reference point，which uses the result data of the developed function block，the test checked whether TCP can meet the requirements when moving around the x，y，and z axis separately.Result shows that the method is feasible and can be also applied to other types of calibration of the robot tool coordinate system.

tool coordinate frame；welding robot； calibration

TP242.2；TP319

A

0254-0037（2016）01-0030-05

10.11936/bjutxb2015020020

2015-02-06

國(guó)家科技重大專(zhuān)項(xiàng)資助項(xiàng)目（2014ZX04001-171）

康存鋒（1970—），男，副教授，主要從事工業(yè)機(jī)器人、工業(yè)控制自動(dòng)化方面的研究，E-mail：kangcunfeng@bjut. edu.cn