3. 基于形状匹配的零件定位

任务目标

本案例实现基于形状匹配的零件定位：给定一张包含已知轮廓零件的灰度图像，通过模板匹配算法在场景图像中定位该零件，输出其像素坐标与旋转角度，并转换为机器人基坐标系下的抓取位姿。核心解决零件随意摆放角度时，机器人无法使用固定示教点抓取的问题。

实现方案

采用基于边缘点梯度方向的形状匹配方法（类似Hausdorff距离的简化版）：

离线阶段：从模板图像提取零件轮廓边缘点集及对应梯度方向；
在线阶段：在场景图像中滑动搜索，计算每个候选位置处边缘点梯度方向与模板的匹配得分；
对得分图做非极大值抑制，取最高峰作为匹配位置；
在匹配位置附近小范围搜索旋转角度，得到最佳角度；
通过标定矩阵将像素坐标转换为机器人基坐标，输出抓取位姿(x, y, θ)。

环境依赖

Python 3.8+，NumPy，OpenCV（含contrib模块）。安装命令：pip install numpy opencv-contrib-python

关键完整代码

import cv2
import numpy as np

def create_template_edges(template_img):
    """从模板图像提取边缘点集和梯度方向"""
    # 高斯模糊降噪
    blurred = cv2.GaussianBlur(template_img, (5, 5), 1.5)
    # Canny边缘检测
    edges = cv2.Canny(blurred, 50, 150)
    # 计算梯度方向（Sobel）
    gx = cv2.Sobel(blurred, cv2.CV_64F, 1, 0, ksize=3)
    gy = cv2.Sobel(blurred, cv2.CV_64F, 0, 1, ksize=3)
    # 提取边缘点坐标和梯度方向
    pts = np.column_stack(np.where(edges > 0))
    angles = np.arctan2(gy[pts[:, 0], pts[:, 1]], gx[pts[:, 0], pts[:, 1]])
    # 归一化模板中心
    center = pts.mean(axis=0)
    pts_norm = pts - center
    return pts_norm, angles, center

def shape_match(scene_img, template_pts, template_angles, angle_range=(-30, 30), angle_step=1):
    """在场景图中搜索模板位置和角度，返回最佳匹配的像素坐标和角度"""
    h, w = scene_img.shape[:2]
    # 计算场景梯度
    blurred = cv2.GaussianBlur(scene_img, (5, 5), 1.5)
    gx = cv2.Sobel(blurred, cv2.CV_64F, 1, 0, ksize=3)
    gy = cv2.Sobel(blurred, cv2.CV_64F, 0, 1, ksize=3)
    scene_angles = np.arctan2(gy, gx)
    scene_mag = np.sqrt(gx**2 + gy**2)
    # 只保留梯度幅值较大的点作为候选
    mask = scene_mag > 30

    best_score = -1
    best_loc = (0, 0)
    best_angle = 0

    # 旋转模板并搜索
    for angle_deg in range(angle_range[0], angle_range[1] + 1, angle_step):
        theta = np.deg2rad(angle_deg)
        cos_t, sin_t = np.cos(theta), np.sin(theta)
        # 旋转模板点
        rot_pts = np.zeros_like(template_pts)
        rot_pts[:, 0] = template_pts[:, 0] * cos_t - template_pts[:, 1] * sin_t
        rot_pts[:, 1] = template_pts[:, 0] * sin_t + template_pts[:, 1] * cos_t
        rot_angles = template_angles + theta

        # 计算模板包围盒，限制搜索范围
        min_y, min_x = rot_pts.min(axis=0).astype(int)
        max_y, max_x = rot_pts.max(axis=0).astype(int)
        th, tw = max_y - min_y + 1, max_x - min_x + 1
        if th >= h or tw >= w:
            continue

        # 滑动搜索
        score_map = np.zeros((h - th + 1, w - tw + 1))
        # 将模板点偏移到局部坐标
        local_pts = rot_pts - np.array([min_y, min_x])

        for y in range(h - th + 1):
            for x in range(w - tw + 1):
                # 提取当前窗口的梯度方向
                window_angles = scene_angles[y:y+th, x:x+tw]
                window_mask = mask[y:y+th, x:x+tw]
                # 计算匹配得分：梯度方向一致性
                valid = 0
                score = 0.0
                for (py, px), ta in zip(local_pts, rot_angles):
                    if 0 <= py < th and 0 <= px < tw:
                        if window_mask[py, px]:
                            diff = abs(window_angles[py, px] - ta)
                            diff = min(diff, 2*np.pi - diff)
                            if diff < np.pi/4:  # 方向差小于45度
                                score += 1.0 - diff/(np.pi/4)
                                valid += 1
                if valid > 0:
                    score_map[y, x] = score / len(template_pts)

        # 找当前角度下的最佳位置
        _, max_val, _, max_loc = cv2.minMaxLoc(score_map)
        if max_val > best_score:
            best_score = max_val
            best_angle = angle_deg
            # 转换为场景图像坐标
            best_loc = (max_loc[0] + min_x, max_loc[1] + min_y)

    return best_loc, best_angle, best_score

