myCobot 600技術案例

Introduction

隨著時代的進步，各種精密的機械臂，人形機器人不斷的問世。我們即將迎來到處都是機器人的高科技時代。為了跟上時代的腳步，我最近入手了一臺myCobot pro 600機械臂，我主要是想要用它來學習機械臂相關得控制以及機器視覺的項目，給以后的實踐中在本文中，我將記錄使用myCobot pro 600結合深度相機來實現物體得跟蹤以及抓取。

接下來我會介紹我使用到的設備

Equipment

myCobot pro 600

myCobot pro 600是一款六個自由度的機械臂，它的工作半徑最大達到600mm，它末端最大負載達到2kg，搭載的電機是諧波減速器。它還是一款以樹莓派4B為核心控制主板的嵌入式機械臂搭配ubuntu20.0的系統。我選擇他的原因有幾個：

1、它不需要額外的電腦進行控制，可以直接使用樹莓派進行控制

2、它的工作半徑和末端負載能力剛好適合我的場景

3、大象機器人為它設計了一款可視化的控制軟件robotflow，可以進行一些拖拽式的編程

SPECIFICATIONS

FS820-E1-Depth camera

深度相機可以捕捉深度信息，可以獲得物體的三維坐標，距離，尺寸和體積。深度相機使用多種算法技術來測量物體的深度，如結構光、時間飛行和立體視覺等。在機器視覺應用中，深度相機可以用于點云分割、物體識別和3D重建等任務。

Project

我使用機械臂有一段時間了，大部分的時間都是用來去執行一些路徑，重復的做一些動作。如果只是這樣的話，機械臂在很多方面都是不能夠勝任人們的工作的。我們工作的時候手和眼睛是需要進行配合，當然機械臂我們要給他配置一個“眼睛”，就是深度相機。

我今天要記錄的是用機械臂+相機來是想物體的抓取，不只是一個平面的抓取，而是一個能夠根據獲取的深度信息進行判斷高度的抓取。

接下來我先簡要介紹一下項目的結構。

Clear Task：首先我們要明確我的目標，做一個深度視覺的機械臂抓取木塊的任務

Hardware preparation：機械臂使用的是mycobot pro 600，深度相機用的是FS820-E1

Hand-eye calibration：進行機械臂和相機之間的手眼標定，以建立它們之間的坐標系轉換關系。通過手眼標定，可以確定機械臂末端執行器的位置和姿態與相機坐標系之間的轉換關系，從而實現精確的視覺引導和抓取。

Object detection and recognition：確定被測物體為木塊，訓練算法識別木塊，通過大量的數據讓機器能夠精準的識別出被測物體

Robotic arm path planning：識別到被測物體之后返回一個物體的三維坐標，給到機械臂執行抓取，要規劃機械臂的運動軌跡達到抓取的點位

Test：完成上邊測試之后，需要進行測試，不斷的進行調整。

Execute：測試完成后，去執行任務

介紹完項目的結構，讓我們開始實現Hand-eye calibration。

Hand-eye calibration

通常情況下，對于機械臂執行視覺類的抓取任務，手眼標定是必需的。手眼標定是一種將機械臂的坐標系與深度相機的坐標系進行關聯的過程。通過手眼標定，可以確定機械臂末端執行器的位置和姿態與相機坐標系之間的轉換關系，從而實現精確的視覺引導和抓取。

第一步

需要準備一個標定板：選擇黑白相間的方格為標定板，棋盤格的幾何結構非常規則，方格之間的邊緣和角點位置可以被精確地計算和模型化。這使得標定算法可以準確地估計相機與棋盤格之間的幾何關系。

第二步

固定深度相機和機械臂

我要做的是眼在手外，就是eye to hand的模式，相機的位置不會隨著機械臂的改變而改變。

第三步

匹配特偵點

將標定板在相機視野范圍內進行多次的旋轉移動，捕獲這些圖片以提供豐富的圖片信息，對每個標定板圖像，使用角點檢測算法來提取標定板上的取方格交叉點的坐標特征點。

用的是相機自帶的RVS（robot vision suite)軟件，里面提前編寫好捕獲圖片，提取特征點，匹配特偵點的方法。

第四步

計算標定坐標值

