專(zhuān)利名稱(chēng)：基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，尤其涉及通過(guò)測(cè)量平板上的點(diǎn)的坐標(biāo)的，基于幾何平面的機(jī)器人工具中心點(diǎn)三分量單獨(dú)校準(zhǔn)的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法。
背景技術(shù)：
機(jī)器人TCP(Tool Center Point)校準(zhǔn)指的是校準(zhǔn)工具中心點(diǎn)在機(jī)器人末端坐標(biāo)系(對(duì)工具被機(jī)器人夾持而言)或機(jī)器人基坐標(biāo)系(對(duì)工具固定的情況而言)下的位置，通常情況下，機(jī)器人TCP的各個(gè)參數(shù)可以通過(guò)兩種方法得到。一種就是把TCP所有的變量都單獨(dú)測(cè)量，然后利用約束條件將TCP的所有變量同時(shí)計(jì)算出來(lái)。另一種方法就是把TCP所有的變量都單獨(dú)分開(kāi)來(lái)測(cè)量，然后單獨(dú)計(jì)算，每次測(cè)量只計(jì)算其中一個(gè)變量。
上述TCP校準(zhǔn)法常用的是“四點(diǎn)法”，在該方法的校準(zhǔn)過(guò)程中，操作者控制機(jī)器人以多個(gè)姿態(tài)(通常為四種)使機(jī)器人工具中心點(diǎn)達(dá)到空間的某一固定點(diǎn)，從而利用約束關(guān)系得到TCP校準(zhǔn)結(jié)果。這種方法，所有待測(cè)變量在測(cè)量中都是相關(guān)的，每一個(gè)變量校準(zhǔn)的精確程度處決于這個(gè)變量在測(cè)量中的相關(guān)程度。在操作上，該方法要求操作者以差異較大的幾種姿態(tài)來(lái)控制機(jī)器人達(dá)到固定空間點(diǎn)，以減小求解的變量的相關(guān)性，但這種方法在操作上很難量化。并且由于校準(zhǔn)過(guò)程中機(jī)器人各關(guān)節(jié)變化大，標(biāo)定過(guò)程中引入了較多的機(jī)器人系統(tǒng)誤差，這會(huì)影響標(biāo)定精度。另外，該方法通常是由人眼判斷工具中心點(diǎn)是否與空間固定點(diǎn)重合，這也使得標(biāo)定過(guò)程中不可避免地引入了人眼判斷誤差，降低了標(biāo)定精度。