def pixel_to_robot(pixel_x, pixel_y, homography):
    """像素坐标转机器人基坐标（使用3x3单应矩阵）"""
    pt = np.array([pixel_x, pixel_y, 1.0])
    robot = homography @ pt
    robot /= robot[2]
    return robot[0], robot[1]

# ============ 主程序 ============
if __name__ == "__main__":
    # 1. 加载模板和场景图像（灰度）
    template = cv2.imread("template.png", cv2.IMREAD_GRAYSCALE)
    scene = cv2.imread("scene.png", cv2.IMREAD_GRAYSCALE)
    assert template is not None and scene is not None, "图像加载失败"

    # 2. 提取模板边缘
    tm_pts, tm_angles, tm_center = create_template_edges(template)

    # 3. 形状匹配
    (loc_x, loc_y), angle, score = shape_match(scene, tm_pts, tm_angles)

    # 4. 计算模板中心在场景中的位置
    theta = np.deg2rad(angle)
    center_scene_x = loc_x + tm_center[1] * np.cos(theta) - tm_center[0] * np.sin(theta)
    center_scene_y = loc_y + tm_center[1] * np.sin(theta) + tm_center[0] * np.cos(theta)

    # 5. 手眼标定矩阵（3x3，由标定获得）
    H = np.array([[0.5, 0, 100],   # 示例矩阵，实际需通过标定获取
                  [0, 0.5, 200],
                  [0, 0, 1]], dtype=np.float64)

    # 6. 转换为机器人抓取位姿
    robot_x, robot_y = pixel_to_robot(center_scene_x, center_scene_y, H)
    robot_theta = np.deg2rad(angle)  # 旋转角度直接映射到机器人末端

    print(f"匹配得分: {score:.3f}")
    print(f"像素位置: ({center_scene_x:.1f}, {center_scene_y:.1f})")
    print(f"旋转角度: {angle:.1f}°")
    print(f"机器人位姿: x={robot_x:.2f}mm, y={robot_y:.2f}mm, θ={robot_theta:.3f}rad")

运行与验证

运行方式：

准备两张灰度图：template.png（仅包含单个零件，背景干净）和scene.png（零件随意摆放，可旋转）；
将代码保存为shape_match.py，与图像放在同一目录；
执行python shape_match.py。

验证方法：

在场景图中手动框选零件实际位置和角度，与程序输出的像素坐标和角度对比，误差应在±3像素和±3°以内；
匹配得分应大于0.5（满分1.0），低于0.3说明匹配失败；
将输出的机器人位姿发送给机器人执行抓取，若抓取点对准零件中心且夹爪方向与零件主轴一致，则验证通过。

注意事项：代码中的单应矩阵H为示例值，实际使用前必须通过九点标定或手眼标定获得真实矩阵；搜索角度范围可根据零件可能出现的旋转范围调整，角度步长越小精度越高但耗时增加。

📚 课程目录