基于結(jié)構(gòu)光的定位抓取系統(tǒng)設(shè)計(jì)

陳勇忠 林曉輝 劉建春，2

（1.廈門理工學(xué)院機(jī)械與汽車工程學(xué)院，福建廈門 361024；2.廈門市智能制造高端裝備研究重點(diǎn)實(shí)驗(yàn)室，福建廈門 361024）

0 引言

近年來(lái)，視覺引導(dǎo)工業(yè)機(jī)器人作業(yè)成為學(xué)者研究的熱點(diǎn)，尤其是在機(jī)器人抓取、安裝等方面應(yīng)用廣泛，其原理是經(jīng)視覺技術(shù)進(jìn)行三維重建后，實(shí)現(xiàn)對(duì)物體的位姿識(shí)別和定位。精確的位姿識(shí)別能有效提高機(jī)器人抓取準(zhǔn)確性，而目前的三維測(cè)量精度和速度仍處于探索研究的過(guò)程中。一般位姿主要包括旋轉(zhuǎn)和平移兩種狀態(tài)，目前常用的位姿計(jì)算方法主要有深度學(xué)習(xí)和點(diǎn)云配準(zhǔn)等[1]。其中，深度學(xué)習(xí)依賴大量樣本且訓(xùn)練耗時(shí)太長(zhǎng)，難以推廣應(yīng)用[2]。點(diǎn)云配準(zhǔn)是基于模板點(diǎn)云與目標(biāo)點(diǎn)云之間的相似特征點(diǎn)，求出旋轉(zhuǎn)平移矩陣，將兩個(gè)點(diǎn)云轉(zhuǎn)換到同一個(gè)坐標(biāo)系下，得到匹配參數(shù)后利用手眼標(biāo)定參數(shù)計(jì)算出機(jī)械手抓取位姿，能夠大幅縮短運(yùn)行時(shí)間，提高配準(zhǔn)精度[3]。本系統(tǒng)采用雙目加投影儀的視覺檢測(cè)方法，通過(guò)處理獲取三維點(diǎn)云數(shù)據(jù)，得到物體位姿信息，最后引導(dǎo)機(jī)械手對(duì)目標(biāo)物體進(jìn)行抓取，提高匹配與抓取精度。

1 定位抓取系統(tǒng)方案設(shè)計(jì)

搭建的定位抓取系統(tǒng)主要由兩個(gè)相機(jī)、一個(gè)DLP4500投影儀、龍門架、機(jī)器人及通信設(shè)備等組成。在實(shí)驗(yàn)平臺(tái)中，為更好地捕獲圖像，通常把投影儀放置在兩個(gè)相機(jī)之間，并調(diào)整好兩個(gè)相機(jī)的角度，滿足兩個(gè)相機(jī)視野的公共視場(chǎng)，使得圖像采集盡可能一致，整體實(shí)驗(yàn)搭建情況如圖1所示。

圖1 實(shí)驗(yàn)系統(tǒng)

整個(gè)定位抓取方案包含了雙目結(jié)構(gòu)光三維重建與點(diǎn)云處理的整體流程，主要步驟如下：

（1）相機(jī)標(biāo)定：首先左右相機(jī)同時(shí)采集10～15張標(biāo)定板圖片，完成相機(jī)標(biāo)定，得到雙目相機(jī)的內(nèi)外參數(shù)以及像素誤差。

（2）三維重建：左右相機(jī)分別采集帶有正弦光柵的12張圖片，基于三頻四步外差法以及相位校正對(duì)相位展開獲得左右相位圖，對(duì)相位圖進(jìn)行極線校正后完成相位匹配，得到待測(cè)物的視差圖。根據(jù)視差圖計(jì)算每個(gè)點(diǎn)的深度信息，獲得Z坐標(biāo)，再計(jì)算出每個(gè)點(diǎn)的X、Y坐標(biāo)，以點(diǎn)云的形式表達(dá)出來(lái)并保存，完成三維重建。

（3）點(diǎn)云處理與位姿估計(jì)：對(duì)三維重建的點(diǎn)云進(jìn)行濾波處理、關(guān)鍵點(diǎn)提取，利用FPFH特征直方圖求取點(diǎn)與鄰域點(diǎn)之間的法線相互關(guān)系，完成點(diǎn)云配準(zhǔn)后得到齊次矩陣，矩陣包含旋轉(zhuǎn)矩陣和平移矩陣。

