七轴开源协作机械臂myArm视觉跟踪技术！

引言
ArUco标记是一种基于二维码的标记，可以被用于高效的场景识别和位置跟踪。这些标记的简单性和高效性使其成为机器视觉领域的理想选择，特别是在需要实时和高精度跟踪的场景中。结合机器学习和先进的图像处理技术，使用ArUco标记的机械臂系统可以实现更高级的自动化功能，如精确定位、导航和复杂动作的执行。

本案例旨在展示结合ArUco标记和机械臂运动控制技术，实现对机械臂的高精度控制和姿态跟踪。通过分析和解释脚本的不同组成部分，本文将探讨如何通过机器视觉识别技术和复杂的数据处理算法，来增强机械臂的操作能力。此外，还将展示机械臂在捕捉和响应环境变化方面的能力，以及如何通过编程和算法优化来提高整体系统的效率和准确性。

技术概述
机械臂-myArm 300 Pi
myArm 300 Pi是大象机器人最新出的一款七自由度的机械臂，搭载树莓派4B 4g芯片，专门为机器人定制了ubuntu mate 20.04操作系统。myArm提供了7自由度的灵活性，使它超越6自由度机器人，让机器人手臂的移动可以如同人类手臂一样灵活。

myArm内置接口可以进行超高难度的肘关节姿态变换，在实践教学中，可以用于机器人姿态研究、机器人运动路径规划学习、机器人冗余自由度的管理和利用、正逆运动学、ROS机器人开发环境、机器人应用开发、编程语言开发和底层数据处理等多种机器人相关的学科教育。开放了树莓派4B开发板和末端Atom近乎100%的硬件接口，可以搭配用户个人的树莓派4B及M5Atom的周边配件，进行个性化的场景开发，以满足不同用户的创意开发。

ArUco 码标记
ArUco 标记是一种二维条形码系统，它在机器视觉领域中被广泛用于标记检测和空间定位。这些标记由黑白图案组成，通常呈正方形，中心包含一个独特的二进制模式，使其能够被计算机视觉系统快速而准确地识别。

ArUco 标记的特点：

唯一性：每个 ArUco 标记具有独特的编码，允许识别系统轻松区分不同的标记
低成本：与其他高级定位系统相比，ArUco 标记不需要昂贵的设备或复杂的安装，可以直接打印ArUco标记。
定位和导航：在机器视觉系统中，ArUco 标记被用作参考点，帮助机械臂或移动机器人定位自身位置或导航至特定位置。
姿态估计：通过分析摄像头捕捉到的 ArUco 标记图像，系统能够计算出标记相对于摄像头的位置和方向（即姿态）。这对于精确控制机械臂或其他自动化设备至关重要。
相关软件和库
操作系统：Ubuntu mate 20.04

编程语言：Python 3.9+

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

pymycobot-控制机械臂运动的库，多种控制接口
OpenCV- 提供了丰富的图像处理和视频分析功能，包括对象检测，面部识别，运动跟踪，图形滤波等
Numpy-是一个核心科学计算哭，它提供了高性能的多维数组对象和工具，用于处理大量数据。
Math-提供了一系列基本的数学运算函数和常量，如三角函数、指数和对数函数、各种数学常数等。
系统设计与实现
物料准备

机械臂的姿态跟踪
定义：姿态跟踪通常指的是监测和记录一个物体在三维空间中的精确位置（平移）和方向（旋转），即其“姿态”。
技术应用：在机械臂的应用中，姿态跟踪涉及到实时监控和控制机械臂自身的各个关节和末端执行器的精确位置和方向。这通常需要复杂的传感器系统和算法，以实现高精度的控制。
用途：姿态跟踪对于执行精密的操作任务非常关键，如在制造业中的装配、焊接、在医疗领域中的外科手术辅助
项目组成部分
整个系统的架构主要分为以下几个部分：

