七軸開(kāi)源協(xié)作機(jī)械臂myArm視覺(jué)跟蹤技術(shù)！

引言
ArUco標(biāo)記是一種基于二維碼的標(biāo)記，可以被用于高效的場(chǎng)景識(shí)別和位置跟蹤。這些標(biāo)記的簡(jiǎn)單性和高效性使其成為機(jī)器視覺(jué)領(lǐng)域的理想選擇，特別是在需要實(shí)時(shí)和高精度跟蹤的場(chǎng)景中。結(jié)合機(jī)器學(xué)習(xí)和先進(jìn)的圖像處理技術(shù)，使用ArUco標(biāo)記的機(jī)械臂系統(tǒng)可以實(shí)現(xiàn)更高級(jí)的自動(dòng)化功能，如精確定位、導(dǎo)航和復(fù)雜動(dòng)作的執(zhí)行。

本案例旨在展示結(jié)合ArUco標(biāo)記和機(jī)械臂運(yùn)動(dòng)控制技術(shù)，實(shí)現(xiàn)對(duì)機(jī)械臂的高精度控制和姿態(tài)跟蹤。通過(guò)分析和解釋腳本的不同組成部分，本文將探討如何通過(guò)機(jī)器視覺(jué)識(shí)別技術(shù)和復(fù)雜的數(shù)據(jù)處理算法，來(lái)增強(qiáng)機(jī)械臂的操作能力。此外，還將展示機(jī)械臂在捕捉和響應(yīng)環(huán)境變化方面的能力，以及如何通過(guò)編程和算法優(yōu)化來(lái)提高整體系統(tǒng)的效率和準(zhǔn)確性。

技術(shù)概述
機(jī)械臂-myArm 300 Pi
myArm 300 Pi是大象機(jī)器人最新出的一款七自由度的機(jī)械臂，搭載樹(shù)莓派4B 4g芯片，專(zhuān)門(mén)為機(jī)器人定制了ubuntu mate 20.04操作系統(tǒng)。myArm提供了7自由度的靈活性，使它超越6自由度機(jī)器人，讓機(jī)器人手臂的移動(dòng)可以如同人類(lèi)手臂一樣靈活。

myArm內(nèi)置接口可以進(jìn)行超高難度的肘關(guān)節(jié)姿態(tài)變換，在實(shí)踐教學(xué)中，可以用于機(jī)器人姿態(tài)研究、機(jī)器人運(yùn)動(dòng)路徑規(guī)劃學(xué)習(xí)、機(jī)器人冗余自由度的管理和利用、正逆運(yùn)動(dòng)學(xué)、ROS機(jī)器人開(kāi)發(fā)環(huán)境、機(jī)器人應(yīng)用開(kāi)發(fā)、編程語(yǔ)言開(kāi)發(fā)和底層數(shù)據(jù)處理等多種機(jī)器人相關(guān)的學(xué)科教育。開(kāi)放了樹(shù)莓派4B開(kāi)發(fā)板和末端Atom近乎100%的硬件接口，可以搭配用戶個(gè)人的樹(shù)莓派4B及M5Atom的周邊配件，進(jìn)行個(gè)性化的場(chǎng)景開(kāi)發(fā)，以滿足不同用戶的創(chuàng)意開(kāi)發(fā)。

ArUco 碼標(biāo)記
ArUco 標(biāo)記是一種二維條形碼系統(tǒng)，它在機(jī)器視覺(jué)領(lǐng)域中被廣泛用于標(biāo)記檢測(cè)和空間定位。這些標(biāo)記由黑白圖案組成，通常呈正方形，中心包含一個(gè)獨(dú)特的二進(jìn)制模式，使其能夠被計(jì)算機(jī)視覺(jué)系統(tǒng)快速而準(zhǔn)確地識(shí)別。

ArUco 標(biāo)記的特點(diǎn)：

唯一性：每個(gè) ArUco 標(biāo)記具有獨(dú)特的編碼，允許識(shí)別系統(tǒng)輕松區(qū)分不同的標(biāo)記
低成本：與其他高級(jí)定位系統(tǒng)相比，ArUco 標(biāo)記不需要昂貴的設(shè)備或復(fù)雜的安裝，可以直接打印ArUco標(biāo)記。
定位和導(dǎo)航：在機(jī)器視覺(jué)系統(tǒng)中，ArUco 標(biāo)記被用作參考點(diǎn)，幫助機(jī)械臂或移動(dòng)機(jī)器人定位自身位置或?qū)Ш街撂囟ㄎ恢谩?br /> 姿態(tài)估計(jì)：通過(guò)分析攝像頭捕捉到的 ArUco 標(biāo)記圖像，系統(tǒng)能夠計(jì)算出標(biāo)記相對(duì)于攝像頭的位置和方向（即姿態(tài)）。這對(duì)于精確控制機(jī)械臂或其他自動(dòng)化設(shè)備至關(guān)重要。
相關(guān)軟件和庫(kù)
操作系統(tǒng)：Ubuntu mate 20.04

