📚 课程目录第4章二维码定位与抓取

4. 二维码定位与抓取

任务目标

本案例实现基于二维码的工件定位与抓取引导：通过相机识别工件上的二维码，解算其在机器人基坐标系下的位置与偏航角，并将位姿数据发送给机械臂完成精确抓取。案例覆盖二维码检测、单目位姿估计、坐标变换与机器人通信的完整闭环。

系统组成与坐标变换

系统采用眼在手外（固定相机）布局。二维码坐标系原点位于码中心，X轴向右，Y轴向上，Z轴垂直码面朝外。通过相机标定获得内参矩阵K和畸变系数，通过手眼标定获得相机到机器人基座的变换矩阵T_base_cam。二维码在相机坐标系下的位姿由PnP解算，再经坐标变换映射到机器人基座坐标系。

环境依赖

Python 3.8+，OpenCV 4.5+（含aruco模块），NumPy，pyrealsense2（如使用RealSense相机）
机械臂通信：UR机器人使用TCP/RTDE，或通过Modbus TCP发送目标位姿
相机标定参数文件 camera_params.npz（含mtx、dist）与手眼标定结果 T_base_cam.npy

关键完整代码

import cv2
import numpy as np
import json
import socket

# ========== 配置 ==========
ARUCO_DICT = cv2.aruco.DICT_4X4_50
TARGET_ID = 0
QR_SIZE = 0.05  # 二维码实际边长（米）
ROBOT_IP = "192.168.1.100"
ROBOT_PORT = 30000

# 加载标定参数
calib = np.load("camera_params.npz")
mtx, dist = calib["mtx"], calib["dist"]
T_base_cam = np.load("T_base_cam.npy")  # 4x4 齐次矩阵

# ========== 二维码检测与位姿解算 ==========
def detect_qr_pose(frame):
    """检测二维码，返回其在相机坐标系下的旋转向量和平移向量"""
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    aruco_dict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT)
    params = cv2.aruco.DetectorParameters()
    corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=params)
    
    if ids is None or TARGET_ID not in ids.flatten():
        return None
    
    idx = list(ids.flatten()).index(TARGET_ID)
    rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
        corners[idx], QR_SIZE, mtx, dist)
    return rvec[0], tvec[0]

# ========== 坐标变换：相机 → 机器人基座 ==========
def cam_to_base(rvec, tvec):
    """将二维码位姿从相机坐标系变换到机器人基座坐标系"""
    R_cam_qr, _ = cv2.Rodrigues(rvec)
    T_cam_qr = np.eye(4)
    T_cam_qr[:3, :3] = R_cam_qr
    T_cam_qr[:3, 3] = tvec.flatten()
    
    T_base_qr = T_base_cam @ T_cam_qr
    
    # 提取位置
    pos = T_base_qr[:3, 3]
    
    # 提取偏航角（绕Z轴旋转）
    R = T_base_qr[:3, :3]
    yaw = np.arctan2(R[1, 0], R[0, 0])
    
    return pos, yaw

# ========== 机械臂通信（UR TCP协议示例） ==========
def send_pose_to_robot(pos, yaw):
    """发送目标位姿到机械臂（UR脚本movel命令）"""
    # 抓取高度补偿：二维码中心到抓取点的Z偏移
    GRASP_Z_OFFSET = -0.03  # 沿工具Z向下3cm
    x, y, z = pos
    z += GRASP_Z_OFFSET
    
    # 固定姿态：末端垂直向下抓取
    rx, ry, rz = 3.14159, 0.0, yaw  # RX=π使工具Z朝下
    
    cmd = f"movel(p[{x:.4f},{y:.4f},{z:.4f},{rx:.4f},{ry:.4f},{rz:.4f}], a=1.2, v=0.25)\n"
    
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.connect((ROBOT_IP, ROBOT_PORT))
    sock.send(cmd.encode())
    sock.close()

# ========== 主流程 ==========
def main():
    cap = cv2.VideoCapture(0)  # 或使用RealSense等其他相机
    if not cap.isOpened():
        print("无法打开相机")
        return
    
    while True:
        ret, frame = cap.read()
        if not ret:
            break
        
        result = detect_qr_pose(frame)
        if result is not None:
            rvec, tvec = result
            pos, yaw = cam_to_base(rvec, tvec)
            print(f"二维码位置: x={pos[0]:.3f}, y={pos[1]:.3f}, z={pos[2]:.3f}, yaw={np.degrees(yaw):.1f}°")
            
            # 发送到机械臂
            send_pose_to_robot(pos, yaw)
            
            # 可视化
            cv2.aruco.drawDetectedMarkers(frame, 
                [np.array([[[c[0][0], c[0][1]], [c[1][0], c[1][1]], 
                            [c[2][0], c[2][1]], [c[3][0], c[3][1]]]]) 
                 for c in [np.array([corners[0][0]])]], 
                np.array([[TARGET_ID]]))
        
        cv2.imshow("QR Detection", frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    
    cap.release()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()

运行与验证

准备标定文件：将相机内参保存为camera_params.npz（键名mtx、dist），手眼标定结果保存为T_base_cam.npy（4×4 ndarray）。
打印测试二维码：使用OpenCV生成ID为0的4×4二维码，边长5cm，贴在工件表面。
运行程序：python qr_grasp.py，将二维码置于相机视野内。
验证通过条件：
- 终端输出二维码在机器人基座坐标系下的三维坐标与偏航角，数值稳定（波动<2mm）；
- 机械臂末端移动到二维码正上方，工具Z轴与码面垂直，抓取点对准码中心；
- 旋转工件90°或180°，程序输出的偏航角相应变化，机械臂仍能正确定向抓取。

常见问题

检测不到二维码：检查二维码是否完整、光照是否均匀、相机曝光是否合适；确认TARGET_ID与打印的码ID一致。
位姿跳变：可对连续帧的位姿做低通滤波（如指数移动平均），或增加检测置信度阈值。
抓取偏移：若抓取点不在码中心，需在send_pose_to_robot中补偿工具中心点（TCP）与码中心的固定偏移量。