睿尔曼机械臂Eye-in-Hand标定实战：从原理到Python代码实现

在工业自动化和机器人技术领域，Eye-in-Hand系统因其高精度和灵活性而备受青睐。这种将相机直接安装在机械臂末端的配置，能够随着机械臂的运动而移动，为精确的视觉引导操作提供了理想解决方案。本文将深入探讨Eye-in-Hand标定的数学原理，并通过Python代码实现完整的标定流程，帮助开发者摆脱对预封装EXE工具的依赖，实现更灵活、更可控的标定过程。

1. Eye-in-Hand系统基础与数学原理

Eye-in-Hand系统的核心在于建立相机坐标系与机械臂末端坐标系之间的精确转换关系。这种关系通常用一个4×4的齐次变换矩阵表示，包含旋转矩阵R（3×3）和平移向量t（3×1）：

[R | t]
[0 | 1]

坐标系转换链是理解手眼标定的关键。当机械臂末端移动时，我们有以下关系链：

1. 机械臂基座到末端的变换：$H^{base}_{end}$
2. 末端到相机的变换（标定目标）：$H^{end}_{cam}$
3. 相机到标定板的变换：$H^{cam}_{board}$

根据坐标系转换的链式法则，可以得到：

$H^{base}{board} = H^{base}{end} \cdot H^{end}{cam} \cdot H^{cam}{board}$

其中：

• $H^{base}_{end}$ 可通过机械臂API获取
• $H^{cam}_{board}$ 可通过相机标定获得
• $H^{end}_{cam}$ 是我们需要求解的手眼变换矩阵

标定方程推导采用Tsai方法，其核心方程为：

$R_{end2base} \cdot R_{cam2end} = R_{cam2end} \cdot R_{board2cam}$

其中：

• $R_{end2base}$：机械臂末端到基座的旋转
• $R_{board2cam}$：标定板到相机的旋转
• $R_{cam2end}$：相机到末端的旋转（待求解）

对应的平移向量方程为：

$(R_{end2base} - I)t_{cam2end} = R_{cam2end}t_{board2cam} - t_{end2base}$

提示：在实际应用中，通常需要至少15-20组不同姿态的数据来保证标定精度，且每组数据间的旋转角度差异应大于30度。

2. 环境准备与数据采集

2.1 硬件配置要求

实现Eye-in-Hand标定需要以下硬件组件：

组件	型号/规格	备注
机械臂	睿尔曼RM65-B	6自由度，重复定位精度±0.05mm
深度相机	Intel Realsense D435	支持RGB和深度图像
标定板	棋盘格12x9	单格尺寸3cm，建议使用玻璃材质
连接件	定制转接板	确保相机牢固安装在机械臂末端

安装注意事项：

1. 相机应安装在机械臂末端法兰上，确保稳固且不遮挡视野
2. 标定板应放置在工作空间内平坦、无反光的表面上
3. 确保所有连接线（USB3.0、电源等）不会限制机械臂运动

2.2 软件环境配置

Python环境需要以下关键库：

# requirements.txt
numpy>=1.21.0
opencv-contrib-python>=4.5.0
pyrealsense2>=2.50.0
scipy>=1.7.0

安装命令：

pip install -r requirements.txt

2.3 数据采集流程

数据采集是标定成功的关键，以下是详细的Python实现：

import cv2
import numpy as np
import pyrealsense2 as rs
from robotic_arm import RM65Controller  # 假设的机械臂控制库

