本發(fā)明涉及四軸機(jī)械手控制領(lǐng)域，具體地說是涉及scara機(jī)械手固定工作平面應(yīng)用場景下的相機(jī)標(biāo)定及運(yùn)動(dòng)過程中的坐標(biāo)變換。

背景技術(shù)：

隨著工業(yè)化的不斷發(fā)展，生產(chǎn)自動(dòng)化水平越來越高，工業(yè)機(jī)器人應(yīng)用到各個(gè)領(lǐng)域，大大提高了生產(chǎn)效率。目前，scara機(jī)械手在工業(yè)相機(jī)的視覺引導(dǎo)下和運(yùn)動(dòng)控制器的控制下可完成工件的高精度定位及裝配。主要應(yīng)用在工業(yè)生產(chǎn)流水線上，比如：pcb板上的具有規(guī)則完整平面元器件的貼裝或插裝、待加工工件的有序拾取并放置等。其主要完成工件的搬運(yùn)和裝配工作，同時(shí)，考慮到流水線上的協(xié)同作業(yè)，scara機(jī)械手的工作范圍還具有可根據(jù)實(shí)際情況靈活調(diào)節(jié)的功能。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是為了提供一種基于scara機(jī)械手在固定工作平面應(yīng)用場景下的相機(jī)標(biāo)定及運(yùn)動(dòng)過程中的坐標(biāo)變換的方法，從而實(shí)現(xiàn)scara機(jī)械手的精確控制。

本發(fā)明及其技術(shù)問題所采用的技術(shù)方案是：

一種基于scara機(jī)械手的相機(jī)標(biāo)定和坐標(biāo)變換的方法，包括步驟：

(1)首先將上視相機(jī)固定安裝在工作平面以下的平臺(tái)上，然后將下視相機(jī)豎直固定安裝在scara機(jī)械手的第二關(guān)節(jié)末端并與機(jī)械手坐標(biāo)系、即世界坐標(biāo)系的z軸平行，此時(shí)下視相機(jī)可隨機(jī)械手在工作范圍內(nèi)移動(dòng)，要求位于機(jī)械手第三關(guān)節(jié)末端的吸嘴能夠在上視相機(jī)中清晰成像；

(2)采用相機(jī)標(biāo)定技術(shù)對上視相機(jī)進(jìn)行標(biāo)定，得到上視相機(jī)的參數(shù)矩陣h1；

(3)采用自制的pcb標(biāo)定板，進(jìn)行上下視的聯(lián)合標(biāo)定，得到下視相機(jī)的參數(shù)矩陣h2；

(4)計(jì)算此時(shí)下視相機(jī)標(biāo)定位置的關(guān)節(jié)角度，通過坐標(biāo)轉(zhuǎn)換公式計(jì)算并記錄此時(shí)吸嘴的世界坐標(biāo)；

(5)下視相機(jī)成功標(biāo)定之后，移動(dòng)下視相機(jī)對pcb板上待貼裝元件位置進(jìn)行拍照，計(jì)算出待貼裝元件的實(shí)際目標(biāo)位置，包括元件中心、元件mark點(diǎn)的世界坐標(biāo)；

(6)移動(dòng)下視相機(jī)拍照待貼裝元件，通過坐標(biāo)轉(zhuǎn)換公式，計(jì)算待貼裝元件的位置，并準(zhǔn)確吸??；

(7)在機(jī)械手移動(dòng)吸嘴成功吸取到規(guī)則元件(暫定為矩形貼片元件)之后，需要進(jìn)行上視相機(jī)過圖校正，運(yùn)用圖像處理算法，準(zhǔn)確識(shí)別元件中心、元件mark點(diǎn)并由標(biāo)定算法獲取兩點(diǎn)的世界坐標(biāo)；

(8)由步驟(5)和(7)，經(jīng)過計(jì)算元件中心與元件mark點(diǎn)組成的向量夾角，旋轉(zhuǎn)u軸，保證元件姿態(tài)與目標(biāo)位置平行；

(9)計(jì)算吸嘴在目標(biāo)位置中的世界坐標(biāo)，即讓待貼裝元件和目標(biāo)位置重合；

(10)此時(shí)即可計(jì)算出目標(biāo)位置點(diǎn)z(xz,yz)的坐標(biāo)，控制scara機(jī)械手的吸嘴移動(dòng)到目標(biāo)位置點(diǎn)z(xz,yz)即可完成正確貼裝。