編程語(yǔ)言：Python 3.9+

主要功能包：pymycobot，OpenCV，numpy，math

pymycobot-控制機(jī)械臂運(yùn)動(dòng)的庫(kù)，多種控制接口
OpenCV- 提供了豐富的圖像處理和視頻分析功能，包括對(duì)象檢測(cè)，面部識(shí)別，運(yùn)動(dòng)跟蹤，圖形濾波等
Numpy-是一個(gè)核心科學(xué)計(jì)算哭，它提供了高性能的多維數(shù)組對(duì)象和工具，用于處理大量數(shù)據(jù)。
Math-提供了一系列基本的數(shù)學(xué)運(yùn)算函數(shù)和常量，如三角函數(shù)、指數(shù)和對(duì)數(shù)函數(shù)、各種數(shù)學(xué)常數(shù)等。
系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)
物料準(zhǔn)備

機(jī)械臂的姿態(tài)跟蹤
定義：姿態(tài)跟蹤通常指的是監(jiān)測(cè)和記錄一個(gè)物體在三維空間中的精確位置（平移）和方向（旋轉(zhuǎn)），即其“姿態(tài)”。
技術(shù)應(yīng)用：在機(jī)械臂的應(yīng)用中，姿態(tài)跟蹤涉及到實(shí)時(shí)監(jiān)控和控制機(jī)械臂自身的各個(gè)關(guān)節(jié)和末端執(zhí)行器的精確位置和方向。這通常需要復(fù)雜的傳感器系統(tǒng)和算法，以實(shí)現(xiàn)高精度的控制。
用途：姿態(tài)跟蹤對(duì)于執(zhí)行精密的操作任務(wù)非常關(guān)鍵，如在制造業(yè)中的裝配、焊接、在醫(yī)療領(lǐng)域中的外科手術(shù)輔助
項(xiàng)目組成部分
整個(gè)系統(tǒng)的架構(gòu)主要分為以下幾個(gè)部分：

1.硬件組成：機(jī)械臂，usb攝像頭以及使用到的設(shè)備。
2.軟件和控制系統(tǒng)：通過(guò)OpenCV識(shí)別ArUco 標(biāo)記，控制算法，機(jī)械臂運(yùn)動(dòng)控制的系統(tǒng)來(lái)實(shí)現(xiàn)案例。
3.數(shù)據(jù)流程：用于圖像的捕捉，圖像處理，數(shù)據(jù)分析和轉(zhuǎn)換，機(jī)械臂的執(zhí)行。

功能實(shí)現(xiàn)
圖像捕捉
使用到OpenCV捕獲圖像的方法

# 初始化攝像頭
cap = cv2.VideoCapture(0) # 0代表默認(rèn)攝像頭的序號(hào)

#讀取圖像幀
ret, frame = cap.read()

