基于三維點(diǎn)云的多指機(jī)械手抓取應(yīng)用研究

關(guān)鍵詞：機(jī)器人；未知物體抓?。稽c(diǎn)云重建；高斯過程

0 引言

近年來，機(jī)器人技術(shù)快速發(fā)展，尤其在工業(yè)制造、康復(fù)醫(yī)療、倉儲(chǔ)物流、家庭服務(wù)等領(lǐng)域有著廣泛的應(yīng)用[1-6]。在工業(yè)制造領(lǐng)域，機(jī)器人完成零部件分揀、組裝等復(fù)雜作業(yè)任務(wù)時(shí)，通常需要先執(zhí)行抓取動(dòng)作。因此，如何實(shí)現(xiàn)對(duì)未知物體的精確感知和準(zhǔn)確抓取，對(duì)機(jī)器人能否快速適應(yīng)不同環(huán)境和任務(wù)需求至關(guān)重要，同時(shí)，對(duì)于提升智能化水平有著重要意義[7-9]。

針對(duì)未知物體的精確感知和準(zhǔn)確抓取等問題，結(jié)合三維點(diǎn)云技術(shù)，提出了一種基于多角度視覺信息的機(jī)器人抓取規(guī)劃方案。首先，利用深度相機(jī)采集未知物體多角度點(diǎn)云圖像，利用點(diǎn)云配準(zhǔn)等方法對(duì)未知物體進(jìn)行提取和三維重建，從而獲取未知物體位置和形狀信息；其次，采用高斯過程擬合未知物體隱式曲面，提出了一種基于隱式曲面的吸引子運(yùn)動(dòng)機(jī)械手動(dòng)態(tài)抓取規(guī)劃方案；最后，通過試驗(yàn)驗(yàn)證了所提出方案的可行性和有效性。

1 基于三維點(diǎn)云的物體重建

1. 1 手眼標(biāo)定

機(jī)械臂執(zhí)行抓取任務(wù)時(shí)，通常必須進(jìn)行手眼標(biāo)定，以確定機(jī)械臂末端執(zhí)行器坐標(biāo)系與相機(jī)坐標(biāo)系之間的變換關(guān)系，從而實(shí)現(xiàn)機(jī)械臂對(duì)目標(biāo)的定位和抓取[10]。圖1為手眼標(biāo)定原理圖。假設(shè)機(jī)械臂末端相對(duì)于基坐標(biāo)系的不同位姿分別為bT （i e ）和 bTe（ j ），此時(shí)，空間中某一點(diǎn)在相機(jī)坐標(biāo)系下的表示分別為cTt（i）和cTt（ j ），該點(diǎn)在基坐標(biāo)系下的表示為bTt。定義相機(jī)坐標(biāo)系與機(jī)械臂末端坐標(biāo)系之間的變換矩陣為eTc，由此可得

進(jìn)而，可由機(jī)械臂末端多個(gè)位姿下的相關(guān)信息擬合得出eTc。

1. 2 物體點(diǎn)云獲取與處理

如圖2（a）所示，由RealSense 深度相機(jī)得到的點(diǎn)云圖包括待抓取物體以及周圍環(huán)境。為提高機(jī)器人抓取效率和準(zhǔn)確度，必須提取出待抓取物體點(diǎn)云圖像。首先，可篩選出桌面以上物體的點(diǎn)云，如圖2（b）所示；其次，采用半徑濾波方法除去桌面點(diǎn)云圖中的噪聲并對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行平滑處理，如圖2（c）所示；為將桌面未知物體點(diǎn)云單獨(dú)提取出來，采用基于距離的聚類算法進(jìn)行分割處理，得到桌面各個(gè)物體的點(diǎn)云圖，如圖2（d）所示。

由于所提取物體點(diǎn)云圖包含的采樣點(diǎn)較多，因此，采用基于體素的降采樣方法進(jìn)行處理，即將點(diǎn)云空間劃分為等大小的體素，每個(gè)體素內(nèi)僅保留1個(gè)點(diǎn)或中心點(diǎn)，這樣既減少了點(diǎn)云數(shù)據(jù)量，也保留了原始點(diǎn)云的形狀和結(jié)構(gòu)[11-12]。以相機(jī)紙盒為例，得到的降采樣點(diǎn)云及各點(diǎn)法向量如圖3所示。

2 機(jī)械手抓取規(guī)劃

2. 1 基于高斯過程的隱式曲面擬合

采用改進(jìn)的高斯隱式曲面（Gaussian Process Im?plicit Surface， GPIS）方法來擬合待抓取物體表面及其法線方向[13]。首先，定義三維空間中的多元函數(shù)y =g （x），其中，x 為三維空間中的點(diǎn)；y = [l nT ]T；l 為從該點(diǎn)位置到曲面表面的有向距離。顯然，當(dāng)x 在物體表面時(shí)，l = 0；當(dāng)x 在物體內(nèi)部時(shí)，l lt; 0；n 為點(diǎn)x 所在位置單位法線。由此可知，輸出l = 0的點(diǎn)組成的曲面即為待抓取物體表面。

