教育场景中的自动化分拣系统！基于大象机器人UltraArm P340机械臂和传送带的实现

#导入库
import cv2.aruco as aruco
 
#加载字典
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
 
#灰度处理并且识别
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)   
                corners, ids, rejectImaPoint = cv.aruco.detectMarkers(
                    gray, self.aruco_dict, parameters=self.aruco_params
                )
 
#检测Aruco码
if len(corners) > 0:
    if ids is not None:
        id = int(ids[0][0])

#姿态估计
ret = cv.aruco.estimatePoseSingleMarkers(
    corners, 0.022, self.camera_matrix, self.dist_coeffs
)
(rvec, tvec) = (ret[0], ret[1])
 
#位置计算
xyz = tvec[0, 0, :]
xyz = [round(xyz[0]*1000 + self.pump_x, 2), round(xyz[1]*1000 + self.pump_y, 2), round(xyz[2]*1000, 2)]
 
#旋转向量处理
try:
    rvec = np.reshape(rvec, (3, 1))
except ValueError as e:
    print("reshape错误：", e)
    print("rvec1=", rvec)
    rvec = np.array([[[-2.86279729, -0.00687534, -0.05316529]]])
    print("rvec2=", rvec)
 
#计算旋转矩阵和欧拉角
rotation_matrix, _ = cv.Rodrigues(rvec)
euler_angles = cv.RQDecomp3x3(rotation_matrix)[0]
yaw_angle = int(euler_angles[2])
 
#返回结果
for i in range(rvec.shape[0]):
    cv.aruco.drawDetectedMarkers(img, corners, ids)
    if num < 100:
        num += 1
    elif num == 100:
        cv.destroyAllWindows()
        print("final_x:", xyz[0])
        print("final_y:", xyz[1])
        print("final_yaw_angle=", -yaw_angle)
        return xyz[0], xyz[1], -yaw_angle, id

from pymycobot.ultraArm import ultraArm
 
#创建实例,com为机械臂的串口号
ua = ultraArm(COM)
 
# 角度控制
ua.send_angles([angle_list],speed)
# 坐标控制mode,控制走直线还是非直线
ua.send_coords([coords_list),speed,mode)
 
#吸泵的使用1-open ;0-close
def pub_pump(self, flag):
    if flag:
        self.ua.set_gpio_state(0)
    else:
        self.ua.set_gpio_state(1)
ua.pub_pump(state)

#初始化设置
self.serial = serial.Serial(port, baudrate, timeout=1)
 
#写入命令
def write_command(self, command):
    self.serial.write(command.encode())
 
#设置传送带的方向
def set_direction(self, direction):
    command = f'DIR {direction}\n'
    self.write_command(command)
 
#设置速度
def set_speed(self, speed):
    command = f'SPD {speed}\n'
    self.write_command(command)
 
#启动和停止
def start(self):
    self.write_command('START\n')
 
def stop(self):
    self.write_command('STOP\n')

  while robot2.count<18:
        data=obj.detect()
        while len(data)<6:
            print("二维码检测数目不对，请确保二维码在相机范围内并且能正确识别")
            time.sleep(1)
            data=obj.detect()          
        
        for i in range(len(data)):
            robot.move(data[i][2],data[i][1]) 
                                 
            conveyer.open_conveyor(100)
            time.sleep(5.2)
            
            conveyer.close_conveyor()
            
            for i in range(3):
                try:
                    print("i=",i)                                
                    pose=cam.detect()
                    if pose is not None: 
                        break
                    
                except Exception as e:
                    if i==2:
                       message()                
                    temp=obj.exception_handling()
                    robot.Special_handling(temp[0][2],temp[0][1],temp[0][3])
                    conveyer.open_conveyor(100)
                    time.sleep(5.2)
                    conveyer.close_conveyor()
            id=robot2.move(pose[0],pose[1],pose[2],pose[3])                   
            robot2.judge(id)