基于機(jī)器視覺(jué)的分揀機(jī)器人6D位姿估計(jì)

趙 嵐, 佘 媛, 溫秀蘭, 李國(guó)成, 張騰飛, 赫忠樂(lè)

(南京工程學(xué)院 自動(dòng)化學(xué)院，江蘇 南京 211167)

1 引 言

分揀機(jī)器人在智能制造領(lǐng)域發(fā)展迅速,被廣泛應(yīng)用于制造加工、自動(dòng)化控制、醫(yī)學(xué)檢測(cè)等領(lǐng)域。傳統(tǒng)分揀機(jī)器人一般通過(guò)人工示教完成指定動(dòng)作,其自動(dòng)化程度低、實(shí)時(shí)性差、可移植性差、開(kāi)發(fā)成本高,難以滿足各行各業(yè)日益復(fù)雜化的生產(chǎn)需求。隨著計(jì)算機(jī)技術(shù)、圖像處理技術(shù)的迅速發(fā)展,眾多研究者將精力投入到視覺(jué)技術(shù)和機(jī)器人控制技術(shù)相結(jié)合的領(lǐng)域。將機(jī)器視覺(jué)應(yīng)用于工業(yè)機(jī)器人的研究最早起源于美國(guó),隨后德國(guó)、日本和瑞典等國(guó)家也加入其中,當(dāng)今工業(yè)機(jī)器人四大家族ABB、KUKA、安川和發(fā)那科的研究和應(yīng)用仍處于世界前沿位置[1]。主流的視覺(jué)傳感器有單目、雙目及深度相機(jī),深度相機(jī)采集的三維點(diǎn)云數(shù)據(jù)不僅包括物體表面的幾何特征、顏色信息,而且還包括深度信息,即采集的信息更接近現(xiàn)實(shí)場(chǎng)景,能夠獲得更高的位姿估計(jì)精度,因此基于三維點(diǎn)云數(shù)據(jù)的物體識(shí)別與6D位姿估計(jì)技術(shù)是目前智能制造領(lǐng)域研究的熱點(diǎn)之一,特別是6D位姿估計(jì)仍然面臨較大挑戰(zhàn)[2]吸引著眾多研究者。

文獻(xiàn)[3]提出了基于樣本一致性的初始對(duì)齊算法,解決了點(diǎn)云配準(zhǔn)效率低的問(wèn)題,但對(duì)噪聲點(diǎn)云數(shù)據(jù)的魯棒性較差;北美工業(yè)研究院使用 Binpicking 傳感器對(duì)工件進(jìn)行激光掃描實(shí)現(xiàn)三維成像,引導(dǎo)機(jī)械臂直接執(zhí)行目標(biāo)工件抓取任務(wù)[4];普林斯頓大學(xué) Andy Zeng 團(tuán)隊(duì)研究提出視圖自我學(xué)習(xí)的視覺(jué)機(jī)器人抓取技術(shù),將三維 CAD 模型與點(diǎn)云圖匹配來(lái)完成目標(biāo)6D 姿態(tài)估計(jì)[5];文獻(xiàn)[6]提出了基于迭代密集融合的6D目標(biāo)位姿估計(jì)方法;文獻(xiàn)[7]研究了基于點(diǎn)對(duì)特征的點(diǎn)云匹配和位姿估計(jì)算法用于散亂堆放物體的位姿估計(jì);為了獲取穩(wěn)定可靠并具有代表性的特征點(diǎn)估計(jì)物體的位姿,文獻(xiàn)[8]提出了基于區(qū)域點(diǎn)云的工件位姿計(jì)算方法;文獻(xiàn)[9]提出了基于改進(jìn)法矢計(jì)算的點(diǎn)云局部描述符SHOT的三維物體識(shí)別與位姿估計(jì)的方法;文獻(xiàn)[10]采用擴(kuò)展的SSD范式從RGB圖像中檢測(cè)三維模型實(shí)例以估計(jì)目標(biāo)6D位姿,該方法可以獲取目標(biāo)位姿,但對(duì)目標(biāo)和模型之間的顏色偏差比較敏感;文獻(xiàn)[11]提出基于三維CAD模型生成測(cè)量對(duì)象點(diǎn)云數(shù)據(jù)庫(kù),通過(guò)投票法識(shí)別并估計(jì)不同物體的6D位姿的方法。