進(jìn)一步地，所述步驟(3)具體包括步驟：上、下相視對pcb標(biāo)定板的上下兩面同一位置拍照，由于上視相機(jī)已經(jīng)標(biāo)定成功，所以可利用上視相機(jī)得到標(biāo)志點(diǎn)的世界坐標(biāo)，并和下視相機(jī)所拍照圖像中的標(biāo)志點(diǎn)一起完成下視相機(jī)的標(biāo)定，得到其參數(shù)矩陣h2。

進(jìn)一步地，所述步驟(4)具體包括步驟：機(jī)械手在控制器的控制下進(jìn)行移動(dòng)拍照，由于機(jī)械手是關(guān)節(jié)型，關(guān)節(jié)的移動(dòng)軌跡呈扇形，所以要準(zhǔn)確獲得此時(shí)圖像中像點(diǎn)的世界坐標(biāo)(xw,yw)，必須知道第一關(guān)節(jié)和第二關(guān)節(jié)的旋轉(zhuǎn)角度α和β，實(shí)際使用的旋轉(zhuǎn)角度δ是此時(shí)位置和下視相機(jī)標(biāo)定位置時(shí)的關(guān)節(jié)角度之差，并折合到第二關(guān)節(jié)上，由于下視相機(jī)與吸嘴的距離固定，所以不管機(jī)械手怎么運(yùn)動(dòng)，下視相機(jī)所拍圖像中同一位置的像點(diǎn)(xp,yp)與吸嘴(xm,ym)的距離也是固定的，并且像點(diǎn)和吸嘴的連線l與機(jī)械手第二關(guān)節(jié)的夾角(實(shí)際使用時(shí)還需考慮坐標(biāo)象限)也是固定不變的；上述旋轉(zhuǎn)折合到第二關(guān)節(jié)上關(guān)節(jié)角度γ和吸嘴的實(shí)時(shí)世界坐標(biāo)(xm,ym)都可由機(jī)械手運(yùn)動(dòng)控制器的庫函數(shù)輸出得到，并由以下坐標(biāo)轉(zhuǎn)換公式，可得到機(jī)械手移動(dòng)之后所拍圖像中的像點(diǎn)(xp,yp)的世界坐標(biāo)(xw,yw)：

進(jìn)一步地，所述步驟(8)中，為了保證準(zhǔn)確貼裝，則待貼裝元件必須和目標(biāo)位置完全重合，這里首先保證待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行，由步驟(5)和(7)，可以得出由元件中心和元件mark點(diǎn)的連線組成的兩個(gè)向量，計(jì)算兩向量的夾角η之后，控制scara機(jī)械手的u軸轉(zhuǎn)動(dòng)η，使待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行。

進(jìn)一步地，所述步驟(9)中，由于元件吸取點(diǎn)與實(shí)際的元件中心有一定偏差，通過步驟(7)，可以得出由元件中心和吸嘴的連線組成的向量通過步驟(8)，已經(jīng)保證了待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行，現(xiàn)假設(shè)有目標(biāo)位置點(diǎn)z(xz,yz)，當(dāng)吸嘴移動(dòng)到此位置，即可保證待貼裝元件和目標(biāo)位置重合，那么由步驟(5)中的元件中心與目標(biāo)位置點(diǎn)z(xz,yz)組成的向量可以使得且

進(jìn)一步地，所述步驟(10)中，所述scara機(jī)械手轉(zhuǎn)動(dòng)關(guān)節(jié)移動(dòng)過程中，只要得到了關(guān)節(jié)旋轉(zhuǎn)折合到第二關(guān)節(jié)上的旋轉(zhuǎn)角度δ，控制scara機(jī)械手的u軸反向轉(zhuǎn)動(dòng)δ即可始終保證待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行，即完成正確貼裝。

本發(fā)明提出的技術(shù)方案具有以下有益效果：

本發(fā)明的新穎之處體現(xiàn)在下視相機(jī)的標(biāo)定上，采用自制的pcb標(biāo)定板，上、下視聯(lián)合標(biāo)定，然后scara機(jī)械手屬于多關(guān)節(jié)型機(jī)械手，它的移動(dòng)過程涉及到關(guān)節(jié)旋轉(zhuǎn)角度的變化，本技術(shù)方案給出了一套完整的坐標(biāo)轉(zhuǎn)換解決辦法。并且針對元件中心的吸取偏差也給出了相應(yīng)的解決辦法。因此本發(fā)明可以實(shí)現(xiàn)基于scara機(jī)械手對元件的準(zhǔn)確吸取和高精度貼裝，可廣泛應(yīng)用在基于scara機(jī)械手的工件裝配、視覺測量、平面定位之中。

附圖說明

下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步說明。