得到了20張不同角度，不同位置的特偵點的數據，接下來使用RVS提供的算法在程序中進行計算得出eye to hand標定的值。記錄下手眼標定的結果等到后面進行機械臂坐標系和相機坐標系轉化的關鍵。

到這里手眼標定就結束了，接下來我們來訓練機器識別被測物體。

Object detection and recognition

首先得采集我們的被測物體，木塊。我們用一個開源軟件labelme，LabelMe是一個流行的開源圖像標注工具，用于創建和編輯圖像標注數據集。旨在簡化圖像標注的過程，并為機器學習和計算機視覺任務提供標注數據。

采集被測物圖片

像前邊標定一樣，采集的圖片越多數據越精準。將他保存在指定的路徑中。

安裝labelme

在python中安裝依賴庫，以便后續的使用

pip install PyQt5
pip install labelme

標注過程

打開labelme，找到我們保存采集圖片的路徑，然后點擊Create Polygons，為木塊繪制紅色的邊框，完成標記后給標記結果明明“wooden block”，一直重復這個步驟為木塊繪制邊框，直至標記完所有采集的圖片。

這是人為標記的木塊，要實現機器識別就得讓機器學會檢測該標記的物體。接下來要訓練AI 模型，RVS軟件中有這個算法，我們只需要將標記好的圖片提交到AI訓練功能，最后會生成一個Train output文件夾，在這里可以獲取標記的權重文件。

AI 推理

AI推理它使得訓練好的模型能夠應用到實際場景中，使用先前經過訓練的參數和權重來處理新的輸入數據，并生成相應的輸出結果。

1）使用 FilterBoxList 算子（重命名為"點云高度排序"）來篩選木塊并按照木塊列表的 Z 軸坐標值進行排序。這個算子的屬性值需要按照下面的要求進行調整。

2）使用 FindElement 算子來獲取適合抓取的平面。在算子屬性中選擇類型為"Plane"，并通過調整 distance_threshold 屬性來選擇合適的平面。可以打開點云可視化屬性來查看所選的平面。

3）使用 MinimumBoundingBox 算子（重命名為"獲得外包框"）來獲取平面的中心點坐標。在算子屬性中選擇類型為"ApproxMVBB"，并給該算子提供一個 ref_pose，這個 ref_pose 連接到先前提到的"TowardsDownPose"算子，它表示繞著 X 軸旋轉 180°，使 Z 軸朝下，以便機器人抓取。可以打開"GetBoxCube"屬性面板中的 box 和 box_pose 可視化屬性來顯示計算出的平面中心點。

總結起來，這個操作的目標是從點云中篩選出木塊并按照高度進行排序，然后找到適合抓取的平面，并計算該平面的中心點坐標。這個過程可能是為了進一步在機器人操作中使用這些信息，例如進行抓取任務或路徑規劃。

被測物體的識別和檢測就完成了。

再結合手眼標定，就能識別出木塊的時候會得到木塊的坐標信息。這樣就獲取到了木塊相對于機械臂的坐標。

下圖是RVS中的仿真模擬3D環境。最左邊帶坐標的是深度相機的坐標系的標志。

Robotic arm path planning

在上一步操作中，我們得到了resultPose，也就是木塊相對于機械臂的坐標，有了坐標就可以對機械臂進行控制，將機械臂的末端運行到被測物體坐標的上方。

下面是部分實現路徑規劃的代碼

import _thread
import socket
import json
import time
import sys
import math
import copy
import numpy as np
from RoboFlowSocket import RoboFlowSocket

#旋轉矩陣和歐拉角之間的轉換
defCvtRotationMatrixToEulerAngle(pdtRotationMatrix):
    pdtEulerAngle = np.zeros(3)

    pdtEulerAngle[2]= np.arctan2(pdtRotationMatrix[1,0], pdtRotationMatrix[0,0])

    fCosRoll = np.cos(pdtEulerAngle[2])
    fSinRoll = np.sin(pdtEulerAngle[2])

    pdtEulerAngle[1]= np.arctan2(-pdtRotationMatrix[2,0],(fCosRoll * pdtRotationMatrix[0,0])+(fSinRoll * pdtRotationMatrix[1,0]))
    pdtEulerAngle[0]= np.arctan2((fSinRoll * pdtRotationMatrix[0,2])-(fCosRoll * pdtRotationMatrix[1,2]),(-fSinRoll * pdtRotationMatrix[0,1])+(fCosRoll * pdtRotationMatrix[1,1]))

