工業(yè)機(jī)器人手眼標(biāo)定綜合實(shí)驗(yàn)設(shè)計(jì)

茅靖峰，丁寅佳，王正堂，吳愛華

（1.南通大學(xué)電氣工程學(xué)院，江蘇南通 226019；2.無(wú)錫信捷電氣股份有限公司，江蘇無(wú)錫 214063）

0 引言

根據(jù)《“十四五”機(jī)器人產(chǎn)業(yè)發(fā)展規(guī)劃》統(tǒng)計(jì)，我國(guó)已經(jīng)連續(xù)8 年成為全球最大的工業(yè)機(jī)器人消費(fèi)國(guó)。隨著應(yīng)用領(lǐng)域的不斷擴(kuò)大，市場(chǎng)規(guī)模還將不斷提升，預(yù)計(jì)到2025 年工業(yè)機(jī)器人年銷量將突破45 萬(wàn)臺(tái)［1］，相關(guān)技術(shù)人才需求井噴式增長(zhǎng)［2-4］。

工業(yè)機(jī)器人技術(shù)課程是一門典型的多學(xué)科深度交叉融合的新興前沿專業(yè)課程［5-6］，涉及數(shù)學(xué)建模、控制、計(jì)算機(jī)、機(jī)械、電氣、電子、人工智能等諸多內(nèi)容［7-9］。為了便于學(xué)生學(xué)習(xí)理解、提高學(xué)生學(xué)習(xí)興趣，增進(jìn)學(xué)生實(shí)操能力和創(chuàng)新素養(yǎng)培養(yǎng)，設(shè)計(jì)具有多學(xué)科交叉、多技術(shù)融通特點(diǎn)的綜合性實(shí)驗(yàn)項(xiàng)目是非常必要的。

針對(duì)由機(jī)器視覺引導(dǎo)的機(jī)械臂抓取技術(shù)教學(xué)內(nèi)容，采用ABB公司的IRB-140 型工業(yè)機(jī)器人和梅卡曼德Mech-Eye DEEP系列的3D相機(jī)，設(shè)計(jì)了基于Visual Studio的工業(yè)機(jī)器人眼在手外模式的機(jī)械臂手眼標(biāo)定綜合實(shí)驗(yàn)，幫助學(xué)生更好地理解手眼標(biāo)定理論知識(shí)。

1 眼在手外手眼標(biāo)定實(shí)驗(yàn)設(shè)計(jì)

1.1 眼在手外手眼標(biāo)定原理

手眼標(biāo)定是指統(tǒng)一視覺系統(tǒng)和機(jī)器人的坐標(biāo)系，將視覺系統(tǒng)所確定的物體位姿轉(zhuǎn)換到機(jī)器人坐標(biāo)系下，由機(jī)器人完成對(duì)物體的作業(yè)。要實(shí)現(xiàn)圖像目標(biāo)點(diǎn)到實(shí)際物體抓取點(diǎn)之間的坐標(biāo)轉(zhuǎn)換，就必須擁有準(zhǔn)確的內(nèi)外參信息。相機(jī)內(nèi)參是相機(jī)內(nèi)部的基本參數(shù)，包括鏡頭焦距、畸變等，一般相機(jī)出廠時(shí)內(nèi)參已標(biāo)定完成，保存在相機(jī)內(nèi)部。相機(jī)外參是機(jī)器人與相機(jī)之間的位姿轉(zhuǎn)換關(guān)系，即手眼關(guān)系，因此相機(jī)外參的標(biāo)定被稱為機(jī)器人的手眼標(biāo)定［10-11］。

眼在手外的標(biāo)定方式是將相機(jī)固定在被拍攝物體上方一定高度的支架上，相機(jī)和機(jī)械臂分離，此時(shí)相對(duì)于機(jī)器人基座相機(jī)是固定的，機(jī)械臂在運(yùn)動(dòng)過程中對(duì)相機(jī)不產(chǎn)生影響，主要標(biāo)定相機(jī)和機(jī)械臂基底的轉(zhuǎn)換矩陣，如圖1 所示。圖1 中，HCAM，ROB為相機(jī)和機(jī)械臂基底的轉(zhuǎn)換矩陣。

