myCobot 600技術(shù)案例

Introduction

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

接下來(lái)我會(huì)介紹我使用到的設(shè)備

Equipment

myCobot pro 600

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

1、它不需要額外的電腦進(jìn)行控制，可以直接使用樹(shù)莓派進(jìn)行控制

2、它的工作半徑和末端負(fù)載能力剛好適合我的場(chǎng)景

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

SPECIFICATIONS

FS820-E1-Depth camera

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

Project

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

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

接下來(lái)我先簡(jiǎn)要介紹一下項(xiàng)目的結(jié)構(gòu)。

Clear Task：首先我們要明確我的目標(biāo)，做一個(gè)深度視覺(jué)的機(jī)械臂抓取木塊的任務(wù)

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

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

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

Robotic arm path planning：識(shí)別到被測(cè)物體之后返回一個(gè)物體的三維坐標(biāo)，給到機(jī)械臂執(zhí)行抓取，要規(guī)劃?rùn)C(jī)械臂的運(yùn)動(dòng)軌跡達(dá)到抓取的點(diǎn)位

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

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

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

Hand-eye calibration

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

第一步

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

第二步

固定深度相機(jī)和機(jī)械臂

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

第三步

匹配特偵點(diǎn)

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

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

第四步

計(jì)算標(biāo)定坐標(biāo)值

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

到這里手眼標(biāo)定就結(jié)束了，接下來(lái)我們來(lái)訓(xùn)練機(jī)器識(shí)別被測(cè)物體。

Object detection and recognition

首先得采集我們的被測(cè)物體，木塊。我們用一個(gè)開(kāi)源軟件labelme，LabelMe是一個(gè)流行的開(kāi)源圖像標(biāo)注工具，用于創(chuàng)建和編輯圖像標(biāo)注數(shù)據(jù)集。旨在簡(jiǎn)化圖像標(biāo)注的過(guò)程，并為機(jī)器學(xué)習(xí)和計(jì)算機(jī)視覺(jué)任務(wù)提供標(biāo)注數(shù)據(jù)。

采集被測(cè)物圖片

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

安裝labelme

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

pip install PyQt5
pip install labelme

標(biāo)注過(guò)程

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

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

AI 推理

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

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

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

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

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

被測(cè)物體的識(shí)別和檢測(cè)就完成了。

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

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

Robotic arm path planning

在上一步操作中，我們得到了resultPose，也就是木塊相對(duì)于機(jī)械臂的坐標(biāo)，有了坐標(biāo)就可以對(duì)機(jī)械臂進(jìn)行控制，將機(jī)械臂的末端運(yùn)行到被測(cè)物體坐標(biāo)的上方。

下面是部分實(shí)現(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]

# 計(jì)算工具坐標(biāo)系在基坐標(biāo)系下的位姿
    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

我們來(lái)一起看看效果如何。

我預(yù)留了一段距離，機(jī)械臂末端到木塊的距離。這一段距離是給機(jī)械安裝上夾爪的距離。我在桌面上放了幾張白紙，那是因?yàn)槲沂褂玫谋粶y(cè)物體跟我放置的桌面的顏色相似度較高，為了能夠快速識(shí)別被測(cè)物體。

遇到的問(wèn)題

1 最一開(kāi)始在考慮機(jī)械臂跟深度相機(jī)如何建立聯(lián)系,兩個(gè)處理器之間建立聯(lián)系就有點(diǎn)復(fù)雜，最后用了python的socket的庫(kù)來(lái)實(shí)現(xiàn)上位機(jī)對(duì)myCobot pro 600的控制。

2 在測(cè)試的時(shí)候會(huì)有個(gè)別的坐標(biāo)出現(xiàn)了偏差，后來(lái)檢測(cè)原因是因?yàn)椋谧鐾晔盅蹣?biāo)定之后，機(jī)械臂和深度相機(jī)的位置一定是要固定住的，不能再標(biāo)定完之后再去做改變。

3 被測(cè)物的識(shí)別的快慢的問(wèn)題。在圖片中我們可以看到桌面上放了幾張白紙，那是因?yàn)槲覀兊谋粶y(cè)物體跟桌面的背景色的紋理有些相似，所以不能快速的識(shí)別出來(lái)。（是為了提高識(shí)別率從才放的白紙）機(jī)器終究還是機(jī)器，抵不過(guò)人類(lèi)的眼睛。

總結(jié)

目前的項(xiàng)目還不夠完善，后續(xù)我考慮加上末端的執(zhí)行器將木塊抓起來(lái)。但是這個(gè)也不能真正的運(yùn)用到實(shí)際場(chǎng)景當(dāng)中使用，是不需要我們來(lái)抓木塊。換個(gè)角度，如果說(shuō)他是一堆零件堆在那，用機(jī)械臂將一大堆的零件進(jìn)行分類(lèi)，給人們提供一個(gè)幫助那就很有用了。因?yàn)樵谝欢央s亂的零件中找零件是一件非常痛苦的事情，如果有遇到過(guò)這個(gè)問(wèn)題的就能明白我在說(shuō)什么了。

如果你有什么想法，或者有趣的建議歡迎在下方評(píng)論留言。你要是覺(jué)得喜歡這篇文章給予我一個(gè)點(diǎn)贊或者評(píng)論吧！

審核編輯 黃宇