3D深度機(jī)器視覺和機(jī)器臂的無序抓取

今天我記錄使用myCobot320 M5跟FS820-E1深度相機(jī)進(jìn)行一個(gè)無序抓取物體的分享。

為什么會(huì)選擇深度相機(jī)和機(jī)械臂做一個(gè)案例呢？

2D相機(jī)（最常見使用的相機(jī)）可以捕捉二維圖像，也就是在水平和垂直方向上的像素值。它們通常用于拍攝靜態(tài)場(chǎng)景或移動(dòng)的物體，并且無法提供深度信息。在機(jī)器視覺應(yīng)用中，2D相機(jī)可以用于圖像分類、目標(biāo)檢測(cè)和識(shí)別等任務(wù)。

相比之下，深度相機(jī)可以捕捉深度信息，因此可以獲得物體的三維信息。這些相機(jī)使用各種技術(shù)來測(cè)量物體的深度，如結(jié)構(gòu)光、時(shí)間飛行和立體視覺等。在機(jī)器視覺應(yīng)用中，3D相機(jī)可以用于點(diǎn)云分割、物體識(shí)別和3D重建等任務(wù)。

2D相機(jī)捕獲到的信息已經(jīng)滿足不了一些特殊的情況，所以換上深度相機(jī)獲得的更多的信息，比如說物體的長(zhǎng)寬高。

讓我們開始今天的主題。

FS820-E1

相機(jī)的環(huán)境搭建

首先我要搭建FS820-E1的開發(fā)環(huán)境，使用的是相機(jī)提供的RVS進(jìn)行開發(fā)?？梢岳肦VS中的視覺算子寫成節(jié)點(diǎn)(Node)快速搭建抓取功能。

RVS工作界面

實(shí)時(shí)采集數(shù)據(jù)

在左上角窗口資源中，找到TyCameraResource 算子添加到算子圖中的 ResourceGroup 中，在算子列表搜索TyCameraAccess，trigger算子分別添加到算子圖中，并根據(jù)需要調(diào)整算子參數(shù)。然后點(diǎn)擊運(yùn)行和屬性面板Trigger->ture即可查看可視化數(shù)據(jù)。沒有報(bào)錯(cuò)能正常顯示即可進(jìn)行下一步。

TyCameraResource 算子

●start 以及 stop 分別用于開啟、關(guān)閉資源算子的線程。auto_start 也是用于開啟資源算子，如果勾選，則僅在打開 RVS 軟件后第一次進(jìn)入運(yùn)行狀態(tài)時(shí)自動(dòng)開啟資源線程。

●reset：在打開資源線程后如果需要更改屬性參數(shù)，需要選中該選項(xiàng)進(jìn)行重置。

TyCameraAccess 算子

●打開cloud、rgb、depth可視化屬性，將 cloud_color設(shè)置為-2，代表真實(shí)顏色

myCobot 320-M5Stack

myCobot 320 是面向用戶自主編程開發(fā)的實(shí)踐型機(jī)器人，產(chǎn)品最大有效臂展 350mm，最大負(fù)載 1KG，重復(fù)定位精度 ±0.5mm。

環(huán)境搭建

需要python 編譯環(huán)境以及控制機(jī)器人的庫(kù)pymycobot

pip install pymycobot --upgrade

ps:使用的PC的顯卡最好是1060 2G獨(dú)顯以上，因?yàn)樾枰憻拡D片識(shí)別等操作，顯卡的性能越好運(yùn)行得越快

無序抓取

接下來是實(shí)現(xiàn)機(jī)械臂的無序抓取，無論物體處于何種姿態(tài)都能過精準(zhǔn)的抓到。下圖是整體的算子圖，也就是unstacking.xml工程文件

手眼標(biāo)定

用棋盤格來進(jìn)行手眼標(biāo)定

準(zhǔn)備：

●準(zhǔn)備棋盤格，算好棋盤格行列數(shù)，以及棋盤格邊長(zhǎng)(mm)

●手眼標(biāo)定分為眼在手上(eye in hand)、眼在手外(eye to hand)。根據(jù)不同情況將標(biāo)定板和相機(jī)固定好。這里選擇eye to hand