（4）抓?。和瓿墒盅蹣?biāo)定得到工件坐標(biāo)系與相機(jī)坐標(biāo)系的轉(zhuǎn)換矩陣，結(jié)合位姿估計(jì)得到的物體與相機(jī)的關(guān)系矩陣，得出機(jī)器人與待抓取結(jié)構(gòu)件的位姿關(guān)系，控制機(jī)器人移動(dòng)到計(jì)算得到的位置完成抓取。

整體流程如圖2所示。

圖2 整體系統(tǒng)流程示意圖

2 系統(tǒng)標(biāo)定

2.1 相機(jī)標(biāo)定

相機(jī)標(biāo)定的目的是獲得相機(jī)的內(nèi)外參數(shù)和畸變參數(shù)，求出世界坐標(biāo)系與像素坐標(biāo)系之間的關(guān)系，常用的標(biāo)定方法是張正友標(biāo)定法[4]。雙目標(biāo)定與單目標(biāo)定的不同之處在于雙目標(biāo)定除了需要單目標(biāo)定的參數(shù)外，還有兩個(gè)相機(jī)之間的相對(duì)位置關(guān)系。一般都是以左相機(jī)為坐標(biāo)參考系，得出右相機(jī)相對(duì)于左相機(jī)的旋轉(zhuǎn)、平移關(guān)系。

左右相機(jī)的位置關(guān)系如下：

聯(lián)合式（1）（2）可得：

求得相機(jī)的外參數(shù)為：

雙目標(biāo)定流程圖如圖3所示，由于相機(jī)是朝下放置，光源在上方，所以對(duì)于標(biāo)定板的反光具有一定的要求，選擇了反光較弱的陶瓷材質(zhì)標(biāo)定板。將標(biāo)定板放置在實(shí)驗(yàn)平臺(tái)上，通過(guò)平面旋轉(zhuǎn)、移動(dòng)和傾斜與平臺(tái)呈現(xiàn)一定角度的標(biāo)定板，在雙目相機(jī)的視野范圍內(nèi)，拍攝15～20張圖片。

圖3 雙目相機(jī)標(biāo)定過(guò)程

使用MATLAB的標(biāo)定工具箱，角點(diǎn)識(shí)別標(biāo)定板完成單目標(biāo)定與雙目標(biāo)定。得到的雙目標(biāo)定各參數(shù)結(jié)果如表1、表2所示。

表1 左右相機(jī)內(nèi)參系數(shù)

表2 系統(tǒng)外參數(shù)

2.2 手眼標(biāo)定

采用Tsai-Lenz算法[5]，將標(biāo)定板固定在機(jī)械手末端，放置于相機(jī)視野范圍內(nèi)，移動(dòng)機(jī)械手末端使標(biāo)定板呈現(xiàn)不同的位姿并完成圖像采集，手眼標(biāo)定系統(tǒng)示意圖如圖4所示。

圖4 手眼標(biāo)定系統(tǒng)示意圖

分別定義TCR、TPC、TMR、TPM為機(jī)械臂基座坐標(biāo)系R與相機(jī)坐標(biāo)系C、相機(jī)坐標(biāo)系C與標(biāo)定板坐標(biāo)系P、機(jī)械臂基座坐標(biāo)系R與機(jī)械臂末端執(zhí)行器坐標(biāo)系M、機(jī)械臂末端執(zhí)行器坐標(biāo)系M與標(biāo)定板坐標(biāo)系P之間的轉(zhuǎn)換關(guān)系。建立4個(gè)坐標(biāo)系之間的運(yùn)動(dòng)方程：

TPC可以通過(guò)相機(jī)標(biāo)定得到，移動(dòng)帶有標(biāo)定板的機(jī)械臂，使標(biāo)定板在相機(jī)視野范圍內(nèi)，根據(jù)相機(jī)內(nèi)參系數(shù)計(jì)算出外部參數(shù)，最后得到在相機(jī)坐標(biāo)系下的標(biāo)定板位姿信息。TMR通過(guò)獲取機(jī)械臂各關(guān)節(jié)的參數(shù)和求解運(yùn)動(dòng)方程可得。所以，手眼標(biāo)定主要求解TCR和TPM的參數(shù)。

機(jī)械臂基座坐標(biāo)系R與相機(jī)坐標(biāo)系C的關(guān)系是保持不變的，標(biāo)定板固定在機(jī)械臂末端，所以機(jī)械臂末端執(zhí)行器坐標(biāo)系M與標(biāo)定板坐標(biāo)系P也保持不變，因此求解的TCR和TPM實(shí)際上是不變的矩陣。