總結(jié)現(xiàn)有研究成果,發(fā)現(xiàn)上述方法多是在特定條件下通過(guò)仿真驗(yàn)證提出方法對(duì)位姿估計(jì)的有效性,直接將提出方法應(yīng)用于估計(jì)機(jī)器人系統(tǒng)實(shí)際位姿的鮮見(jiàn)報(bào)道。針對(duì)基于機(jī)器視覺(jué)的機(jī)器人分揀系統(tǒng)獲得的原始點(diǎn)云信息量大、位姿估計(jì)效率和精度低等問(wèn)題,本文在對(duì)原始點(diǎn)云進(jìn)行預(yù)處理的基礎(chǔ)上,提出采用采樣一致性初始配準(zhǔn)算法(sampling consensus initial alignment,SAC-IA)進(jìn)行位姿粗配準(zhǔn),再依次利用迭代最近點(diǎn)(iterative closest point,ICP)及正態(tài)分布變換(normal distributions transform,NDT)算法進(jìn)行增強(qiáng)精細(xì)配準(zhǔn)的遞進(jìn)策略,獲得目標(biāo)點(diǎn)6D位姿以實(shí)現(xiàn)機(jī)器人快速準(zhǔn)確完成分揀任務(wù)。

2 基于機(jī)器視覺(jué)的分揀機(jī)器人平臺(tái)

本文設(shè)計(jì)的基于機(jī)器視覺(jué)的分揀機(jī)器人平臺(tái)如圖1所示,由機(jī)器人、視覺(jué)傳感器、末端執(zhí)行器、分揀實(shí)驗(yàn)臺(tái)、上位機(jī)控制系統(tǒng)等組成。

其中機(jī)器人選擇小型TX60史陶比爾串聯(lián)六自由度機(jī)械臂;視覺(jué)傳感器采用Kinect深度相機(jī),置于機(jī)械臂上方距離分揀平面約2 m,采集待分揀物體現(xiàn)場(chǎng)輸出彩色和深度3D圖像;末端執(zhí)行器設(shè)計(jì)了氣動(dòng)吸盤和MHZ2-32D氣動(dòng)二指夾爪兩種,根據(jù)待分揀物體不同自行選擇。Kinect相機(jī)通過(guò)USB2.0傳送至計(jì)算機(jī),上位機(jī)軟件經(jīng)系統(tǒng)初始化、物體識(shí)別、物體定位和通信,輸出分揀目標(biāo)物體的顏色、形狀和位姿信息。上位機(jī)軟件與分揀機(jī)器人控制器CS8C通過(guò)RS232串口通信方式傳遞目標(biāo)物體信息,控制器通過(guò)PLC控制機(jī)械臂運(yùn)動(dòng)及末端執(zhí)行器完成分揀任務(wù),分揀對(duì)象為長(zhǎng)方體、圓柱體、齒輪等。

3 基于點(diǎn)云配準(zhǔn)的6D位姿估計(jì)

機(jī)器人在執(zhí)行分揀動(dòng)作時(shí),其控制器采用點(diǎn)到點(diǎn)運(yùn)動(dòng)形式,即需要將待分揀物體的6D位姿傳送給機(jī)器人控制器,控制機(jī)器人末端到達(dá)指定位姿完成分揀動(dòng)作。其中位置坐標(biāo)用于確定機(jī)器人抓取位置,姿態(tài)坐標(biāo)用于確定機(jī)器人以什么姿態(tài)去抓取。6D位姿估計(jì)流程如圖2所示,即首先將Kinect深度相機(jī)采集來(lái)的待分揀物體3D點(diǎn)云數(shù)據(jù)進(jìn)行點(diǎn)云濾波、點(diǎn)云分割等預(yù)處理,再依次采用SAC-IA進(jìn)行位姿粗配置、ICP及NDT算法實(shí)現(xiàn)位姿增強(qiáng)精配準(zhǔn),獲得6D目標(biāo)位姿點(diǎn)。

圖2 6D目標(biāo)位姿估計(jì)流程圖

3.1 SAC-IA粗配準(zhǔn)

點(diǎn)云位姿的粗配準(zhǔn)采用SAC-IA[12],通過(guò)對(duì)候選對(duì)應(yīng)關(guān)系大量采樣,快速尋找一個(gè)最優(yōu)變換,實(shí)現(xiàn)初步配準(zhǔn)。具體實(shí)現(xiàn)方法為:

(1) 首先對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行降采樣,獲得源點(diǎn)云P∈RM×3和目標(biāo)點(diǎn)云Q∈RN×3。

