面向機(jī)器人抓取過(guò)程中目標(biāo)位姿估計(jì)方法*

李樹(shù)春， 張 靜,2， 張 華， 劉滿祿,2， 楊厚易， 劉理想

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽(yáng) 621000;2.中國(guó)科學(xué)技術(shù)大學(xué) 信息科學(xué)技術(shù)學(xué)院，安徽 合肥 230026)

0 引 言

視覺(jué)引導(dǎo)機(jī)械臂進(jìn)行抓取任務(wù)是目前機(jī)械臂抓取的主要方式。抓取過(guò)程分為目標(biāo)識(shí)別、目標(biāo)位姿估計(jì)和機(jī)械臂抓取。其中，目標(biāo)位姿估計(jì)精度決定了抓取的效果。Lin C等人[1]采用雙目視覺(jué)，通過(guò)雙目立體視覺(jué)算法，對(duì)目標(biāo)進(jìn)行位姿估計(jì)，并采用快速特征點(diǎn)提取和描述(oriented FAST and rotated BRIEF,ORB)算法動(dòng)態(tài)查找遮擋。隨著Xtion[2]，Kinect等廉價(jià)傳感器的出現(xiàn)，大量研究者開(kāi)始采用深度信息進(jìn)行目標(biāo)位姿估計(jì)。呂強(qiáng)等人[3]將RGB圖像和深度圖像結(jié)合，利用RGB圖像提取(normal aligned radial feature，NARF)和尺度不變特征變換(scale-invariant feature transform,SIFT)特征點(diǎn)，采用隨機(jī)采樣一致性(random sample consensus,RANSAC)算法進(jìn)行位姿估計(jì)。楊邱滟等人[4]利用Kinect相機(jī)對(duì)仿人機(jī)器人抓取進(jìn)行目標(biāo)定位。Besl P J等人[5]利用迭代最近點(diǎn)(iterative closest point，ICP)算法對(duì)剛性物體進(jìn)行位姿估計(jì)，但是該方法在有遮擋的情況下會(huì)存在局部最小解問(wèn)題。Lu J等人[6]提出了一種基于正態(tài)分布變換(normal distribution transform，NDT)算法的變尺度體素位姿估計(jì)法。

針對(duì)非結(jié)構(gòu)化環(huán)境，本文使用深度相機(jī)Kinect獲取點(diǎn)云圖像，提取快速點(diǎn)特征直方圖(fast point feature histograms,FPFH)描述子，結(jié)合奇異值分解算法和隨機(jī)采樣一致算法來(lái)對(duì)位姿進(jìn)行估計(jì)。

1 機(jī)器人抓取系統(tǒng)

系統(tǒng)硬件平臺(tái)如圖1，由Microsoft Kinect深度相機(jī)，6自由度的輕量級(jí)機(jī)器人KINOVA MICO2和上位機(jī)組成。

圖1 系統(tǒng)硬件平臺(tái)

系統(tǒng)工作流程如圖2，離線制作模板。

圖2 系統(tǒng)工作流程

2 目標(biāo)位姿估計(jì)

在對(duì)目標(biāo)進(jìn)行位姿估計(jì)前，先對(duì)場(chǎng)景進(jìn)行體素濾波減少運(yùn)算量，然后采用分割算法將目標(biāo)與場(chǎng)景分割。

2.1 FPFH特征點(diǎn)提取

PFH描述符[7]通過(guò)考慮基于k領(lǐng)域中的點(diǎn)與其估計(jì)的表面法線之間的關(guān)系,來(lái)盡可能多地捕獲點(diǎn)云的表面變化，適應(yīng)鄰域中存在不同采樣密度或噪聲水平[8]。

點(diǎn)云空間中的一對(duì)點(diǎn)pt和ps，如圖3。

圖3 點(diǎn)云空間中的一對(duì)點(diǎn)

FPFH描述子是在PFH基礎(chǔ)上提出的，相對(duì)PFH描述子，F(xiàn)PFH的總體復(fù)雜度大大降低，且能保存PFH的大多數(shù)信息。FPFH描述子將α,φ,θ分別劃分為11個(gè)區(qū)間，每11個(gè)區(qū)間的概率和為100 %，33 個(gè)區(qū)間概率總和為 300 %，如圖4。