由式（6）可知，標(biāo)定板移動(dòng)為任何姿態(tài)TPM都是固定值。

拍攝n次不同位姿的標(biāo)定板圖像，可得如下方程組：

整理方程組，用X代替TCR，可得：

式（8）用于求解AX=XB，AX主要與R和M有關(guān)，XB主要與C和P有關(guān)。最后將問(wèn)題轉(zhuǎn)換成求解旋轉(zhuǎn)、平移矩陣，即完成手眼標(biāo)定。得到的標(biāo)定誤差如表3所示。

表3 手眼標(biāo)定誤差

3 三維重建

異構(gòu)件的三維重建整體流程如圖5所示，首先對(duì)左右相機(jī)采集不同姿態(tài)的標(biāo)定板圖片，完成相機(jī)標(biāo)定。

圖5 三維重建流程

其次將正弦光柵投影到異構(gòu)件表面，并利用左右相機(jī)獲取圖像，如圖6所示。

圖6 實(shí)際場(chǎng)景圖

根據(jù)捕捉到的圖像分別進(jìn)行包裹相位的計(jì)算，完成相位校正和極線校正，通過(guò)相位匹配計(jì)算得到異構(gòu)件的視差圖，最后結(jié)合雙目系統(tǒng)求解得到異構(gòu)件的三維點(diǎn)云，完成三維重建，如圖7所示。

圖7 三維重建圖

4 位姿估計(jì)與定位抓取

為能高效率地實(shí)現(xiàn)位姿估計(jì)，完成定位，將流程分成兩個(gè)部分，模板點(diǎn)云采用離線操作，場(chǎng)景點(diǎn)云的位姿矩陣計(jì)算則利用在線部分的三維重建與模板點(diǎn)云進(jìn)行配準(zhǔn)。位姿估計(jì)流程圖如圖8所示。

圖8 位姿估計(jì)流程圖

實(shí)驗(yàn)方法步驟：

（1）建立離線模板點(diǎn)云庫(kù)，重建不同的異構(gòu)件點(diǎn)云，將重建效果好的點(diǎn)云經(jīng)過(guò)處理后導(dǎo)入模板庫(kù)。

（2）重建場(chǎng)景點(diǎn)云后完成點(diǎn)云預(yù)處理，并對(duì)其進(jìn)行歐式距離的聚類分割，根據(jù)抓取的優(yōu)先順序?qū)雽?duì)應(yīng)的模板，使用SAC-IA+ICP完成配準(zhǔn)，配準(zhǔn)誤差最小的即為匹配成功對(duì)。

（3）獲取位姿估計(jì)信息，即機(jī)器人的抓取位姿。在得到點(diǎn)云配準(zhǔn)的旋轉(zhuǎn)、平移矩陣后與手眼標(biāo)定矩陣結(jié)合，計(jì)算得到待抓取物體與工件坐標(biāo)系的位姿關(guān)系，將該關(guān)系的位姿矩陣寫入Rapid語(yǔ)言，機(jī)器人根據(jù)獲取到的位姿信息移動(dòng)到指定位置，將待抓取物吸附后移動(dòng)到指定放置地點(diǎn)。

4個(gè)異構(gòu)件在相機(jī)視野范圍內(nèi)隨意分散擺放，經(jīng)過(guò)三維重建、點(diǎn)云預(yù)處理、點(diǎn)云分割及點(diǎn)云配準(zhǔn)后，得到圖9所示點(diǎn)云分割結(jié)果圖和圖10所示點(diǎn)云配準(zhǔn)示意圖。

圖9 點(diǎn)云分割結(jié)果圖

圖10 點(diǎn)云配準(zhǔn)示意圖

根據(jù)配準(zhǔn)結(jié)果獲取抓取位姿后，控制機(jī)器人完成抓取。多種類異構(gòu)件的抓取示意圖如圖11所示。

圖11 不同種類的異構(gòu)件抓取

5 結(jié)語(yǔ)

本文以異構(gòu)件為研究對(duì)象對(duì)整個(gè)測(cè)量系統(tǒng)的關(guān)鍵技術(shù)進(jìn)行研究，搭建雙目結(jié)構(gòu)光系統(tǒng)，利用該系統(tǒng)獲得帶有場(chǎng)景的點(diǎn)云數(shù)據(jù)，通過(guò)點(diǎn)云處理得到的位姿估計(jì)結(jié)果引導(dǎo)機(jī)械臂完成抓取，驗(yàn)證了整體方案設(shè)計(jì)的可行性。