(2) 分別提取點(diǎn)云P和Q的法向量,并計(jì)算配準(zhǔn)點(diǎn)云的快速點(diǎn)云特征直方圖(FPFH),即從待配準(zhǔn)的源點(diǎn)云P中選取n個(gè)采樣點(diǎn),提取采樣點(diǎn)相互之間距離須滿足大于給定的最小距離閾值d,以保證采樣點(diǎn)具有不同的FPFH特征。

(3) 查找目標(biāo)點(diǎn)云Q中與源點(diǎn)云P采樣點(diǎn)具有相似FPFH特征的1個(gè)或多個(gè)點(diǎn),從相似點(diǎn)中隨機(jī)選取1個(gè)點(diǎn)作為點(diǎn)云P在目標(biāo)點(diǎn)云Q中的一一對(duì)應(yīng)點(diǎn)。

(1)

式中:s為一預(yù)先給定值;li為第i組對(duì)應(yīng)點(diǎn)變換之后的距離差。

3.2 ICP點(diǎn)云精配準(zhǔn)

經(jīng)SAC-IA完成點(diǎn)云初始配準(zhǔn),使得匹配點(diǎn)之間的距離在一定的閾值內(nèi),再采用ICP將大致對(duì)齊的點(diǎn)云對(duì)精準(zhǔn)對(duì)齊[13],實(shí)現(xiàn)過(guò)程為:

(1) 將經(jīng)過(guò)SAC-IA粗配準(zhǔn)后的源點(diǎn)云作為新的源點(diǎn)云P,目標(biāo)點(diǎn)云Q保持不變;

(2)

(3) 采用方向向量閾值剔除錯(cuò)誤的對(duì)應(yīng)點(diǎn)對(duì);

(4) 計(jì)算剛性變換矩陣T*,使對(duì)應(yīng)點(diǎn)之間的距離誤差最小,即

(3)

(5) 將變換矩陣T*作用于源點(diǎn)云,更新源點(diǎn)云為P′;

(6) 計(jì)算P′與Q的距離誤差,如果誤差小于設(shè)定值或迭代次數(shù)大于設(shè)定值,迭代結(jié)束,否則重復(fù)上述步驟直至滿足收斂條件。

3.3 NDT點(diǎn)云配準(zhǔn)

NDT算法也是以迭代的方式進(jìn)行配準(zhǔn)[14,15],但相比SAC-IA算法,NDT算法具有更強(qiáng)的魯棒性,其點(diǎn)云配準(zhǔn)流程如圖3所示。

圖3 NDT點(diǎn)云配準(zhǔn)

首先,初始化經(jīng)ICP配準(zhǔn)后的點(diǎn)云數(shù)據(jù),確保配準(zhǔn)所用點(diǎn)云是無(wú)噪聲和無(wú)離群點(diǎn)的可用點(diǎn)云;把初始化點(diǎn)云劃分規(guī)則均勻且大小固定的三維單元格,通過(guò)正態(tài)分布表示體素單元中每個(gè)三維點(diǎn)位置測(cè)量樣本概率分布p(x)為:

(4)

式中:c為常量;q為均值向量;C協(xié)方差矩陣;xi(i=1,2,…,n)為體素單元格中的所有點(diǎn)云。

經(jīng)SAC-IA位姿粗配準(zhǔn)、ICP精配準(zhǔn)及NDT增強(qiáng)精配準(zhǔn),獲得高精度6D位姿。

4 實(shí)驗(yàn)與結(jié)果

4.1 分揀目標(biāo)物體預(yù)處理

分揀目標(biāo)物體預(yù)處理過(guò)程包括原始點(diǎn)云獲取、點(diǎn)云濾波、點(diǎn)云分割等主要步驟,圖4為分揀目標(biāo)物體預(yù)處理過(guò)程點(diǎn)云圖。

圖4 分揀目標(biāo)物體預(yù)處理過(guò)程點(diǎn)云圖