高斯過程回歸的核心思想為：對(duì)于給定的輸入x，輸出y 可看作是隨機(jī)變量，其均值和方差可用高斯分布來表示y ～ N [ f （x），σ2 ]。假設(shè)有n 組訓(xùn)練數(shù)據(jù)（X，Y），其中，X = （x1，x2，…，xn ），Y =（y1，y2，…，yn ）。由于f （x） 未知，假設(shè)其服從高斯分布， 即f （x） ～ N [m （x），k （xa，xb ） ]， 其中， m （x）是f （x）的均值函數(shù)；k （xa，xb ）是f （x）的協(xié)方差函數(shù)，其定義如下

在訓(xùn)練過程中，通過最大化后驗(yàn)概率來確定f （x） 的參數(shù)。對(duì)于新的輸入x*，可用訓(xùn)練數(shù)據(jù)和先驗(yàn)分布求出x* 的后驗(yàn)分布。假設(shè)該分布是均值為μ*、方差為σ2*的高斯分布，那么有

式中，k* = [ k （x*，x1 ），k （x*，x2 ），…，k （x*，xn ） ]；K為n × n 階矩陣，其元素為Ki，j = k （xi，xj ）；In 為n ×n 階的單位矩陣；σ2 為噪聲項(xiàng)的方差。

由待抓取物體三維點(diǎn)云可獲取其表面點(diǎn)云P ={ p1，p2，…，pm } 所對(duì)應(yīng)的法線N ={ n1，n2，…，nm }，其中，m 為采樣點(diǎn)數(shù)量。令pmax為點(diǎn)云中距離最遠(yuǎn)的兩個(gè)點(diǎn)之間的距離；-p為點(diǎn)云中心。對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行歸一化處理，有

實(shí)際試驗(yàn)過程中，為了使模型通過基于高斯過程的方法擬合出來的表面更好，需要增加一組內(nèi)部點(diǎn)xin = [ 0 0 0 ]T，對(duì)應(yīng)的yin = [ -1 0 0 0 ]T 和均勻分布在單位球面上的外部采樣點(diǎn)xouti = ri，對(duì)應(yīng)的youti = [1 r Ti ]T，其中，ri 為從中心點(diǎn)出發(fā)、指向單位球面上外部采樣點(diǎn)的單位向量。由未知物體點(diǎn)云信息，將以上物體內(nèi)部和若干物體表面點(diǎn)作為訓(xùn)練數(shù)據(jù)進(jìn)行高斯過程回歸，即可擬合出未知物體點(diǎn)云基于高斯過程的隱式曲面。

2. 2 基于吸引子的動(dòng)態(tài)抓取規(guī)劃

試驗(yàn)平臺(tái)使用三指機(jī)械手，因此，抓取中引入3個(gè)吸引子，吸引子被約束在前文所建立的高斯隱式曲面表面上。吸引子會(huì)在該表面搜索與指尖法向量相反的抓取位置，并牽引指尖至對(duì)應(yīng)的抓取點(diǎn)進(jìn)行抓取。機(jī)械手初始掌姿對(duì)于執(zhí)行抓取動(dòng)作至關(guān)重要，不同形狀物體應(yīng)采用合適的抓握掌姿[14]。因此，如圖4所示，對(duì)初始抓握掌姿規(guī)定如下：對(duì)于長方體類物體，即|zmax - zmid| lt; |ymax - ymid|amp;amp;|xmax - xmid|，采用掌心朝下的抓握掌姿；對(duì)于圓柱體類物體，即|zmax -zmid| gt; |ymax - ymid|amp;amp;|xmax - xmid| 時(shí)， 采用橫向的抓握掌姿。其中，xmid、ymid、zmid 分別為點(diǎn)云中心在x、y、z 軸上的坐標(biāo)；xmax、ymax、zmax 分別為物體點(diǎn)云在x、y、z 軸上距離點(diǎn)云中心距離最遠(yuǎn)點(diǎn)的坐標(biāo)。