發(fā)明內(nèi)容
本發(fā)明的目的是把機(jī)器人TCP所有的變量都單獨(dú)分開(kāi)來(lái)測(cè)量和計(jì)算，從而容易控制測(cè)量過(guò)程中校準(zhǔn)的精確程度。
為實(shí)現(xiàn)上述目的，本發(fā)明提供了一種基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，機(jī)器人末端固定一個(gè)測(cè)量用探頭，一個(gè)表面平整的平板位于機(jī)器人的活動(dòng)范圍之內(nèi)，本方法包括以下步驟步驟1所述機(jī)器人保持一定的姿態(tài)平動(dòng)，用所述探頭接觸所述平板上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)，得到這些點(diǎn)的坐標(biāo)和位置，利用這些坐標(biāo)和位置計(jì)算所述表面所在平面的法向量；步驟2機(jī)器人末端坐標(biāo)系的X、Y和Z軸方向分別與所述表面所在平面的法向量方向相同時(shí)，分別用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后與法向量方向相反，分別用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；步驟3利用步驟2測(cè)量出的所有點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人工具中心點(diǎn)在機(jī)器人末端坐標(biāo)系下的位置。
所述步驟3具體為，首先機(jī)器人末端坐標(biāo)系的X軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，然后機(jī)器人末端坐標(biāo)系的Y軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著機(jī)器人末端坐標(biāo)系的Z軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；再后機(jī)器人末端坐標(biāo)系的X軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，然后機(jī)器人末端坐標(biāo)系的Y軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，最后機(jī)器人末端坐標(biāo)系的Z軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)。
所述步驟中的機(jī)器人為六自由度的機(jī)器人。所述步驟2之后中，機(jī)器人改變姿態(tài)，用所述探頭接觸所述平板，測(cè)量所述點(diǎn)的坐標(biāo)。所述步驟中通過(guò)讀取機(jī)器人控制器而得到所述點(diǎn)的坐標(biāo)和位置。
本發(fā)明還提供了一種通過(guò)測(cè)平板上點(diǎn)的坐標(biāo)的機(jī)器人的固定工具中心點(diǎn)三分量校準(zhǔn)法，機(jī)器人末端固定一個(gè)平板，該平板的兩個(gè)表面均為光滑且平行，為第一表面和第二表面，一個(gè)測(cè)量用的探頭固定于機(jī)器人的活動(dòng)范圍之內(nèi)，本方法包括以下步驟步驟1機(jī)器人保持一定的姿態(tài)平動(dòng)，使得探頭接觸平板的第一表面上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)，得到這些點(diǎn)的坐標(biāo)和位置，利用這些坐標(biāo)和位置計(jì)算該第一表面的法向量；步驟2機(jī)器人基座坐標(biāo)系的X、Y和Z軸方向分別與所述第一表面所在平面的法向量方向相反時(shí)，分別用所述探頭接觸所述第一表面的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后與法向量方向相同，分別用所述探頭接觸所述第二表面的一點(diǎn)，分別測(cè)量所述點(diǎn)的坐標(biāo)；步驟3利用步驟3測(cè)量出的所有點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人工具中心點(diǎn)在機(jī)器人基座標(biāo)系下的位置。
所述步驟3具體為，首先所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的X軸方向相同時(shí)，用所述探頭接觸所述第一表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的X軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Y軸方向相同時(shí)，用所述探頭接觸所述第一表面的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著將所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Y軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；最后所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Z軸方向相同時(shí)，用所述探頭接觸所述第一表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Z軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)。
所述步驟中的機(jī)器人為六自由度的機(jī)器人。所述步驟2之后中，機(jī)器人改變姿態(tài)，用所述探頭接觸所述平板，測(cè)量所述點(diǎn)的坐標(biāo)。所述步驟中通過(guò)讀取機(jī)器人控制器而得到所述點(diǎn)的坐標(biāo)和位置。
因此，本發(fā)明將TCP所有的變量都單獨(dú)分開(kāi)來(lái)測(cè)量和計(jì)算，每次測(cè)量只測(cè)量和計(jì)算其中一個(gè)變量。由于所有的變量都被單獨(dú)分離開(kāi)，所以很容易控制測(cè)量過(guò)程中校準(zhǔn)的精確程度。