#顯示圖像
cv2.imshow('video", frame)
def capture_video():
   cap = cv2.VideoCapture(0)
   if not cap.isOpened():
       print("Can't open camera")
       return
   
   try:
       while True:
           ret, frame = cap.read()
           if not ret:
               print("Can't read the pic from camera")
               break
           cv2.imshow('Video Capture', frame)
           # enter 'q'  quit 
           if cv2.waitKey(1) & 0xFF == ord('q'):
               break
   finally:
       cap.release()
       cv2.destroyAllWindows()

圖像處理與ArUco標(biāo)記識(shí)別
對(duì)攝像頭捕獲的圖像進(jìn)行處理以及對(duì)ArUco的標(biāo)記碼進(jìn)行識(shí)別

#檢測(cè)ArUco標(biāo)記
   def detect_marker_corners(self, frame: np.ndarray) -> Tuple[NDArray, NDArray, NDArray]:
       # 灰度化
       gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
       corners : Any
       ids : Any
       rejectedImgPoints : Any
       corners, ids, rejectedImgPoints = self.detector.detectMarkers(gray)
       return corners, ids, rejectedImgPoints
       
 #在圖像中標(biāo)記出ArUco碼，并且在每個(gè)標(biāo)志上繪制坐標(biāo)軸  
   def draw_marker(self, frame: np.ndarray, corners, tvecs, rvecs, ids) -> None:
       # cv2.aruco.drawDetectedMarkers(frame, corners, None, borderColor=(0, 255, 0))
       cv2.aruco.drawDetectedMarkers(frame, corners, ids, borderColor=(0, 200, 200))
       for i in range(len(ids)):
           corner, tvec, rvec, marker_id = corners[i], tvecs[i], rvecs[i], ids[i]
           cv2.drawFrameAxes(frame, self.mtx, self.dist, rvec, tvec, 30, 2)
           
while True:
           ret, frame = cap.read()
           corners, ids, rejectedImgPoints = aruco_detector.detect_marker_corners(frame)
            if ids is not None:
               detector.draw_marker(frame, corners, tvecs, rvecs, ids)
               ArucoDetector.draw_position_info(frame, corners, tvecs)           
           cv2.imshow('Video Capture', frame)
       
           # enter 'q'  quit 
           if cv2.waitKey(1) & 0xFF == ord('q'):
               break

數(shù)據(jù)解析與處理
因?yàn)樵摪咐龅氖亲藨B(tài)跟蹤，所以我們?cè)跈z測(cè)ArUco標(biāo)記的時(shí)候得檢測(cè)該姿態(tài)，標(biāo)注每個(gè)旋轉(zhuǎn)向量（rvec）和平移向量（tvecs），這些向量描述了標(biāo)記相對(duì)于攝像頭的三位位置和方向。

   def estimatePoseSingleMarkers(self, corners):
       """
       This will estimate the rvec and tvec for each of the marker corners detected by:
          corners, ids, rejectedImgPoints = detector.detectMarkers(image)
       corners - is an array of detected corners for each detected marker in the image
       marker_size - is the size of the detected markers
       mtx - is the camera matrix
       distortion - is the camera distortion matrix
       RETURN list of rvecs, tvecs, and trash (so that it corresponds to the old estimatePoseSingleMarkers())
       """
       marker_points = np.array([[-self.marker_size / 2, self.marker_size / 2, 0],
                                 [self.marker_size / 2, self.marker_size / 2, 0],
                                 [self.marker_size / 2, -self.marker_size / 2, 0],
                                 [-self.marker_size / 2, -self.marker_size / 2, 0]], dtype=np.float32)
       rvecs = []
       tvecs = []
       for corner in corners:
           corner : np.ndarray
           retval, rvec, tvec = cv2.solvePnP(marker_points, corner, self.mtx, self.dist, None, None, False, 
                                             cv2.SOLVEPNP_IPPE_SQUARE)
           if retval:
               rvecs.append(rvec)
               tvecs.append(tvec)

       rvecs = np.array(rvecs)
       tvecs = np.array(tvecs)
       (rvecs - tvecs).any()
       return rvecs, tvecs

捕獲數(shù)據(jù)的時(shí)候會(huì)大量的數(shù)據(jù)，為了提高檢測(cè)的準(zhǔn)確性需要使用濾波器來(lái)進(jìn)行對(duì)數(shù)據(jù)的處理。

用到了中值濾波器，平均濾波器還有二階濾波器。

中值濾波器：中值濾波器非常有效于去除所謂的“椒鹽”噪聲，同時(shí)保持信號(hào)的邊緣信息。它在圖像處理中常用于去除噪點(diǎn)，同時(shí)不會(huì)使圖像模糊。

平均濾波器：平均濾波器常用于去除隨機(jī)噪聲、平滑和軟化數(shù)據(jù)。在圖像處理中，它可以用于平滑圖像，但可能會(huì)導(dǎo)致邊緣信息丟失，可以看到圖像處理過(guò)后會(huì)模糊一些。

二階濾波器：精確控制信號(hào)頻率成分時(shí)使用，例如在信號(hào)處理和控制系統(tǒng)中，用于減少振蕩和提高穩(wěn)定性，特別是在姿態(tài)估計(jì)和精確運(yùn)動(dòng)控制中。

def median_filter(pos, filter, filter_len):
   if not np.any(filter):
       # 如果濾波器為空，用pos填充濾波器
       filter[:] = pos

   # 將pos加入濾波器
   filter[filter_len - 1] = pos

   # 移動(dòng)濾波器中的元素
   for i in range(filter_len - 1):
       filter[i] = filter[i + 1]

   # 計(jì)算中值并存儲(chǔ)到輸出數(shù)組中
   output = np.median(filter)

   return output

def Average_filter(pos, filter, filter_len):
   if not np.any(filter):
       # 如果濾波器為空，用pos填充濾波器
       filter[:] = pos

   # 將pos加入濾波器
   filter[filter_len - 1] = pos

   # 移動(dòng)濾波器中的元素
   for i in range(filter_len - 1):
       filter[i] = filter[i + 1]

   # 計(jì)算中值并存儲(chǔ)到輸出數(shù)組中
   output = np.mean(filter)

   return output