眼在手外的標(biāo)定方式需要將相機(jī)架設(shè)在距離被拍攝物體上方一定高度的固定支架上，此時(shí)相機(jī)不能隨機(jī)器人運(yùn)動(dòng)，并且不干涉機(jī)器人的運(yùn)動(dòng)空間，避免機(jī)器人與相機(jī)發(fā)生碰撞。將標(biāo)定板固定在機(jī)械臂末端，組成的手眼標(biāo)定坐標(biāo)系有機(jī)械臂基坐標(biāo)系base（ba）、機(jī)械臂末端坐標(biāo)系end（e）、相機(jī)坐標(biāo)系camera（c）和標(biāo)定板坐標(biāo)系board（b）。圖2 為手眼標(biāo)定坐標(biāo)系示意圖。圖2 中，K為所要求解的手眼矩陣，A、B為根據(jù)標(biāo)定圖片列出等式后最終化簡(jiǎn)得到的2 個(gè)參數(shù)矩陣。

圖2 手眼標(biāo)定坐標(biāo)系示意圖

眼在手外手眼標(biāo)定的目標(biāo)是求解相機(jī)坐標(biāo)系到機(jī)械臂基坐標(biāo)系的變換矩陣，記為Mba，c，也稱為手眼矩陣。使用相機(jī)拍攝n（n＞3）張不同機(jī)械臂姿態(tài)下的標(biāo)定板圖片，每張圖片均應(yīng)符合

式中：Mba，e為機(jī)械臂末端坐標(biāo)系到機(jī)械臂基坐標(biāo)系的變換矩陣；Me，b為標(biāo)定板坐標(biāo)系到機(jī)械臂末端坐標(biāo)系的變換矩陣；Mb，c為相機(jī)坐標(biāo)系到標(biāo)定板坐標(biāo)系的變換矩陣。

標(biāo)定板是固定在機(jī)械臂末端的，對(duì)于每張圖片標(biāo)定板坐標(biāo)系到機(jī)械臂末端坐標(biāo)系變換矩陣都相同，即Me，b不變。將式（1）進(jìn)行變形，可得

對(duì)于第1 張和第2 張標(biāo)定圖片都有式（2）關(guān)系，因此根據(jù)Me，b相等可得到以下等式：

式中：下標(biāo)1 表示第1 張標(biāo)定圖片；下標(biāo)2 表示第2 張標(biāo)定圖片。對(duì)式（3）進(jìn)行化簡(jiǎn)，等式兩邊分別左乘M（ba，e）2、右乘M（b，c）1，可得

因此，對(duì)于n張標(biāo)定圖片可以得到（n-1）個(gè)等式：

式中：Mc（1→2）為相機(jī)坐標(biāo)系下第1 張標(biāo)定圖片到第2張標(biāo)定圖片的變換矩陣；Mba（1→2）為基坐標(biāo)下機(jī)械臂末端位姿1 到位姿2 的變換矩陣。因此，方程組（5）可以化簡(jiǎn)為：

Mba，c即為K，表示所要求解的相機(jī)坐標(biāo)系到機(jī)械臂基坐標(biāo)系的變換矩陣；Mba（n-1→n）即為參數(shù)矩陣A，表示基坐標(biāo)系下機(jī)械臂末端位姿（n-1）到位姿n的變換矩陣；Mc（n-1→n）即為參數(shù)矩陣B，表示相機(jī)坐標(biāo)系下第（n-1）張標(biāo)定圖片到第n張標(biāo)定圖片的變換矩陣。方程組（9）可簡(jiǎn)化為一個(gè)包含（n-1）個(gè)方程的經(jīng)典方程組，即AK=KB［12］。通過求解參數(shù)矩陣A和參數(shù)矩陣B，即可計(jì)算出手眼變換矩陣K。

1.2 眼在手外手眼標(biāo)定實(shí)驗(yàn)步驟

1.2.1 求取機(jī)械臂末端位姿變換矩陣

任意一個(gè)三維空間中的定向（orientation）都可以被表示為一個(gè)繞某個(gè)特定軸的旋轉(zhuǎn)，給定旋轉(zhuǎn)軸和旋轉(zhuǎn)角度就可以把其他形式的旋轉(zhuǎn)表示如歐拉角、旋轉(zhuǎn)矩陣轉(zhuǎn)化為四元數(shù)或者將四元數(shù)轉(zhuǎn)化為其他形式的旋轉(zhuǎn)表示［13］。將位置信息轉(zhuǎn)化為3 ×1 的平移矩陣，四元數(shù)轉(zhuǎn)化為3 ×3 的旋轉(zhuǎn)矩陣，就可以得到由平移矩陣和旋轉(zhuǎn)矩陣組成的機(jī)械臂末端到基坐標(biāo)系的4 ×4 的變換矩陣。

