本文重點介紹了一款專為機器人教育而設(shè)計的具有動態(tài)跟蹤功能的創(chuàng)客友好型機械臂

硬件組件

M5Stack ESP32 Basic Core IoT Development Kit

Raspberry Pi 4 Model B

Espressif ESP32S

Elephant Robotics myCobot 320 m5

引言

今天文章的重點是使用myCobot 320機械臂重新創(chuàng)建一個視覺跟蹤案例，該案例由Elephant Robotics作為官方解決方案提供，使用戶能夠快速上手并逐步跟進(jìn)，以查看如何重現(xiàn)結(jié)果并識別任何潛在問題。 設(shè)備

myCobot 320 M5Stack

myCobot 320 M5Stack是six-degree-of-freedom的機械臂，工作半徑為350mm，末端執(zhí)行器最大有效載荷為1kg，支持各種主流編程語言和操作系統(tǒng)，本文主要使用Python來控制機械臂。

從圖像中我們可以看到，該裝置由一個 myCobot 機械臂和一個用于捕獲圖像數(shù)據(jù)的相機組成。機械臂的具體參數(shù)如下。

照相機


你不需要使用和我一樣的相機；關(guān)鍵是它可以安裝在機械臂的末端執(zhí)行器上，并且可以通過USB電纜獲取圖像數(shù)據(jù)。對于這個設(shè)置，我使用了myCobot Pro Camera Flange，這是大象機器人專門為myCobot改裝的末端執(zhí)行器相機。

環(huán)境設(shè)置
●操作系統(tǒng)：Windows 10
●編程語言：Python
●IDE：PyCharm
●庫：Numpy、OpenCV、STag、pymycobot、json、time（推薦這些庫的最新版本）
知識介紹
openCV
OpenCV（開源計算機視覺庫）是一個用于計算機視覺和機器學(xué)習(xí)的開源軟件庫，廣泛應(yīng)用于圖像處理、視頻分析和目標(biāo)檢測。OpenCV為開發(fā)人員提供了一套豐富的工具，用于處理圖像數(shù)據(jù)和實現(xiàn)復(fù)雜的視覺算法。在機械臂的背景下，OpenCV可用于視覺跟蹤，其中相機實時捕捉目標(biāo)，分析和提取目標(biāo)的位置和運動軌跡。然后機械臂根據(jù)這些信息調(diào)整其運動，實現(xiàn)精確的對象抓取和操縱。這種視覺跟蹤技術(shù)廣泛應(yīng)用于自動化、工業(yè)機器人和智能制造。
Stag
STag標(biāo)記是一種二維條碼系統(tǒng)，廣泛用于機器視覺中的標(biāo)記檢測和空間定位。這些標(biāo)記由黑白圖案組成，通常呈方形，中心有獨特的二進(jìn)制圖案，使它們能夠被計算機視覺系統(tǒng)快速準(zhǔn)確地識別。
手眼校準(zhǔn)-手眼配置（Hand-Eye Calibration – Eye-in-Hand Configuration）
手眼校準(zhǔn)涉及確定相機（眼睛）和機械臂末端執(zhí)行器（手）之間的精確空間和方向關(guān)系。在這種情況下，我們主要討論的是“手眼”場景，它指的是本項目中遇到的情況。手眼校準(zhǔn)對于建立相機相對于機械臂末端執(zhí)行器的定位方式至關(guān)重要。在手眼配置中，相機安裝在機械臂末端執(zhí)行器上，因此視野和相機角度隨著機械臂的移動而變化。目標(biāo)是計算相機坐標(biāo)系和機械臂末端執(zhí)行器坐標(biāo)系之間的變換。這使得機器人能夠通過相機感知周圍環(huán)境，并執(zhí)行目標(biāo)跟蹤和精確抓取等任務(wù)。