根據(jù)未知物體表面確定機(jī)械手初始位置、掌姿以及初始手指構(gòu)型后，將每個(gè)指尖中心點(diǎn)到點(diǎn)云圖中心點(diǎn)連線與基于高斯過程所擬合的未知物體隱式曲面交點(diǎn)作為3個(gè)吸引子的初始位置。機(jī)械手和對(duì)應(yīng)的吸引子構(gòu)成的動(dòng)態(tài)系統(tǒng)開始對(duì)吸引子的運(yùn)動(dòng)做出規(guī)劃，其目的是使吸引子移動(dòng)到物體表面法向方向與指尖法向方向相反的位置，并在機(jī)械手指尖位置與吸引子位置重合的同時(shí)，將指尖姿態(tài)調(diào)整為自身法向量與吸引子法向量的反方向重合。如圖5所示，在抓取規(guī)劃的迭代過程中，吸引子完成保持在物體表面以及搜索物體表面法向的目標(biāo)，其運(yùn)動(dòng)可分解為在曲面局部切面的運(yùn)動(dòng)以及沿著法向量方向的運(yùn)動(dòng)，其運(yùn)動(dòng)速度分別定義為vtani 和vnmli（i = 1，2，3）。機(jī)械手3個(gè)手指指尖運(yùn)動(dòng)速度定義為vfini，其運(yùn)動(dòng)目標(biāo)是使指尖位置與吸引子位置重合，且指尖法向量與未知物體的隱式曲面法向量相反。

3 機(jī)器人抓取試驗(yàn)

所搭建的機(jī)器人視覺抓取試驗(yàn)平臺(tái)主要包括：6自由度艾力特協(xié)作機(jī)器臂Elite-EC66、RealSenseD455深度相機(jī)、DoraHand-3F三指機(jī)械手以及待抓取物體。控制系統(tǒng)采用ROS，支持C++、Python等多種編程語言。DoraHand-3F有8個(gè)主動(dòng)自由度，末端抓取力為25 N，最大抓取負(fù)載為6 kg，可通過Ether?net接口進(jìn)行控制。將RealSense D455深度相機(jī)安裝于DoraHand-3F機(jī)械手法蘭盤支架，內(nèi)置有雙目攝像頭和深度攝像頭，理想測量距離在0. 6～6 m，其深度攝像頭FOV水平視角為87°，垂直視角為58°，分辨率可達(dá)1 280×720，能夠?qū)崿F(xiàn)高精度的三維深度數(shù)據(jù)捕捉和圖像采集。

選擇所獲取的相機(jī)紙盒點(diǎn)云進(jìn)行抓取計(jì)算，則機(jī)械手3 個(gè)抓取點(diǎn)的位置和指尖向量分別為P1[-0. 109 0. 482 -0. 007]，N1[0. 329 -0. 911 0. 250]，P2[-0. 020 0. 508 -0. 005]， N2[-0. 010 -0. 968 0. 251]，P3[-0. 071 0. 405 0. 003]， N3[0. 121 0. 992 -0. 045]。其中，Pn 表示抓取點(diǎn)的位置；Nn 表示該點(diǎn)指尖法向量。由此，可計(jì)算出手掌姿態(tài)Tpalm 和機(jī)械手8個(gè)關(guān)節(jié)的具體角度qpos，分別為

圖6 所示為由Elite 機(jī)械臂和DoraHand-3F 機(jī)械手共同實(shí)現(xiàn)的對(duì)桌面上相機(jī)紙盒的實(shí)際抓取過程。機(jī)械臂和機(jī)械手在接收到初始信號(hào)后，機(jī)械手張開手指并按照規(guī)劃路徑運(yùn)動(dòng)靠近待抓取物體；當(dāng)?shù)竭_(dá)相應(yīng)掌姿位置時(shí)，機(jī)械手將其手指閉合至規(guī)劃出的期望指關(guān)節(jié)角度；隨后機(jī)械臂運(yùn)動(dòng)回原位，實(shí)現(xiàn)了對(duì)桌面未知物體的抓取。以上抓取過程僅需10 s，其中，手指抓取閉合動(dòng)作約2 s 時(shí)間。對(duì)相機(jī)紙盒重復(fù)上述抓取試驗(yàn)，7 次試驗(yàn)中僅1 次抓取失敗，成功率約為85%。同時(shí)，對(duì)于圓柱體、立方體等不同形狀的未知物體均成功完成抓取試驗(yàn)。

4 結(jié)語

1） 搭建了機(jī)械手抓取試驗(yàn)平臺(tái)，針對(duì)機(jī)器人對(duì)未知物體的抓取問題，提出了一種基于多角度視覺信息的機(jī)器人抓取規(guī)劃方案。

2） 該方案結(jié)合三維點(diǎn)云技術(shù)對(duì)未知物體進(jìn)行重建以獲取其位置與形狀信息；同時(shí)，基于高斯過程擬合物體曲面并基于吸引子運(yùn)動(dòng)的動(dòng)態(tài)抓取方案，實(shí)現(xiàn)機(jī)械手對(duì)未知物體的特制化抓取。

3） 由平臺(tái)試驗(yàn)驗(yàn)證了所提出抓取方案具有速度快、精度高、適應(yīng)性強(qiáng)等特點(diǎn)，能夠滿足不同環(huán)境和任務(wù)需求，因此，在工業(yè)制造領(lǐng)域有著廣闊應(yīng)用前景。