1.硬件组成：机械臂，usb摄像头以及使用到的设备。
2.软件和控制系统：通过OpenCV识别ArUco 标记，控制算法，机械臂运动控制的系统来实现案例。
3.数据流程：用于图像的捕捉，图像处理，数据分析和转换，机械臂的执行。

功能实现
图像捕捉
使用到OpenCV捕获图像的方法

 

# 初始化摄像头
cap = cv2.VideoCapture(0) # 0代表默认摄像头的序号

#读取图像帧
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标记识别
对摄像头捕获的图像进行处理以及对ArUco的标记码进行识别

 

#检测ArUco标记
   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
       
 #在图像中标记出ArUco码，并且在每个标志上绘制坐标轴  
   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

 

数据解析与处理
因为该案例要做的是姿态跟踪，所以我们在检测ArUco标记的时候得检测该姿态，标注每个旋转向量（rvec）和平移向量（tvecs），这些向量描述了标记相对于摄像头的三位位置和方向。

 

   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

 

捕获数据的时候会大量的数据，为了提高检测的准确性需要使用滤波器来进行对数据的处理。

用到了中值滤波器，平均滤波器还有二阶滤波器。

中值滤波器：中值滤波器非常有效于去除所谓的“椒盐”噪声，同时保持信号的边缘信息。它在图像处理中常用于去除噪点，同时不会使图像模糊。

平均滤波器：平均滤波器常用于去除随机噪声、平滑和软化数据。在图像处理中，它可以用于平滑图像，但可能会导致边缘信息丢失，可以看到图像处理过后会模糊一些。

二阶滤波器：精确控制信号频率成分时使用，例如在信号处理和控制系统中，用于减少振荡和提高稳定性，特别是在姿态估计和精确运动控制中。

 

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

   # 将pos加入滤波器
   filter[filter_len - 1] = pos

   # 移动滤波器中的元素
   for i in range(filter_len - 1):
       filter[i] = filter[i + 1]

   # 计算中值并存储到输出数组中
   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

   # 移动滤波器中的元素
   for i in range(filter_len - 1):
       filter[i] = filter[i + 1]

   # 计算中值并存储到输出数组中
   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 截止频率 （设计的滤波器频率）
   fs = 100  # Hz 斩波频率  (采样频率）
   Ksi = 10  # 品质因数
   
   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

 

从检测到的标记（如ArUco标记）中提取机械臂或相机的姿态信息，并对提取的角度数据进行滤波处理，最终获得目标的坐标。

机械臂控制命令生成
在机械臂运动控制的方式上，我们得设置它的运动模式

 

# 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)

 

在获取到目标坐标，就得发送给机械臂去执行命令。

from pymycobot import MyArm
arm = MyArm("COM11",debug=False)
# 发送坐标控制机械臂运动
arm.send_coords(target_coords, 10, 2)
关键技术点
关键的技术点主要在几个方面：

ArUco 检测：
ArUco 标记的检测是整个系统运行的基础。通过摄像头识别这些标记，系统能够获取关于标记位置和方向的关键信息。这些信息对于机械臂的精确控制和操作至关重要，尤其是在需要精确位置调节的应用中，如在自动化、机器人编程和增强现实中。

使用图像处理技术，用openCV库从摄像头捕获的图像中识别标记，并且提取他们的位置和姿态信息。

滤波技术：
在处理图像数据或机械臂传感器数据时，滤波技术是保证数据质量和系统稳定性的关键。它们帮助去除数据中的噪声和误差，从而提高系统的准确性和可靠性。

机械臂控制：
在开始实现机械臂姿态跟踪前提，需要设置其运动模式。确保机械臂的运动与预期任务相匹配、提高操作的精度和可靠性非常关键。通过调整坐标系统、工具参考点和指令执行方式，可以使机械臂更加适应特定的操作环境和任务需求。

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

总结
在该项目中，深入了解图像处理和机器视觉的原理，特别是ArUco标记检测和位姿最终方面。可以掌握各种滤波技术的应用，理解它们在提高数据质量和系统性能中的重要。总而言之，该项目可以实践应用各个方面。

审核编辑 黄宇