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

硬件組件

M5Stack ESP32 Basic Core IoT Development Kit

Raspberry Pi 4 Model B

Espressif ESP32S

Elephant Robotics myCobot 320 m5

引言

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

myCobot 320 M5Stack

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

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

照相機(jī)


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

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

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

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

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ù)每個(gè)位置的數(shù)據(jù)，可以導(dǎo)出兩種轉(zhuǎn)換：

●Ai是機(jī)械臂末端執(zhí)行器在不同位置的變換矩陣，代表末端執(zhí)行器的運(yùn)動(dòng)。

●Bi是相機(jī)在相機(jī)坐標(biāo)系中觀察到的物體的變換矩陣，代表相機(jī)的運(yùn)動(dòng)。

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

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

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

●Ai代表機(jī)械臂末端執(zhí)行器的運(yùn)動(dòng)（從世界框架到末端執(zhí)行器框架）。

●Bi表示相機(jī)的運(yùn)動(dòng)（在相機(jī)坐標(biāo)系中看到的物體的運(yùn)動(dòng)）。

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

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

保存收集到的數(shù)據(jù)并計(jì)算結(jié)果后，就可以實(shí)現(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. 視覺(jué)跟蹤

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

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

基于從識(shí)別出的代碼中返回的坐標(biāo)，機(jī)械臂相應(yīng)地移動(dòng)，沿末端執(zhí)行器的 XYZ 軸進(jìn)行運(yùn)動(dòng)，以實(shí)現(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é)

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