眼校準(zhǔn)步驟
1.相機姿態(tài)變化：在手眼配置中，相機的視角隨著機械臂的每次移動而變化。通過移動機械臂，可以捕捉校準(zhǔn)對象的多個視點，從而產(chǎn)生相機不同姿態(tài)的數(shù)據(jù)。
2.數(shù)據(jù)收集：機械臂被移動到幾個不同的位置，每次，它捕獲校準(zhǔn)板或特定物體的圖像。末端執(zhí)行器的姿態(tài)（由編碼器提供）和物體的姿態(tài)（通過圖像處理計算）被記錄下來。
3、攝像機與末端執(zhí)行器關(guān)系的求解：利用最小二乘等算法，計算攝像機與末端執(zhí)行器之間的變換矩陣，建立它們之間的坐標(biāo)變換關(guān)系。
用代碼實現(xiàn)該過程主要分為兩部分：校準(zhǔn)過程和跟蹤運動模塊。
校準(zhǔn)過程
1.坐標(biāo)轉(zhuǎn)換
在手眼標(biāo)定過程中，涉及不同坐標(biāo)系之間的變換。關(guān)鍵坐標(biāo)系如下：
●世界幀（W）：通常固定在環(huán)境中的參考幀。
●底架（B）：固定在機械臂底座的框架，用于表示手臂的姿勢。
●末端執(zhí)行器框架（E）：當(dāng)相機安裝在機械臂末端執(zhí)行器上時，該框架代表手臂末端執(zhí)行器的姿勢。
●相機框架（C）：固定在相機上的框架，用于描述相機看到的物體的姿勢。
在手眼標(biāo)定中，目標(biāo)是解決攝像機框架和機械臂末端執(zhí)行器框架之間的變換矩陣。這使得攝像機檢測到的物體的姿態(tài)被轉(zhuǎn)換成機械臂末端執(zhí)行器坐標(biāo)，從而實現(xiàn)對目標(biāo)物體的精確操作。

def eyes_in_hand_calculate(self, pose, tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr):
 
        tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr = map(np.array, [tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr])
        # Convert pose from degrees to radians
        euler = np.array(pose) * np.pi / 180
        Rbe = self.CvtEulerAngleToRotationMatrix(euler)
        Reb = Rbe.T
        
        A = np.hstack([(Mc2 - Mc1).reshape(-1, 1), 
                    (Mc3 - Mc1).reshape(-1, 1), 
                    (Mc3 - Mc2).reshape(-1, 1)])
        
        b = Reb @ np.hstack([(tbe1 - tbe2).reshape(-1, 1), 
                            (tbe1 - tbe3).reshape(-1, 1), 
                            (tbe2 - tbe3).reshape(-1, 1)])
        
        U, S, Vt = svd(A @ b.T)
        Rce = Vt.T @ U.T
        
        tce = Reb @ (Mr - (1/3)*(tbe1 + tbe2 + tbe3) - (1/3)*(Rbe @ Rce @ (Mc1 + Mc2 + Mc3)))
        
        eyes_in_hand_matrix = np.vstack([np.hstack([Rce, tce.reshape(-1, 1)]), np.array([0, 0, 0, 1])])
        return eyes_in_hand_matrix

2.數(shù)據(jù)收集

通過將機械臂移動到不同的位置，收集有關(guān)機械臂末端執(zhí)行器的各種位置和相機觀察的數(shù)據(jù)。

在您的代碼中，機械臂的姿勢是通過調(diào)用“ml.get_coords（）”方法獲得的，而相機的位置數(shù)據(jù)是通過“stag_identify（）”函數(shù)收集的，該函數(shù)識別標(biāo)記對象。

def reg_get(self, ml):
    for i in range(30):
        Mc_all = self.stag_identify()  
    tbe_all = ml.get_coords()  
    ...
    return Mc, tbe

3.坐標(biāo)變換矩陣

根據(jù)每個位置的數(shù)據(jù)，可以導(dǎo)出兩種轉(zhuǎn)換：