數(shù)據(jù)錄制

點(diǎn)擊左上角加載，打開unstacking_runtime/HandEyeCalibration/HandEyeCalibration.xml

在屬性面板正確填寫標(biāo)定板的行列數(shù)，和標(biāo)定板格子的單位長(zhǎng)度,和數(shù)據(jù)保存的文件路徑

啟動(dòng)相機(jī)工程和機(jī)械臂開始進(jìn)行標(biāo)定.

標(biāo)定前確保相機(jī)能完整識(shí)別完整的棋盤格，以及標(biāo)定過程中，棋盤格是固定的，不能發(fā)生移動(dòng)。運(yùn)行完成會(huì)得到18組數(shù)據(jù)。

計(jì)算標(biāo)定結(jié)果

positional error 在 0.005（5 毫米）以內(nèi)，則比較理想

坐標(biāo)系轉(zhuǎn)換

此操作旨在將點(diǎn)云所處的坐標(biāo)系——相機(jī) rgb 鏡頭坐標(biāo)系轉(zhuǎn)換至機(jī)器人坐標(biāo)系，這一轉(zhuǎn)換涉及相機(jī)外參及手眼標(biāo)定結(jié)果。

步驟：

●1）在算子圖中右鍵選擇在此處導(dǎo)入Group XML，導(dǎo)入RVSCommonGroup 中的HandToEye_Depth2Robot.group.xml。需要注意的是，除了該文件之外 ，還有HandInEye_Depth2Robot.group.xml。

●2）加載手眼標(biāo)定數(shù)據(jù)組的pose端口與HandToEye_Depth2Robot組的rgb2robot 端口連接。

●3）拖入 LoadCalibFile 算子，用于加載標(biāo)定文件，finshed 端口連接至HandToEye_Depth2Robot組的start端口；extrinsic_pose端口與rgb2depth 端口連接；start端口與InitTrigger端口finished端口連接。具體連接如下：

點(diǎn)擊 Group，找到 rgb2tcp 算子，在屬性面板的 pose 屬性處，粘貼手眼標(biāo)定的結(jié)果。

●5）通過前述步驟，我們已經(jīng)獲取了相機(jī) rgb 鏡頭轉(zhuǎn)機(jī)器人坐標(biāo)系的矩陣 rgb2robot 和相機(jī)深度鏡頭轉(zhuǎn)機(jī)器人坐標(biāo)系的矩陣 depth2robot，此處我們將相機(jī)深度鏡頭坐標(biāo)系下點(diǎn)云轉(zhuǎn)換至機(jī)器人坐標(biāo)系下。

●6）首先拖入 Transform 算子，type 屬性選擇“PointCloud”，將 depth2robot 端口連接至該算子的pose 輸入端口，將 LoadLocalData 算子組的 pointcloud 端口連接到本算子的同名輸入端口。

AI訓(xùn)練

采集訓(xùn)練圖像

打開 unstacking_runtime/MaskRCNN/ty_ai_savedata.xml，內(nèi)容基本與錄制 RGB 圖像一致，在這里我們只需要調(diào)整 EmitSring 中的 string 參數(shù)，設(shè)置為我們想要的路徑即可。點(diǎn)擊 Capture 錄制圖像。當(dāng)然數(shù)據(jù)越多那是越好，越穩(wěn)定。

標(biāo)注訓(xùn)練模型

目前為已錄制好的 RGB 標(biāo)注，我們推薦使用 labelme 這款軟件，本文檔提供一種 labelme 的安裝方法。

●1.按照官網(wǎng)安裝pip

https://pip.pypa.io/en/stable/installation/

●2.安裝PyQt5

pip install PyQt5

●3.安裝labelme

pip install labelme

標(biāo)注前準(zhǔn)備

首先確定任務(wù)目標(biāo)，明確在檢測(cè)過程中什么物體需要被檢測(cè)，什么物體不需要被檢測(cè)，從而有針對(duì)性的進(jìn)行標(biāo)注。

