趙 嵐, 佘 媛, 溫秀蘭, 李國成, 張騰飛, 赫忠樂

(南京工程學院 自動化學院，江蘇 南京 211167)

1 引 言

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

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

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

2 基于機器視覺的分揀機器人平臺

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

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

3 基于點云配準的6D位姿估計

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

圖2 6D目標位姿估計流程圖

3.1 SAC-IA粗配準

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

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

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

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

(1)

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

3.2 ICP點云精配準

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

(1) 將經(jīng)過SAC-IA粗配準后的源點云作為新的源點云P,目標點云Q保持不變;

(2)

(3) 采用方向向量閾值剔除錯誤的對應(yīng)點對;

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

(3)

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

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

3.3 NDT點云配準

NDT算法也是以迭代的方式進行配準[14,15],但相比SAC-IA算法,NDT算法具有更強的魯棒性,其點云配準流程如圖3所示。

圖3 NDT點云配準

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

(4)

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

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

4 實驗與結(jié)果

4.1 分揀目標物體預處理

分揀目標物體預處理過程包括原始點云獲取、點云濾波、點云分割等主要步驟,圖4為分揀目標物體預處理過程點云圖。

圖4 分揀目標物體預處理過程點云圖

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

4.2 分揀目標物體點云配準實驗

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

圖5 圓柱體點云數(shù)據(jù)配準過程

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

齒輪點云數(shù)據(jù)配準過程如圖6所示。獲得的齒輪點云數(shù)據(jù)相對于目標點云的轉(zhuǎn)換矩陣為:

圖6 齒輪點云數(shù)據(jù)配準過程

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

表1 點云處理過程

4.3 6D位姿精度校驗

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

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

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

表2 目標6D位姿估計

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

4.4 系統(tǒng)分揀實驗

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

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

圖8 機器人分揀操作過程

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

5 結(jié) 論

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