圖4 點(diǎn)特征直方圖

FPFH算法流程:

步驟1：對(duì)于每個(gè)查詢點(diǎn)pq，計(jì)算出該待求點(diǎn)與其所有鄰域點(diǎn)之間的相對(duì)關(guān)系，記作SPFH(pq)；

步驟2：重新確定每個(gè)點(diǎn)k鄰域，并通過(guò) SPFH特征，來(lái)估計(jì) FPFH特征，如下式所示

(1)

式中wk為第K個(gè)鄰域點(diǎn)SPFH 特征的加權(quán)值。1/wk為pq與其第K個(gè)鄰域點(diǎn)的距離值，用于表示點(diǎn)對(duì)(pq,pk)的關(guān)系。

2.2 旋轉(zhuǎn)位移矩陣求取

在Rd空間中有兩個(gè)點(diǎn)集P={P1,P2,…,Pn}和Q={q1,q2,…,qn},要找到這兩個(gè)點(diǎn)集之間的旋轉(zhuǎn)位移矩陣，可以把旋轉(zhuǎn)位移用R和T表示

(2)

計(jì)算旋轉(zhuǎn)位移矩陣的步驟：

步驟1：計(jì)算兩個(gè)點(diǎn)集的重心

(3)

步驟2: 計(jì)算兩個(gè)點(diǎn)集中每一個(gè)點(diǎn)減去重心后的偏移向量

xi=pi-,yi=qi-,i=1,2…,n

(4)

步驟3:計(jì)算X和Y的協(xié)方差矩陣

S=XTY

(5)

式中X和Y由向量xi,yi構(gòu)成。

步驟4：對(duì)S進(jìn)行奇異值分解，根據(jù)性質(zhì)可得旋轉(zhuǎn)矩陣

S=U∑VT

(6)

R=VUT

(7)

步驟5：由旋轉(zhuǎn)矩陣可得位移

t=-R

(8)

2.3 RANSAC算法

RANSAC算法是一種通過(guò)對(duì)觀測(cè)數(shù)據(jù)進(jìn)行隨機(jī)采樣來(lái)估計(jì)模型參數(shù)的學(xué)習(xí)方法。給定一個(gè)數(shù)據(jù)集，其數(shù)據(jù)元素包含正確數(shù)據(jù)(inliers)和異常數(shù)據(jù)(outliers)，RANSAC使用投票方案來(lái)尋找最佳擬合結(jié)果。

算法：RANSAC(P,Q)

Input:P,Q

Output:Rtfinal

Paranms:k,d,inlier

1:fori←1 tokdo ∥k為采樣次數(shù)

2014年，在全國(guó)留學(xué)工作會(huì)議上習(xí)近平重點(diǎn)指出：“留學(xué)工作要適應(yīng)國(guó)家發(fā)展大勢(shì)與黨和國(guó)家工作大局……培養(yǎng)造就更多優(yōu)秀人才，努力開(kāi)創(chuàng)留學(xué)工作新局面。”習(xí)近平這番話也為接下來(lái)的“一帶一路”背景下高校來(lái)華留學(xué)生教育工作指明了方向。目前在“一帶一路”背景下，我國(guó)高校為加強(qiáng)來(lái)華留學(xué)生教育采取很多有效教育措施，具體措施主要如下。

2：{p1,p2,p3}=Random(P)

3:{q1,q2,q3}=Fpfh{q1,q2,q3}

4：Ifd{p1,p2,p3}/d{q1,q2,q3}>d

Then∥由3個(gè)點(diǎn)構(gòu)成的三角形邊長(zhǎng)比，d為比的最大閾值

5：Rt=Rt{p1,p2,p3,q1,q2,q3}

∥利用2.2節(jié)算法求解旋轉(zhuǎn)位移矩陣

6：Q1=RP+t∥將初點(diǎn)云移動(dòng)

7：Inlier=Euclidean(Q1,Q)

∥以兩點(diǎn)云集的歐式距離作為得分

8：Rtfinal←inliermin(Rt)

∥將得分最低的旋轉(zhuǎn)位移矩陣作為所得

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