給定的標(biāo)注條件無需過分苛刻，不要按照人的思維去考慮，而是按照自己主觀設(shè)定的標(biāo)注思路是否便于落實(shí)代碼。

標(biāo)注過程

●終端輸出labelme，打開軟件點(diǎn)擊OpenDir,選擇我們標(biāo)注的路徑(在3.2.1采集訓(xùn)圖像Emit算子string路徑)

●點(diǎn)擊Create Polygons，為木塊繪制紅色的邊框

●完成后會(huì)彈出命名框，第一次請(qǐng)命名 wooden block，后續(xù)同類直接選擇

●當(dāng)圖像內(nèi)所有箱子標(biāo)注完成后，點(diǎn)擊 Save 進(jìn)行保存，默認(rèn)當(dāng)前文件夾，默認(rèn)名稱，隨后選擇 Next Image 切換到下一個(gè)圖像

訓(xùn)練AI模型

開unstacking_runtime/MaskRCNN/ty_ai_train.xml，這里只需要調(diào)整 data_directory 和classnames _filepath 路徑。點(diǎn)擊 start_train按鈕即開始訓(xùn)練。

最終會(huì)生成一個(gè) train output 文件夾在這個(gè)文件夾中有命名為 model fial,pth是所需要的權(quán)重文件。

AI推理

1）拖入一個(gè) Emit 算子，type 屬性選擇“pose”，重命名為“抓取參考Pose”，將 pose_roll 輸入入“3.141592654”。這個(gè)算子在后續(xù)的算子中使用。將該算子中 pose 端口與計(jì)算抓取點(diǎn)組down_pose 端口連接

2）雙擊展開計(jì)算抓取點(diǎn)組，需要預(yù)先使用 MaskRCNN 網(wǎng)絡(luò)對(duì)數(shù)據(jù)進(jìn)行訓(xùn)練，將其中的AIDetectGPU 算子的 type 更改為MaskRCNN 并對(duì)應(yīng)修改其余配置文件參數(shù)。由于 AI 推理算子在正式運(yùn)行前需要初始化運(yùn)行一次，所以需要在算子前額外添加一個(gè) Trigger(type 為 InitTrigger)。

3）AI 推理算子會(huì)獲得目標(biāo)在 2D 圖像中的位置區(qū)域（即掩碼圖，對(duì)應(yīng)的是 obj_list 端口），之后我們需要將這些位置區(qū)域轉(zhuǎn)換到 3D 點(diǎn)云中，這一環(huán)節(jié)對(duì)應(yīng)的是 計(jì)算抓取點(diǎn) 組中的 ProjectMask 算子。對(duì)于 ProjectMask 算子，不僅需要給入 AI 推理算子獲得的 obj_list,還需要給入 2D 圖對(duì)應(yīng)的點(diǎn)云、2D圖采圖時(shí)所用的 rgb 鏡頭坐標(biāo)系同點(diǎn)云坐標(biāo)系的轉(zhuǎn)換矩陣、相機(jī) rgb 鏡頭的內(nèi)參。這里已經(jīng)將點(diǎn)云轉(zhuǎn)換到了機(jī)器人坐標(biāo)系，所以需要輸入 rgb 鏡頭到機(jī)器人坐標(biāo)系的轉(zhuǎn)換矩陣。相機(jī)的 rgb 鏡頭內(nèi)參可以直接從相機(jī)參數(shù)文件中讀取。算子運(yùn)行完成后，會(huì)獲得所有檢測(cè)目標(biāo)的點(diǎn)云列表。

機(jī)械臂定位抓取

定位識(shí)別

根據(jù) AI 推理后的流程，已經(jīng)獲得了在機(jī)器人坐標(biāo)系下所有檢測(cè)目標(biāo)的點(diǎn)云列表。接下來要獲得它的點(diǎn)云中心坐標(biāo)。

1）雙擊展開 計(jì)算抓取點(diǎn) 組中 尋找目標(biāo) 組。需要先篩選木塊，并按照木塊列表的 Z 軸坐標(biāo)值進(jìn)行篩選，篩選出最上層的木塊，并對(duì)上層木塊進(jìn)行排序。因此這里使用 FilterBoxList 算子，重命名為“點(diǎn)云高度排序”，該算子的屬性值調(diào)整如下：