class DataCollector:
    def __init__(self):
        # 初始化相机
        self.pipeline = rs.pipeline()
        config = rs.config()
        config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        self.pipeline.start(config)
        
        # 初始化机械臂控制器
        self.arm = RM65Controller(ip="192.168.1.18")
        
        # 标定板参数
        self.pattern_size = (11, 8)  # 内部角点数(格子数-1)
        self.square_size = 0.03  # 单位：米
    
    def collect_data(self, num_samples=20):
        poses = []
        image_points = []
        object_points = []
        
        # 生成标定板3D坐标(假设标定板在Z=0平面)
        objp = np.zeros((self.pattern_size[0]*self.pattern_size[1], 3), np.float32)
        objp[:,:2] = np.mgrid[0:self.pattern_size[0], 0:self.pattern_size[1]].T.reshape(-1,2)
        objp *= self.square_size
        
        print("开始数据采集，按's'保存当前帧，按'q'退出")
        
        while len(poses) < num_samples:
            frames = self.pipeline.wait_for_frames()
            color_frame = frames.get_color_frame()
            if not color_frame:
                continue
                
            img = np.asanyarray(color_frame.get_data())
            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
            
            # 查找棋盘格角点
            ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, 
                cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE)
            
            if ret:
                # 亚像素精确化
                criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
                corners = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
                
                # 显示结果
                cv2.drawChessboardCorners(img, self.pattern_size, corners, ret)
                cv2.imshow('Calibration', img)
                
                key = cv2.waitKey(1) & 0xFF
                if key == ord('s'):
                    # 获取当前机械臂末端位姿
                    pose = self.arm.get_end_effector_pose()  # [x,y,z,rx,ry,rz]
                    
                    poses.append(pose)
                    image_points.append(corners)
                    object_points.append(objp)
                    
                    print(f"已采集 {len(poses)}/{num_samples} 组数据")
                    
                elif key == ord('q'):
                    break
        
        cv2.destroyAllWindows()
        return np.array(poses), image_points, object_points

采集技巧：

• 确保标定板在相机视野中占据足够大的面积（建议30%-70%）
• 每次移动后，标定板应与相机光轴形成不同角度（建议15°-75°）
• 在X、Y、Z三个轴上都要有足够的平移和旋转变化
• 避免采集模糊、过曝或低对比度的图像

3. 标定算法实现与OpenCV函数解析

3.1 相机内参标定

在计算手眼矩阵前，需要先标定相机内参：

def calibrate_camera(image_points, object_points, image_size):
    """
    标定相机内参和畸变系数
    :param image_points: 角点像素坐标列表
    :param object_points: 角点世界坐标列表
    :param image_size: 图像尺寸 (width, height)
    :return: 相机矩阵, 畸变系数, 旋转向量, 平移向量
    """
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
        object_points, image_points, image_size, None, None)
    
    print("相机内参矩阵:")
    print(mtx)
    print("\n畸变系数:")
    print(dist)
    
    return mtx, dist, rvecs, tvecs

3.2 机械臂位姿数据处理

机械臂返回的位姿通常为6维向量(x,y,z,rx,ry,rz)，需要转换为齐次变换矩阵：

def pose_to_homogeneous(pose):
    """
    将机械臂位姿(x,y,z,rx,ry,rz)转换为4x4齐次变换矩阵
    :param pose: [x,y,z,rx,ry,rz]
    :return: 4x4齐次变换矩阵
    """
    x, y, z, rx, ry, rz = pose
    
    # 欧拉角转旋转矩阵(Z-Y-X顺序)
    R = euler_to_rotation_matrix(rx, ry, rz)
    
    # 构建齐次变换矩阵
    H = np.eye(4)
    H[:3, :3] = R
    H[:3, 3] = [x, y, z]
    
    return H

def euler_to_rotation_matrix(rx, ry, rz):
    """ 欧拉角转旋转矩阵 """
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(rx), -np.sin(rx)],
                   [0, np.sin(rx), np.cos(rx)]])
    
    Ry = np.array([[np.cos(ry), 0, np.sin(ry)],
                   [0, 1, 0],
                   [-np.sin(ry), 0, np.cos(ry)]])
    
    Rz = np.array([[np.cos(rz), -np.sin(rz), 0],
                   [np.sin(rz), np.cos(rz), 0],
                   [0, 0, 1]])
    
    return Rz @ Ry @ Rx

3.3 手眼标定核心算法

使用OpenCV的calibrateHandEye函数实现标定：