return pdtEulerAngle

defCvtEulerAngleToRotationMatrix(ptrEulerAngle):
    ptrSinAngle = np.sin(ptrEulerAngle)
    ptrCosAngle = np.cos(ptrEulerAngle)

    ptrRotationMatrix = np.zeros((3,3))
    ptrRotationMatrix[0,0]= ptrCosAngle[2]* ptrCosAngle[1]
    ptrRotationMatrix[0,1]= ptrCosAngle[2]* ptrSinAngle[1]* ptrSinAngle[0]- ptrSinAngle[2]* ptrCosAngle[0]
    ptrRotationMatrix[0,2]= ptrCosAngle[2]* ptrSinAngle[1]* ptrCosAngle[0]+ ptrSinAngle[2]* ptrSinAngle[0]
    ptrRotationMatrix[1,0]= ptrSinAngle[2]* ptrCosAngle[1]
    ptrRotationMatrix[1,1]= ptrSinAngle[2]* ptrSinAngle[1]* ptrSinAngle[0]+ ptrCosAngle[2]* ptrCosAngle[0]
    ptrRotationMatrix[1,2]= ptrSinAngle[2]* ptrSinAngle[1]* ptrCosAngle[0]- ptrCosAngle[2]* ptrSinAngle[0]
    ptrRotationMatrix[2,0]=-ptrSinAngle[1]
    ptrRotationMatrix[2,1]= ptrCosAngle[1]* ptrSinAngle[0]
    ptrRotationMatrix[2,2]= ptrCosAngle[1]* ptrCosAngle[0]

return ptrRotationMatrix

# 抓取位置的路徑規劃
defcompute_end_effector_pose(current_pose, tool_pose):
# 從位姿矩陣中提取旋轉矩陣和平移向量
    current_rotation = current_pose[:3,:3]
    current_translation = current_pose[:3,3]
    tool_rotation = tool_pose[:3,:3]
    tool_translation = tool_pose[:3,3]

# 計算工具坐標系在基坐標系下的位姿
    new_rotation = np.dot(current_rotation, tool_rotation)
    new_translation = np.dot(current_rotation, tool_translation)+ current_translation

# 組合旋轉矩陣和平移向量得到新的位姿矩陣
    new_pose = np.eye(4)
    new_pose[:3,:3]= new_rotation
    new_pose[:3,3]= new_translation

return new_pose

我們來一起看看效果如何。

我預留了一段距離，機械臂末端到木塊的距離。這一段距離是給機械安裝上夾爪的距離。我在桌面上放了幾張白紙，那是因為我使用的被測物體跟我放置的桌面的顏色相似度較高，為了能夠快速識別被測物體。

遇到的問題

1 最一開始在考慮機械臂跟深度相機如何建立聯系,兩個處理器之間建立聯系就有點復雜，最后用了python的socket的庫來實現上位機對myCobot pro 600的控制。

2 在測試的時候會有個別的坐標出現了偏差，后來檢測原因是因為，在做完手眼標定之后，機械臂和深度相機的位置一定是要固定住的，不能再標定完之后再去做改變。

3 被測物的識別的快慢的問題。在圖片中我們可以看到桌面上放了幾張白紙，那是因為我們的被測物體跟桌面的背景色的紋理有些相似，所以不能快速的識別出來。（是為了提高識別率從才放的白紙）機器終究還是機器，抵不過人類的眼睛。

總結

目前的項目還不夠完善，后續我考慮加上末端的執行器將木塊抓起來。但是這個也不能真正的運用到實際場景當中使用，是不需要我們來抓木塊。換個角度，如果說他是一堆零件堆在那，用機械臂將一大堆的零件進行分類，給人們提供一個幫助那就很有用了。因為在一堆雜亂的零件中找零件是一件非常痛苦的事情，如果有遇到過這個問題的就能明白我在說什么了。

如果你有什么想法，或者有趣的建議歡迎在下方評論留言。你要是覺得喜歡這篇文章給予我一個點贊或者評論吧！

審核編輯 黃宇