圖1為本發(fā)明基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法實(shí)施例1的結(jié)構(gòu)示意圖。
圖2為本發(fā)明基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法實(shí)施例1的方法流程圖。
圖3為本發(fā)明基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法實(shí)施例2的結(jié)構(gòu)示意圖。
圖4為本發(fā)明基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法實(shí)施例2的方法流程圖。
具體實(shí)施例方式
本發(fā)明的思想是把機(jī)器人TCP的所有的變量都單獨(dú)分開(kāi)來(lái)計(jì)算，每次測(cè)量只計(jì)算其中一個(gè)變量，基于所有的變量都被單獨(dú)分離開(kāi)，從而容易控制測(cè)量過(guò)程中校準(zhǔn)的精確程度。
下面用實(shí)施例，對(duì)本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)描述。
實(shí)施例1如圖1所示，為本發(fā)明實(shí)施例1的結(jié)構(gòu)示意圖，機(jī)器人1為六自由度工業(yè)機(jī)器人，底座10為機(jī)器人基座，tool0為機(jī)器人末端11的坐標(biāo)系，測(cè)量用的探頭2固定在機(jī)器人末端上。還包括一個(gè)厚度已知的表面平整的平板3。如圖2所示，為本發(fā)明機(jī)器人TCP三分量校準(zhǔn)法的流程圖，包括如下步驟步驟101將一個(gè)表面平整的平板置于機(jī)器人的活動(dòng)范圍內(nèi)；步驟102機(jī)器人保持一定的運(yùn)動(dòng)方向平動(dòng)，測(cè)量所述表面上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)的坐標(biāo)，利用這些坐標(biāo)計(jì)算所述表面所在平面的法向量；可以證明在對(duì)平面法向量的測(cè)量中，一旦機(jī)器人保持一定的運(yùn)動(dòng)方向進(jìn)行測(cè)量，平面法向量的精確值與TCP的初始值是沒(méi)有關(guān)系的。因此既然平面法向量的測(cè)量與未知的TCP值沒(méi)有關(guān)系，那么法向量方向的測(cè)量它就是準(zhǔn)確的。
步驟103首先機(jī)器人末端坐標(biāo)系的X軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，然后機(jī)器人末端坐標(biāo)系的Y軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著機(jī)器人末端坐標(biāo)系的Z軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；再后機(jī)器人末端坐標(biāo)系的X軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，然后機(jī)器人末端坐標(biāo)系的Y軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，最后機(jī)器人末端坐標(biāo)系的Z軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；步驟104利用步驟103測(cè)量出的六個(gè)點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人TCP的TX、TY和TZ三個(gè)分量。
下面證明步驟102中機(jī)器人保持同樣的運(yùn)動(dòng)方向進(jìn)行測(cè)量，平面法向量的精確值與TCP的初始值是沒(méi)有關(guān)系的。
假設(shè)機(jī)器人的TCP的值是(Tx、Ty、Tz)。用接觸式觸發(fā)探頭測(cè)量的測(cè)量點(diǎn)相對(duì)于機(jī)器人的底座的坐標(biāo)取決于以下公式
x＝r11*Tx+r12*Ty+r13*Tz+Pxy＝r21*Tx+r22*Ty+r23*Tz+Py(1)z＝r31*Tx+r32*Ty+r33*Tz+Pz其中(rij，i＝1，2，3；j＝1，2，3)和(Px，Py，Pz)表示末端tool0相對(duì)于機(jī)器人的底座的方向和位置，這些值可以從機(jī)器人控制器上實(shí)時(shí)讀取。
確定一個(gè)平面，至少需要測(cè)量不在同一直線(xiàn)上的三個(gè)點(diǎn)，此處用三個(gè)點(diǎn)分析。三個(gè)以上的點(diǎn)所決定的平面是基于最小二乘法得到。
因此平板上的三個(gè)測(cè)量點(diǎn)的坐標(biāo)取決于以下公式xi＝r11*Tx+r12*Ty+r13*Tz+Pxiyi＝r21*Tx+r22*Ty+r23*Tz+Pyi(2)zi＝r31*Tx+r32*Ty+r33*Tz+Pzii＝1，2，3；在這三個(gè)測(cè)量點(diǎn)的測(cè)量過(guò)程中，機(jī)器人一直保持著相同的運(yùn)動(dòng)方向，因此rij是不變的。
平面的法向量計(jì)算如下假設(shè)平面的方程是A*x+B*y+C*z＝1； (3)平面的參數(shù)A、B和C由測(cè)量得到的三個(gè)測(cè)量點(diǎn)的坐標(biāo)(x1，y1，z1)，(x2，y2，z2)，(x3，y3，z3)決定。
令A(yù)=Δ1Δ]]>B=Δ2Δ...(4)]]>C=Δ3Δ]]>Δ1=1,y1,z11,y2,z21,y3,z3,]]>Δ2=x1,1,z1x2,1,z2x3,1,z3,]]>Δ3=x1,y1,1x2,y2,1x3,y3,1]]>
則平面法向量(nx，ny，nz)和偏移量是nx=Δ1(Δ1)2+(Δ2)2+(Δ3)2]]>ny=Δ2(Δ1)2+(Δ2)2+(Δ3)2...(5)]]>nz=Δ3(Δ1)2+(Δ2)2+(Δ3)2]]>d=Δ(Δ1)2+(Δ2)2+(Δ3)2]]>又，Δ1=1,y1,z11,y2,z21,y3,z3=1,y1,z10,y2-y1,z2-z10,y3-y1,z3-z1=(y2-y1)*(z3-z1)-(z2-z1)*(y3-y1)...(6)]]>(6)P表示的＝(py2-py1)*(pz3-pz1)-(pz2-pz1)*(py3-py1)；是機(jī)器人末端點(diǎn)(Tool0)的坐標(biāo)。
上面的數(shù)學(xué)公式推導(dǎo)過(guò)程說(shuō)明了Δ1和(Tx、Ty、Tz)無(wú)關(guān)，同理可以證明Δ2和Δ3與(Tx、Ty、Tz)也無(wú)關(guān)，所以(5)式中的平面法向量(nx，ny，nz)也與(Tx、Ty、Tz)無(wú)關(guān)。
步驟103的測(cè)量和步驟104的計(jì)算方法如下利用(1)～(6)式可以證明平面的法向量與TCP的準(zhǔn)確值無(wú)關(guān)，然后將法向量計(jì)算出來(lái)，現(xiàn)設(shè)為(nx，ny，nz)，使用機(jī)器人控制程序(如ABB機(jī)器人的Rapid程序)使機(jī)器人接觸平板上的一點(diǎn)，它的TCP位置由下式?jīng)Q定x＝r11*Tx+r12*Ty+r13*Tz+Pxy＝r21*Tx+r22*Ty+r23*Tz+Py(7)z＝r31*Tx+r32*Ty+r33*Tz+Pz其中(rrij，i＝1.2.3，j＝1，2，3)和(Px，Py，Pz)表示點(diǎn)末端tool0相對(duì)于機(jī)器人底部的方向和位置，這些值從機(jī)器人控制器上實(shí)時(shí)讀取。偏移量就可以用以下的公式來(lái)計(jì)算
d＝nx*x+ny*y+nz*z＝nx*(r11*Tx+r12*Ty+r13*Tz+px)+ny*(r21*Tx+r22*Ty+r23*Tz+Py)+nz*(r31*Tx+r32*Ty+r33*Tz+Pz)； (8)1、Tx的校準(zhǔn)第一步使機(jī)器人坐標(biāo)系的X軸與平板平整表面所在平面的法向量方向相同，即(r11，r21，r31)＝(nx，ny，nz)然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，把上式代入式(8)，由于旋轉(zhuǎn)矩陣rij是正交矩陣，Ty和Tz將被消去，于是方程(8)變成d1＝Tx+nx*px1+ny*py1+nz*pz1(9)第二步使機(jī)器人坐標(biāo)系的X軸與平板平整表面所在平面的法向量方向相反，即(r11，r21，r31)＝-(nx，ny，nz)然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)。
于是方程(8)將變?yōu)閐2＝-Tx+nx*px2+ny*py2+nz*pz2d2＝-Tx+nx*px2+ny*py2+nz*pz2(10)聯(lián)立方程(9)和(10)，得到Tx=(d1-d2)-nx*(px1-px2)-ny*(py1-py2)-nz*(pz1-pz2)2...(11)]]>因?yàn)閮纱螠y(cè)量用的是同一平面，則d1＝d2，于是Tx=-nx*(px1-px2)-ny*(py1-py2)-nz*(pz1-pz2)2...(12)]]>(px1，py1，pz1)是第一個(gè)點(diǎn)相對(duì)于機(jī)器人底部的坐標(biāo)，從機(jī)器人控制器上實(shí)時(shí)讀取，(px2，py2，pz2)是第二個(gè)點(diǎn)相對(duì)于機(jī)器人底部的坐標(biāo)，從機(jī)器人控制器上實(shí)時(shí)讀取，(nx，ny，nz)是平面的法向量，均已知。
2、Ty的校準(zhǔn)
同理，Ty的校準(zhǔn)和Tx的校準(zhǔn)相同，第一步使機(jī)器人坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相同，即(r11，r21，r31)＝(nx，ny，nz)然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，第二步使機(jī)器人坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相反，即(r11，r21，r31)＝-(nx，ny，nz)然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，則結(jié)果Ty=-nx*(px1-px2)-ny*(py1-py2)-nz*(pz1-pz2)2]]>3、Tz的校準(zhǔn)同理，Tz的校準(zhǔn)也和Tx的校準(zhǔn)是一樣的，在測(cè)量中，分別使第一步使機(jī)器人坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相同，即(r11，r21，r31)＝(nx，ny，nz)用探頭接觸該表面上的一點(diǎn)，然后測(cè)量該點(diǎn)，第二步使機(jī)器人坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相反，即(r11，r21，r31)＝-(nx，ny，nz)然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，則結(jié)果Tz=-nx*(px1-px2)-ny*(py1-py2)-nz*(pz1-pz2)2]]>在機(jī)器人標(biāo)定其中最后一個(gè)量(如本實(shí)施例TCP中的Tz)時(shí)，為避免機(jī)器人關(guān)節(jié)運(yùn)動(dòng)過(guò)大，引入過(guò)大機(jī)器人系統(tǒng)誤差，可以通過(guò)僅控制工具末端點(diǎn)接觸到其中一個(gè)面來(lái)計(jì)算。
計(jì)算方法如下
以最后計(jì)算Tz為例，在標(biāo)定完Tx和Ty后，由式(8)可知，平板表面方程的偏移量是Tz的函數(shù)，改變機(jī)器人的姿態(tài)，使工具末端再次接觸同一平板表面點(diǎn)，得到平板表面方程的偏移量的另外一個(gè)表達(dá)式。消去兩式中的d，可建立方程求解Tz.
實(shí)施例2如圖3所示，為本發(fā)明實(shí)施例2的結(jié)構(gòu)示意圖，機(jī)器人1為六自由度工業(yè)機(jī)器人，底座10為機(jī)器人基座，tool0為機(jī)器人末端11的坐標(biāo)系，一個(gè)表面平整的平板3固定在機(jī)器人末端上，該平板的兩個(gè)表面為第一表面和第二表面，而且相互平行，地面放置探頭2。如圖4所示，為本發(fā)明機(jī)器人的固定TCP三分量校準(zhǔn)法的流程示意圖，包括如下步驟步驟201將測(cè)量用的探頭安置在機(jī)器人的活動(dòng)范圍內(nèi)；步驟202機(jī)器人保持一定的運(yùn)動(dòng)方向平動(dòng)，測(cè)量所述第一表面上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)的坐標(biāo)，利用這些坐標(biāo)計(jì)算所述表面所在平面的法向量；可以證明在對(duì)平面法向量的測(cè)量中，一旦機(jī)器人保持一定的姿態(tài)進(jìn)行測(cè)量，平面法向量的精確值與TCP的初始值是沒(méi)有關(guān)系的。因此既然平面法向量的測(cè)量與未知的TCP值沒(méi)有關(guān)系，那么法向量方向的測(cè)量它就是準(zhǔn)確的。
步驟203首先所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的X軸方向相同時(shí)，用所述探頭接觸所述第一表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的X軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Y軸方向相同時(shí)，用所述探頭接觸所述第一表面的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著將所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Y軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；最后所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Z軸方向相同時(shí)，用所述探頭接觸所述第一表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Z軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；步驟204利用步驟203測(cè)量出的六個(gè)點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人TCP的TX、TY和TZ三個(gè)分量。
下面證明步驟202中機(jī)器人保持同樣的運(yùn)動(dòng)方向進(jìn)行測(cè)量，平面法向量的精確值與TCP的初始值是沒(méi)有關(guān)系的。
假設(shè)機(jī)器人的TCP的值是T＝(Tx，Ty，Tz)T。當(dāng)平板和探頭接觸時(shí)，有下列關(guān)系成立R0·Xt+T0＝T其中，R0和T0分別是Tool0對(duì)于基座的旋轉(zhuǎn)和平移變換，Xt是探頭探測(cè)的點(diǎn)在Tool0坐標(biāo)系下的坐標(biāo).
讓機(jī)器人基座保持同一姿態(tài)平行探測(cè)三個(gè)不在同一條直線(xiàn)上的點(diǎn)，則有如下關(guān)系成立R0·Xt1+T01=TR0·Xt2+T02=TR0·Xt3+T03=T]]>這里R0，T01，T02，T03可以從機(jī)器人的控制器上讀出，Xt1，Xt2，Xt3為探頭在平板面上探測(cè)到的三點(diǎn)在Tool0坐標(biāo)系下的位置。
將上三等式兩兩相減，可以得到R0·(Xt1-Xt2)=T02-T01R0·(Xt1-Xt3)=T03-T01]]>如此可以得到關(guān)系平板的法向量NN=(Xt1-Xt2)×(Xt1-Xt3)||(Xt1-Xt2)×(Xt1-Xt3)||=R0-1((T02-T01)×(T03-T01))||R0-1((T02-T0)×(T03-T01))||]]>此關(guān)系式表明，平板的法向量與TCP的位置沒(méi)有關(guān)系.其中Xt1，Xt2，Xt3確定一個(gè)平面，至少需要測(cè)量不在同一直線(xiàn)上的三個(gè)點(diǎn)，此處用三個(gè)點(diǎn)分析。三個(gè)以上的點(diǎn)所決定的平面是基于最小二乘法得到。
固定TCP的三分量的校準(zhǔn)原理如下描述.
假設(shè)平板的表面所在的平面可表示為N·X＝d其中N為平面的單位法向量，X為T(mén)ool0下的坐標(biāo).
令Tool0=R0T001]]>為機(jī)器人末端到基座的變換，Tool0-1=R0-1-R0-1·T001]]>同時(shí)令R0＝(r1，r2，r3)-1，則有關(guān)系N·(Tool0-1·T1)=N·(R0-1·T-R0-1·T0)=d]]>即有關(guān)系N·((r1,r2,r3)·TxTyTz-(r1,r2,r3)·T0xT0yT0z)=d]]>移動(dòng)機(jī)器人，讓r1＝N，可以得到Tx-T01x＝d1再移動(dòng)機(jī)器人，讓r1＝-N，可以得到-Tx+T02x＝d2如此，可以計(jì)算出Tx=T01x+T02x+(d1-d2)2]]>同樣的方法，我們可以得到Ty=T01y+T02y+(d1-d2)2]]>Tz=T01z+T02z+(d1-d2)2]]>由于d1-d2為平板的厚度，為已知量，并且T01x，T02x，T01y，T02y，T01z，T02z，可從機(jī)器人控制器上讀出，故我們可以得到分離地得到固定TCP的三分量.
具體的實(shí)施時(shí)候，可以用機(jī)器人控制程序(如ABB機(jī)器人的Rapid程序)使機(jī)器人接觸平板上的一點(diǎn)，先校準(zhǔn)平板的法向量然后再校準(zhǔn)TCP位置.
步驟203的測(cè)量和步驟204的計(jì)算方法如下1、平板法向量的校準(zhǔn)使用機(jī)器人控制程序(如ABB機(jī)器人的Rapid程序)使機(jī)器人末端持的平板的第一表面接觸到探頭，并讓探頭在平面上移動(dòng)(注意機(jī)器人末端的姿態(tài)保持不變)，采集到三個(gè)不在同一條直線(xiàn)上的三點(diǎn)的坐標(biāo)，然后由以上計(jì)算平板平面法向量的方法計(jì)算出平板的單位法向量.
2、固定TCP三分量Tx，TY，Tz的校準(zhǔn)2.1 Tx的校準(zhǔn)第一步使機(jī)器人基座坐標(biāo)系的X軸與平板第一表面所在平面的法向量方向相同，即r1＝N然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，則該點(diǎn)滿(mǎn)足關(guān)系式Tx-T01x＝d1第二步使機(jī)器人基座坐標(biāo)系的X軸與平板第一表面所在平面的法向量方向相反，即r1＝-N然后用探頭接觸第二表面上的一點(diǎn)，測(cè)量該點(diǎn)，同樣的該點(diǎn)也滿(mǎn)足關(guān)系式-Tx+T02x＝d2由上面的兩個(gè)等式，可以得到Tx=T01x+T02x+(d1-d2)2]]>由于d1-d2為平板厚度，為已知量，且可以從控制器上讀出機(jī)器人末端的T01x，T02x，則可以完成Tx的校準(zhǔn)。
2.2 Ty的校準(zhǔn)
Ty的校準(zhǔn)和Tx的校準(zhǔn)原理相同，具體的操作如下.
第一步使機(jī)器人基座坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相同，即r2＝N然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，則該點(diǎn)滿(mǎn)足關(guān)系式Ty-T01y＝d1第二步使機(jī)器人基座坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相反，即r2＝-N然后用探頭接觸第二表面上的一點(diǎn)，測(cè)量該點(diǎn)，同樣的該點(diǎn)也滿(mǎn)足關(guān)系式-Ty+T02y＝d2由上面的兩個(gè)等式，可以得到Ty=T01y+T02y+(d1-d2)2]]>由于d1-d2為平板厚度，為已知量，且可以從控制器上讀出機(jī)器人末端的T01y，T02y，則可以完成Ty的校準(zhǔn).
2.3 Tz的校準(zhǔn)Tz的校準(zhǔn)也和Tx的校準(zhǔn)是一樣的，在測(cè)量中，分別使第一步使機(jī)器人基座坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相同，即r3＝N然后用探頭接觸該表面上的一點(diǎn)，測(cè)量該點(diǎn)，則該點(diǎn)滿(mǎn)足關(guān)系式Tz-T01z＝d1第二步使機(jī)器人基座坐標(biāo)系的Y軸與平板平整表面所在平面的法向量方向相反，即r3＝-N然后用探頭接觸第二表面上的一點(diǎn)，測(cè)量該點(diǎn)，同樣的該點(diǎn)也滿(mǎn)足關(guān)系式-Tz+T02z＝d2
由上面的兩個(gè)等式，可以得到Tz=T01z+T02z+(d1-d2)2]]>由于d1-d2為平板厚度，為已知量，且可以從控制器上讀出機(jī)器人末端的T01z，T02z，則可以完成Tz的校準(zhǔn).
在機(jī)器人標(biāo)定其中最后一個(gè)量(如本實(shí)施例TCP中的Tz)時(shí)，為避免機(jī)器人關(guān)節(jié)運(yùn)動(dòng)過(guò)大，引入過(guò)大機(jī)器人系統(tǒng)誤差，可以通過(guò)僅控制工具末端點(diǎn)接觸到其中一個(gè)面來(lái)計(jì)算。
計(jì)算方法如下以最后計(jì)算Tz為例，在標(biāo)定完Tx和Ty后，可知平板表面方程的偏移量d是Tz的函數(shù)d=N·(R0-1·TxTyTz-R0-1·T0)]]>其中，N為平面的法向量，R0，T0為機(jī)器人末端相對(duì)于基座的旋轉(zhuǎn)和平移關(guān)系。
改變機(jī)器人的姿態(tài)，使工具末端再次接觸同一平板表面點(diǎn)，得到平板表面方程的偏移量的另外一個(gè)表達(dá)式。消去兩式中的d，可建立方程求解Tz.
因此，本發(fā)明將TCP所有的變量都單獨(dú)分開(kāi)來(lái)計(jì)算，每次測(cè)量只計(jì)算其中一個(gè)變量。由于所有的變量都被單獨(dú)分離開(kāi)，所以很容易控制測(cè)量過(guò)程中校準(zhǔn)的精確程度。
最后所應(yīng)說(shuō)明的是，以上實(shí)施例僅用以說(shuō)明本發(fā)明的技術(shù)方案而非限制，盡管參照較佳實(shí)施例對(duì)本發(fā)明進(jìn)行了詳細(xì)說(shuō)明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對(duì)本發(fā)明的技術(shù)方案進(jìn)行修改或者等同替換，而不脫離本發(fā)明技術(shù)方案的精神和范圍。
權(quán)利要求
1.一種基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，機(jī)器人末端固定一個(gè)測(cè)量用探頭，一個(gè)表面平整且厚度已知的平板置于機(jī)器人的活動(dòng)范圍之內(nèi)，本方法包括以下步驟步驟1所述機(jī)器人保持一定的姿態(tài)平動(dòng)，用所述探頭接觸所述平板上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)，得到這些點(diǎn)的坐標(biāo)和位置，利用這些坐標(biāo)和位置計(jì)算所述表面所在平面的法向量；步驟2機(jī)器人末端坐標(biāo)系的X、Y和Z軸方向分別與所述表面所在平面的法向量方向相同時(shí)，分別用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后與法向量方向相反，分別用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；步驟3利用步驟2測(cè)量出的所有點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人工具中心點(diǎn)在機(jī)器人末端坐標(biāo)系下的位置。
2.根據(jù)權(quán)利要求1所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟3具體為，首先機(jī)器人末端坐標(biāo)系的X軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，然后機(jī)器人末端坐標(biāo)系的Y軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著機(jī)器人末端坐標(biāo)系的Z軸方向與所述表面所在平面的法向量方向相同時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；再后機(jī)器人末端坐標(biāo)系的X軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，然后機(jī)器人末端坐標(biāo)系的Y軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，最后機(jī)器人末端坐標(biāo)系的Z軸方向與所述表面所在平面的法向量方向相反時(shí)，用所述探頭接觸所述平板的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)。
3.根據(jù)權(quán)利要求1或2所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟中的機(jī)器人為六自由度的機(jī)器人。
4.根據(jù)權(quán)利要求1或2所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟2之后中，機(jī)器人改變姿態(tài)，用所述探頭接觸所述平板，測(cè)量所述點(diǎn)的坐標(biāo)。
5.根據(jù)權(quán)利要求1或2所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟中通過(guò)讀取機(jī)器人控制器而得到所述點(diǎn)的坐標(biāo)和位置。
6.一種基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，機(jī)器人末端固定一個(gè)平板，該平板的兩個(gè)表面均為光滑且平行，為第一表面和第二表面，一個(gè)測(cè)量用的探頭固定于機(jī)器人的活動(dòng)范圍之內(nèi)，本方法包括以下步驟步驟1機(jī)器人保持一定的姿態(tài)平動(dòng)，使得探頭接觸平板的第一表面上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)，得到這些點(diǎn)的坐標(biāo)和位置，利用這些坐標(biāo)和位置計(jì)算該第一表面的法向量；步驟2機(jī)器人基座坐標(biāo)系的X、Y和Z軸方向分別與所述第一表面所在平面的法向量方向相同時(shí)，分別用所述探頭接觸所述第一表面的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后機(jī)器人基座坐標(biāo)系的X、Y和Z軸方向分別與所述第一表面所在平面的法向量方向相反，分別用所述探頭接觸所述第二表面的一點(diǎn)，分別測(cè)量所述點(diǎn)的坐標(biāo)；步驟3利用步驟2測(cè)量出的所有點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人工具中心點(diǎn)在機(jī)器人基座標(biāo)系下的位置。
7.根據(jù)權(quán)利要求6所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟3具體為，首先所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的X軸方向相同時(shí)，用所述探頭接觸所述第一表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的X軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；然后所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Y軸方向相同時(shí)，用所述探頭接觸所述第一表面的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著將所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Y軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)；最后所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Z軸方向相同時(shí)，用所述探頭接觸所述第一表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)，接著所述第一表面所在平面的法向量方向與機(jī)器人基座坐標(biāo)系的Z軸方向相反時(shí)，用所述探頭接觸所述第二表面上的一點(diǎn)，測(cè)量所述點(diǎn)的坐標(biāo)。
8.根據(jù)權(quán)利要求6或7所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟中的機(jī)器人為六自由度的機(jī)器人。
9.根據(jù)權(quán)利要求6或7所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟2中，機(jī)器人改變姿態(tài)，用所述探頭接觸所述平板，測(cè)量所述點(diǎn)的坐標(biāo)。
10.根據(jù)權(quán)利要求6或7所述的基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，其特征在于所述步驟中通過(guò)讀取機(jī)器人控制器而得到所述點(diǎn)的坐標(biāo)和位置。
全文摘要
本發(fā)明涉及一種基于平板測(cè)量的機(jī)器人工具中心點(diǎn)三分量校準(zhǔn)法，機(jī)器人末端固定一個(gè)測(cè)量用探頭，一個(gè)表面平整的平板位于機(jī)器人的活動(dòng)范圍之內(nèi)，包括以下步驟步驟1機(jī)器人保持一定的姿態(tài)平動(dòng)，用探頭接觸平板上不在一條直線(xiàn)上的三個(gè)或三個(gè)以上的點(diǎn)，得到這些點(diǎn)的坐標(biāo)和位置，利用這些坐標(biāo)和位置計(jì)算表面所在平面的法向量；步驟2機(jī)器人末端坐標(biāo)系的X、Y和Z軸方向分別與表面所在平面的法向量方向相同時(shí)，分別用探頭接觸平板的一點(diǎn)，測(cè)量點(diǎn)的坐標(biāo)；然后與法向量方向相反，分別用探頭接觸平板的一點(diǎn)，測(cè)量點(diǎn)的坐標(biāo)；步驟3利用步驟2測(cè)量出的點(diǎn)的坐標(biāo)計(jì)算出機(jī)器人工具中心點(diǎn)在機(jī)器人末端坐標(biāo)系下的位置。
文檔編號(hào)G01B21/22GK1903525SQ20051008704
公開(kāi)日2007年1月31日 申請(qǐng)日期2005年7月25日 優(yōu)先權(quán)日2005年7月25日
發(fā)明者湯青, 甘中學(xué), 吳水華, 寧隨軍, 孫云權(quán) 申請(qǐng)人:廊坊智通機(jī)器人系統(tǒng)有限公司