myCobot 600技術(shù)案例

Introduction

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

接下來我會介紹我使用到的設(shè)備

Equipment

myCobot pro 600

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

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

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

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

SPECIFICATIONS

FS820-E1-Depth camera

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

Project

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

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

接下來我先簡要介紹一下項目的結(jié)構(gòu)。

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

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

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

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

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

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

Execute：測試完成后，去執(zhí)行任務(wù)

介紹完項目的結(jié)構(gòu)，讓我們開始實現(xiàn)Hand-eye calibration。

Hand-eye calibration

通常情況下，對于機械臂執(zhí)行視覺類的抓取任務(wù)，手眼標定是必需的。手眼標定是一種將機械臂的坐標系與深度相機的坐標系進行關(guān)聯(lián)的過程。通過手眼標定，可以確定機械臂末端執(zhí)行器的位置和姿態(tài)與相機坐標系之間的轉(zhuǎn)換關(guān)系，從而實現(xiàn)精確的視覺引導和抓取。

第一步

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

第二步

固定深度相機和機械臂

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

第三步

匹配特偵點

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

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

第四步

計算標定坐標值

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

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

Object detection and recognition

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

采集被測物圖片

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

安裝labelme

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

pip install PyQt5
pip install labelme

標注過程

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

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

AI 推理

AI推理它使得訓練好的模型能夠應用到實際場景中，使用先前經(jīng)過訓練的參數(shù)和權(quán)重來處理新的輸入數(shù)據(jù)，并生成相應的輸出結(jié)果。

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

2）使用 FindElement 算子來獲取適合抓取的平面。在算子屬性中選擇類型為"Plane"，并通過調(diào)整 distance_threshold 屬性來選擇合適的平面?？梢源蜷_點云可視化屬性來查看所選的平面。

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

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

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

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

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

Robotic arm path planning

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

下面是部分實現(xiàn)路徑規(guī)劃的代碼

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

#旋轉(zhuǎn)矩陣和歐拉角之間的轉(zhuǎn)換
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

# 抓取位置的路徑規(guī)劃
defcompute_end_effector_pose(current_pose, tool_pose):
# 從位姿矩陣中提取旋轉(zhuǎn)矩陣和平移向量
    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

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

return new_pose

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

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

遇到的問題

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

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

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

總結(jié)

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

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

審核編輯 黃宇