圖1是本發(fā)明的實(shí)施例提供的基于scara機(jī)械手的相機(jī)標(biāo)定和坐標(biāo)轉(zhuǎn)換方法的流程圖。

圖2是過圖校正的示意圖。

圖3是下視相機(jī)標(biāo)定在移動(dòng)過程中的坐標(biāo)變換示意圖。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施例對本發(fā)明的發(fā)明目的作進(jìn)一步詳細(xì)地描述，實(shí)施例不能在此一一贅述，但本發(fā)明的實(shí)施方式并不因此限定于以下實(shí)施例。

如圖1所示，一種基于scara機(jī)械手的相機(jī)標(biāo)定和坐標(biāo)變換的方法，包括步驟：

步驟s1：首先將上視相機(jī)固定安裝在工作平面以下的平臺(tái)上，然后將下視相機(jī)豎直固定安裝在scara機(jī)械手的第二關(guān)節(jié)末端并與機(jī)械手坐標(biāo)系、即世界坐標(biāo)系的z軸平行，此時(shí)下視相機(jī)可隨機(jī)械手在工作范圍內(nèi)移動(dòng)，要求位于機(jī)械手第三關(guān)節(jié)末端的吸嘴能夠在上視相機(jī)中清晰成像。

步驟s2：采用相機(jī)標(biāo)定技術(shù)對上視相機(jī)進(jìn)行標(biāo)定，得到上視相機(jī)的參數(shù)矩陣h1。

步驟s3：采用自制的pcb標(biāo)定板，進(jìn)行上下視的聯(lián)合標(biāo)定，即上、下視相機(jī)對著pcb上下兩面同一塊區(qū)域拍照，由于上視相機(jī)已標(biāo)定好，可利用上視相機(jī)得到pcb標(biāo)志點(diǎn)的世界坐標(biāo)，并和下視相機(jī)所拍照圖像中的標(biāo)志點(diǎn)一起完成下視相機(jī)的標(biāo)定，得到下視相機(jī)的參數(shù)矩陣h2。

步驟s4：計(jì)算此時(shí)下視相機(jī)標(biāo)定位置的關(guān)節(jié)角度，并記錄此時(shí)吸嘴的世界坐標(biāo)，即機(jī)械手在控制器的控制下進(jìn)行移動(dòng)拍照，由于機(jī)械手是關(guān)節(jié)型，關(guān)節(jié)的移動(dòng)軌跡呈扇形，所以要準(zhǔn)確獲得此時(shí)圖像中像點(diǎn)的世界坐標(biāo)(xw,yw)，必須知道第一關(guān)節(jié)和第二關(guān)節(jié)的旋轉(zhuǎn)角度α和β，實(shí)際使用的旋轉(zhuǎn)角度δ是此時(shí)位置和下視相機(jī)標(biāo)定位置時(shí)的關(guān)節(jié)角度之差，并折合到第二關(guān)節(jié)上，由于下視相機(jī)與吸嘴的距離固定，所以不管機(jī)械手怎么運(yùn)動(dòng)，下視相機(jī)所拍圖像中同一位置的像點(diǎn)(xp,yp)與吸嘴(xm,ym)的距離也是固定的，并且像點(diǎn)和吸嘴的連線l與機(jī)械手第二關(guān)節(jié)的夾角(實(shí)際使用時(shí)還需考慮坐標(biāo)象限)也是固定不變的；上述旋轉(zhuǎn)折合到第二關(guān)節(jié)上關(guān)節(jié)角度γ和吸嘴的實(shí)時(shí)世界坐標(biāo)(xm,ym)都可由機(jī)械手運(yùn)動(dòng)控制器的庫函數(shù)輸出得到，并由以下坐標(biāo)轉(zhuǎn)換公式(18)，可得到機(jī)械手移動(dòng)之后所拍圖像中的像點(diǎn)(xp,yp)的世界坐標(biāo)(xw,yw)。

步驟s5：獲取元件中心、元件mark點(diǎn)的世界坐標(biāo),即下視相機(jī)成功標(biāo)定之后，移動(dòng)下視相機(jī)對pcb板上待貼裝元件位置進(jìn)行拍照，計(jì)算出待貼裝元件的實(shí)際目標(biāo)位置,包括元件中心、元件mark點(diǎn)的世界坐標(biāo)。

步驟s6：移動(dòng)下視相機(jī)拍照待貼裝元件，通過坐標(biāo)轉(zhuǎn)換公式，計(jì)算待貼裝元件的位置，并準(zhǔn)確吸取。