圖4(a)為由Kinect相機(jī)獲得的原始點(diǎn)云數(shù)據(jù),不僅密度不均勻,而且有稀疏的離群點(diǎn)。首先采用直通濾波器實(shí)現(xiàn)背景點(diǎn)云和分揀平面點(diǎn)云分離,再使用Statistical Outlier Removal濾波器對(duì)每個(gè)點(diǎn)的鄰域進(jìn)行統(tǒng)計(jì)分析,修剪掉不符合標(biāo)準(zhǔn)的點(diǎn),以解決點(diǎn)云數(shù)據(jù)中產(chǎn)生的離群點(diǎn)問(wèn)題?？紤]到Kinect深度相機(jī)生成的點(diǎn)云數(shù)據(jù)具有顏色RGB信息,因此先用歐幾里得分割法對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行預(yù)處理,將單個(gè)分揀目標(biāo)分離出來(lái);對(duì)于有重疊部分的點(diǎn)云使用基于法線和基于顏色兩種方法的區(qū)域生長(zhǎng)法進(jìn)行分割。原始點(diǎn)云數(shù)據(jù)經(jīng)直通濾波及Statistical Outlier Removal濾波器濾波、去除背景信息、裁剪目標(biāo)區(qū)域、提取ROI點(diǎn)云如圖4(b)所示,圖4(c)為經(jīng)背景分割等預(yù)處理操作后獲得的點(diǎn)云,保留了分揀目標(biāo)物體的點(diǎn)云信息,背景分割完成后,再對(duì)圓柱體、長(zhǎng)方體、齒輪等單個(gè)目標(biāo)進(jìn)行分割。

4.2 分揀目標(biāo)物體點(diǎn)云配準(zhǔn)實(shí)驗(yàn)

搭建的機(jī)器人分揀系統(tǒng)應(yīng)用平臺(tái)選擇Windows 7操作系統(tǒng)、編譯環(huán)境VS2010,使用MFC上位機(jī)平臺(tái)進(jìn)行軟件開(kāi)發(fā)。利用提出方法分別對(duì)圓柱體、長(zhǎng)方體及齒輪的點(diǎn)云平面進(jìn)行粗配置、增強(qiáng)精配準(zhǔn)操作,不斷將原始點(diǎn)云數(shù)據(jù)向目標(biāo)點(diǎn)云數(shù)據(jù)迭代,最終使原始點(diǎn)云數(shù)據(jù)最大限度地接近目標(biāo)點(diǎn)云數(shù)據(jù),并輸出配準(zhǔn)點(diǎn)云相對(duì)于目標(biāo)點(diǎn)云的轉(zhuǎn)換矩陣。圖5為對(duì)所分割的圓柱體點(diǎn)云數(shù)據(jù)進(jìn)行增強(qiáng)精細(xì)配準(zhǔn)過(guò)程,圖5(a)～圖5(d)分別為使用SAC-IA粗配準(zhǔn)、ICP精配準(zhǔn)、NDT點(diǎn)云配準(zhǔn)及最終的配準(zhǔn)效果,圖中綠色點(diǎn)云表示原始點(diǎn)云,紅色點(diǎn)云表示目標(biāo)點(diǎn)云,白色點(diǎn)云表示配準(zhǔn)結(jié)果。

圖5 圓柱體點(diǎn)云數(shù)據(jù)配準(zhǔn)過(guò)程

點(diǎn)云配準(zhǔn)后獲得的圓柱體點(diǎn)云數(shù)據(jù)相對(duì)于目標(biāo)點(diǎn)云的轉(zhuǎn)換矩陣為:

齒輪點(diǎn)云數(shù)據(jù)配準(zhǔn)過(guò)程如圖6所示。獲得的齒輪點(diǎn)云數(shù)據(jù)相對(duì)于目標(biāo)點(diǎn)云的轉(zhuǎn)換矩陣為:

圖6 齒輪點(diǎn)云數(shù)據(jù)配準(zhǔn)過(guò)程

由點(diǎn)云配準(zhǔn)圖可見(jiàn),經(jīng)SAC-IA粗配準(zhǔn)原始點(diǎn)云有少量點(diǎn)云數(shù)據(jù)配準(zhǔn)到目標(biāo)點(diǎn)云上,再使用ICP精配準(zhǔn),大部分原始點(diǎn)云配準(zhǔn)到目標(biāo)點(diǎn)云上,最后使用NDT增強(qiáng)精配準(zhǔn),白色配準(zhǔn)結(jié)果點(diǎn)云與紅色目標(biāo)點(diǎn)云重合度在經(jīng)典ICP點(diǎn)云配準(zhǔn)算法基礎(chǔ)上進(jìn)一步提高。通過(guò)程序記錄點(diǎn)云處理算法的每一步處理所需時(shí)間如表1所示,針對(duì)本系統(tǒng)所分揀的物體,點(diǎn)云處理算法總用時(shí)5 s以內(nèi),能夠滿足分揀實(shí)驗(yàn)要求。

