第一次把Realsense D435装到睿尔曼机械臂末端的时候我以为最难的环节是后续的抓取算法。结果一跑起来就傻眼了相机明明看到工件在画面中央换算给机械臂之后机械臂却总是偏出好几厘米有时候那个目标点甚至直接落到工作台外面去。排查半天图像识别没毛病机械臂运动精度也没问题真正的问题是相机坐标系和机械臂基座坐标系之间缺了一个关键的变换矩阵也就是手眼标定矩阵。这套用Python实现的标定流程我从环境配置到精度验证完整跑了一遍。做完之后最大的感受是很多人卡住的地方根本不是标定算法本身而是前面数据采集步骤做得太糙导致后面求出来的矩阵就是废纸。这篇文章我会把5个关键步骤完整拆开讲清楚每一步在做什么、为什么必须这么做顺便把我踩过的坑和排查链路一并写出来给后面做同类项目的朋友一个可以直接参考的作业。1. 手眼标定的底层逻辑两套坐标系之间缺一座桥1.1 从一次失败的抓取实验说起当时我搭的实验场景其实很简单机械臂末端装了一个D435工作台上放一个ArUco标定板。相机识别到标定板后我算出了标定板在相机坐标系下的位姿接着直接把这个位姿当成机械臂基座坐标系下的坐标发给机械臂让它移动过去。结果机械臂到的位置和相机看到的位置对不上偏差肉眼可见。问题出在哪相机有自己的一套三维坐标系机械臂也有自己的一套三维坐标系。你从相机里拿到的坐标是相对于相机光心的而机械臂执行运动时用的是基座坐标系。这两套坐标系之间存在一个固定的平移加旋转关系不知道这个关系你就没办法把相机看到的坐标正确翻译成机械臂应该去的坐标。手眼标定就是用来求解这个固定变换关系的业内一般叫手眼矩阵。1.2 眼在手上与眼在手外选择决定了后续公式手眼标定按照相机安装位置分成两种典型场景必须要先分清楚因为后面数据采集和公式求解的处理方式完全不同。眼在手上Eye-in-Hand相机固定在机械臂末端跟着机械臂一起动。相机能看到的目标范围会随手臂姿态变化适合近距离操作和抓取场景。D435这种体积和重量都比较适中的相机装在睿尔曼这类轻量级协作臂末端是常见的做法。眼在手外Eye-to-Hand相机固定安装在外部支架上机械臂在相机视野里运动。这种方案视野稳定机械臂末端负载负担小但容易被机械臂自身遮挡适合大范围工位的全局定位。这篇文章的主线是眼在手上因为这是D435和睿尔曼机械臂配合时最常见的装配方式。眼在手上的场景下我们要求解的是相机坐标系在机械臂末端坐标系下的位姿也就是相机的安装偏移量和旋转量有了它才能把相机看到的物体坐标换算到机械臂基座坐标系里。1.3 AXXB这个方程到底在说什么手眼标定最后落到数学上就是一个经典的AXXB问题。我用眼在手上的场景来解释。机械臂运动到某个位姿时我们能得到两个变换关系机械臂末端在基座坐标系下的位姿这个直接读机械臂SDK就能拿到记作T_BE。标定板在相机坐标系下的位姿这个通过识别标定板并用PnP求解得到记作T_CT。我们要找的是相机在末端坐标系下的位姿记作X也就是T_EC。关键点在于标定板固定在工作台上不动那么标定板在机械臂基座坐标系下的位姿应该是固定的无论机械臂怎么动都不变。于是就有了恒等式T_BT T_BE × X × T_CT同一个恒等式在机械臂两个不同位姿下各写一遍联立之后一化简就得到了AXXB的形式。这里的A由两组机械臂末部位姿算出B由两组视觉测量结果算出X就是我们要求的手眼矩阵。OpenCV的calibrateHandEye函数做的就是这件事你只需要给它喂对数据它负责求解X。2. 标定前先把这几样东西准备好2.1 Realsense D435的驱动、Python库与内参获取D435的使用不复杂但有几个前置条件容易出问题。首先硬件上D435一定要插在USB 3.0口上USB 2.0接口虽然能识别设备但帧率会被压得很低甚至直接打不开彩色流。如果你是虚拟机里跑强烈建议换物理机RealSense的UVC协议在虚拟机里经常出现掉帧或者设备丢失的情况。Python这边装官方库就行pip install pyrealsense2 numpy opencv-contrib-python这三个库是后面的主力。装好之后先做一件事把相机的内参读出来。D435出厂自带标定数据但考虑到运输震动、温度变化等因素在做手眼标定之前我建议用OpenCV自带的张正友标定法用棋盘格单独做一次相机内参标定把焦距和主点坐标标定得更准一些。这个内参后面会用于PnP求解标定板的位姿内参不准后面全白搭。读取D435内参的代码很简单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) intr color_profile.as_video_stream_profile().get_intrinsics() camera_matrix np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]]) dist_coeffs np.array(intr.coeffs)需要注意D435的intr.coeffs返回的畸变系数已经是OpenCV格式可以直接用。如果你自己用棋盘格重新标定了相机就用你标定出来的camera_matrix和dist_coeffs替换上面的值。2.2 睿尔曼机械臂的Python通信与位姿读取睿尔曼机械臂的通信方式很直接默认是TCP协议机械臂默认IP一般是192.168.1.18端口8080。第一次连的时候记得把电脑的有线网卡IP手动设成192.168.1.x的网段比如192.168.1.100不然SDK连不上控制器。官方提供了Python SDK的Demo里面封装好了移动、查询位姿这些常用指令。我在项目里主要用的是查询当前末端位姿的接口它会返回一组数据通常包含位置x、y、z和姿态rx、ry、rz单位是毫米和度。# 伪代码以你拿到的官方SDK实际接口为准 import rm_sdk arm rm_sdk.RmArm(192.168.1.18, 8080) pose arm.get_actual_tcp_pose() # pose 形如 [x, y, z, rx, ry, rz]单位 mm 和 deg拿到欧拉角之后要转成旋转矩阵。这一步有个非常容易踩的坑欧拉角的旋转顺序必须确认。睿尔曼默认返回的欧拉角一般是ZYX顺序但不同固件版本可能有差异。我的建议是优先用SDK里能直接返回旋转矩阵或四元数的接口如果只有欧拉角一定要先用官方文档确认旋转顺序再转旋转矩阵。顺序搞错标定出来的结果误差会大得离谱。2.3 标定板的选择与精度控制标定板我推荐ArUco它比棋盘格更适合手眼标定原因很实际ArUco可以从任意角度识别不需要整块板子都在画面里部分遮挡也能识别还可以用多个marker同时估计位姿鲁棒性比棋盘格好很多。ArUco标定板的生成可以用OpenCV但单独生成一张图方便打印import cv2 dictionary cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250) board cv2.aruco.GridBoard_create(5, 7, 0.03, 0.005, dictionary) # 5列7行每个格子3cm间隔0.5cm img board.generateImage((700, 500), None, 10) cv2.imwrite(aruco_board.png, img)打印ArUco板的时候我建议用激光打印机不要用喷墨喷墨打印出的黑色边缘会洇开导致marker的实际边长和理论值有偏差。打印完用游标卡尺量一下实际边长把测量值写进代码里。另外一定要把纸贴在硬质平板上比如亚克力板或者铝板纸面一旦轻微弯曲PnP解出来的位姿就会有系统性偏差这种偏差后续非常难排查。3. 五个关键步骤的完整实操链路3.1 步骤一搭好基础环境确认版本兼容很多人在这里就被卡住了核心问题是opencv-python和opencv-contrib-python的冲突。ArUco模块在OpenCV的contrib包里如果你同时装了opencv-python和opencv-contrib-python后装的会覆盖先装的import cv2时很容易出现模块加载错乱。正确做法是只保留opencv-contrib-pythonpip uninstall opencv-python -y pip install opencv-contrib-pythonPython版本建议3.8到3.10新版Python对RealSense SDK的支持稳定性我没有仔细验证过但3.9和3.10在D435和睿尔曼SDK的兼容性上是经过大量用户验证过的稳妥选择。另外注意如果你用的是OpenCV 4.7及以上版本ArUco的旧API比如cv2.aruco.Dictionary_get已经不建议使用新API改成cv2.aruco.getPredefinedDictionary配合cv2.aruco.ArucoDetector。下面两个版本的代码我都会给出避免版本不同导致踩坑。3.2 步骤二在图像中稳定检测标定板并估计位姿这一步的核心目标是拿到标定板在相机坐标系下的位姿T_CT也就是旋转向量rvec和平移向量tvec。先说ArUco检测。以OpenCV 4.7以上版本为例import cv2 import numpy as np # 新API4.7 dictionary cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250) detector_params cv2.aruco.DetectorParameters() detector cv2.aruco.ArucoDetector(dictionary, detector_params) # 如果还是旧版本用这两行替代 # dictionary cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) # detector_params cv2.aruco.DetectorParameters_create() def detect_board_pose(color_image, camera_matrix, dist_coeffs, marker_length0.03): gray cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) corners, ids, _ detector.detectMarkers(gray) if ids is None or len(ids) 4: return None # estimatePoseSingleMarkers 对每个marker分别估计位姿 rvecs, tvecs, _ cv2.aruco.estimatePoseSingleMarkers( corners, marker_length, camera_matrix, dist_coeffs) # 多个marker的结果做平均提高抗噪声能力 rvec np.mean(rvecs, axis0).reshape(3) tvec np.mean(tvecs, axis0).reshape(3) return rvec, tvec代码里几个细节值得展开marker_length是marker的实际物理边长单位米。这个值必须是你打印后实测的边长不是理论值。我的标定板设计的是0.03m实测是0.0302m别看只差0.2mm放在PnP解算里对几十厘米的工作距离来说误差会被放大不少。estimatePoseSingleMarkers每个marker都能解出一个位姿多个marker时取平均能有效抑制单个marker识别抖动带来的误差。但我提醒一句如果某个marker因为反光没被识别到简单平均会引入偏差这时候可以用cv2.aruco.refineDetectedMarkers做亚像素精化或者直接丢弃少于4个marker的帧。如果画面中的ArUco板在运动过程中频繁出现检测失败先排查曝光问题。D435的自动曝光在光照不均匀时会让marker边缘发白或发黑建议固定曝光值关闭自动曝光from pyrealsense2 import rs # 在开启pipeline之前设置 sensor pipeline.get_active_profile().get_device().query_sensors()[1] # RGB sensor sensor.set_option(rs.option.enable_auto_exposure, 0) sensor.set_option(rs.option.exposure, 200) # 具体值根据现场亮度调3.3 步骤三规划机械臂运动轨迹同步采集数据这一步是整个标定流程里最关键也最容易翻车的一步直接决定了最终结果的精度。所谓手眼标定要的数据本质就是一组配对数据每一帧画面中测得的标定板位姿T_CT加上同一时刻机械臂末端在基座坐标系下的位姿T_BE。至少需要15到20组这样的配对数据。采集时要遵循几个原则姿态要足够多样。机械臂每次运动到目标点后除了位置平移还要改变末端的姿态绕不同轴转个10到30度。千万不要只做平移不做旋转。AXXB这个方程之所以能解靠的是多组位姿之间的旋转差异如果所有位姿的姿态几乎一样方程会退化成病态解出来的矩阵数值上不可信。标定板要完整出现在画面中。每个采样位置都要确认ArUco板在画面内且足够大。一般建议marker在画面中占据适当比例不要太小否则检测精度会下降。机械臂运动要稳、要慢。运动到位后等一两秒等机械臂完全静止后再触发相机拍照和读取位姿。机械臂运动的瞬间读取位姿误差会很大这一毫秒级别的不同步最后反映到手眼矩阵上可能是几毫米的偏差。采集程序的伪代码如下# 连接机械臂、打开相机 pose_list [] # 机械臂末端位姿 target_list [] # 标定板在相机坐标系下的位姿 sample_poses [...] # 预先规划好的10~20个目标位姿含位置和姿态变化 for pose_cmd in sample_poses: arm.move_to(pose_cmd) time.sleep(1.5) # 等待机械臂完全静止 # 同步读取 tcp_pose arm.get_actual_tcp_pose() color_frame pipeline.wait_for_frames().get_color_frame() color_image np.asanyarray(color_frame.get_data()) result detect_board_pose(color_image, camera_matrix, dist_coeffs) if result is None: print(标定板未识别到跳过这一组) continue rvec, tvec result pose_list.append(tcp_pose) target_list.append((rvec, tvec))我实测下来的经验是宁可每组数据都花两秒去等机械臂稳定也不要为了赶时间在运动过程中抓数据。同步性在手眼标定里的优先级高于一切。3.4 步骤四组装数据并调用OpenCV的calibrateHandEye求解数据采集完成后就要组装成calibrateHandEye需要的格式。在眼在手上的场景下需要准备四个数组R_gripper2base一系列3x3旋转矩阵。对应机械臂末端坐标系到基座坐标系的旋转部分也就是从SDK返回的位姿里提取出来的旋转矩阵。t_gripper2base一系列3x1平移向量。同样的旋转矩阵对应的平移部分。R_target2cam一系列3x3旋转矩阵。对应ArUco标定板到相机坐标系的旋转部分从rvec转换而来。t_target2cam一系列3x1平移向量。对应rvec对应的tvec。组装和调用的代码from scipy.spatial.transform import Rotation as R import cv2 import numpy as np # 假设收集好了 pose_list 和 target_list R_gripper2base_list [] t_gripper2base_list [] R_target2cam_list [] t_target2cam_list [] for tcp_pose, (rvec, tvec) in zip(pose_list, target_list): # 机械臂位姿位置单位mm转m角度deg转rad注意欧拉角顺序 x, y, z, rx, ry, rz tcp_pose rot R.from_euler(ZYX, [rz, ry, rx], degreesTrue) # 根据SDK文档确认顺序 R_gripper2base_list.append(rot.as_matrix()) t_gripper2base_list.append(np.array([x/1000.0, y/1000.0, z/1000.0])) # 视觉测量结果 R_target2cam_list.append(R.from_rotvec(rvec).as_matrix()) t_target2cam_list.append(tvec.reshape(3)) R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( R_gripper2base_list, t_gripper2base_list, R_target2cam_list, t_target2cam_list, methodcv2.CALIB_HAND_EYE_TSAI ) # 组装成4x4齐次矩阵就是最终的手眼矩阵 T_cam2gripper np.eye(4) T_cam2gripper[:3, :3] R_cam2gripper T_cam2gripper[:3, 3] t_cam2gripper.flatten()关于求解方法我直接用的CALIB_HAND_EYE_TSAI也就是经典Tsai-Lenz算法。OpenCV还提供了Park、Horaud、Andreff、Daniilidis等方法。我实测在姿态覆盖充分的情况下几种方法的结果差异很小。如果某个方法解出的矩阵明显异常大概率不是算法的锅而是数据里有脏数据。不过在噪声明显偏大的时候Tsai-Lenz的表现相对稳定一些。这里有个单位问题特别想单独拎出来说睿尔曼SDK返回的位置单位是毫米必须除以1000换算成米不然手眼矩阵的平移部分直接差了一个数量级。有朋友在这个问题上调试了整整一天最后发现只是忘了mm和m的换算。3.5 步骤五验证标定结果必要时重新采集用完calibrateHandEye拿到手眼矩阵后先别急着欢呼还有最后一步验证。我推荐一个非常实用的留出验证法采集数据时单独留出3到5组数据不参与求解专门用来验证。验证逻辑是这样的由于标定板固定在世界坐标系中不动那么对于任意一组数据标定板在机械臂基座坐标系下的位姿都应该是同一个值。利用手眼矩阵X可以把每一组数据都换算到基座坐标系下T_BT T_BE × X × T_CT如果手眼矩阵正确所有验证组算出来的T_BT应该非常接近误差在3到5毫米以内。如果几组验证数据的计算结果散成一团说明标定有问题需要排查原始数据。def compute_board_in_base(tcp_pose, rvec, tvec, T_cam2gripper): # 机械臂末端在基座下的齐次矩阵 x, y, z, rx, ry, rz tcp_pose rot R.from_euler(ZYX, [rz, ry, rx], degreesTrue).as_matrix() T_gripper2base np.eye(4) T_gripper2base[:3, :3] rot T_gripper2base[:3, 3] [x/1000.0, y/1000.0, z/1000.0] # 标定板在相机下的齐次矩阵 T_target2cam np.eye(4) T_target2cam[:3, :3] R.from_rotvec(rvec).as_matrix() T_target2cam[:3, 3] tvec.flatten() # 换算到基座坐标系 T_target2base T_gripper2base T_cam2gripper T_target2cam return T_target2base # 把验证组都代进去比较xyz值的一致性如果验证不过关我的排查顺序是先看数据姿态覆盖够不够丰富再看标定板检测有没有跳变最后看欧拉角转换有没有搞错顺序。按这个顺序排查大概率能定位到问题。4. 数据质量是最隐蔽的坑常见故障与排查链路4.1 姿态覆盖不足导致方程病态求解这是我遇到过的比较隐蔽的问题。有一批数据是机械臂在大概相同的姿态下只做平移采集的结果标定出来的矩阵在验证时误差非常大。原因在于AXXB要解出唯一的X需要数据中的旋转信息足够丰富。如果所有位姿的姿态几乎一样等价于用同一组旋转约束去解方程解出来的矩阵在数学上不唯一数值上就是一个病态结果。这个问题的排查办法有两个打印出所有采样点末端姿态的旋转矩阵计算它们之间的相对旋转角度。如果两两之间的角度普遍小于10度那就是数据姿态覆盖不足需要重新规划轨迹。用多组不同子集分别做标定对比结果。比如从20组数据里随机抽15组标定多做几次如果每次解出来的手眼矩阵都差很多说明数据本身信息量不够。我当时重新规划轨迹之后给每个采样点加上了绕末端xyz轴的不同转角尽可能让机械臂末端指向更多的方向问题立刻消失了。4.2 欧拉角转换顺序错误最难察觉的一类错误睿尔曼返回的rx,ry,rz到底按什么顺序旋转不同机器人品牌甚至不同固件版本都可能不同。常见的顺序有ZYX、XYZ、ZYZ。一旦转换顺序和机械臂内部定义不一致旋转矩阵就是错的后面标定直接崩。我建议在正式采集之前先做一个快速自检把机械臂末端手动移动到某个已知姿态用SDK读到欧拉角转成旋转矩阵后把一个已知向量比如基座Z轴单位向量乘过去看看能不能对应上机械臂当前末端的实际朝向。用这个方法验证欧拉角顺序的正确性10分钟就能排查清楚。4.3 数据同步问题机械臂和相机各拍各的手眼标定要求同一时刻的机械臂位姿和相机图像配对如果两边的数据不是同一时刻采的相当于拿不同时间点的数据硬凑方程误差是必然的。最稳妥的做法是采集时就让机械臂完全静止然后依次读取机械臂位姿和相机帧。我之前用过一个偷懒的做法机械臂边运动边采数据然后拿时间戳去对齐。结果发现相机帧率和SDK位姿返回的缓存时间戳根本不是同一套时钟对齐精度非常差。后来改成移动到目标点、等待完全停止、分别读取的保守策略效果立竿见影。4.4 OpenCV版本升级导致ArUco接口不兼容在OpenCV 4.7之后旧的ArUco API被新接口取代。如果你在旧教程上复制了cv2.aruco.Dictionary_get在新版本上会直接报错module cv2.aruco has no attribute Dictionary_get。这个报错很好排查网上直接搜错误信息就能找到原因。但有一个更隐蔽的情况如果你同时装了opencv-python和opencv-contrib-python因为两个包的cv2模块会互相覆盖你import的cv2可能来自其中任意一个包ArUco模块时有时无非常诡异。解决办法在前面也说过只保留opencv-contrib-python一个包即可。5. 标定完成的下一步把像素坐标换算成机械臂抓取坐标手眼标定不是终点标定完拿到矩阵接下来要把它真正用到抓取流程里。这里给出一个完整的坐标变换链路。假设场景是机械臂末端装了D435相机相机识别到工作台上的一个工件比如一个ArUco码贴在工件表面现在要把工件在相机坐标系下的位置换算到机械臂基座坐标系下发给机械臂去抓取。整个变换链是这样相机识别工件得到工件在相机坐标系下的位姿T_obj2cam。从机械臂SDK实时读取当前末端在基座坐标系下的位姿T_gripper2base。用手眼标定得到的XT_cam2gripper把工件坐标转换到末端坐标系T_obj2gripper X × T_obj2cam。再转换到基座坐标系T_obj2base T_gripper2base × T_obj2gripper。翻译成代码T_obj2base T_gripper2base T_cam2gripper T_obj2cam拿到T_obj2base之后提取位置分量发给机械臂的move指令机械臂就能准确到达工件上方。如果要做完整的抓取还需要考虑机械臂末端工具夹爪、吸盘等的TCP偏移那是另一个工具标定的问题但手眼矩阵部分到这里就闭环了。我在项目里跑通之后又顺手做了个扩展实验给D435的深度图对齐到彩色图用深度值替换PnP解出来的距离信息验证了几次发现在60厘米左右的工作距离下深度图对齐后的坐标和PnP解算结果能对上误差在5毫米量级作为粗定位足够用了。正式抓取场合建议还是用标定板加PnP的方式做精定位。最后再分享一个经验手眼标定不是一次性的活如果你的相机和机械臂之间的安装连接因为碰撞或者拆装发生了变化原来标定的矩阵就不能用了需要重新标定。我习惯在最开始就给标定流程单独封装一个脚本随时可以重跑。这套流程跑熟了之后从装好相机到拿到验证通过的手眼矩阵一上午的时间就够了。