2）獲取平面，使用 FindElement，type 選擇“Plane”，獲得點(diǎn)云中適合抓取的平面。調(diào)整算子屬性distance_threshold 來調(diào)整所選取的平面。打開 cloud 可視化屬性來查看選取的平面。

3）獲取平面中心點(diǎn)，使用 MInimumBoundingBox 算子，重命名為“獲得外包框”，type 屬性選擇“ApproxMVBB”獲得一個(gè)方便機(jī)器人抓取的坐標(biāo)中心點(diǎn)。這里需要給該算子一個(gè) ref_pose，這里連接在3.3.4進(jìn)行AI推理中提到的“TowardsDownPose”，表示繞著 X 軸旋轉(zhuǎn) 180°，使 Z 軸朝下，便于機(jī)器人抓取。打開“GetBoxCube”屬性面板 box 和 box_pose 可視化屬性即可顯示計(jì)算出的平面中心點(diǎn)。

4）調(diào)整木塊方向，使用AdjustBoxNode算子，該算子的作用是，選擇長(zhǎng)度大于寬度的物體，將物體位姿進(jìn)行改變，這里選擇yaw選擇90°

這樣就能夠獲取到坐標(biāo)了

機(jī)械臂的抓取

在完成上述操作后，已經(jīng)獲得了目標(biāo)點(diǎn)坐標(biāo)，需要通過機(jī)器人和RVS軟件建立連接并進(jìn)行 tcp通訊。進(jìn)行實(shí)際抓取。

1）編寫TCP通訊代碼(RobotControl_Elephant.py)，以下部分為截取，該代碼實(shí)現(xiàn)RVS軟件和機(jī)械臂的TCP通訊

#CAPTURE print("***get pose***%s"%time.asctime()) capture_cmd = "GET_POSES n" capture_bytes=bytes(capture_cmd,encoding="utf-8") sock_rvs.send(capture_bytes) #recv CAPTURE data = sock_rvs.recv(socket_buf_len) print("---------------------------接收的數(shù)據(jù)----------------------------") print(data) print("***data end***%s"%data[-1:]) print("***capture_receive***%s"%time.asctime()) if int(data[-1:]) == 1: print("***received CAPTURE result***n") if int(data[-1:]) == 2: print("***All finished!***" #P_FLAG = bool(1-P_FLAG) #print("切換拍照位") continue #break

2）將目標(biāo)點(diǎn)進(jìn)行調(diào)整坐標(biāo)?例，將 ScalePose 算?的 type 設(shè)置為 Normal ，分別調(diào)整 pose 的（ X 、Y 、Z ）和（ Roll 、Pitch 、Yaw）?例。scale_rpy：修改 pose 中 r p y 的單位。設(shè)：57.2957795 。即從將弧度切換為?度。

3）最后，將ScalePose的 finished 和pose_list端口連接到最外層算子組的 MirrorOutput 端口, 并連接回 HandEyeTCPServer算子。至此，項(xiàng)目文件的編輯已經(jīng)完成。

效果展示

完成以上步驟，在unstacking.xml工程下，點(diǎn)擊運(yùn)行，同時(shí)運(yùn)行RobotControl_Elephant.py文件，識(shí)別到多個(gè)木塊選取其中一個(gè)木塊位姿就會(huì)發(fā)送給機(jī)械臂進(jìn)行夾取。

總結(jié)

總的來說這只是深度相機(jī)的一小點(diǎn)功能，后續(xù)甚至考慮將這幾個(gè)物體疊在一起又或者其他的不規(guī)則形狀來體現(xiàn)出它性能的強(qiáng)大。提前訓(xùn)練好模型，就能實(shí)現(xiàn)想要的效果。你期待我用它來做些什么呢？歡迎在地下留言，你們的點(diǎn)贊和關(guān)注將是我更新的動(dòng)力！

審核編輯黃宇