機(jī)械臂末端的位姿信息可以在示教器中獲得，示教器中的位姿信息如圖3 所示。圖3 中，X、Y、Z表示位置信息，q1、q2、q3、q4為四元數(shù)，用來(lái)描述三維空間中的姿態(tài)。

圖3 示教器中的位姿信息

圖3（a）為位姿1 信息圖，X、Y、Z分別為784.94、12.72、634.18 mm，q1、q2、q3、q4分別為0.005 29、0.928 74、0.370 40、-0.014 76；圖3（b）為位姿2 信息圖，X、Y、Z分別為1 083.48、-322.93、634.19 mm，q1、q2、q3、q4分別為0.005 29、0.928 74、0.370 41、-0.014 74。

參數(shù)矩陣A 是基坐標(biāo)系下機(jī)械臂末端位姿（n-1）到位姿n的變換矩陣，可以將某個(gè)位姿下對(duì)應(yīng)的姿態(tài)信息和位置信息轉(zhuǎn)化為旋轉(zhuǎn)矩陣RA和平移矩陣TA，然后得到基坐標(biāo)系下的變換矩陣MA，分別為：

根據(jù)式（12），可得到位姿1 的變換矩陣MA1、位姿2 的變換矩陣MA2。將位姿1 到位姿2 的變換矩陣設(shè)為A1，即MA1A1=MA2，可以得到，此時(shí)以圖3 中位姿1 和位姿2為例，得到MA1、MA2和A1，分別為：

在實(shí)際實(shí)驗(yàn)中，首先通過示教器，獲取n=20 種不同位姿下機(jī)械臂的位姿信息，構(gòu)造出具有19 個(gè)等式的方程組（9）；然后使用Visual Studio編程軟件，將20 種位姿根據(jù)式（10）～（12），計(jì)算得到20 個(gè)基坐標(biāo)系下的位姿矩陣MAn，即表示基坐標(biāo)系下機(jī)械臂末端位姿（n-1）到位姿n的變換矩陣；最后通過20 個(gè)位姿變換矩陣之間的轉(zhuǎn)換，計(jì)算得到19 個(gè)位姿變換矩陣An。

1.2.2 求取標(biāo)定板圖片變換矩陣

拍攝標(biāo)定板圖片前需要選取一個(gè)正常標(biāo)定板。標(biāo)定板上圓點(diǎn)清晰，沒有明顯的刮花痕跡，標(biāo)定板沒有明顯彎曲變形。圖4 為實(shí)驗(yàn)標(biāo)定板示意圖和標(biāo)定現(xiàn)場(chǎng)圖，實(shí)驗(yàn)所用標(biāo)定板是圓形標(biāo)定圖案。

圖4 實(shí)驗(yàn)標(biāo)定板示意圖和標(biāo)定現(xiàn)場(chǎng)圖

圖4（a）為實(shí)驗(yàn)所用的標(biāo)定板示意圖，標(biāo)定板上的圓形圖案是非對(duì)稱的，共有4 行5 列，20 個(gè)圓，圓心距為100 mm。拍攝的標(biāo)定板圖片實(shí)際上是標(biāo)記標(biāo)定板上20 個(gè)圓的圓心坐標(biāo)。將標(biāo)定板安裝在機(jī)械臂末端進(jìn)行拍攝。圖4（b）為標(biāo)定現(xiàn)場(chǎng)，采用ABB 公司的IRB-140 型工業(yè)機(jī)器人和梅卡曼德Mech-Eye DEEP系列的3D相機(jī)。

Microsoft Visual Studio 是支持多種程序設(shè)計(jì)語(yǔ)言的集成開發(fā)環(huán)境，所寫的目標(biāo)代碼適用于微軟支持的所有平臺(tái)，可用于編輯、調(diào)試并生成代碼，然后發(fā)布應(yīng)用。使用Visual Studio編程軟件調(diào)用OpenCV 庫(kù)中的findCirclesGrid（）函數(shù)來(lái)獲取標(biāo)定板中圓心點(diǎn)坐標(biāo)，findCirclesGrid（）函數(shù)返回的是圓心列表，即識(shí)別出標(biāo)定板中的圓，并獲取到圓心坐標(biāo)。

