基于Kinova機械臂RGBD相機的目標抓取

黃 偉, 郭瑞科, 劉大勇, 康 柯, 周忠達, 胡海東

1. 中國空間技術研究院杭州中心,杭州 310024

2. 北京控制工程研究所,北京 100094

0 引 言

對物體感知和操作是目前機械臂領域研究的熱點.隨著科學技術的發展,執行重復性勞動已經由人工轉向機械臂,然而機械臂通常不具備目標識別和定位的能力,因此,需要為機械臂配置一雙“眼睛”實現更強大的功能.

在軌服務如氣液加注、故障修復等空間應用場景[1], 通過裝有手眼相機的機械臂對故障航天器進行識別、定位,再進行修復,可以有效延長在軌航天器的使用壽命, 降低航天器運行成本, 對航天事業發展具有重要意義.

機器視覺可以實現物體的檢測和定位,充當機械臂的“眼睛”.相機作為機器人的眼睛進行圖像的采集、目標捕獲和定位等功能,在工業應用中往往安裝于傳送帶正上方,相機至傳送帶的垂直距離固定不變,只進行二維平面的定位[2-3].黃夢濤等[4]在快遞行業中采用在物體表面貼二維碼進行定位,通過ar_track_alvar功能包獲取二維碼相對于機械臂工具坐標系的位姿.但貼二維碼定位方法存在以下不足:物體表面需平整,以便于粘貼二維碼,并需要預先輸入二維碼的大小等參數;不適用于表面彎曲或者體積較小的物體等.毛成林等[5]提出一種結合TCP(tool central point)標定過程同步標定深度相機手眼關系的新方法,減少了TCP和手眼關系獨立標定累積誤差影響.此外,杜惠斌等[6]提出一種利用低成本3D 打印球作為標定件對機械臂與RGBD 相機進行手眼標定的方法,這些方法集中介紹深度相機的標定問題,沒有提及深度相機與彩色相機配準問題.與上述方法不同,本文的創新之處是在xyz3個方向定位,涉及姿態信息且闡述了深度相機與彩色相機配準,使得距離方向檢測更加準確.本文采用HSV方法[7]進行圖像分割,獲得彩色圖像中目標位置,通過彩色圖與深度圖的配準關系,結合深度相機的深度信息以及機械臂與相機的變換關系,完成目標物體三維定位和抓取過程.

1 系統介紹

本文采用Kinova機械臂進行目標抓取任務.Kinova官網提供開源的機器人操作系統(robot operating system,ROS)程序功能包,包括機械臂控制的ROS_KORTEX功能包和相機采集的ROS_KORTEX_VISION功能包,方便用戶二次開發和使用.ROS 操作方便、功能強大,非常適用于機器人多節點多任務的復雜場景.自 2010 年誕生以來,ROS 就受到了學術界和工業界的歡迎,如今已經廣泛應用于機械臂、移動底盤和無人機等許多種類的機器人上[8].ROS以節點為基本程序單元,可以很方便地實現機器人的運動、路徑規劃,與視覺、控制等模塊之間的通訊也有相應的功能包,極大擴展了應用范圍.

目標檢測和定位常見的工具有OpenCV、Halcon和Matlab圖像處理包等,而Halcon、Matlab等由于收費等原因,實際應用范圍相對小.本文采用OpenCV實現.

綜上所述,本實驗平臺采用Ubuntu 18+ROS Melodic 開發,基于ROS_KORTEX功能包實現Kinova機械臂運動控制和目標抓取,配合ROS的MoveIt功能包可在Rviz中實現可視化.視覺定位方面,使用OpenCV,采用HSV方法,進行圖像分割處理,在圖像坐標系下定位,結合深度相機完成目標檢測定位.該方法對物體表面形狀和體積無特別要求,可實現更通用的目標抓取.

2 相機標定、配準與三維定位

2.1 相機內參標定及手眼標定

相機標定是通過物理空間點及其對應的像素點,獲得相機的位置信息和內部參數的過程,完成相機標定后系統才能根據目標的圖像像素位置計算得到其物理空間坐標,從而為機械臂的抓取指定物理目標位置.相機標定包含相機內參和外參2個部分:相機內參表示從相機坐標系到圖像坐標系的轉換關系,由相機加工、裝配過程或物理屬性決定;相機外參表示世界坐標系到相機坐標系的轉換關系.通常將機械臂與相機的標定稱為手眼標定,分為眼在手上和眼在手外.

Kinova機械臂的相機安裝于末端執行器上方,官方提供的ROS驅動程序中已經內置了相機內參和外參.相機坐標系和末端執行器工具坐標系之間的變換矩陣,是一個固定值,由安裝位置決定,而相機坐標系相對于世界坐標系(基座)的關系可以通過ROS的TF(transform)樹直接讀取,因此不需要進行額外的手眼標定步驟.