def hand_eye_calibration(arm_poses, rvecs, tvecs):
    """
    执行手眼标定
    :param arm_poses: 机械臂位姿列表(x,y,z,rx,ry,rz)
    :param rvecs: 相机标定得到的旋转向量
    :param tvecs: 相机标定得到的平移向量
    :return: 相机到机械臂末端的旋转矩阵和平移向量
    """
    # 转换机械臂位姿为齐次变换矩阵
    H_end2base = [pose_to_homogeneous(pose) for pose in arm_poses]
    
    # 转换相机标定结果为旋转矩阵
    R_board2cam = [cv2.Rodrigues(rvec)[0] for rvec in rvecs]
    t_board2cam = [tvec.reshape(3,1) for tvec in tvecs]
    
    # 准备calibrateHandEye输入
    R_end2base = [H[:3,:3] for H in H_end2base]
    t_end2base = [H[:3,3].reshape(3,1) for H in H_end2base]
    
    # 执行手眼标定(TSAI方法)
    R_cam2end, t_cam2end = cv2.calibrateHandEye(
        R_gripper2base=R_end2base,
        t_gripper2base=t_end2base,
        R_target2cam=R_board2cam,
        t_target2cam=t_board2cam,
        method=cv2.CALIB_HAND_EYE_TSAI)
    
    return R_cam2end, t_cam2end

方法选择：OpenCV提供了多种手眼标定算法：

方法	特点	适用场景
CALIB_HAND_EYE_TSAI	经典方法，计算效率高	大多数常规应用
CALIB_HAND_EYE_PARK	改进的线性方法	需要更高精度的场景
CALIB_HAND_EYE_HORAUD	非线性优化方法	数据噪声较大的情况
CALIB_HAND_EYE_ANDREFF	基于旋转和平移分离	特殊运动约束场景

3.4 标定结果验证

标定完成后，需要对结果进行验证：

def verify_calibration(R_cam2end, t_cam2end, arm_poses, rvecs, tvecs):
    """
    验证标定结果精度
    :param R_cam2end: 相机到末端的旋转矩阵
    :param t_cam2end: 相机到末端的平移向量
    :param arm_poses: 机械臂位姿列表
    :param rvecs: 相机旋转向量
    :param tvecs: 相机平移向量
    """
    errors = []
    H_cam2end = np.eye(4)
    H_cam2end[:3,:3] = R_cam2end
    H_cam2end[:3,3] = t_cam2end.flatten()
    
    for i in range(len(arm_poses)):
        # 获取机械臂末端到基座的变换
        H_end2base = pose_to_homogeneous(arm_poses[i])
        
        # 获取标定板到相机的变换
        R_board2cam = cv2.Rodrigues(rvecs[i])[0]
        t_board2cam = tvecs[i]
        H_board2cam = np.eye(4)
        H_board2cam[:3,:3] = R_board2cam
        H_board2cam[:3,3] = t_board2cam.flatten()
        
        # 计算标定板到基座的理论变换
        H_board2base_theory = H_end2base @ H_cam2end @ H_board2cam
        
        # 由于标定板位置固定，理论上H_board2base应该相同
        if i == 0:
            H_board2base_ref = H_board2base_theory
        else:
            # 计算误差
            error = np.linalg.norm(H_board2base_ref[:3,3] - H_board2base_theory[:3,3])
            errors.append(error)
    
    avg_error = np.mean(errors)
    print(f"平均重投影误差: {avg_error*1000:.2f} mm")
    return avg_error

注意：良好的标定结果平均误差应小于5mm。如果误差过大，建议检查数据采集质量或增加样本数量。

4. 标定结果在抓取任务中的应用

4.1 坐标转换实践

获得手眼矩阵后，可以实现物体从相机坐标系到机械臂基坐标系的转换：

def transform_point_to_base(cam_point, R_cam2end, t_cam2end, arm_pose):
    """
    将相机坐标系下的点转换到机械臂基坐标系
    :param cam_point: 相机坐标系下的3D点 [x,y,z]
    :param R_cam2end: 相机到末端的旋转矩阵
    :param t_cam2end: 相机到末端的平移向量
    :param arm_pose: 当前机械臂位姿 [x,y,z,rx,ry,rz]
    :return: 基坐标系下的3D点
    """
    # 构建相机到末端的齐次变换矩阵
    H_cam2end = np.eye(4)
    H_cam2end[:3,:3] = R_cam2end
    H_cam2end[:3,3] = t_cam2end.flatten()
    
    # 构建末端到基座的齐次变换矩阵
    H_end2base = pose_to_homogeneous(arm_pose)
    
    # 将点转换为齐次坐标
    point = np.append(cam_point, 1).reshape(4,1)
    
    # 坐标转换
    base_point = H_end2base @ H_cam2end @ point
    
    return base_point[:3].flatten()