步驟s7：識(shí)別元件中心、元件mark點(diǎn)并由標(biāo)定算法獲取兩點(diǎn)的世界坐標(biāo)，即在機(jī)械手移動(dòng)吸嘴成功吸取到規(guī)則元件(暫定為矩形貼片元件)之后，需要進(jìn)行上視相機(jī)過圖校正，運(yùn)用圖像處理算法，可以準(zhǔn)確識(shí)別元件中心、元件mark點(diǎn)并由標(biāo)定算法獲取兩點(diǎn)的世界坐標(biāo)。

步驟s8：由步驟(5)和(7)，經(jīng)過計(jì)算元件中心與元件mark點(diǎn)組成的向量夾角，旋轉(zhuǎn)u軸，保證元件姿態(tài)與目標(biāo)位置平行，為了保證準(zhǔn)確貼裝，則待貼裝元件必須和目標(biāo)位置完全重合。這里首先保證待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行，由步驟(5)和(7)，可以得出由元件中心和元件mark點(diǎn)的連線組成的兩個(gè)向量，計(jì)算兩向量的夾角η之后，控制scara機(jī)械手的u軸轉(zhuǎn)動(dòng)η，使待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行。

步驟s9：計(jì)算吸嘴在目標(biāo)位置中的坐標(biāo)，即讓待貼裝元件和目標(biāo)位置重合，由于元件吸取點(diǎn)與實(shí)際的元件中心有一定偏差，通過步驟(7)，可以得出由元件中心和吸嘴的連線組成的向量通過步驟(8)，已經(jīng)保證了待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行，現(xiàn)假設(shè)有目標(biāo)位置點(diǎn)z(xz,yz)，當(dāng)吸嘴移動(dòng)到此位置，即可保證待貼裝元件和目標(biāo)位置重合，那么由步驟(5)中的元件中心與目標(biāo)位置點(diǎn)z(xz,yz)組成的向量可以使得且

步驟s10：scara機(jī)械手移動(dòng)到目標(biāo)點(diǎn)z(xz,yz)，并在后續(xù)的scara機(jī)械手轉(zhuǎn)動(dòng)關(guān)節(jié)移動(dòng)過程中，只要得到了關(guān)節(jié)旋轉(zhuǎn)折合到第二關(guān)節(jié)上的旋轉(zhuǎn)角度δ，控制scara機(jī)械手的u軸反向轉(zhuǎn)動(dòng)δ即可始終保證待貼裝元件的姿態(tài)和目標(biāo)位置輪廓平行，從而完成貼裝。

圖2是過圖校正的示意圖，下面對在機(jī)械手移動(dòng)過程中的不同位置涉及到的坐標(biāo)轉(zhuǎn)換加以闡述：

圖2中涉及4個(gè)不同位置p0、p1、p2、p3(由元件的角點(diǎn)mp標(biāo)識(shí))。

在p3處(目標(biāo)位置)時(shí)：

由攝像頭可得坐標(biāo)并得到向量

在p0處(校準(zhǔn)位置)時(shí)：

由攝像頭可得坐標(biāo)，吸取點(diǎn)b(xb,yb)，并得到向量

進(jìn)而求解出與的夾角θ：

然后判斷旋轉(zhuǎn)角θ的方向，利用和的向量積，可得

此時(shí)：

若式(2)>0，則u軸逆時(shí)針旋轉(zhuǎn)θ；

若式(2)<0，則u軸順時(shí)針旋轉(zhuǎn)θ；

在p1處時(shí)：

由攝像頭可得坐標(biāo)可得向量和模

由平行且相等可得如下等式：

由(3)，(4)解得b'點(diǎn)坐標(biāo)：

由上式可得出關(guān)于b'點(diǎn)的4組解，對其進(jìn)行分類討論：

若的橫坐標(biāo)則xb'取+(“+”指的是xb'表達(dá)式中的根式前取“+”)。(下同)

若的橫坐標(biāo)則xb'取-。

若的縱坐標(biāo)則yb'取+。

若的縱坐標(biāo)則yb'取-。

在p2處時(shí)：

由圖2所示，在從p1移動(dòng)到p2的過程中，由關(guān)節(jié)轉(zhuǎn)動(dòng)可得：

α0+α1+α2＝δ(7)

其中關(guān)節(jié)j1和j2轉(zhuǎn)動(dòng)的α0，α1，α2，可由機(jī)械手本體自帶的庫函數(shù)獲得。

圖3為下視相機(jī)標(biāo)定在移動(dòng)過程中的坐標(biāo)變換示意圖。

