📚 课程目录第2章基于颜色分割的工件定位

2. 基于颜色分割的工件定位

任务目标

本案例实现基于颜色分割的工件定位：从相机图像中分割出目标颜色的工件区域，计算其像素坐标（图像中心），并通过手眼标定得到的变换矩阵将像素坐标转换到机器人基座标系，最终输出可供机器人使用的三维坐标。

环境与依赖

Python 3.8+，OpenCV（cv2）、NumPy
相机已内参标定（获取 camera_matrix 与 dist_coeffs）
手眼标定已完成，得到从相机坐标系到机器人基座标系的齐次变换矩阵 cam2base（4×4）
工件放置于已知高度的平面上（本案例假设 Z=0 平面）

核心实现

import cv2
import numpy as np

def pixel_to_base(u, v, Z_w, cam2base, camera_matrix, dist_coeffs):
    """
    将像素坐标 (u,v) 转换到机器人基座标系。
    Z_w: 工件所在平面在机器人基座标系中的 Z 值（已知常数）
    """
    # 去畸变并归一化相机坐标
    p = np.array([[[u, v]]], dtype=np.float64)
    undistorted = cv2.undistortPoints(p, camera_matrix, dist_coeffs, P=camera_matrix)
    x_cam = undistorted[0][0][0]
    y_cam = undistorted[0][0][1]

    # 构造相机坐标系下的方向向量（Z=1 平面）
    dir_cam = np.array([x_cam, y_cam, 1.0])

    # 相机到基座的旋转与平移
    R = cam2base[:3, :3]
    t = cam2base[:3, 3]

    # 方向向量转换到基座坐标系
    dir_base = R @ dir_cam

    # 与 Z=Z_w 平面求交
    if abs(dir_base[2]) < 1e-12:
        return None  # 视线与平面平行，无交点
    s = (Z_w - t[2]) / dir_base[2]
    X_base = t[0] + s * dir_base[0]
    Y_base = t[1] + s * dir_base[1]
    return np.array([X_base, Y_base, Z_w])

def locate_workpiece(image, lower_color, upper_color, cam2base, camera_matrix, dist_coeffs, Z_w=0.0):
    """
    基于颜色分割定位工件，返回像素坐标与基座标系坐标。
    lower_color/upper_color: HSV 颜色范围（如红色: (0,100,100)-(10,255,255)）
    """
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, lower_color, upper_color)

    # 形态学去噪
    kernel = np.ones((5,5), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None

    # 取最大轮廓作为目标工件
    largest = max(contours, key=cv2.contourArea)
    if cv2.contourArea(largest) < 100:  # 面积阈值过滤噪声
        return None

    M = cv2.moments(largest)
    if M["m00"] == 0:
        return None
    u = int(M["m10"] / M["m00"])
    v = int(M["m01"] / M["m00"])

    # 像素坐标 → 基座标系
    base_pos = pixel_to_base(u, v, Z_w, cam2base, camera_matrix, dist_coeffs)
    return {"pixel": (u, v), "base": base_pos, "contour_area": cv2.contourArea(largest)}

# ========== 使用示例 ==========
if __name__ == "__main__":
    # 以下为示例参数，实际使用时需替换为真实标定结果
    camera_matrix = np.array([[800, 0, 320],
                              [0, 800, 240],
                              [0, 0, 1]], dtype=np.float64)
    dist_coeffs = np.zeros((4,1))
    # 示例变换：相机在基座上方1m处，无旋转（实际需手眼标定获得）
    cam2base = np.eye(4)
    cam2base[2,3] = 1.0

    img = cv2.imread("workpiece.png")  # 输入图像
    if img is None:
        print("图像读取失败")
    else:
        # 红色工件示例
        result = locate_workpiece(img, (0,100,100), (10,255,255),
                                  cam2base, camera_matrix, dist_coeffs, Z_w=0.0)
        if result:
            print(f"像素坐标: {result['pixel']}")
            print(f"基座标系坐标: {result['base']}")
        else:
            print("未检测到目标工件")

运行与验证

输入图像：准备一张包含单一颜色工件的图像 workpiece.png，背景颜色与工件颜色差异明显。
调整HSV范围：若工件为红色，使用示例范围；其他颜色需通过 cv2.cvtColor 观察HSV值后调整 lower_color/upper_color。
替换标定参数：将 camera_matrix、dist_coeffs、cam2base 替换为实际标定值。
运行：python locate_workpiece.py

通过条件：程序输出工件的像素坐标和基座标系坐标；将输出的基座坐标与工件在机器人工作台上的实际测量位置对比，误差在±5mm以内（取决于标定精度）即判定通过。若未检测到工件，检查HSV范围是否覆盖工件颜色、面积阈值是否过大。

常见问题

检测到多个区域：代码取最大轮廓，若工件非最大区域，需增加颜色范围精确性或改用其他筛选条件。
坐标偏差大：优先检查 cam2base 标定精度和 Z_w 是否与实际平面一致。
光照影响：HSV分割对光照敏感，建议使用稳定光源或增加光照补偿预处理。