4.2 实际应用案例：视觉引导抓取

结合手眼标定结果，实现完整的视觉引导抓取流程：

class VisionGuidedGrasping:
    def __init__(self, R_cam2end, t_cam2end):
        self.R_cam2end = R_cam2end
        self.t_cam2end = t_cam2end
        
        # 初始化机械臂和相机
        self.arm = RM65Controller(ip="192.168.1.18")
        self.pipeline = rs.pipeline()
        config = rs.config()
        config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
        config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        self.pipeline.start(config)
    
    def detect_object(self):
        """ 检测目标物体并返回其在相机坐标系中的位置 """
        # 获取帧数据
        frames = self.pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()
        depth_frame = frames.get_depth_frame()
        
        # 此处简化为返回固定值，实际应实现物体检测算法
        # 例如使用YOLO等检测物体并获取中心点坐标
        u, v = 320, 240  # 假设物体在图像中心
        depth = depth_frame.get_distance(u, v)
        
        # 将像素坐标转换为相机坐标系3D点
        intrinsics = color_frame.profile.as_video_stream_profile().intrinsics
        cam_point = rs.rs2_deproject_pixel_to_point(intrinsics, [u, v], depth)
        
        return np.array(cam_point)
    
    def execute_grasp(self, object_point):
        """ 执行抓取动作 """
        # 获取当前机械臂位姿
        current_pose = self.arm.get_end_effector_pose()
        
        # 将物体坐标转换到基坐标系
        base_point = transform_point_to_base(object_point, self.R_cam2end, 
                                           self.t_cam2end, current_pose)
        
        print(f"物体在基坐标系中的位置: {base_point}")
        
        # 规划抓取路径(简化版)
        approach_point = base_point.copy()
        approach_point[2] += 0.1  # 在物体上方10cm处接近
        
        grasp_point = base_point.copy()
        grasp_point[2] -= 0.02  # 抓取位置，略低于物体中心
        
        # 执行运动
        self.arm.move_to(approach_point, speed=50)  # 移动到接近点
        self.arm.move_to(grasp_point, speed=20)     # 缓慢移动到抓取点
        self.arm.gripper_close()                    # 闭合夹爪
        self.arm.move_to(approach_point, speed=30)  # 抬升物体
        
        print("抓取完成")

优化建议：

1. 在实际应用中，应考虑机械臂运动学约束和碰撞检测
2. 可以加入视觉伺服控制，在接近过程中持续校正位置
3. 对于易碎物品，应降低接近速度和抓取力度

4.3 常见问题排查

问题1：标定误差过大

可能原因及解决方案：

• 数据采集不足：确保至少15组数据，且姿态变化充分
• 标定板检测不准确：检查角点检测是否正确，尝试调整检测参数
• 机械臂位姿误差：检查机械臂的重复定位精度是否达标

问题2：calibrateHandEye报错"Not enough informative motions"

解决方法：

• 确保机械臂在数据采集过程中有足够大的旋转运动（>30°）
• 在X、Y、Z三个轴上都要有足够的旋转变化
• 增加数据采集数量（建议20组以上）

问题3：抓取位置偏差

调试步骤：

1. 验证手眼标定结果的重投影误差
2. 检查物体检测算法的精度
3. 确认机械臂的工具坐标系(TCP)标定是否正确
4. 检查机械臂与相机的安装是否牢固

在实际项目中，我们曾遇到一个典型案例：客户反馈抓取位置总是有约3cm的偏差。经过排查发现，问题根源是机械臂末端的工具坐标系标定不准确，重新进行TCP标定后问题解决。这提醒我们，在视觉引导系统中，各个环节的标定都至关重要。