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

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

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

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

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

讓我們開始今天的主題。

FS820-E1

相機的環境搭建

首先我要搭建FS820-E1的開發環境，使用的是相機提供的RVS進行開發。可以利用RVS中的視覺算子寫成節點(Node)快速搭建抓取功能。

RVS工作界面

實時采集數據

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

TyCameraResource 算子

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

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

TyCameraAccess 算子

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

myCobot 320-M5Stack

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

環境搭建

需要python 編譯環境以及控制機器人的庫pymycobot

pip install pymycobot --upgrade

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

無序抓取

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

手眼標定

用棋盤格來進行手眼標定

準備：

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

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

數據錄制

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

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

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

標定前確保相機能完整識別完整的棋盤格，以及標定過程中，棋盤格是固定的，不能發生移動。運行完成會得到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 錄制圖像。當然數據越多那是越好，越穩定。

標注訓練模型

目前為已錄制好的 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，后續同類直接選擇

●當圖像內所有箱子標注完成后，點擊 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”。這個算子在后續的算子中使用。將該算子中 pose 端口與計算抓取點組down_pose 端口連接

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

3）AI 推理算子會獲得目標在 2D 圖像中的位置區域（即掩碼圖，對應的是 obj_list 端口），之后我們需要將這些位置區域轉換到 3D 點云中，這一環節對應的是 計算抓取點 組中的 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文件，識別到多個木塊選取其中一個木塊位姿就會發送給機械臂進行夾取。

總結

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

審核編輯黃宇