def twoorder_filter_single_input(input):
   global prev1
   global prev2
   global prev_out1
   global prev_out2

   if np.array_equal(prev1, np.zeros(3)):
       output, prev1, prev_out1 = input, input, input
       return output
   
   if np.array_equal(prev2, np.zeros(3)):
       prev2, prev_out2 = prev1, prev_out1
       output, prev1, prev_out1 = input, input, input
       return output
   
   fc = 20   # Hz 截止頻率 （設(shè)計(jì)的濾波器頻率）
   fs = 100  # Hz 斬波頻率  (采樣頻率）
   Ksi = 10  # 品質(zhì)因數(shù)
   
   temp1 = (2 * 3.14159 * fc)**2
   temp2 = (2 * fs)**2
   temp3 = 8 * 3.14159 * fs * Ksi * fc
   temp4 = temp2 + temp3 + temp1
   
   K1 = temp1 / temp4
   K2 = 2 * K1
   K3 = K1
   K4 = 2 * (temp1 - temp2) / temp4
   K5 = (temp1 + temp2 - temp3) / temp4
   
   
   output = K1 * prev2 + K2 * prev1 + K3 * input - K4 * prev_out2 - K5 * prev_out1

   # 更新全局變量
   prev2, prev1, prev_out2, prev_out1 = prev1, input, prev_out1, output
   
   return output

從檢測(cè)到的標(biāo)記（如ArUco標(biāo)記）中提取機(jī)械臂或相機(jī)的姿態(tài)信息，并對(duì)提取的角度數(shù)據(jù)進(jìn)行濾波處理，最終獲得目標(biāo)的坐標(biāo)。

機(jī)械臂控制命令生成
在機(jī)械臂運(yùn)動(dòng)控制的方式上，我們得設(shè)置它的運(yùn)動(dòng)模式

# Set end coordinate system 1-tool
arm.set_end_type(1)
time.sleep(0.03)
# Set tool coordinate system
arm.set_tool_reference([-50, 0, 20, 0, 0, 0])
time.sleep(0.03)
# Set command refresh mode
arm.set_fresh_mode(0)
time.sleep(0.03)

在獲取到目標(biāo)坐標(biāo)，就得發(fā)送給機(jī)械臂去執(zhí)行命令。

from pymycobot import MyArm
arm = MyArm("COM11",debug=False)
# 發(fā)送坐標(biāo)控制機(jī)械臂運(yùn)動(dòng)
arm.send_coords(target_coords, 10, 2)
關(guān)鍵技術(shù)點(diǎn)
關(guān)鍵的技術(shù)點(diǎn)主要在幾個(gè)方面：

ArUco 檢測(cè)：
ArUco 標(biāo)記的檢測(cè)是整個(gè)系統(tǒng)運(yùn)行的基礎(chǔ)。通過(guò)攝像頭識(shí)別這些標(biāo)記，系統(tǒng)能夠獲取關(guān)于標(biāo)記位置和方向的關(guān)鍵信息。這些信息對(duì)于機(jī)械臂的精確控制和操作至關(guān)重要，尤其是在需要精確位置調(diào)節(jié)的應(yīng)用中，如在自動(dòng)化、機(jī)器人編程和增強(qiáng)現(xiàn)實(shí)中。

使用圖像處理技術(shù)，用openCV庫(kù)從攝像頭捕獲的圖像中識(shí)別標(biāo)記，并且提取他們的位置和姿態(tài)信息。

濾波技術(shù)：
在處理圖像數(shù)據(jù)或機(jī)械臂傳感器數(shù)據(jù)時(shí)，濾波技術(shù)是保證數(shù)據(jù)質(zhì)量和系統(tǒng)穩(wěn)定性的關(guān)鍵。它們幫助去除數(shù)據(jù)中的噪聲和誤差，從而提高系統(tǒng)的準(zhǔn)確性和可靠性。

機(jī)械臂控制：
在開(kāi)始實(shí)現(xiàn)機(jī)械臂姿態(tài)跟蹤前提，需要設(shè)置其運(yùn)動(dòng)模式。確保機(jī)械臂的運(yùn)動(dòng)與預(yù)期任務(wù)相匹配、提高操作的精度和可靠性非常關(guān)鍵。通過(guò)調(diào)整坐標(biāo)系統(tǒng)、工具參考點(diǎn)和指令執(zhí)行方式，可以使機(jī)械臂更加適應(yīng)特定的操作環(huán)境和任務(wù)需求。

https://twitter.com/i/status/1733806097050558951

總結(jié)
在該項(xiàng)目中，深入了解圖像處理和機(jī)器視覺(jué)的原理，特別是ArUco標(biāo)記檢測(cè)和位姿最終方面?？梢哉莆崭鞣N濾波技術(shù)的應(yīng)用，理解它們?cè)谔岣邤?shù)據(jù)質(zhì)量和系統(tǒng)性能中的重要?？偠灾?，該項(xiàng)目可以實(shí)踐應(yīng)用各個(gè)方面。

審核編輯 黃宇