做机器人抓取项目第一步往往不是写抓取规划而是先把相机的坐标系和机械臂的坐标系“对齐”。这个对齐过程就是业内常说的手眼标定。最近我用Python配合Intel RealSense D435深度相机和睿尔曼机械臂把整套标定流程完整走了一遍包括原理推导、数据采集、代码实现和结果验证踩了不少坑也沉淀出一些经验。这篇文章就围绕这套组合展开给你一条可以直接照着操作的路线末尾也会把这些年做手眼标定最容易忽略的细节整理出来。文章面向两类读者一类是刚接触机器人视觉的开发者想快速搞清楚手眼标定到底在算什么、代码该怎么写另一类是已经在做视觉引导项目但标定精度一直不理想的人希望借这篇文章对照排查自己的数据采集和代码流程。我尽量用最直白的话解释原理代码部分也会逐步讲清楚保证你能在看完后自己复现一遍。1. 手眼标定的本质把“眼睛”坐标系和“手”坐标系对齐1.1 相机装在哪里决定了你要标定什么先明确一个基本概念。相机和机械臂之间的相对安装方式决定了你要求解什么样的变换关系网上很多教程含糊带过导致很多人拿到代码却不知道怎么替换自己的数据。第一种是眼在手上eye-in-hand相机固定在机械臂末端法兰上跟着机械臂一起动。这种方案适合近距离抓取、视觉伺服因为目标物永远在相机视野内而且可以靠近观察遮挡少、深度精度高。第二种是眼在手外eye-to-hand相机固定在工作空间外面机械臂在视野范围内运动。这种方案适合全局定位、工位监控机械臂运动不遮挡相机视野。我做这个项目时选择的是眼在手上。原因很直接RealSense D435的重量只有一百多克体积也小装到睿尔曼末端法兰上非常轻松负载完全不是问题而且D435本身就是为近距离环境感知设计的正好匹配抓取场景。如果你做的是工作台全景抓取相机挂在天花板上更合理那就改成眼在手外标定。虽然本文代码以眼在手上为例但后面的求解逻辑两者基本一致只是输入矩阵的顺序不同我在对应位置会单独说明。1.2 坐标变换链和AXXB是怎么来的搞清楚了安装方式接下来就是核心问题了手眼标定到底要求解什么以眼在手上为例你会在工作台面上固定一张标定板ArUco标签或者棋盘格。这时候存在四条坐标变换关系(T_{base}^{end})机械臂末端在机械臂基座坐标系下的位姿这个通过睿尔曼的SDK可以直接读到(T_{end}^{cam})相机在机械臂末端坐标系下的位姿也就是我们要标定的手眼矩阵(T_{cam}^{marker})标定板在相机坐标系下的位姿这个通过OpenCV识别ArUco标签计算得到(T_{base}^{marker})标定板在机械臂基座坐标系下的位姿因为标定板固定在工作台上不动所以这个值是恒定的。这四个变换可以串联成一个闭合链[ T_{base}^{marker} T_{base}^{end} \cdot T_{end}^{cam} \cdot T_{cam}^{marker} ]因为标定板固定不动所以 (T_{base}^{marker}) 是常量。机械臂运动到两个不同位置时分别有[ T_{base}^{end1} \cdot T_{end}^{cam} \cdot T_{cam}^{marker1} T_{base}^{end2} \cdot T_{end}^{cam} \cdot T_{cam}^{marker2} ]把未知量整理到等式一边就变成经典的AXXB形式[ (T_{base}^{end2})^{-1} \cdot T_{base}^{end1} \cdot X X \cdot T_{cam}^{marker2} \cdot (T_{cam}^{marker1})^{-1} ]其中X就是我们要标定的 (T_{end}^{cam})。等号左边是机械臂末端两次位姿之间的相对变换右边是标定板在相机坐标系下两次观测之间的相对变换。理论上只要两组数据就能列出方程但实际上一组数据受噪声影响太大所以至少要采集十几组不同位姿的数据用最小二乘的思想去求解最优值。OpenCV里封装好的calibrateHandEye函数就是干这件事的。这里有个非常关键的细节很多人拿到代码就开跑但不知道A矩阵和B矩阵分别对应哪一侧。我上面给的是眼在手上的推导对应OpenCV函数里的R_gripper2base和R_target2cam。如果你做的是眼在手外输入顺序要反过来。具体怎么传后面的代码部分会直接演示。2. 准备工作软硬件清单和三个容易踩的坑2.1 我用到的硬件和软件选型在动手写代码之前先把环境搭好。这套方案里涉及的软硬件并不复杂但型号匹配和版本兼容问题不容忽视。我实际使用的配置如下。项目型号/版本备注相机Intel RealSense D435深度彩色主动立体视觉机械臂睿尔曼协作机械臂带官方Python SDK支持TCP控制标定板ArUco 词典DICT_6X6_250单个二维码尺寸50mm系统Windows 11 / Ubuntu 20.04两种环境都实测过Python3.8 ~ 3.10实测稳定OpenCVopencv-contrib-python 4.6.0.66这个版本包含aruco模块RealSense SDKpyrealsense2 2.54.2对应固件版本D435相机我多说两句。它是主动红外立体视觉方案室内光照条件下深度质量稳定彩色分辨率最高1920×1080深度分辨率1280×720。手眼标定其实只用它的彩色流识别ArUco标签就行深度流是后续抓取时才用到。用它的好处是同一套SDK可以同时拿到内外参省去很多转换步骤。2.2 环境配置的三个坑这块看起来简单实际上我在这上面耽误过不少时间。先说最容易踩的三个坑。第一个坑是pyrealsense2的Python包版本兼容问题。RealSense SDK更新频繁不同版本对Python版本的支持范围不一样有些新版只支持到3.9有些则要3.10以上。建议安装前先确认一下你本机Python版本别装完发现import直接报错。第二个坑是opencv-python和opencv-contrib-python不能同时存在。ArUco模块在contrib版本里如果电脑上两个包都装了import cv2的时候会互相覆盖轻则找不到cv2.aruco重则整个cv2起不来。安装前先检查一下环境把不需要的那个卸载干净。第三个坑是睿尔曼SDK的初始化方式。不同批次的睿尔曼机械臂支持的连接方式略有差异有的是网口有的是串口SDK初始化时要把IP地址或者串口号填对。如果是网口连接建议先用机械臂自带的调试软件确认通讯正常再运行Python脚本避免把网络问题误判成SDK问题。3. 核心代码实现一步一步来3.1 相机内参准备手眼标定需要相机内参因为只有知道相机内部的光学参数才能从像素坐标反推出标定板在相机坐标系下的位姿。D435的SDK可以直接读取彩色流的内参不需要额外跑棋盘格标定这是它方便的地方。import pyrealsense2 as rs import numpy as np pipeline rs.pipeline() config rs.config() config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) pipeline.start(config) profile pipeline.get_active_profile() color_profile profile.get_stream(rs.stream.color) intrinsics color_profile.as_video_stream_profile().get_intrinsics() camera_matrix np.array([ [intrinsics.fx, 0, intrinsics.ppx], [0, intrinsics.fy, intrinsics.ppy], [0, 0, 1] ], dtypenp.float64) dist_coeffs np.array(intrinsics.coeffs, dtypenp.float64) print(相机内参矩阵:\n, camera_matrix) print(畸变系数:, dist_coeffs)代码里我把内参组织成OpenCV风格的camera_matrix后面识别ArUco标签时直接传给它。D435的畸变模型是Brown-Conrady模型coeffs里包含径向和切向畸变系数OpenCV的estimatePoseSingleMarkers函数也能直接兼容。这个过程我建议单独跑一次确认标定板在画面中清晰可见且没有反光。D435的彩色相机对光线比较敏感太暗的环境会导致ArUco检测不稳定有条件的话补个均匀白光效果会好很多。3.2 识别ArUco标定板并获取位姿有了内参下一步就是写一个函数输入一帧图像输出标定板在相机坐标系下的4x4变换矩阵。import cv2 ARUCO_DICT cv2.aruco.DICT_6X6_250 MARKER_LENGTH 0.05 # 标定板边长单位米 def detect_marker_pose(image, camera_matrix, dist_coeffs): aruco_dict cv2.aruco.Dictionary_get(ARUCO_DICT) parameters cv2.aruco.DetectorParameters_create() corners, ids, rejected cv2.aruco.detectMarkers( image, aruco_dict, parametersparameters ) if ids is None: return None # 老版本接口返回三个值新版本返回两个值这里做兼容 ret cv2.aruco.estimatePoseSingleMarkers( corners, MARKER_LENGTH, camera_matrix, dist_coeffs ) if len(ret) 3: rvecs, tvecs, _ ret else: rvecs, tvecs ret # 假设画面中只有一个标定板取第一个 rvec rvecs[0].reshape(3) tvec tvecs[0].reshape(3) R, _ cv2.Rodrigues(rvec) # 旋转向量转旋转矩阵 T_cam2marker np.eye(4) T_cam2marker[:3, :3] R T_cam2marker[:3, 3] tvec return T_cam2marker这里我多说一句。OpenCV在不同版本里estimatePoseSingleMarkers的返回格式不一样4.6版本返回三个值4.8版本之后有的接口只返回两个值网上代码经常因此报错。我在代码里加了兼容处理你直接复制过去跑基本不会出问题。还有个细节是标定板尺寸。MARKER_LENGTH是ArUco标签的物理边长必须用卡尺量准单位是米。这个值直接参与位姿解算如果量偏了1毫米标定结果和实际抓取精度都会跟着偏而且偏肉眼看不太出来排查起来很麻烦。我建议打印标定板时尽量选用高精度打印机量尺寸时多量几条边取平均。3.3 获取机械臂末端位姿我们需要的另一个关键输入是机械臂末端在基座坐标系下的位姿。睿尔曼的Python SDK提供了接口来读取当前末端姿态不同版本SDK的类名和函数名可能有点差异但核心逻辑是一致的拿到位置和姿态数据然后组装成4x4齐次变换矩阵。def get_robot_end_transform(): # 假设已经初始化了睿尔曼机械臂SDK并且读取到了末端位姿 # 这里的API调用以实际SDK为准返回值示例仅供参考 pose rm_api.get_current_end_pose() # 位置单位通常为毫米要换算成米 x pose[position][x] / 1000.0 y pose[position][y] / 1000.0 z pose[position][z] / 1000.0 # 姿态通常是欧拉角单位可能是弧度也可能是角度 # 一定要确认SDK的旋转顺序和单位这里按常见的ZYX欧拉角处理 rx pose[euler][rx] ry pose[euler][ry] rz pose[euler][rz] R euler_to_rotation_matrix(rx, ry, rz) T_base2end np.eye(4) T_base2end[:3, :3] R T_base2end[:3, 3] [x, y, z] return T_base2end def euler_to_rotation_matrix(rx, ry, rz): # 按ZYX顺序计算旋转矩阵 cx, sx np.cos(rx), np.sin(rx) cy, sy np.cos(ry), np.sin(ry) cz, sz np.cos(rz), np.sin(rz) Rx np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]]) Ry np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]]) Rz np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]]) return Rz Ry Rx这个函数有两个重点。第一个是单位统一机械臂SDK返回的位置单位可能是毫米但手眼标定使用的单位都是米不换算的话结果完全不对。第二个是欧拉角的旋转顺序和单位不同机械臂厂家对欧拉角的定义并不一样有的用ZYX有的用XYZ有的是角度制有的是弧度制一定要翻SDK文档确认清楚。我早期做另一个品牌机械臂时吃了这个亏标定出的手眼矩阵旋转部分怎么都不对最后才发现是欧拉角顺序写反了。3.4 数据采集多组位姿的录制与筛选手眼标定本质上是个多组数据的拟合过程数据质量直接决定结果精度。我的做法是分两步先写一个采集脚本控制机械臂依次运动到预设的多个位置在每个位置同时记录末端位姿和图像中的标定板位姿然后实时显示检测结果方便人工筛选。def collect_data(num_poses20): data [] while len(data) num_poses: input(将机械臂移动到新的姿态后按回车...) # 读取机械臂末端位姿 T_base2end get_robot_end_transform() # 拍摄一张彩色图并检测标定板 frames pipeline.wait_for_frames() color_frame frames.get_color_frame() image np.asanyarray(color_frame.get_data()) T_cam2marker detect_marker_pose(image, camera_matrix, dist_coeffs) if T_cam2marker is None: print(当前视野没有检测到标定板换个方向再试) continue # 简单筛选末端姿态差异太小会降低标定精度 if len(data) 0: last_T data[-1][T_base2end] delta_pos np.linalg.norm(T_base2end[:3, 3] - last_T[:3, 3]) delta_R np.trace(T_base2end[:3, :3].T last_T[:3, :3]) delta_angle np.degrees(np.arccos(np.clip((delta_R - 1) / 2, -1, 1))) if delta_pos 0.05 and delta_angle 5: print(变化量太小建议移动距离大于5cm或旋转角度大于5度) continue data.append({ T_base2end: T_base2end, T_cam2marker: T_cam2marker }) print(f已采集 {len(data)}/{num_poses} 组) # 可视化当前图像和检测结果 cv2.aruco.drawDetectedMarkers(image, corners, ids) cv2.imshow(tag, image) if cv2.waitKey(1) 0xFF ord(q): break return data关于采集多少个点有人说越多越好实际上不是。我实测下来20组左右就足够稳定再多数据反而可能因为机械臂末端靠近奇异点而引入局部误差。关键不是点数多少而是点位分布的多样性。数据筛选那块我做了个简单判断相邻两个采样点之间末端位置至少相差5厘米或者姿态变化至少5度避免两次采集过于接近导致方程退化。3.5 求解AXXB调用OpenCV的calibrateHandEye数据记录完成后就到了核心求解环节。OpenCV提供了现成的calibrateHandEye函数内部实现了Tsai、Park、Horaud等多种算法我们不需要自己解矩阵方程但要把数据转换成它需要的格式。def solve_handeye(data): R_gripper2base [] t_gripper2base [] R_target2cam [] t_target2cam [] for item in data: T_b2e item[T_base2end] T_c2m item[T_cam2marker] R_gripper2base.append(T_b2e[:3, :3]) t_gripper2base.append(T_b2e[:3, 3]) R_target2cam.append(T_c2m[:3, :3]) t_target2cam.append(T_c2m[:3, 3]) R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, methodcv2.CALIB_HAND_EYE_TSAI ) T_cam2end np.eye(4) T_cam2end[:3, :3] R_cam2gripper T_cam2end[:3, 3] t_cam2gripper.flatten() return T_cam2end为什么这里要把旋转矩阵和平移向量拆开传因为calibrateHandEye只处理旋转矩阵列表和平移向量列表不接收变换矩阵。我见过有人直接传入4x4矩阵列表然后报错就是这个原因。方法选择上Tsai算法收敛快、对噪声相对稳定是最常用的默认选择。如果标定结果不够理想可以换成cv2.CALIB_HAND_EYE_PARK或者cv2.CALIB_HAND_EYE_HORAUD交叉验证一下。多跑几种方法结果差异很小说明数据质量好如果差异很大多半是数据采集有问题先别急着怀疑算法。到这一步手眼矩阵 (T_{cam}^{end}) 就算出来了。保存到本地文件时建议连同时间戳、机械臂型号等元信息一起存方便后续追溯。4. 数据采集策略与避坑经验4.1 什么样的数据集才是好数据玩过手眼标定的人都有体会同样是标定有人一次就准有人反复折腾还是漂。区别往往不在代码而在数据采集策略。我总结了几条“黄金法则”供你对照自己采集的数据集检查。第一让标定板在工作空间的不同位置出现。不要只固定在一个点附近转机械臂末端要带着相机在X、Y、Z三个方向移动覆盖整个工作空间。这样标定出的手眼矩阵才对整个空间都有效而不是只在某个局部区域准确。第二姿态变化要丰富。相机相对于标定板的角度要尽量多样俯仰、偏航、滚转都要有避免所有数据都在同一个平面内旋转。我自己的经验是如果末端始终水平移动、不转动相机标定出的旋转部分误差会偏大。第三每次移动幅度别太小。两个采样点之间的末端变换如果太小方程组的条件数会很差数值上不稳定。上面代码里我加了变化量筛选就是避免这种问题。第四图像中标定板要清晰可识别。光照均匀、没有运动模糊、标定板占比适中。D435的彩色相机对强烈反光很敏感金属台面、亚克力板反射过来的光会让ArUco标签边缘发白直接导致检测位姿抖动。4.2 我踩过的坑和排查思路这里分享一下我在实际项目中遇到过的典型问题按出现频率排序。第一个坑旋转向量和旋转矩阵混用。estimatePoseSingleMarkers返回的是旋转向量而calibrateHandEye需要旋转矩阵。很多人漏了一步cv2.Rodrigues转换直接喂进去结果标定出的矩阵张得乱七八糟。这个报错其实不报但结果一验证就完蛋是最隐蔽的坑。第二个坑单位不统一。机械臂返回的位置单位是毫米ArUco标定板的尺寸单位是米两个单位混在一起解出来的平移向量会差1000倍。我见过有人标定出来的平移量是几十米检查半天才找到原因。第三个坑把标准板拿在手里移动而不是固定在工作台面上。如果标定板在采集过程中移动了那 (T_{base}^{marker}) 就不再是常量整个方程推导的前提就不成立。操作时一定要把标定板用胶带或夹具固定好采集过程中不能动。第四个坑一次采集多个目标物。ArUco检测如果画面中有多个标签ids会返回多个如果不加筛选直接取tvecs[0]可能每次都取的是不同的那个标签导致数据完全错乱。我的建议是画面中只放一个标签或者固定选取唯一ID的标签。第五个坑标定板太小或者太大。标定板在画面中占比太小位姿解算精度差太大则边缘容易出视野检测不稳定。以D435在1280×720分辨率下工作建议标定板边长在30到80毫米之间具体看工作距离。我把这些常见问题整理成了一个排查表遇到问题可以按表格快速对号入座。现象可能原因排查方式标定出的平移向量数值异常大单位没统一检查毫米和米换算旋转矩阵不正交旋转向量未转矩阵检查Rodrigues转换标定结果只在局部区域准数据分布太集中扩大采样空间范围标定板检测经常失败光照不足或反光调整照明更换标定板材质换了几种算法结果差异大数据质量差增加姿态多样性剔除异常点5. 标定结果怎么验证才靠谱5.1 重投影与闭环验证标定完不是万事大吉必须要验证。验证方法分两层第一层是数学层面的闭环验证第二层是真实场景的抓取测试。闭环验证的原理很简单利用手眼矩阵把相机坐标系下观测到的标定板位姿变换到机械臂基座坐标系下理论上它应该是一个固定值不随机械臂运动而改变。随机选取一组没有参与标定的数据来算def validate_handeye(data, T_cam2end, indices): errors [] for i in indices: T_b2e data[i][T_base2end] T_c2m data[i][T_cam2marker] # 闭环计算标定板在基座坐标系下的位姿 T_b2m T_b2e T_cam2end T_c2m # 以第一组为基准计算位置误差 if errors: delta T_b2m[:3, 3] - ref_T[:3, 3] errors.append(np.linalg.norm(delta)) else: ref_T T_b2m return errors # 用未参与求解的几组数据验证 test_idx range(15, 20) errors validate_handeye(data, T_cam2end, test_idx) print(闭环位置误差(mm):, [round(e * 1000, 2) for e in errors])如果位置误差都在10毫米以内说明标定结果基本可信。如果误差有几十毫米那就要回头查数据质量、内参是否准确、机械臂末端位姿读取是否对了。这里要注意闭环误差包含机械臂本身的绝对定位误差。协作机械臂虽然重复定位精度高但绝对定位精度一般不会太高可能本身就有几毫米的误差来源所以误差在5到10毫米内都可以接受。如果要做更高精度的视觉引导可能需要单独做机械臂的标定补偿。5.2 真实场景测试视觉引导抓一次数学验证通过了只是第一步我强烈建议再做一次真实场景测试。方法也简单在机械臂工作空间内放一个物体先用相机识别它得出物体在相机坐标系下的位置再通过手眼矩阵变换到基座坐标系让机械臂末端移动到这个位置。具体流程是这样的先用相机检测到目标物体的位置和姿态然后计算 (T_{base}^{target} T_{base}^{end} \cdot T_{cam}^{end} \cdot T_{cam}^{target})把这个结果发给机械臂执行。如果手眼矩阵正确末端应该能准确到达目标位置上方。如果偏了看偏的方向和大小通常就能判断问题是出在旋转部分还是平移部分。比如目标点始终在同一方向偏移固定的量大概率是平移部分估计不准如果在不同位置偏移方向不同可能是旋转部分有问题。这一步我称之为“手眼标定的毕业考试”很多理论上觉得已经没问题的人一抓取就原形毕露。原因是闭环验证用的数据本身是同一套采集体系的存在系统性偏差而真实场景测试完全独立能暴露标定中的隐藏问题。5.3 标定精度不够时从哪里下手优化遇到精度不够的情况不要急着怀疑OpenCV的算法绝大多数问题出在采集环节和标定板本身。我给你的排查顺序是先看数据质量再看内参精度最后才看算法和参数调整。数据质量方面先把采集的20组数据画出来看末端位姿是否分布在足够的空间范围内标定板观测值是否有明显跳变。如果某组数据的标定板位姿和前后几组差异很大大概率是该帧图像拍模糊了或者光照突变直接删掉重采一组。内参精度方面D435出厂内参虽然能用但如果你想追求极致精度可以先用棋盘格在目标工作距离附近重新标定一遍相机内参。尤其是畸变系数出厂默认值在短距离下误差可能被放大重标定后一般能看到明显改善。还有一个很容易被忽略的点机械臂末端和相机之间连接的刚性。如果相机支架是3D打印的软性材料或者螺丝没拧紧相机在运动过程中会轻微晃动这种物理层面的误差是任何算法都救不回来的。所以安装时务必用金属支架并固定牢靠。如果以上都排除了可以试着在calibrateHandEye中换一种方法对比结果。Tsai、Park、Horaud三种算法在不同噪声模型下表现略有差异理论上多方法结果的一致性是数据质量良好的标志。如果差异很大回看第4.2节的数据集问题数据质量是根因。我自己做这类项目时最深的体会是手眼标定是一项“七分数据、三分算法”的工作。很多人在网上找代码、调参数花了一整天最后发现问题不过是标定板尺寸量错了或者单位没有换算。与其反复折腾算法不如把时间花在数据采集上保证采样点分布好、数据干净标定结果基本不会差到哪里去。整套流程走完后你把保存的 (T_{cam}^{end}) 矩阵加载到自己的视觉抓取程序里后续每个目标点的坐标变换就都有了基准。如果换了一台相机或者重新拆装了相机支架记得重新标定一次这个矩阵和安装位置强相关不能一套标定结果永久通用。最后再分享一个小经验在项目初期先花半天时间把标定流程跑通包括数据采集脚本、标定求解脚本和验证脚本都写好后面做视觉抓取时你会省下大把调试时间。手眼标定是整个机器人视觉系统的地基地基稳了后面的抓取、码垛、装配才能做得顺畅。这套Python RealSense D435 睿尔曼机械臂的组合在实际项目中表现稳定希望这篇文章能帮你少走几步弯路。