圖5 為標(biāo)定圖片識(shí)別出的圓心點(diǎn)及得到的二維坐標(biāo)。將二維坐標(biāo)映射到三維點(diǎn)云上，即可獲取點(diǎn)云上圓心點(diǎn)的三維坐標(biāo)。圖6 為映射的三維點(diǎn)云及對(duì)應(yīng)的三維坐標(biāo)。

圖5 識(shí)別到的圓心點(diǎn)及其坐標(biāo)

圖6 映射的點(diǎn)云及對(duì)應(yīng)的三維坐標(biāo)

機(jī)械臂在n種位姿下拍攝對(duì)應(yīng)的n張標(biāo)定圖片，每張圖片依次進(jìn)行如上操作均可以獲得n組三維坐標(biāo)。參數(shù)矩陣B 表示相機(jī)坐標(biāo)系下第（n-1）張標(biāo)定圖片到第n張標(biāo)定圖片的變換矩陣，可以轉(zhuǎn)換為第（n-1）組三維點(diǎn)集到第n組三維點(diǎn)集的變換矩陣，即求2個(gè)點(diǎn)集之間的旋轉(zhuǎn)矩陣RB和平移矩陣TB。將第1組的點(diǎn)集設(shè)為：L=｛l1，l2，…，lm｝，第2 組的點(diǎn)集設(shè)為：S=｛s1，s2，…，sm｝，其中每個(gè)點(diǎn)都是（x，y，z）三維的。首先，分別計(jì)算每個(gè)集合的中心點(diǎn)lc和sc：

式中，m表示標(biāo)定點(diǎn)的數(shù)量，即拍攝的每張標(biāo)定圖片上的圓心數(shù)量。其次，將點(diǎn)集中所有的點(diǎn)減去中心點(diǎn)，生成新的點(diǎn)集L′和S′，即：

接著，計(jì)算新的點(diǎn)集之間的協(xié)方差矩陣

最后，通過對(duì)協(xié)方差矩陣進(jìn)行奇異值分解［14］，獲得對(duì)應(yīng)的奇異向量U和V即：

式中，Σ 為對(duì)角陣，對(duì)角元素為奇異值。此時(shí)，旋轉(zhuǎn)矩陣RB、平移矩陣TB和B分別為：

以拍攝2 張標(biāo)定圖片為例，使用Visual Studio 編程軟件分別提取標(biāo)定圖片中的圓心坐標(biāo)，然后映射為三維坐標(biāo)，最后通過這2 組三維點(diǎn)集就可以得到第1個(gè)參數(shù)矩陣B1：

實(shí)際實(shí)驗(yàn)中，拍攝20 張標(biāo)定圖片，拍攝的每張標(biāo)定圖片上的圓有20 個(gè)，即m=20。經(jīng)過上述操作，由20 張標(biāo)定圖片信息可以計(jì)算得到參數(shù)矩陣B。

1.2.3 求取手眼矩陣

將方程組（9）轉(zhuǎn)化為

式中：Mba，ij為機(jī)器人在基坐標(biāo)系下i時(shí)刻的末端位姿到j(luò)時(shí)刻的末端位姿的變換矩陣；Mc，ij為相機(jī)坐標(biāo)系下i時(shí)刻的標(biāo)定板上標(biāo)記點(diǎn)到j(luò)時(shí)刻的標(biāo)定板上標(biāo)記點(diǎn)的變換矩陣；K 為所求解的相機(jī)到機(jī)械臂基底的齊次變換矩陣，由3 ×3 的旋轉(zhuǎn)矩陣R 和3 ×1 的平移矩陣T組成。旋轉(zhuǎn)矩陣R為正交矩陣，公式如下所示：

在使用Tsai 兩步法［15］求解K 時(shí)，先求解旋轉(zhuǎn)部分R，再使用旋轉(zhuǎn)部分求解平移部分T。將式（20）展開得