如圖3所示，其中j1和j2為可自由移動(dòng)的轉(zhuǎn)軸。o-xy為不考慮z方向時(shí)的齊次世界坐標(biāo)系xy。l1、l2、l3、l4為輔助線，平行于o-xy的x軸。p1和p2為機(jī)械臂的兩個(gè)位置。u1和u2為像素坐標(biāo)對應(yīng)的世界坐標(biāo)的位置。l1和l2分別為u1和u2到吸嘴中心的連線長度。

由攝像機(jī)和機(jī)器人末端的相對位置x不變，我們可以知道l1和l2對應(yīng)的直線和j2的夾角應(yīng)該保持不變即：

β1＝β2(8)

我們通過分析，工作平面上的點(diǎn)都在同一水平面下，當(dāng)機(jī)器人末端帶著攝像機(jī)在xy方向上平移運(yùn)動(dòng)時(shí)，由小孔成像的基本原理我們可知，圖像坐標(biāo)中的兩個(gè)像素點(diǎn)在對應(yīng)的世界坐標(biāo)中的距離是保持不變的。從而由攝像機(jī)和機(jī)器人末端的相對位置保持不變的條件下，圖像坐標(biāo)中的標(biāo)志點(diǎn)對應(yīng)的世界坐標(biāo)和機(jī)器人末端吸嘴中心的距離是恒定不變的即：

l1＝l2(9)

l1：為u1到吸嘴中心的連線長度；l2：為u2到吸嘴中心的連線長度。

如圖3所示，我們在機(jī)器人末端的p1位置對小臂相機(jī)ccd2進(jìn)行固定相機(jī)的標(biāo)定。由于工作平面在同一水平面，且攝像機(jī)光軸垂直于工作平面，所以在世界坐標(biāo)系下，任意標(biāo)志點(diǎn)p的zw＝0。在這種情況下，世界坐標(biāo)系與攝像機(jī)坐標(biāo)系是平行平面的對應(yīng)關(guān)系，因此其模型可以簡化，攝像機(jī)外參數(shù)矩陣的旋轉(zhuǎn)部分退化為平面旋轉(zhuǎn)，且我們在固定的工作平面內(nèi)不關(guān)心z向的變化。從而我們可以利用前面固定相機(jī)的標(biāo)定方法，求出圖像坐標(biāo)與不考慮z方向時(shí)的齊次世界坐標(biāo)的關(guān)系矩陣h。

假設(shè)當(dāng)機(jī)器人末端帶著攝像機(jī)在xy方向上平移運(yùn)動(dòng)到了p2位置，在拍攝的圖像中有一點(diǎn)u2其像素坐標(biāo)為(u1,v1)，對應(yīng)在p1位置的相同像素坐標(biāo)點(diǎn)為u1。我們可以通過關(guān)系矩陣h獲得在u1處的世界坐標(biāo)，公式如下：

而吸嘴中心坐標(biāo)(x1,y1)可以通過控制器獲得，這樣通過獲得u1對應(yīng)的世界坐標(biāo)為(xp1,yp1)從而可以算出l1和α1：

其中：

當(dāng)xp1-x1<0時(shí)，

當(dāng)xp1-x1>0時(shí)，

由l1、l2平行，所以可以得到如下公式：

β1＝π-θ1+α1(13)

而θ1可以通過機(jī)械臂獲得：

同理可得

β2＝π-θ2+α2(14)

由(8)式可得：

α2＝θ2-θ1+α1(15)

在p2位置時(shí)吸嘴中心坐標(biāo)為(x2,y2)可以通過控制器獲得，u2對應(yīng)的世界坐標(biāo)為(xp2,yp2)。由l2和α2分別為直角三角形中的斜邊和夾角可知(xp2,yp2)：

xp2＝x2+l2×sin(α2)(16)

yp2＝y(tǒng)2+l2×cos(α2)(17)

由(15)、(16)和(17)可以得到如下式：

式中：

(x1,y1)：下視相機(jī)標(biāo)定位置時(shí)的吸嘴中心坐標(biāo)；

(x2,y2)：p2位置時(shí)的吸嘴中心坐標(biāo)；

(xp1,yp1)：u1對應(yīng)的世界坐標(biāo)；

(xp2,yp2)：u2對應(yīng)的世界坐標(biāo)。

本發(fā)明的上述實(shí)施例僅僅是為清楚地說明本發(fā)明所作的舉例，而并非是對本發(fā)明的實(shí)施方式的限定。對于所屬領(lǐng)域的普通技術(shù)人員來說，在上述說明的基礎(chǔ)上還可以做出其它不同形式的變化或變動(dòng)。這里無需也無法對所有的實(shí)施方式予以窮舉。凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明權(quán)利要求的保護(hù)范圍之內(nèi)。