●Ai是機械臂末端執(zhí)行器在不同位置的變換矩陣，代表末端執(zhí)行器的運動。

●Bi是相機在相機坐標(biāo)系中觀察到的物體的變換矩陣，代表相機的運動。

這些變換矩陣通過視覺系統(tǒng)和機械臂系統(tǒng)（使用'get_coords'）獲得。

4.求解校準(zhǔn)矩陣

根據(jù)校準(zhǔn)模型：

●Ai代表機械臂末端執(zhí)行器的運動（從世界框架到末端執(zhí)行器框架）。

●Bi表示相機的運動（在相機坐標(biāo)系中看到的物體的運動）。

●Xce是待求解的手眼校準(zhǔn)矩陣，代表相機和機械臂末端執(zhí)行器之間的剛體變換。

通過收集Ai和Bi的多個位置，可以使用最小二乘法來求解Xce。雖然代碼中沒有顯示這部分邏輯，但通?？梢允褂肧VD分解等方法來解決。

保存收集到的數(shù)據(jù)并計算結(jié)果后，就可以實現(xiàn)后續(xù)的跟蹤功能。

[[0.9825202432037423, 0.03775722308035847, 0.1822864882543945, -21.50838594386444], [-0.04022441808787263, 0.9991420672993772, 0.009855229181470597, -0.6545263884052905], [-0.1817579926285262, -0.017015330087522124, 0.9831960692850951, 59.71321654600654], [0.0, 0.0, 0.0, 1.0]]

5. 視覺跟蹤

手眼校準(zhǔn)的輸出是一個剛體變換矩陣，用于描述相機和機械臂的末端執(zhí)行器之間的空間關(guān)系。該矩陣構(gòu)成了機械臂視覺控制和操作的基礎(chǔ)。利用這個矩陣，機械臂可以將視覺系統(tǒng)感知到的物體位置轉(zhuǎn)換為自己的坐標(biāo)系。前面提到的 STag 代碼是使用 OpenCV 算法識別的。

def stag_robot_identify(self, ml):
        marker_pos_pack = self.stag_identify()
        target_coords = ml.get_coords() 
        while (target_coords is None):
            target_coords = ml.get_coords()
        # print("current_coords", target_coords)
        cur_coords = np.array(target_coords.copy())
        cur_coords[-3:] *= (np.pi / 180)  
        fact_bcl = self.Eyes_in_hand(cur_coords, marker_pos_pack, self.EyesInHand_matrix) 
        
        for i in range(3):
            target_coords[i] = fact_bcl[i]
        
        return target_coords

基于從識別出的代碼中返回的坐標(biāo)，機械臂相應(yīng)地移動，沿末端執(zhí)行器的 XYZ 軸進(jìn)行運動，以實現(xiàn)跟蹤目標(biāo)。

def vision_trace_loop(self, ml):
    mc.set_fresh_mode(1)
    time.sleep(1)
 
    ml.send_angles(self.origin_mycbot_horizontal, 50) 
    self.wait() 
    time.sleep(1)
 
    origin = ml.get_coords()  
        while 1:
        target_coords = self.stag_robot_identify(ml) 
        target_coords[0] -= 300  
        self.coord_limit(target_coords)  
        print(target_coords)
        for i in range(3):
            target_coords[i+3] = origin[i+3]  
        ml.send_coords(target_coords, 30)

總結(jié)

總體來說，在運行這段代碼的時候，可能還是會出現(xiàn)一些小插曲，有些功能沒有完全解釋清楚。使用過許多手眼校準(zhǔn)方法后，我發(fā)現(xiàn)這是更直接的自動校準(zhǔn)方法之一，盡管它缺乏一些精度，但可以通過優(yōu)化來改進(jìn)。總的來說，這個案例值得探索，特別是對于那些對機械臂和視覺系統(tǒng)有一定了解的人來說！