式中：Rba，ij為機(jī)器人在基坐標(biāo)系下i時(shí)刻的末端位姿到j(luò)時(shí)刻的末端位姿的旋轉(zhuǎn)矩陣；Tba，ij為機(jī)器人在基坐標(biāo)系下i時(shí)刻的末端位姿到j(luò)時(shí)刻的末端位姿的平移矩陣；Rc，ij為相機(jī)坐標(biāo)系下i時(shí)刻的標(biāo)定板上標(biāo)記點(diǎn)到j(luò)時(shí)刻的標(biāo)定板上標(biāo)記點(diǎn)的旋轉(zhuǎn)矩陣；Tc，ij為相機(jī)坐標(biāo)系下i時(shí)刻的標(biāo)定板上標(biāo)記點(diǎn)到j(luò)時(shí)刻的標(biāo)定板上標(biāo)記點(diǎn)的平移矩陣。將式（22）化簡(jiǎn)后得到

將式（23）展開后得：

對(duì)式（24）化簡(jiǎn)后可得：

式中，I表示單位矩陣。使用Visual Studio編程軟件編寫程序，采用Tsai算法求解得到手眼標(biāo)定的旋轉(zhuǎn)矩陣R和平移矩陣T分別為：

K的旋轉(zhuǎn)部分為正交矩陣，需要驗(yàn)證所求手眼矩陣中旋轉(zhuǎn)矩陣R是否為正交矩陣。若R為正交矩陣，則滿足RRT=I，即RT=R-1。因此，只需驗(yàn)證RR-1是否為單位陣。計(jì)算RR-1，可得

RR-1=I滿足條件，即R為正交矩陣。故可得

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

對(duì)于標(biāo)定好的外參數(shù)據(jù)，需要對(duì)得到手眼矩陣后的標(biāo)定結(jié)果進(jìn)行誤差分析，同時(shí)判斷是否滿足精度要求。標(biāo)定板上標(biāo)記的是20 個(gè)圓心坐標(biāo)，映射到三維點(diǎn)云上就是20 組三維坐標(biāo)。將20 個(gè)圓分為5 個(gè)部分，如圖7 所示，分別為左上、左下、中間、右上和右下5 個(gè)部分，每個(gè)部分有4 個(gè)圓。

圖7 標(biāo)定板上圓劃分區(qū)域示意圖

將標(biāo)定板上的圓心映射到三維點(diǎn)云上后每部分用4 組三維坐標(biāo)的平均值來(lái)代替。將相機(jī)坐標(biāo)系下5 個(gè)部分的標(biāo)記點(diǎn)通過手眼矩陣和機(jī)械臂末端姿態(tài)信息轉(zhuǎn)換到機(jī)械臂基坐標(biāo)系下，與實(shí)際值進(jìn)行對(duì)比。

點(diǎn)云的誤差參考標(biāo)準(zhǔn)為：一般工程場(chǎng)景下，要求所有數(shù)據(jù)點(diǎn)誤差小于3 mm；高精度場(chǎng)景下，要求所有數(shù)據(jù)點(diǎn)誤差小于2 mm；碼垛拆垛場(chǎng)景下，要求所有數(shù)據(jù)點(diǎn)誤差小于5 mm。表1 為手眼標(biāo)定數(shù)據(jù)。通過實(shí)驗(yàn)數(shù)據(jù)求得手眼矩陣并分析得到手眼標(biāo)定在3 個(gè)方向的總平均誤差為±2 mm左右，符合各種場(chǎng)景要求。

表1 手眼標(biāo)定數(shù)據(jù) mm

3 結(jié)語(yǔ)

采用ABB 公司的IRB-140 型工業(yè)機(jī)器人和梅卡曼德Mech-Eye DEEP 系列的3D 相機(jī)，以眼在手外的手眼標(biāo)定方法，運(yùn)用機(jī)械臂、相機(jī)、標(biāo)定板和Visual Studio編寫程序，開發(fā)了工業(yè)機(jī)器人眼在手外模式的機(jī)械臂手眼標(biāo)定綜合實(shí)驗(yàn)。實(shí)踐證明，通過眼在手外的手眼標(biāo)定系統(tǒng)，獲取機(jī)械臂末端位姿變換矩陣、標(biāo)定板圖片變換矩陣和手眼矩陣，并進(jìn)行手眼標(biāo)定誤差分析，可幫助學(xué)生加深眼在手外的手眼標(biāo)定知識(shí)理解。