3.1 雜亂無(wú)遮擋環(huán)境實(shí)驗(yàn)

在無(wú)遮擋的環(huán)境中，首先對(duì)環(huán)境進(jìn)行濾波處理，分割出目標(biāo)物體，分別使用本文算法、NDT算法和ICP算法在相同的環(huán)境對(duì)同一物體進(jìn)行位姿估計(jì)，物體以任意姿態(tài)放置，如圖5。3種算法位姿估計(jì)誤差如圖6所示。

圖5 目標(biāo)物體任意姿態(tài)放置

圖6 3種算法位姿估計(jì)誤差

由圖6可以看出：在16種任意姿態(tài)下，NDT算法對(duì)旋轉(zhuǎn)量的估計(jì)誤差變化不大，但對(duì)平移量的誤差估計(jì)出現(xiàn)了起伏。ICP算法對(duì)個(gè)別位姿會(huì)出現(xiàn)較大的偏差。針對(duì)任意姿態(tài)，經(jīng)過(guò)多次實(shí)驗(yàn)，本文提出的算法位姿估計(jì)的平移量誤差為4.86 mm，旋轉(zhuǎn)量誤差為1.96°；NDT算法的平移量誤差為8.86 mm，旋轉(zhuǎn)量誤差為6.16°；ICP算法平移量誤差為10.03 mm，旋轉(zhuǎn)量誤差為12.49°。

3.2 機(jī)械臂遮擋目標(biāo)物體實(shí)驗(yàn)

針對(duì)Eye-to-Hand模型易出現(xiàn)機(jī)械臂遮擋目標(biāo)物體的問(wèn)題，本文設(shè)計(jì)了實(shí)驗(yàn)，機(jī)械臂在不同遮擋率下遮擋住物體，利用本文算法、NDT算法和ICP算法進(jìn)行位姿的估計(jì)。實(shí)驗(yàn)結(jié)果如圖7。

圖7 不同遮擋率下的位姿估計(jì)誤差

由圖7可以看出：隨著遮擋率的增高，3種算法對(duì)平移量和旋轉(zhuǎn)量估計(jì)的誤差都在增加。由于NDT算法利用概率分布來(lái)計(jì)算位姿并沒(méi)有計(jì)算點(diǎn)云的特征，在遮擋導(dǎo)致物體信息缺失情況下對(duì)物體位姿估計(jì)的效果并不好。ICP算法通過(guò)歐氏距離來(lái)進(jìn)行位姿估計(jì)，但同樣缺失點(diǎn)云特征，在物體信息缺失時(shí)易出現(xiàn)偏差。本文提出的算法是利用點(diǎn)云的特征信息來(lái)進(jìn)行位姿估計(jì)的，所含有的信息量豐富，即使部分信息缺失也能有效估計(jì)位姿。圖8和圖9是在10%遮擋率下3種算法對(duì)位姿估計(jì)的效果。其中圖8中淺色的點(diǎn)云表示初始位姿，深色點(diǎn)云為彩色圖像轉(zhuǎn)換后的點(diǎn)云。圖9中淺色點(diǎn)云是將初始點(diǎn)云經(jīng)過(guò)估計(jì)的位姿移動(dòng)后得到。可以看到本文初始點(diǎn)云經(jīng)過(guò)旋轉(zhuǎn)位移與移動(dòng)后的點(diǎn)云基本重合。而NDT和ICP算法的重合效果較差。

圖8 采集點(diǎn)云數(shù)據(jù)

圖9 3種算法估計(jì)效果

4 結(jié) 論

通過(guò)與NDT算法和ICP算法進(jìn)行對(duì)比實(shí)驗(yàn)，本文提出的算法在對(duì)位姿估計(jì)精度和穩(wěn)定性上都優(yōu)于NDT算法和ICP算法，并在Eye-to-Hand模型中機(jī)械臂存在遮擋物體問(wèn)題上對(duì)位姿估計(jì)的效果也要優(yōu)于其他兩種算法。在后續(xù)的工作，可以在保持位姿估計(jì)的精度情況下,對(duì)算法的時(shí)間復(fù)雜度進(jìn)行優(yōu)化，使算法的實(shí)時(shí)性效果更優(yōu)。