今天我記錄使用myCobot320 M5跟FS820-E1深度相機進行一個無序抓取物體的分享。

為什么會選擇深度相機和機械臂做一個案例呢？

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

相比之下，深度相機可以捕捉深度信息，因此可以獲得物體的三維信息。這些相機使用各種技術來測量物體的深度，如結構光、時間飛行和立體視覺等。在機器視覺應用中，3D相機可以用于點云分割、物體識別和3D重建等任務。

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

讓我們開始今天的主題。

FS820-E1

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

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

RVS工作界面

實時采集數據

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

TyCameraResource 算子

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

●reset：在打開資源線程后如果需要更改屬性參數，需要選中該選項進行重置。

TyCameraAccess 算子

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

myCobot 320-M5Stack

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

環(huán)境搭建

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

pip install pymycobot --upgrade

ps:使用的PC的顯卡最好是1060 2G獨顯以上，因為需要鍛煉圖片識別等操作，顯卡的性能越好運行得越快

無序抓取

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

手眼標定

用棋盤格來進行手眼標定

準備：

●準備棋盤格，算好棋盤格行列數，以及棋盤格邊長(mm)

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

數據錄制

點擊左上角加載，打開unstacking_runtime/HandEyeCalibration/HandEyeCalibration.xml

在屬性面板正確填寫標定板的行列數，和標定板格子的單位長度,和數據保存的文件路徑

啟動相機工程和機械臂開始進行標定.

標定前確保相機能完整識別完整的棋盤格，以及標定過程中，棋盤格是固定的，不能發(fā)生移動。運行完成會得到18組數據。

計算標定結果

positional error 在 0.005（5 毫米）以內，則比較理想

坐標系轉換

此操作旨在將點云所處的坐標系——相機 rgb 鏡頭坐標系轉換至機器人坐標系，這一轉換涉及相機外參及手眼標定結果。

步驟：

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

●2）加載手眼標定數據組的pose端口與HandToEye_Depth2Robot組的rgb2robot 端口連接。

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

點擊 Group，找到 rgb2tcp 算子，在屬性面板的 pose 屬性處，粘貼手眼標定的結果。

●5）通過前述步驟，我們已經獲取了相機 rgb 鏡頭轉機器人坐標系的矩陣 rgb2robot 和相機深度鏡頭轉機器人坐標系的矩陣 depth2robot，此處我們將相機深度鏡頭坐標系下點云轉換至機器人坐標系下。

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

AI訓練

采集訓練圖像

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

標注訓練模型

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

●1.按照官網安裝pip

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

●2.安裝PyQt5

pip install PyQt5

●3.安裝labelme

pip install labelme

標注前準備

首先確定任務目標，明確在檢測過程中什么物體需要被檢測，什么物體不需要被檢測，從而有針對性的進行標注。

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

標注過程

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

●點擊Create Polygons，為木塊繪制紅色的邊框

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

●當圖像內所有箱子標注完成后，點擊 Save 進行保存，默認當前文件夾，默認名稱，隨后選擇 Next Image 切換到下一個圖像

訓練AI模型

開unstacking_runtime/MaskRCNN/ty_ai_train.xml，這里只需要調整 data_directory 和classnames _filepath 路徑。點擊 start_train按鈕即開始訓練。

最終會生成一個 train output 文件夾在這個文件夾中有命名為 model fial,pth是所需要的權重文件。

AI推理

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

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

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

機械臂定位抓取

定位識別

根據 AI 推理后的流程，已經獲得了在機器人坐標系下所有檢測目標的點云列表。接下來要獲得它的點云中心坐標。

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

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

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

4）調整木塊方向，使用AdjustBoxNode算子，該算子的作用是，選擇長度大于寬度的物體，將物體位姿進行改變，這里選擇yaw選擇90°

這樣就能夠獲取到坐標了

機械臂的抓取

在完成上述操作后，已經獲得了目標點坐標，需要通過機器人和RVS軟件建立連接并進行 tcp通訊。進行實際抓取。

1）編寫TCP通訊代碼(RobotControl_Elephant.py)，以下部分為截取，該代碼實現RVS軟件和機械臂的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("---------------------------接收的數據----------------------------") 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）將目標點進行調整坐標?例，將 ScalePose 算?的 type 設置為 Normal ，分別調整 pose 的（ X 、Y 、Z ）和（ Roll 、Pitch 、Yaw）?例。scale_rpy：修改 pose 中 r p y 的單位。設：57.2957795 。即從將弧度切換為?度。

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

效果展示

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

總結

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

審核編輯黃宇