2.2 彩色相機和深度相機(RGBD)的配準

本節首先介紹ROS圖像消息與OpenCV圖像格式的轉換方法,其次介紹彩色相機和深度相機圖像之間的配準方法,實現彩色相機目標像素的三維定位.最后,基于HSV顏色空間進行目標檢測、圖像分割以及空間定位方法.

(1)圖像獲取和格式轉換.

Kinova機械臂內置的RGB相機和深度相機獲取圖像信息后, ROS系統的ROS_KORTEX_VISION功能包將按30 fps(幀每秒)的頻率發布實時的圖像信息.對于RGB相機,ROS通過/camera/color/image_raw 話題發布“bgr8”格式的Image類型消息.對于深度相機,ROS會通過/camera/depth/image_raw話題發布“16UC1”格式的Image類型消息,其中每個像素的深度數據以毫米為單位.

上述Image類型消息OpenCV不能直接讀取和識別,需要在程序中訂閱相應的圖像信息通過CvBridge實現不同圖像格式間的轉換,并將其轉換為OpenCV可以處理的Mat矩陣格式.

(2)深度圖像和彩色圖像的配準.

彩色相機和深度相機分別獲取到彩色、深度圖像后,需要對2幅圖像進行配準,實際就是讓深度圖和彩色圖重合在一起,即將深度圖像的圖像坐標系轉換到彩色圖像的圖像坐標系下,從而能夠確定RGB圖像內特定位置像素的深度信息(即獲取每個像素的RGBD信息).首先,找出深度相機圖像坐標系和彩色相機圖像坐標系之間的轉換關系.

對于深度相機D,從深度相機坐標系到圖像坐標系的變換可以表示為

(1)

其中,XD、YD和ZD是相機坐標系下的三維坐標,uD、vD是圖像坐標系下的坐標,fx、fy、u0和v0是相機內參.整理為4×4齊次矩陣

(2)

(3)

深度相機與彩色相機的坐標變換矩陣M通過以下方式獲取.世界坐標系下的點P變換到深度相機坐標系和RGB相機坐標系下,得到

PD=RDP+TD

(4)

PR=RRP+TR

(5)

將式(4)中的P用PD、TD和RD表示,代入式(5),得到

(6)

對比式(4)和(5),可得

(7)

T=TR-RTD

(8)

R和T即是矩陣M的旋轉和平移部分.

這樣世界坐標系下的點在深度相機和彩色相機坐標系下有如下關系:

(9)

由式(2)、(3)和(4)可以得到2相機像素之間的轉換關系

(10)

其中,ZD是深度相機至目標物的距離,ZR是彩色相機至目標物的距離.由于深度相機和彩色相機至目標物的距離幾乎相等,即ZD≈ZR.式(10)可簡化為

(11)

其中,

(12)

將公式展開,可得深度相機圖像坐標系轉換至彩色相機圖像坐標系的解析解

(13)

至此,深度圖像坐標系和彩色圖像坐標系的關系已經確定.

2.3 基于彩色相機和深度相機(RGBD)的三維定位

通過下面2個步驟計算RGB圖像坐標系下特定像素的空間位置:

根據轉換關系,遍歷彩色圖像的每一個像素,找到其對應的深度圖像坐標并記錄數據,便可得到配準后的深度圖,如圖1(a)所示.

圖1 配準后的Depth/RGB圖Fig.1 Depth/RGB map after match

結合像素位置、深度信息及彩色相機的內外參矩陣,以及ROS發布的TF樹,便能夠得到彩色圖像目標像素相對于世界坐標系的位置.如下圖1(b)所示,圖中的紅點表示目標像素位置,左上角顯示了當前像素對應的世界坐標.

3 抓取實驗

3.1 目標的檢測與定位

視覺定位方法通常有以下幾種:模板匹配方法(如基于點云迭代最近點算法)、機器視覺與深度學習相結合方法和基于圖像特征方法.每種方法都有相應的應用場合和優缺點.

本文采用紅色和藍色正方體作為被抓取物體,邊長約為5 cm.視覺定位算法屬于圖像特征方法.和傳統的圖像特征提取方法不同,不僅得到二維圖像的坐標信息,還利用深度相機計算得到物體相對于機械臂基坐標系的三維坐標.采用基于目標物體的顏色進行圖像分割,并根據形態學方法分析獲得特定顏色目標在圖像坐標系中的位姿信息,結合2.3節的三維定位方法,可以實時輸出目標相對于世界坐標系的三維位姿.目標分割是在HSV色彩空間(如圖2[8])內執行的,根據顏色的直觀特性創建的一種顏色空間,也稱六角錐體模型(hexcone model),其顏色參數分別是色調(hue)、飽和度(saturation)和明度(value).

