# 从原理到实践：用Python打通睿尔曼RM65-B与Realsense D435i的手眼标定全链路 最近在实验室部署一个基于视觉的抓取项目，核心任务之一就是让睿尔曼RM65-B机械臂的“手”和英特尔的Realsense D435i深度相机的“眼”协同工作。这听起来像是让两个来自不同星球的工程师说同一种语言——机械臂的世界由关节角度和末端位姿定义，而相机看到的是像素和三维点云。手眼标定，就是为它们搭建一座精准的“翻译桥梁”。网上能找到的代码跑起来总有些磕绊，不是路径报错就是参数对不上，调试过程颇费周折。经过几轮实战和代码重构，我梳理出了一套更清晰、更健壮的Python实现方案，不仅附上可直接运行的代码，更关键的是拆解每一步背后的数学原理和工程考量，让你知其然更知其所以然。 ## 1. 手眼标定：不只是代码，更是空间关系的数学对话 在开始敲代码之前，我们得先搞明白到底要解决什么问题。手眼标定，核心目标是求解一个固定的变换矩阵。这个矩阵描述了相机坐标系（Camera Frame）与机械臂末端工具坐标系（Tool Frame）之间的相对位置和姿态关系。一旦我们知道了这个关系，相机看到的任何一个物体在相机坐标系下的三维坐标，都可以通过这个变换矩阵，转换到机械臂末端坐标系下，从而指导机械臂“伸手”去抓取。 对于“眼在手上”（Eye-in-Hand）这种我们常用的配置，相机固定在机械臂末端，随着机械臂一起运动。标定过程可以抽象为这样一个方程： **A * X = X * B** 这个看似简洁的方程，是整个标定的灵魂。我们来拆解一下： - **X**： 这就是我们要求解的未知数，即相机坐标系到工具坐标系的变换矩阵（4x4的齐次变换矩阵）。 - **A**： 表示机械臂末端在两个不同位姿之间的**运动变换**。这个数据我们可以从机械臂控制器精确地读取到。 - **B**： 表示相机在两个不同位姿下，“看到”的标定板之间的**运动变换**。这个需要通过相机拍摄的图像，结合相机内参和标定板模型计算出来。 > 注意：这个方程成立的前提是，在整个标定过程中，标定板在机器人基坐标系下的位置是固定不变的。机械臂带着相机运动，从多个不同角度观测同一个静止的标定板。 我们的任务，就是通过采集多组（通常大于20组）的 (A, B) 配对数据，来求解出那个最优的 X。这本质上是一个优化问题，常用**Tsai-Lenz方法**或**Daniilidis方法**来求解。在后续的代码中，我们会使用 `scipy` 库中的优化器来实际计算。 为了更直观地理解数据流，可以参考下面的标定数据采集与处理流程： | 步骤 | 数据来源 | 数据形式 | 处理目标 | | :--- | :--- | :--- | :--- | | **1. 机械臂位姿采集** | 睿尔曼RM65-B控制器 | 末端位姿 (x, y, z, rx, ry, rz) | 转换为齐次变换矩阵，得到序列 A_i | | **2. 相机图像采集** | Realsense D435i RGB相机 | 标定板图像 (RGB) | 提取角点像素坐标 | | **3. 相机运动计算** | 图像角点 + 相机内参 + 标定板模型 | 求解PnP得到相机相对于标定板的位姿 | 计算不同图像间的变换，得到序列 B_i | | **4. 手眼矩阵求解** | 配对序列 {A_i, B_i} | 利用手眼标定方程 A*X = X*B | 求解最优的变换矩阵 X (旋转R和平移t) | | **5. 验证与应用** | 计算得到的X矩阵 | 4x4齐次变换矩阵 | 将相机坐标系下的点云转换到机械臂工具坐标系 | ## 2. 环境搭建与准备工作：避开初学者的那些坑 工欲善其事，必先利其器。一个干净、兼容的环境能避免很多莫名其妙的错误。我强烈建议使用 **Anaconda** 或 **Miniconda** 来创建独立的Python环境，避免与系统或其他项目的包产生冲突。 ```bash # 创建一个新的conda环境，命名为hand_eye，指定Python版本为3.8 conda create -n hand_eye python=3.8 conda activate hand_eye ``` 接下来安装核心依赖库。这里需要特别注意 `pyrealsense2` 的版本，不同版本对D435i的支持和API可能有细微差别。经过测试，以下版本组合最为稳定： ```bash # 安装科学计算和图像处理基础库 pip install numpy==1.24.2 scipy==1.10.1 opencv-python==4.8.1.78 opencv-contrib-python==4.8.1.78 # 安装英特尔RealSense SDK的Python封装 # 注意：pyrealsense2的安装方式取决于你的操作系统 # 对于Windows，通常使用预编译的wheel文件 pip install pyrealsense2==2.53.1.4623 # 可选但推荐：用于更美观的进度提示和数据处理 pip install tqdm ``` 除了软件，硬件和标定板的准备同样关键： - **睿尔曼RM65-B**：确保机械臂已上电，并通过网线与你的电脑处于同一局域网。你需要知道机械臂控制器的IP地址，并准备好通过TCP/IP或ROS等方式读取末端位姿的接口。本文示例代码将基于一个模拟的位姿读取函数，你需要根据实际SDK替换为真实接口。 - **Realsense D435i**：使用原装Type-C数据线连接电脑。首次连接时，系统可能会自动安装驱动。可以通过英特尔官方提供的 `RealSense Viewer` 工具验证相机是否能正常启动并获取彩色、深度流。 - **标定板**：我们使用经典的棋盘格标定板。你可以在 [calib.io](https://calib.io/pages/camera-calibration-pattern-generator) 这个网站自定义生成。我的建议是： - **尺寸**： 打印在A3纸上，保证足够的物理尺寸，便于相机在较远距离也能清晰拍摄。 - **参数**： 内角点数量设置为 **11列 x 8行**（即棋盘格内部黑白格相交的点）。这意味着你打印的棋盘格是12格宽，9格高。 - **方格大小**： 设置为 **30毫米**。这个值至关重要，后续所有计算都基于此物理尺寸。请务必用游标卡尺测量打印后棋盘格的实际方格边长，并以**米**为单位（0.03米）填入代码。 将打印好的标定板贴在一块平整、坚硬的亚克力板或木板上，确保它在整个标定过程中**绝对固定**，任何微小的移动都会导致标定失败。 ## 3. 数据采集：同步的艺术与实操细节 数据采集是标定成功的基础，核心要求是**同步**——为每一张成功拍摄的标定板图像，记录下拍摄瞬间机械臂末端的精确位姿。我重构后的采集脚本结构更清晰，将相机操作、机械臂通信和文件保存逻辑分离。 首先，我们来看一下项目目录结构，这有助于理解代码组织： ``` hand_eye_calibration_rm65_d435i/ ├── data_collection/ │ ├── camera_capture.py # 封装Realsense相机操作 │ ├── robot_interface.py # 封装睿尔曼机械臂位姿读取（需根据实际API填充） │ ├── data_synchronizer.py # 主采集逻辑，处理同步与用户交互 │ └── config.yaml # 配置文件，集中管理路径和参数 ├── calibration/ │ ├── calibrator.py # 核心标定算法实现 │ ├── utils.py # 齐次变换、坐标转换等工具函数 │ └── evaluate.py # 标定结果评估脚本 ├── outputs/ # 自动生成的目录，存放采集的图像和位姿文件 │ ├── images/ # 按序号保存的标定板图像 │ └── poses.csv # 同步记录的机械臂末端位姿 └── run_calibration.py # 一键运行的主脚本 ``` 采集程序 `data_synchronizer.py` 的核心循环逻辑如下，它清晰地展示了“按下按键 -> 同步保存”的流程： ```python import cv2 from camera_capture import D435iCamera from robot_interface import RM65Robot import json import os def main_collection_loop(): # 初始化 camera = D435iCamera(resolution=(1280, 720)) robot = RM65Robot(ip="192.168.1.100") # 替换为实际IP data_dir = "./outputs" os.makedirs(os.path.join(data_dir, "images"), exist_ok=True) pose_list = [] image_count = 0 print("数据采集开始！按 's' 保存当前帧和位姿，按 'q' 退出。") print("请确保标定板静止，并缓慢、多角度移动机械臂。") try: while True: # 1. 获取当前彩色图像帧 color_frame = camera.get_color_frame() if color_frame is None: continue # 2. 实时显示图像，方便用户观察标定板是否在视野内且清晰 display_frame = color_frame.copy() cv2.putText(display_frame, f"Captured: {image_count}", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.imshow("Calibration Data Collection", display_frame) key = cv2.waitKey(1) & 0xFF if key == ord('s'): # 3. 同步时刻：获取机械臂当前末端位姿 current_pose = robot.get_current_pose() # 返回 [x, y, z, rx, ry, rz] (单位：米/弧度) if current_pose is None: print("错误：无法读取机械臂位姿，请检查连接。") continue # 4. 保存图像和位姿 image_filename = os.path.join(data_dir, "images", f"frame_{image_count:04d}.png") cv2.imwrite(image_filename, color_frame) pose_list.append(current_pose) print(f"已保存第 {image_count} 组数据: 图像 -> {image_filename}, 位姿 -> {current_pose}") image_count += 1 elif key == ord('q'): break finally: # 5. 退出时保存所有位姿到CSV文件 camera.release() cv2.destroyAllWindows() save_poses_to_csv(pose_list, os.path.join(data_dir, "poses.csv")) print(f"采集结束。共保存 {image_count} 组数据。位姿已保存至 poses.csv") ``` 在采集过程中，有几点**实战经验**值得分享： - **运动策略**： 不要做单纯的平移或单纯的旋转。让机械臂末端带着相机做**既包含平移也包含旋转**的复合运动，并且旋转角度尽可能大（例如绕X、Y、Z轴各旋转正负30度以上）。这样采集到的 `A` 矩阵数据才具有充分的多样性，有利于方程求解的稳定性。 - **图像质量**： 确保每一张保存的图像中，**整个棋盘格都清晰可见**，且角点检测算法（后续步骤）能够稳定识别。光照要均匀，避免反光和阴影遮盖角点。 - **数据量**： 至少采集 **20-30组** 有效数据。数据太少容易过拟合或求解不稳定。我通常采集40-50组，然后可以随机剔除一些进行鲁棒性验证。 ## 4. 核心标定算法实现与代码逐行解析 数据采集完成后，我们进入核心的标定计算环节。这部分代码封装在 `calibration/calibrator.py` 中。我们不仅要求出矩阵，还要理解每一步在做什么。 首先，我们需要从采集的图像中计算相机相对于标定板的位姿（即 `B` 矩阵序列）。这需要两个前提：**相机内参**和**标定板三维模型**。 ```python import cv2 import numpy as np from scipy.optimize import least_squares from .utils import pose_to_homogeneous_matrix, homogeneous_matrix_to_pose class HandEyeCalibrator: def __init__(self, chessboard_size=(11, 8), square_size=0.03): """ 初始化标定器 :param chessboard_size: 棋盘格内角点数量 (宽度, 高度) :param square_size: 棋盘格方格物理边长，单位：米 """ self.chessboard_size = chessboard_size self.square_size = square_size # 生成标定板的三维角点坐标 (对象坐标系，Z=0) self.objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32) self.objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2) self.objp *= square_size # 缩放到实际物理尺寸 def compute_camera_poses(self, image_paths, camera_matrix, dist_coeffs): """ 计算每张图像对应的相机位姿 (相对于标定板) :param image_paths: 图像文件路径列表 :param camera_matrix: 相机内参矩阵 3x3 :param dist_coeffs: 相机畸变系数 :return: 相机位姿列表 (旋转向量+平移向量) [rvec, tvec] """ camera_poses = [] for img_path in image_paths: img = cv2.imread(img_path) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 查找棋盘格角点 ret, corners = cv2.findChessboardCorners(gray, self.chessboard_size, None) if ret: # 亚像素级角点精确化 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners_refined = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) # 求解PnP：已知3D点(objp)、2D投影点(corners_refined)、相机内参，求相机位姿 ret, rvec, tvec = cv2.solvePnP(self.objp, corners_refined, camera_matrix, dist_coeffs) if ret: camera_poses.append((rvec.flatten(), tvec.flatten())) else: print(f"警告：无法求解图像 {img_path} 的PnP。") else: print(f"警告：未在图像 {img_path} 中找到棋盘格角点。") return camera_poses ``` 得到相机位姿序列后，我们需要计算机械臂的运动 `A` 和相机的运动 `B`。假设我们采集了N组数据，那么对于连续的两组数据 i 和 j，有： - **A_ij** = inv(T_i) * T_j。其中 T_i, T_j 是机械臂末端在第i,j时刻的位姿变换矩阵（相对于基座）。这表示从第i个位姿**运动到**第j个位姿的变换。 - **B_ij** = inv(C_i) * C_j。其中 C_i, C_j 是相机在第i,j时刻的位姿变换矩阵（相对于标定板）。这表示从第i个视角**变换到**第j个视角时，相机自身的运动。 手眼标定方程 `A * X = X * B` 对每一对 (i, j) 都成立。我们的目标是找到最优的 `X`，使得所有等式尽可能同时成立。这通常转化为一个最小二乘优化问题。以下是使用 `scipy.optimize.least_squares` 的实现核心： ```python def calibrate_hand_eye(self, robot_poses, camera_poses): """ 执行手眼标定 :param robot_poses: 机械臂末端位姿列表，每个元素为 [x, y, z, rx, ry, rz] (米，弧度) :param camera_poses: 相机位姿列表，每个元素为 (rvec, tvec) :return: 最优的变换矩阵 X (4x4), 以及重投影误差 """ # 将位姿列表转换为齐次变换矩阵列表 T_robot = [pose_to_homogeneous_matrix(pose) for pose in robot_poses] T_camera = [pose_to_homogeneous_matrix(rvec, tvec) for (rvec, tvec) in camera_poses] # 构建运动对 (A_ij, B_ij)。通常使用所有可能的组合或一个固定参考帧。 A_list, B_list = [], [] ref_idx = 0 # 选择第一组数据作为参考 for i in range(len(T_robot)): if i == ref_idx: continue A = np.linalg.inv(T_robot[ref_idx]) @ T_robot[i] # A_0i B = np.linalg.inv(T_camera[ref_idx]) @ T_camera[i] # B_0i A_list.append(A) B_list.append(B) # 定义优化目标函数：最小化 || A * X - X * B ||^2 的Frobenius范数 def objective_func(params): # params: 将X矩阵的旋转部分(3x1旋转向量)和平移部分(3x1)拼接成一维数组 rvec_x = params[:3] tvec_x = params[3:6] X_est = pose_to_homogeneous_matrix(rvec_x, tvec_x) error = [] for A, B in zip(A_list, B_list): # 计算残差：A*X - X*B residual = A @ X_est - X_est @ B # 将4x4矩阵残差展平为16维向量 error.extend(residual.flatten()) return np.array(error) # 初始猜测：假设相机坐标系和工具坐标系大致对齐，有少量偏移 initial_guess = np.zeros(6) # [rx, ry, rz, tx, ty, tz] initial_guess[5] = 0.1 # 假设相机在工具坐标系Z轴正方向0.1米处 # 运行最小二乘优化 result = least_squares(objective_func, initial_guess, verbose=2, max_nfev=500) print(f"优化状态: {result.status}") print(f"优化信息: {result.message}") # 从优化结果中重构最终的变换矩阵 X rvec_opt = result.x[:3] tvec_opt = result.x[3:6] X_final = pose_to_homogeneous_matrix(rvec_opt, tvec_opt) # 计算最终的平均重投影误差作为标定质量评估 final_error = objective_func(result.x) mean_error = np.mean(np.abs(final_error)) print(f"平均变换矩阵残差: {mean_error:.6f}") return X_final, mean_error ``` ## 5. 结果验证、误差分析与工程化建议 得到变换矩阵 `X` 后，**千万不要直接投入使用**。必须进行严格的验证，因为一个错误的标定结果比没有标定更危险，可能导致机械臂撞机。 一种有效的验证方法是“重投影检查”： 1. 从采集的数据中随机挑选几组（或全部）未参与标定的数据。 2. 对于每组数据，已知： - 机械臂末端位姿 `T_robot` - 相机拍摄的标定板图像，我们可以检测出标定板角点在**图像像素坐标系**下的坐标 `pixel_points`。 - 标定板角点在**标定板坐标系**下的3D坐标 `obj_points`。 - 我们刚求出的手眼矩阵 `X`。 3. 利用以下链条进行坐标变换和重投影： - 将标定板角点的3D坐标 `obj_points`，通过相机外参（即拍摄该图像时相机相对于标定板的位姿 `T_cam_to_board`）变换到**相机坐标系**。 - 此时，利用公式 `P_tool = X * P_camera`，可以将相机坐标系下的点 `P_camera` 变换到**机械臂工具坐标系**。 - 同时，我们从机械臂控制器读取到的末端位姿 `T_robot` 实际上是工具坐标系相对于机器人基坐标系的变换。因此，`P_base = T_robot * P_tool`。 - 理论上，对于同一个物理角点，无论从哪张图像计算，其变换到机器人基坐标系下的坐标 `P_base` 应该是一个**固定值**（因为标定板固定）。我们可以计算多张图像得到的同一个角点的 `P_base` 坐标的方差或标准差，作为标定精度的衡量。 为了方便使用，我将整个流程封装成了一个一键运行脚本 `run_calibration.py`。你只需要在配置文件 `config.yaml` 中设置好路径和参数，运行后即可得到标定结果和评估报告。 ```yaml # config.yaml 示例 paths: image_dir: "./outputs/images" pose_file: "./outputs/poses.csv" camera_intrinsics: "./camera_intrinsics.yaml" # 相机内参文件，需提前标定 calibration: chessboard_width: 11 # 内角点列数 chessboard_height: 8 # 内角点行数 square_size_m: 0.03 # 方格物理边长，单位：米 robot: # 睿尔曼机械臂位姿格式： [x, y, z, rx, ry, rz] (米，弧度) # 如果实际读取的数据顺序或单位不同，需要在此处或代码中转换 ``` 最后，分享几个提升标定成功率和精度的**工程化技巧**： - **相机内参标定**： 本文假设你已经有了准确的D435i相机内参（焦距 `fx, fy`，主点 `cx, cy` 和畸变系数）。如果还没有，务必先用OpenCV的 `cv2.calibrateCamera` 对相机进行单独标定，并将结果保存到 `camera_intrinsics.yaml`。不准确的内参会直接导致PnP求解误差，传递到手眼标定。 - **机械臂位姿精度**： 确保从睿尔曼控制器读取的末端位姿是**在同一个坐标系下**（通常是基坐标系），并且单位统一（平移为米，旋转为弧度）。位姿的噪声和误差会直接影响 `A` 矩阵的准确性。 - **多组数据融合**： 可以运行多次独立的标定流程（从采集到计算），然后取几次结果的平均值，或者使用RANSAC等鲁棒算法剔除异常数据对。 - **可视化检查**： 在采集数据时，实时在图像上绘制检测到的角点（使用 `cv2.drawChessboardCorners`），确保每次按下‘s’键时角点检测都是正确和完整的。