表1 點(diǎn)云處理過(guò)程

4.3 6D位姿精度校驗(yàn)

為了驗(yàn)證本文基于機(jī)器視覺(jué)的分揀機(jī)器人6D位姿估計(jì)精度,采用Leica AT960激光跟蹤儀測(cè)量待分揀物體中心獲得6D理論位姿,測(cè)量環(huán)境如圖7所示。Leica AT960測(cè)量精度為15 μm+6 μm/m,配置有標(biāo)準(zhǔn)靶球、TMAC探測(cè)器及Spatial Analyzer(SA)測(cè)量分析軟件,能夠同時(shí)測(cè)量位置和姿態(tài)[16]。

圖7 6D位姿測(cè)量環(huán)境

考慮到高精度儀器同樣存在測(cè)量誤差,使用激光跟蹤儀對(duì)待分揀物體中心重復(fù)測(cè)量10次,取其平均值作為理論位姿。為了便于比較,表2同時(shí)列出了經(jīng)本文提出的增強(qiáng)精配準(zhǔn)策略生成圓柱體和齒輪中心的實(shí)際位置PR和姿態(tài)DR及采用Leica AT960激光跟蹤儀10次測(cè)量獲得的平均位置PT和平均姿態(tài)DT及其標(biāo)準(zhǔn)差SP和姿態(tài)SD。

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

由表2可見(jiàn),本系統(tǒng)由本文提出的增強(qiáng)精配準(zhǔn)策略估計(jì)的6D位姿與理論位置和姿態(tài)相比較其誤差分別控制在1.5 mm和2°之內(nèi),證明所提方法能夠獲得較高的6D位姿精度以滿足系統(tǒng)分揀需求。

4.4 系統(tǒng)分揀實(shí)驗(yàn)

機(jī)器人在執(zhí)行分揀任務(wù)時(shí),首先從Kinect相機(jī)獲取原始點(diǎn)云數(shù)據(jù),對(duì)采集到的原始點(diǎn)云進(jìn)行裁剪、邊緣提取等預(yù)處理,提前ROI并進(jìn)行點(diǎn)云分割等操作,再經(jīng)SAC-IA、ICP及NDT增強(qiáng)精配準(zhǔn)處理獲得目標(biāo)物體的6D位姿,將位姿點(diǎn)發(fā)送到機(jī)器人控制器中,控制器驅(qū)動(dòng)機(jī)械臂完成抓取操作。

分揀機(jī)器人操作過(guò)程見(jiàn)圖8所示,圖8(a)和圖8(b)分別給出了機(jī)器人末端采用吸盤時(shí)的定位、分揀過(guò)程,圖8(c)和圖8(d)給出了末端采用二指夾爪時(shí)的抓取、放置過(guò)程。

圖8 機(jī)器人分揀操作過(guò)程

由表1、表2和圖8可見(jiàn),本文提出的增強(qiáng)精配準(zhǔn)策略不僅用時(shí)短,而且6D位姿估計(jì)精度高,在各坐標(biāo)軸的位置和角度能夠滿足系統(tǒng)分揀需求。

5 結(jié) 論

為了解決傳統(tǒng)分揀機(jī)器人通過(guò)人工示教完成指定動(dòng)作其自動(dòng)化程度低、實(shí)時(shí)性差、可移植性弱等問(wèn)題,本文設(shè)計(jì)了基于機(jī)器視覺(jué)的分揀機(jī)器人實(shí)驗(yàn)平臺(tái),提出了基于粗配置及增強(qiáng)精配準(zhǔn)混合遞進(jìn)策略的6D位姿估計(jì)方法,將配準(zhǔn)獲得的6D位姿與激光跟蹤儀實(shí)測(cè)位姿進(jìn)行比較,并對(duì)大量實(shí)物進(jìn)行抓取分揀,結(jié)果證實(shí):通過(guò)遞進(jìn)配準(zhǔn)策略能夠快速準(zhǔn)確獲得待抓取目標(biāo)在各坐標(biāo)軸的位置和姿態(tài),滿足分揀機(jī)器人的實(shí)際需求,本文提出的方法適于在基于機(jī)器視覺(jué)的機(jī)器人裝配、打磨等有高精度6D位姿估計(jì)場(chǎng)合推廣應(yīng)用。