圖2 HSV色彩空間示意圖Fig.2 HSV color space

紅色、藍色和綠色物體,其對應的HSV色彩范圍為:

紅色:H(0-8)∪(160-180)、S(80-255)和V(50-220)

藍色:H(90-124)、S(43-255)和V(26-255)

綠色:H(35-77)、S(43-255)和V(26-255).

目標檢測的圖像處理過程如圖3所示,圖3(a)是原圖,圖3(b)進行HSV分割,圖3(c)進行形態學開運算和最小外接矩形獲取,圖3(d)計算目標深度信息.具體步驟如下:

圖3 圖像處理過程Fig.3 Image processing process

1)篩選像素.基于配準后的深度圖,篩選與相機距離小于1m的像素,從而消除距離相機較遠部分背景對目標檢測的干擾.之后,將經過初步篩選的圖像轉換為HSV色彩空間,并進一步篩選HSV在特定范圍的所有像素點,得到包含所有藍色像素的掩膜(如圖3(b)).

2)獲取外接矩形.通過形態學開運算,先以10×10像素大小的圖像算子進行腐蝕,再以10×10像素大小的圖像算子進行膨脹,去除噪點,之后獲取圖像中體積最大的外輪廓,并計算其最小外接矩形(如圖3(c)).

3)獲取三維坐標.取外接矩形中心位置像素并計算其相對于世界坐標系的三維坐標,將該值作為抓取目標的空間坐標.此外,根據外接矩形與圖像坐標系x軸的夾角,可以確定目標在執行器z軸方向的姿態(如圖3(d)).

3.2 基于位置控制的目標抓取

3.1節通過顏色空間分析圖像獲取目標像素坐標,結合深度相機獲取三維位置,以便使用機械臂進行目標抓取.具體的抓取流程,如圖4所示.

圖4 抓取流程圖Fig.4 Capture flowchart

首先,機械臂移動末端執行器至垂直向下正視桌面的初始姿態,從而能夠觀察到放置于桌面的目標物體.

接著,系統將利用基于顏色空間HSV功能包檢測相機畫面中的正方體.當畫面中存在紅色或藍色正方體時,系統會將正方體的中心位置和相機坐標系間的轉換關系以TF消息的形式發布.機械臂控制程序通過訂閱TF話題,得到正方體在世界坐標系下的三維位置與姿態.

獲取到目標位置信息后,功能包程序將詢問操作者是否執行抓取任務.

確認執行抓取任務后,程序會根據正方體位置計算結果引導末端執行器移動至正方體正上方,同時調整其z軸方向的姿態,使得執行器的抓手與目標正方體的棱邊平行.結合目標位置與位姿,程序調用ROS_KORTEX API的位置控制接口,生成抓取路徑并執行目標的抓取,最終將正方體放置于指定的位置.完成全部的抓取和放置動作后,程序將控制機械臂回到初始姿態,并重新檢測是否存在抓取目標信息.

正方體實物抓取如圖5所示.圖5(b)為Rviz中顯示的可視化虛擬機械臂,與實際機械臂會保持同步運動.圖5(c)為視覺檢測窗口,系統將通過HSV顏色分割檢測藍色和紅色正方體的位置與姿態,正方體的輪廓與正方體中心點的三維位置在圖中通過綠色標注顯示.實物抓取實驗過程中,在相機視野內隨機擺放正方體,連續試驗50次,均抓取成功,成功率100%,說明基于顏色空間的方法在分割紅色和藍色正方體上具有很高的可靠性;此外,從被檢測到的正方體外接矩形框(圖5(c)綠色框所示)與實際正方體邊線的重合度,可以看出檢測過程的準確度較高.

圖5 目標抓取過程Fig.5 Target capture process

4 結 論

本文針對在軌服務與維護應用場景負載輕、近距離感知和精準定位等特點.采用Kinova機械臂及自帶的RGBD相機進行感知和抓取實驗.介紹了機械臂系統和相機標定的問題,詳細闡述彩色相機RGB圖像與深度相機深度圖的匹配問題以及三維空間定位問題,通過彩色圖像和深度圖像的配準提高定位準確性.在實驗環節介紹了顏色空間的圖像處理和分割,得到目標的二維坐標點和旋轉角度,結合深度圖獲得三維坐標信息.通過機械臂的控制模塊成功實現基于位置控制的目標抓取,為在軌服務提供了理論驗證.