📚 课程目录第4章眼在手上标定数据采集

4. 眼在手上标定数据采集

任务目标

本案例实现眼在手上（Eye-in-Hand）标定的数据采集环节：控制机器人依次运动到预设的多个位姿，在每个位姿下采集标定板图像，并同步记录机器人末端位姿（位置+姿态）。采集结果将作为后续手眼标定求解的输入数据。

实现方案

使用Python编写采集控制脚本，通过TCP/IP与机器人控制器通信（以UR机器人为例），通过USB相机（OpenCV）采集图像。核心逻辑为：定义一组末端位姿 → 逐位姿运动 → 等待到位 → 拍照并保存 → 记录当前末端位姿到JSON文件。

环境依赖

Python 3.8+，安装 opencv-python、numpy、urx（UR机器人通信库）
UR机器人（或支持TCP运动控制的工业机器人），相机固定于机器人末端法兰
标定板（棋盘格或圆点阵）固定在工作空间内，确保所有采集位姿下标定板均完整出现在视野中

关键完整代码

import cv2
import json
import time
import numpy as np
import urx

# ========== 配置参数 ==========
ROBOT_IP = "192.168.1.10"        # 机器人IP
CAMERA_ID = 0                    # 相机ID
SAVE_DIR = "./calib_data/"        # 保存目录
NUM_POSES = 8                    # 采集位姿数量
MOVE_SPEED = 0.2                 # 运动速度 m/s
MOVE_ACC = 0.2                   # 运动加速度 m/s²

# 预设的8个末端位姿 [x, y, z, rx, ry, rz]（位置单位：米，姿态单位：弧度）
# 实际使用时需根据机器人工作空间和标定板位置调整
POSE_LIST = [
    [0.40, 0.00, 0.35, 0.0, 0.0, 0.0],
    [0.40, 0.05, 0.35, 0.0, 0.0, 0.1],
    [0.40, -0.05, 0.35, 0.0, 0.0, -0.1],
    [0.40, 0.00, 0.40, 0.0, 0.1, 0.0],
    [0.40, 0.00, 0.30, 0.0, -0.1, 0.0],
    [0.35, 0.00, 0.35, 0.1, 0.0, 0.0],
    [0.45, 0.00, 0.35, -0.1, 0.0, 0.0],
    [0.40, 0.00, 0.35, 0.0, 0.0, 0.2],
]

# ========== 主程序 ==========
def main():
    import os
    os.makedirs(SAVE_DIR, exist_ok=True)

    # 连接机器人
    robot = urx.Robot(ROBOT_IP)
    robot.set_tcp((0, 0, 0.1, 0, 0, 0))  # 设置工具TCP（相机安装偏移）
    robot.set_payload(0.5)

    # 打开相机
    cap = cv2.VideoCapture(CAMERA_ID)
    if not cap.isOpened():
        print("错误：无法打开相机")
        robot.close()
        return

    # 等待相机就绪
    time.sleep(1)

    collected = []

    try:
        for i, pose in enumerate(POSE_LIST[:NUM_POSES]):
            print(f"运动到位姿 {i+1}/{NUM_POSES}: {pose}")

            # 运动到目标位姿（位姿为末端在基坐标系下的表示）
            robot.movep(pose, acc=MOVE_ACC, vel=MOVE_SPEED, relative=False)
            time.sleep(0.5)  # 等待运动稳定

            # 采集图像
            ret, frame = cap.read()
            if not ret:
                print(f"警告：位姿 {i+1} 图像采集失败")
                continue

            # 保存图像
            img_path = f"{SAVE_DIR}pose_{i+1:02d}.png"
            cv2.imwrite(img_path, frame)

            # 读取当前实际末端位姿（运动到位后读取，确保与图像同步）
            actual_pose = robot.getl()
            actual_pose = [round(v, 6) for v in actual_pose]

            # 记录数据
            data_entry = {
                "image_path": img_path,
                "end_effector_pose": actual_pose  # [x, y, z, rx, ry, rz]
            }
            collected.append(data_entry)
            print(f"  图像已保存: {img_path}")
            print(f"  末端位姿: {actual_pose}")

        # 保存标定数据到JSON
        json_path = f"{SAVE_DIR}calib_data.json"
        with open(json_path, "w") as f:
            json.dump(collected, f, indent=2)
        print(f"\n数据采集完成，共 {len(collected)} 组数据，已保存至 {json_path}")

    finally:
        cap.release()
        robot.close()

if __name__ == "__main__":
    main()

运行方式

将机器人置于手动模式，确认TCP参数与相机安装偏移一致（代码中已设置TCP偏移为Z轴0.1m，需按实际安装修改）
将标定板固定在工作空间内，调整初始位姿使标定板完整出现在相机视野中央
根据实际工作空间修改 POSE_LIST 中的位姿值，确保所有位姿下标定板均可见
运行脚本：python collect_calib_data.py

验证方法

程序正常结束后，检查 calib_data/ 目录下是否生成 pose_01.png ~ pose_08.png 共8张图像
打开 calib_data.json，确认包含8组数据，每组含 image_path 和 end_effector_pose 字段
逐张查看图像，确认标定板完整出现在每张图像中且无明显运动模糊
检查记录的末端位姿与预设位姿基本一致（存在微小偏差属正常），且各姿态差异足够大（保证标定求解的矩阵条件数良好）

常见问题

图像模糊：增加运动到位后的等待时间（time.sleep），或降低运动速度
标定板部分出视野：调整预设位姿范围，使标定板始终位于图像中心区域
机器人连接失败：确认机器人IP正确，且已开启远程控制接口（UR需在设置中启用Dashboard Server）