开头部分我想从实际场景切入——很多做机械臂避障的人卡住的第一关往往不是路径规划算法本身而是手眼标定。Moveit里的避障算法再先进给你的是一个建立在错误坐标系下的点云那一切规划都是空中楼阁。这篇文章就聚焦UR5机械臂配合深度相机做避障时绕不开的手眼标定环节完整走一遍从标定板准备、数据采集、数学求解到最终把点云对齐到机器人坐标系的整个流程。这套方法我在项目里反复用过多次也踩过不少坑。这篇东西既是给系列文章补上“感知坐标系的最后一块拼图”也可以单独拿出来当一份实操手册看。适合正在做机械臂抓取、避障、无序分拣或者任何需要把相机点云和机器人运动学关联起来的工程师参考。1. 手眼标定先搞清楚你这套系统是眼在手上还是眼在手外先说一个我见过太多人搞反的概念。手眼标定虽然叫“手眼”但手眼关系的核心不是相机和手之间物理距离多远而是相机坐标系和机械臂末端或基座坐标系之间的位姿变换关系。在做标定之前必须明确自己的系统属于哪种构型因为两种构型的数学模型和求解方式完全不同。1.1 两种经典构型的本质区别手眼标定分两种眼在手上eye-in-hand和眼在手外eye-to-hand。构型相机安装位置待求矩阵方程形式眼在手上固定在机械臂末端相机到末端的变换 (T_{cam}^{tool})(AX XB)眼在手外固定在工作空间外部相机到机械臂基座的变换 (T_{cam}^{base})(AX ZB)眼在手上的典型应用是近距离精细抓取或视觉伺服——相机跟着机械臂走靠近工件后才能看清细节泛光干扰少精度较高。但它有个天然问题相机视野随着机械臂运动而变化如果你做的是全工作空间的避障规划眼在手上的点云只覆盖局部很难为全局避障提供完整的环境信息。眼在手外则是把深度相机固定在工作空间上方或侧方类似监控视角一次能看到机械臂、工件和大部分障碍物。这个构型下待求的是相机坐标系到机械臂基座坐标系的固定变换矩阵只要标定一次后续相机不动、机器人不动变换关系就始终成立。1.2 避障场景下我为什么选眼在手外UR5配合Moveit做避障尤其是动态环境避障我强烈建议用眼在手外。原因有三第一全局感知完整性。避障算法的输入必须包含机械臂本身、目标物、障碍物三者的空间关系。眼在手外的俯视或斜视视角天然提供了整个场景的全局点云Moveit的规划场景Planning Scene可以直接用这份点云构建碰撞世界。第二标定之后稳定性高。眼在手外一次标定只要相机和机械臂底座没有相对位移变换矩阵一劳永逸。不像眼在手上每次更换末端工具都可能影响手眼关系需要重新标定。第三点云对齐逻辑直观。眼在手外时点云从相机坐标系变换到机器人基座坐标系是一个固定的4x4齐次变换矩阵。点云中的每个点直接表达了在机器人基座系下的三维坐标无论是喂给Moveit做碰撞检测还是自己写RRT、PRM的避障算法数据天然是“对齐好”的。当然眼在手外也有代价——机械臂自身会遮挡部分视野尤其是UR5这种六轴臂运动到某些构型时手臂本身会挡住目标点。这个问题我在后面点云处理部分会专门讲怎么用模型过滤来规避。1.3 本系列方案的硬件安装参考我自己的实机方案是这样的深度相机我用的是Intel RealSense D435这个系列深度图质量稳定SDK也成熟安装在UR5底座斜上方约1.5米处向下倾斜30度左右保证整个机械臂工作空间都在视野内。安装时有一点特别重要相机支架必须有足够的刚性。别用那种细长的万向臂或者塑料支架相机哪怕有1毫米的位移在1.5米工作距离下反映到点云上就是几个毫米的误差对标定结果的影响是灾难性的。建议用铝型材或者CNC加工的固定支架装好之后用记号笔在底座上做个标记方便日后检查相机是否被误碰移位。2. 数学原理不讲虚的AXXB与AXZB到底在解什么噪声从哪来很多人拿到OpenCV的calibrateHandEye函数就直接调但完全不清楚背后的数学模型导致出错了也不知道怎么排查。这一节把数学讲透。2.1 坐标变换链从标定板到机器人基座的闭环眼在手外的场景下我们每次采集数据时实际上建立了这样一条坐标链[ T_{board}^{cam} \rightarrow T_{cam}^{base} \rightarrow T_{end}^{base} \rightarrow T_{board}^{end} ]其中(T_{board}^{cam})标定板坐标系到相机坐标系的变换通过检测标定板角点并用solvePnP求得(T_{cam}^{base})就是我们要求的矩阵相机到机器人基座的固定变换(T_{end}^{base})机械臂末端到基座的变换直接从UR5的控制接口读取(T_{board}^{end})标定板坐标系到机械臂末端的变换这个值在标定过程中是常量——因为标定板固定不动机械臂末端无论如何运动标定板相对于末端的关系始终不变。这个“常量”约束就是我们能够列方程求解的核心。2.2 从推导看为什么要多采集位姿把上面的变换链从两个方向展开。第一次采集时机械臂末端在姿态1有[ T_{board}^{cam1} \cdot T_{cam}^{base} T_{end1}^{base} \cdot T_{board}^{end} ]第二次采集时机械臂末端在姿态2有[ T_{board}^{cam2} \cdot T_{cam}^{base} T_{end2}^{base} \cdot T_{board}^{end} ]两次等式右边都有同一个未知量 (T_{board}^{end})把它消掉[ (T_{end2}^{base})^{-1} \cdot T_{board}^{cam2} \cdot T_{cam}^{base} (T_{end1}^{base})^{-1} \cdot T_{board}^{cam1} \cdot T_{cam}^{base} ]整理成经典形式[ A \cdot X X \cdot B ]其中 (A (T_{end1}^{base})^{-1} \cdot T_{end2}^{base}) 是机械臂两次运动之间的变换(B T_{board}^{cam1} \cdot (T_{board}^{cam2})^{-1}) 是标定板两次在相机系下的变换(X T_{cam}^{base}) 是待求的手眼矩阵。单组方程有无穷多解所以必须采集多组不同位姿的数据构成超定方程组用最小二乘法求出最优解。这也是为什么网上所有手眼标定的教程都强调“至少采集15到20组位姿”。2.3 旋转和平移的噪声特性为什么平移更难标准手眼标定的求解通常分两步先解旋转部分再解平移部分。旋转部分的约束关系是 (R_A \cdot R_X R_X \cdot R_B)这个方程对旋转轴的约束比较强即使有噪声解出来的旋转矩阵也相对稳定。平移部分则是一个线性方程组对噪声极其敏感尤其是当机械臂两次运动的旋转角度太小时平移部分的病态程度会急剧上升。所以你在实际标定时会看到一个现象标定结果里旋转矩阵的误差可能只有零点几度但平移向量的误差可能达到一两厘米。这不一定是你代码写错了很可能是数据采集时旋转角度变化不够大、位姿太相似导致的。2.4 求解算法的选型参考OpenCV的cv2.calibrateHandEye提供了四种方法分别是Tsai-Lenz、Park、Horaud和Andreff。实际测试下来我推荐用Park方法methodcv2.CALIB_HAND_EYE_PARK它在旋转和平移的联合优化上表现比较均衡对噪声的鲁棒性比Tsai-Lenz好。如果追求速度且数据质量高Tsai-Lenz也够用。但别用Andreff——它虽然一次性联合求解旋转和平移但实际工程中遇到退化位姿时稳定性较差。3. 标定板和采集工作这一步做不好后面全是白费数学再漂亮数据采集不严谨结果照样一塌糊涂。这个环节我栽过好几次跟头把经验教训都整理出来。3.1 标定板选择ChArUco板为什么比棋盘格好用很多人直接打印一张棋盘格就开始标定然后发现图像边缘区域的角点检测经常失败。我的建议是直接用ChArUco板。ChArUco板结合了棋盘格和ArUco码的优点每个角点周围都有唯一的编码信息带来三个核心优势第一支持部分遮挡检测。棋盘格只要画面里有一个角点被机械臂遮挡整张板的角点提取就可能失败。ChArUco板只要能看到足够多的ArUco码就能重建所有角点的位置这在机械臂工作空间内采集数据时非常实用——因为机械臂本身就很容易挡到标定板。第二角点方向唯一。棋盘格的角点图案是对称的检测时可能出现方向歧义而ChArUco板每个角点有独特的编码上下文solvePnP的初始姿态估计更稳定。第三支持多板同屏。如果你工作空间大可以放多张ChArUco板同时检测扩展有效标定区域。打印ChArUco板时用cv2.aruco.generateImageMarker配合自定义字典生成然后使用激光打印别用喷墨。喷墨打印的黑白边界有晕染角点检测的亚像素精度会受影响。3.2 标定板的物理准备打印出来的板子不要直接拿在手里晃要贴在刚性平板上。我用的是3mm铝板表面贴哑光相纸再覆一层哑光膜防反光。如果板子有翘曲角点的世界坐标和实际物理位置就会有偏差直接引入系统误差。尺寸选择上标定板边长占图像对角线长度的1/3到1/2比较合适。我用的板子是8x6格子格子边长30mm配合D435在1.5米工作距离下使用角点清晰可辨。3.3 采集规则和数量我在采集数据时有一套固定的流程确认标定板固定不动——我直接用胶带把它贴在墙上或者用三脚架架住保证在整个采集过程中标定板纹丝不动。控制UR5末端不一定要夹爪用法兰盘上的参考尖点就行移动到标定板附近的多个位姿确保相机能看到标定板且标定板尽量出现在图像的不同区域。每个位姿下记录当前机械臂末端位姿从UR的RTDE接口读四元数xyz 当前深度相机的彩色图。至少采集20组数据每组之间机械臂的姿态变化要足够大——旋转角度差至少在30度以上最好混合不同高度、不同俯仰角。让标定板出现在图像的边缘区域也采集几组这能显著提高标定结果在全视野范围内的适用性。3.4 最容易忽略的不同步问题采集数据时相机图像和机械臂位姿必须是同一时刻的。如果你先拍10张图再让机械臂动10个位置然后去读位姿这数据就是废的。正确做法让机械臂在每个位姿停顿1到2秒然后同时触发相机采集和RTDE位姿读取。我用Python写了一个简单的同步脚本机械臂移动到目标位姿后发送一个ready信号相机和RTDE同时采样然后才运动到下一位姿。如果机械臂运动过程中有抖动或者还在微调读到的位姿和图像就不匹配。UR5的重复定位精度虽然很高但运动过程中的轨迹插值会导致位姿数据变化所以一定要等机械臂完全静止再采。3.5 相机内参手眼标定的隐藏前提做手眼标定之前必须先完成相机内参标定尤其是畸变系数否则solvePnP求出来的 (T_{board}^{cam}) 会带有系统性偏差手眼标定再怎么做精度也上不去。RealSense D435出厂自带内参但也要自己再标一遍更稳妥因为温度变化、镜片松动都可能导致内参漂移。用cv2.calibrateCamera对同一个ChArUco板采集20到30张不同角度的图像求出新的内参矩阵和畸变系数然后在后续所有代码里用这个自标定的内参别用出厂内置的。注意RealSense的深度图和彩色图有视差偏移采集时要用rs.align把深度图对齐到彩色图坐标系上否则后面点云和图像对不上手眼标定做了也白做。4. 完整的标定与点云对齐代码一步步照抄就能跑代码部分我直接给出完整流程每一步都有注释。用的环境是Ubuntu 20.04 Python 3.8 OpenCV 4.5 RealSense SDK 2.0 UR RTDE。4.1 数据采集脚本首先是一段采集数据的代码骨架它负责采集彩色图、对齐深度图、同步读取UR位姿。import cv2 import numpy as np import pyrealsense2 as rs from ur_rtde import rtde_receive # --- 初始化RealSense --- pipeline rs.pipeline() config rs.config() config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) profile pipeline.start(config) # 深度图对齐到彩色图 align rs.align(rs.stream.color) # --- 初始化UR RTDE --- rtde_r rtde_receive.RTDEReceive(192.168.1.10) # UR5 IP # --- 相机内参自标定结果 --- camera_matrix np.array([[930.0, 0, 640.0], [0, 930.0, 360.0], [0, 0, 1.0]]) dist_coeffs np.array([0.1, -0.05, 0.0, 0.0, 0.0]) # --- 生成ChArUco板 --- aruco_dict cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) board cv2.aruco.CharucoBoard((8, 6), 0.03, 0.02, aruco_dict) board_ids board.ids # 循环采集 save_rgb [] save_poses [] for i in range(30): input(f移动到第{i1}个位姿后按回车采集...) # 等机械臂静止 time.sleep(1.0) # 采集图像 frames pipeline.wait_for_frames() aligned_frames align.process(frames) color_frame aligned_frames.get_color_frame() color_image np.asanyarray(color_frame.get_data()) # 读取UR位姿TCP在法兰盘中心 tcp_pose rtde_r.getActualTCPPose() # [x, y, z, rx, ry, rz] 旋转向量形式 # 保存 save_rgb.append(color_image.copy()) save_poses.append(tcp_pose) print(f第{i1}组: tcp {tcp_pose}) np.save(rgb_images.npy, np.array(save_rgb)) np.save(tcp_poses.npy, np.array(save_poses))这一步要特别注意UR的RTDE返回的是旋转向量rx,ry,rz不是四元数也不是欧拉角。后面要么把旋转向量转旋转矩阵要么转四元数要看OpenCV的calibrateHandEye需要的格式。4.2 角点检测与solvePnP对每一张彩色图检测ChArUco角点然后用solvePnP求标定板坐标系到相机坐标系的变换。def detect_charuco_pose(color_image, camera_matrix, dist_coeffs, board): # 灰度化 gray cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) # 检测ArUco marker detector_params cv2.aruco.DetectorParameters() detector cv2.aruco.ArucoDetector(board.dictionary, detector_params) corners, ids, rejected detector.detectMarkers(gray) if ids is None or len(ids) 4: return None, None # 插值ChArUco角点 ret, charuco_corners, charuco_ids cv2.aruco.interpolateCornersCharuco( corners, ids, gray, board) if not ret or charuco_corners is None: return None, None # 如果角点太少放弃这一帧 if len(charuco_corners) 10: return None, None # 用PnP求标定板到相机的变换 obj_points board.getChessboardCorners() # 世界坐标系标定板系下的角点坐标 obj_points obj_points[charuco_ids.flatten()] retval, rvec, tvec cv2.solvePnP(obj_points, charuco_corners, camera_matrix, dist_coeffs) # 构建4x4变换矩阵 T_board_cam R, _ cv2.Rodrigues(rvec) T_board_cam np.eye(4) T_board_cam[:3, :3] R T_board_cam[:3, 3] tvec.flatten() return T_board_cam, charuco_corners小坑提醒ChArUco板的obj_points用的是角点索引来索引的charuco_ids是角点在板上的唯一编号用obj_points[charuco_ids.flatten()]才能对齐到正确的世界坐标。我一开始直接用了整个obj_points数组导致结果完全错误排查了快两小时。4.3 读取UR位姿并组装数据UR的RTDE返回的是[tcp_x, tcp_y, tcp_z, rx, ry, rz]其中旋转向量表示TCP相对于基座的旋转。转成4x4变换矩阵 (T_{end}^{base})。def to_transformation_matrix(tcp_pose): x, y, z, rx, ry, rz tcp_pose R, _ cv2.Rodrigues(np.array([rx, ry, rz])) T np.eye(4) T[:3, :3] R T[:3, 3] [x, y, z] return T # 遍历所有采集的数据 T_board_cam_list [] T_end_base_list [] for i in range(len(save_rgb)): T_bc, corners detect_charuco_pose(save_rgb[i], camera_matrix, dist_coeffs, board) if T_bc is None: continue T_eb to_transformation_matrix(save_poses[i]) T_board_cam_list.append(T_bc) T_end_base_list.append(T_eb) print(f成功检测到标定板的帧数: {len(T_board_cam_list)})4.4 调用calibrateHandEye求解手眼矩阵眼在手外构型下OpenCV的cv2.calibrateHandEye函数要求输入的是机器人末端到基座的变换从基座看末端和标定板到相机的变换从相机看标定板然后直接输出T_cam_base。等等这里有个容易搞混的关键点。cv2.calibrateHandEye的官方用法是针对眼在手上的参数是R_gripper2base和t_gripper2base末端到基座以及R_target2cam和t_target2cam标定板到相机输出是R_cam2gripper和t_cam2gripper即相机到末端的变换。对于眼在手外我们需要的是T_cam2base也就是相机到基座的变换。OpenCV没有直接提供eye-to-hand的专用接口但是数学变换是等价的只需要把参数做些调整。眼在手外时方程改为 (AX ZB)。但如果你把“标定板固定在环境中”这个事实和“相机固定在环境中”这个事实互换角色实际上可以把眼在手外转换成一个眼在手上的求逆问题。具体做法把标定板想象成“假机械臂末端”把相机想象成“假标定板”。这时候待求矩阵 (X T_{base}^{cam} (T_{cam}^{base})^{-1})。也就是说把R_gripper2base传标定板的旋转矩阵因为是标定板在动——“假末端”R_target2cam传相机在基座系的旋转——但相机在基座系下是固定的我们不知道。这是行不通的。换个思路。变换链 (T_{board}^{cam} \cdot T_{cam}^{base} T_{end}^{base} \cdot T_{board}^{end})我把它改写成[ T_{cam}^{base} (T_{board}^{cam})^{-1} \cdot T_{end}^{base} \cdot T_{board}^{end} ]或者把未知的 (T_{board}^{end}) 消掉变成[ (T_{board}^{cam2})^{-1} \cdot T_{board}^{cam1} \cdot T_{cam}^{base} T_{end2}^{base} \cdot (T_{end1}^{base})^{-1} \cdot T_{cam}^{base} ]整理成 (A \cdot X X \cdot B) 的形式后可以直接复用[ A_i (T_{end_{i1}}^{base})^{-1} \cdot T_{end_i}^{base} ] [ B_i (T_{board_{i1}}^{cam})^{-1} \cdot T_{board_i}^{cam} ]注意这里的自变量是 (T_{cam}^{base})和之前推导时定义的 (A)、(B) 正好互换了位置但形式仍然是 (AX XB)。OpenCV的calibrateHandEye照样可以解但参数传入要小心# 组装连续两帧的相对运动 A_list [] # 机械臂末端的相对运动从第i1帧到第i帧 B_list [] # 标定板的相对运动从第i1帧到第i帧 for i in range(len(T_end_base_list) - 1): # 机械臂末端从位姿i1到位姿i的相对变换从base系下看 A_i np.linalg.inv(T_end_base_list[i1]) T_end_base_list[i] # 标定板从位姿i1到位姿i的相对变换从cam系下看 B_i np.linalg.inv(T_board_cam_list[i1]) T_board_cam_list[i] A_list.append(A_i) B_list.append(B_i) # 分离旋转和平移 R_A [A[:3, :3] for A in A_list] t_A [A[:3, 3] for A in A_list] R_B [B[:3, :3] for B in B_list] t_B [B[:3, 3] for B in B_list] # 求解 R_X, t_X cv2.calibrateHandEye( R_A, t_A, R_B, t_B, methodcv2.CALIB_HAND_EYE_PARK ) T_cam_base np.eye(4) T_cam_base[:3, :3] R_X T_cam_base[:3, 3] t_X.flatten() print(相机到基座的变换矩阵) print(T_cam_base)我还是把完整的更稳做法写一遍以避免直接对calibrateHandEye的输入输出约定产生歧义。在OpenCV的资料里函数输出的是R_cam2gripper和t_cam2gripper。对眼在手外等价变换后可以求得T_gripper2cam最后再对整体取逆即可。代码如下# 注意这里R_gripper2base传的是T_end_base末端到基座 # R_target2cam传的是T_board_cam标定板到相机 R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( [T_end_base_list[i][:3, :3] for i in range(len(T_end_base_list))], [T_end_base_list[i][:3, 3] for i in range(len(T_end_base_list))], [T_board_cam_list[i][:3, :3] for i in range(len(T_board_cam_list))], [T_board_cam_list[i][:3, 3] for i in range(len(T_board_cam_list))], methodcv2.CALIB_HAND_EYE_PARK ) # 直接用这个结果就是T_cam2gripper但我们要的是T_cam2base # 利用公式 T_cam_base T_gripper_base T_cam_gripper T_gripper_base T_end_base_list[0] # 用任意一帧的末端位姿 T_cam_gripper np.eye(4) T_cam_gripper[:3, :3] R_cam2gripper T_cam_gripper[:3, 3] t_cam2gripper.flatten() # T_cam_base T_gripper_base T_cam_gripper T_cam_base T_gripper_base T_cam_gripper等等这里要重新捋一下。T_cam_gripper是相机坐标系下末端的位置——不对R_cam2gripper表示的是“在gripper坐标系下表达的cam坐标系的旋转”所以T_cam_gripper实际是“cam在gripper系下的位姿”即 (T_{cam}^{gripper})。那 (T_{cam}^{base} T_{end}^{base} \cdot T_{cam}^{end}) 对任意一帧都成立因为cam和base都是固定的end是变化的但等式右边的两个矩阵相乘结果应该一致。所以T_base_gripper T_end_base_list[0] # 即T_end_base # T_cam_base T_end_base T_cam_end T_cam_end T_cam_gripper # 注意这里T_cam_gripper T_cam_end T_cam_base T_end_base_list[0] T_cam_end但T_cam_end这个名字在OpenCV输出是T_cam2gripper含义是“cam在gripper系下的位姿”与T_cam2gripper符号一致。OK这样写应该没问题。我建议在做完求解后用一个独立的验证流程反向检查一遍见第5章。4.5 点云变换到机器人基座坐标系拿到T_cam_base后点云的变换就是一个齐次坐标乘法的问题。def transform_pointcloud(points_cam, T_cam_base): points_cam: (N, 3) 相机坐标系下的点云 T_cam_base: (4, 4) 相机到机器人基座的变换矩阵 # 转齐次坐标 ones np.ones((points_cam.shape[0], 1)) points_hom np.hstack([points_cam, ones]) # (N, 4) # 变换 points_base (T_cam_base points_hom.T).T # (N, 4) # 转回3维 points_base points_base[:, :3] return points_base从RealSense获取点云时注意深度图的坐标系是相机光学坐标系Z轴朝前而OpenCV的坐标系是图像坐标系Z轴朝前但Y轴朝下。如果你直接用rs2::pointcloud生成的点云xyz都在相机坐标系下直接用上面的变换即可。如果是自己从深度图生成点云要特别注意坐标轴方向的约定否则会出现点云上下颠倒或者前后翻转的问题。RealSense SDK的rs2.deproject_pixel_to_point返回的点已经是相机坐标系下的3D坐标直接用。4.6 在Moveit中显示对齐后的点云点云变换到基座坐标系后就可以直接发布到Moveit的Planning Scene中用于避障。import moveit_commander from moveit_msgs.msg import CollisionObject, PlanningScene import rospy def publish_pointcloud_to_moveit(points_base): rospy.init_node(publish_cloud_to_scene) planning_scene PlanningScene() # 构造CollisionObject用点云表示障碍物 co CollisionObject() co.id obstacle_cloud co.header.frame_id base_link co.header.stamp rospy.Time.now() # 用octomap或者直接mesh表示 # 这里简化直接把点云发布为PointCloud2消息 pub rospy.Publisher(/pointcloud_in_base, PointCloud2, queue_size1) # ... 发布逻辑更推荐的做法是把点云转成OctoMap再加载到Planning Scene中因为Moveit的碰撞检测对原始点云需要构建FCL网格计算量较大。OctoMap体素化之后既能保留障碍物轮廓又能大幅降低碰撞检测的计算开销。下一篇文章我会专门讲这部分。5. 标定验证与翻车现场别只盯着重投影误差标定完成只是第一步验证标定结果是否可靠才是真正拉开差距的地方。很多人在这一步草草了事直接用标定结果去跑避障结果机械臂撞了才发现标定有问题回头还得排查是标定错还是避障算法错。我分享一套我自己常用的验证链路。5.1 第一关重投影误差检验把求解出的T_cam_base代回到变换链中对每一帧数据重新计算标定板角点的重投影位置看误差有多大。def check_reprojection_error(T_cam_base, T_end_base_list, T_board_cam_list): errors [] for i in range(len(T_end_base_list)): # 从T_cam_base和T_end_base反推T_board_end T_board_end_est np.linalg.inv(T_end_base_list[i]) T_cam_base T_board_cam_list[i] # T_board_end应该是一个常量因为板和末端都固定 # 这里用所有帧的平均值作为真值 # ... # 或者换个更直接的方法 # 用T_end_base和T_cam_base预测标定板在相机系下的位姿 T_board_cam_pred np.linalg.inv(T_cam_base) T_end_base_list[i] T_board_end_mean # 然后把T_board_cam_pred投影到图像平面和检测到的角点做比对 return np.mean(errors)通常重投影误差在1像素以内说明标定解算没问题如果超过3像素说明数据采集中存在较大不一致要回头检查是否有不同步帧、标定板是否发生位移、UR位姿读数是否准确等。5.2 第二关空间点验证重投影只能验证标定结果在“图像平面”上自洽但无法暴露深度方向Z轴的误差。更严格的办法是用机械臂TCP去触碰点云中的空间点。具体操作把标定板放在工作空间某个位置让UR5末端装一个尖锥工具去触碰标定板上的某个角点。从机器人示教器读到这个角点在机器人基座坐标系下的坐标 (P_{base}^{true})。同时用深度相机拍下这个角点通过相机内参和深度值得到它在相机坐标系下的坐标 (P_{cam}^{meas})。用标定得到的 (T_{cam}^{base}) 把 (P_{cam}^{meas}) 变换到基座系和 (P_{base}^{true}) 比对。误差在5毫米以内基本可接受10毫米以上就需要重新标定。这个测试最好在工作空间的多个区域做因为手眼标定结果在不同区域精度会有差异。5.3 翻车现场一位姿不同步导致的整体偏移症状标定出来的旋转矩阵看起来合理但平移向量明显偏大或者偏小整体点云位置偏移。原因排查相机图像和UR位姿不是在同一时刻采集的机械臂运动中微小的位姿变化被当成了静态度量。特征就是误差方向随机械臂运动方向变化——机械臂向X运动时误差偏向-X运动时误差偏-。修复确保机械臂在每个位姿完全静止后再采集或者使用更可靠的同步机制比如RTDE和相机在同一线程里用time.time()打时间戳对齐。5.4 翻车现场二标定板太小导致角点检测不稳定症状标定板检测帧率低检测到的角点数忽多忽少标定结果跳动大。原因工作距离太远标定板在图像中占比过小ArUco码解码不稳定。修复换大标定板或者把相机靠近一点重新安装。保证标定板在图像中占对角线长度的1/3到1/2ArUco码在图像中至少要覆盖20x20像素。5.5 翻车现场三退化位姿组合导致解算崩溃症状标定结果误差很大而且每次运行结果都不一样。原因采集时机械臂位姿变化太小特别是旋转角度变化不够。想象一下如果机械臂末端每次都只是平移了一点点所有数据几乎都在描述同一个视角这时候矩阵方程的约束条件不够会出现退化现象。修复采集数据时确保机械臂的俯仰角和偏航角有足够大的变化范围至少30度以上。最好设计一套标准采集路径——机械臂在标定板前方画一个球面轨迹每次指向标定板的角度都有明显差异。5.6 翻车现场四UR的TCP和实际末端工具不匹配症状如果机械臂末端装了夹爪或其他工具直接读TCP位姿法兰盘中心可能和你期望的“手眼”参考点不一致。修复标定时有两种选择——要么把工具坐标系的TCP设到法兰盘中心保证RTDE读出来的就是法兰盘位姿要么在UR上配置好工具坐标系让RTDE读出来的TCP包含工具偏移。关键是手眼标定公式里的(T_{end}^{base})必须和实际使用的坐标系一致。如果你要算的是“相机到基座”的关系那么(T_{end}^{base})直接用法兰盘位姿即可后续做点云对齐也不涉及末端工具。但如果你要算“相机到工具”的关系就需要把工具坐标系的值传入。5.7 翻车现场五深度图和彩色图没对齐症状点云在物体边缘有严重的“飞点”或者点云位置和彩色图看起来对不上。原因RealSense的深度传感器和RGB传感器物理位置不同视角有视差。如果不做rs.align直接用深度图生成的点云会和彩色图的坐标系有偏差导致PnP算出来的位姿不准。修复在采集数据之前就做深度图对齐并且对齐后的深度图在尺寸上和彩色图保持一致。标定板角点的像素坐标用于PnP深度值用于空间定位两者必须对应同一个坐标系。6. 进阶优化把标定精度从“能跑”提升到“好用”完成基本标定后如果精度还满足不了避障需求还可以从几个方向继续优化。6.1 多点平均法不要只做一次标定就完事可以采集两到三次独立的数据集分别标定得到多个T_cam_base然后对旋转矩阵求平均把旋转矩阵转四元数后球面平均或者直接对数映射平均对平移向量求加权平均。这样做的好处是可以规避单次采集中偶发的异常数据帧。我实测中三次标定结果如果差异很大旋转超过1度或平移超过5毫米说明数据采集过程有问题需要先排查再继续。6.2 非线性优化精修OpenCV的calibrateHandEye只做了两步线性求解可以在此基础上再用非线性优化比如Levenberg-Marquardt把重投影误差作为代价函数进一步精修。from scipy.optimize import least_squares def cost_function(params, T_end_base_list, T_board_cam_list): # 把T_cam_base参数化旋转向量平移 rvec params[:3] tvec params[3:] R, _ cv2.Rodrigues(rvec) T_cam_base np.eye(4) T_cam_base[:3, :3] R T_cam_base[:3, 3] tvec # 用T_board_end的一致性构造误差 errors [] T_board_end_list [] for i in range(len(T_end_base_list)): T_board_end np.linalg.inv(T_end_base_list[i]) T_cam_base T_board_cam_list[i] T_board_end_list.append(T_board_end) mean_T_board_end np.mean(T_board_end_list, axis0) for T_board_end in T_board_end_list: err (T_board_end[:3, :3] - mean_T_board_end[:3, :3]).flatten() err np.append(err, T_board_end[:3, 3] - mean_T_board_end[:3, 3]) errors.extend(err) return np.array(errors) # 初始值用OpenCV的结果 init_params np.hstack([cv2.Rodrigues(T_cam_base[:3, :3])[0].flatten(), T_cam_base[:3, 3]]) result least_squares(cost_function, init_params, args(T_end_base_list, T_board_cam_list))这套优化代码不需要单独安装什么库scipy就够了。优化后我一般能看到重投影误差下降20%到30%。但要注意非线性优化只能在数据本身一致性好时发挥作用如果采集时有几帧不同步的数据优化反而会把整体结果带偏。6.3 基于标定结果的动态检查标定完成后我习惯在工作空间固定几个标记点比如贴几个ArUco码在桌面上每次开机后让机械臂末端去触碰这些标记点快速验证标定结果是否仍然有效。如果发现误差突然变大优先检查相机支架是否被碰歪用水平尺和激光笔检查相机螺丝是否松动RealSense的镜头外壳很容易因热胀冷缩松动工作环境温度是否变化过大温度变化会导致相机内参漂移这一套检查流程只要3分钟但能省下后面排查了一整天才发现标定失效的时间。7. 避障链路中的点云预处理标定完了不等于能直接用标定结果正确是点云能到机器人坐标系的前提但避障算法真正需要的是干净、降噪、结构化的场景数据。直接拿原始点云去跑避障效果会惨不忍睹。7.1 直通滤波把采集范围缩小到工作空间相机视野里通常包含大量无关信息远处的墙壁、地面、各种杂物。这些点云如果不过滤掉不仅会增加Moveit碰撞检测的计算负担还可能被误识别为障碍物导致规划失败。def passthrough_filter(points_base, x_range, y_range, z_range): mask ( (points_base[:, 0] x_range[0]) (points_base[:, 0] x_range[1]) (points_base[:, 1] y_range[0]) (points_base[:, 1] y_range[1]) (points_base[:, 2] z_range[0]) (points_base[:, 2] z_range[1]) ) return points_base[mask]UR5的工作范围是一个半径约850mm的球体我会把直通滤波的范围设置成比这个稍大一圈的立方体区域既保证机械臂可达范围内的障碍物全部被保留又过滤掉远处无用点云。7.2 体素滤波降采样RealSense D435在1280x720分辨率下每帧产生约92万个点。全量点云直接用于碰撞检测FCL的碰撞检测开销会非常大Moveit的规划周期会被拖到几秒甚至更久。降采样是必须的。import open3d as o3d def voxel_downsample(points_base, voxel_size0.01): pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(points_base) pcd pcd.voxel_down_sample(voxel_size) return np.asarray(pcd.points)voxel_size取**0.01m1厘米**对避障来说完全够用机械臂末端碰不到障碍物的精度要求在厘米级就够了。体素滤波后点云数量能降到2到5万点碰撞检测速度大幅提升。7.3 去噪和动目标处理深度相机最烦人的问题是飞点和边缘拖影。RealSense可以用rs.temporal_filter和rs.hole_filling_filter做预处理能有效减少空洞和飞点。对于动态障碍物比如人或者移动的料车我建议让避障系统以低频周期性更新点云比如2Hz而不是每帧都更新。这样既能跟上动态环境变化又不会因为点云抖动导致Moveit反复重新规划。7.4 机械臂自遮挡的处理眼在手外构型下机械臂本身会出现在点云中。如果直接把包含机械臂的点云传给Moveit做避障Moveit会认为自己和自己碰撞导致规划失败。解决方案有两种检测并删除机械臂自身点云可以通过Moveit的Planning Scene拿到当前机械臂各连杆的包围盒或网格模型然后把落在这些区域内的点云删除。用PassThrough或裁剪区域手动抠除机械臂区域在固定安装下这个区域是确定的可以直接写死。第一种方法更通用复杂度也更高我在下一篇避障实战系列里会专门写这部分。这里先提个醒看到机械臂点云出现在Moveit障碍物里不要惊慌这是正常现象需要过滤。8. 一次完整的标定实战从数据采集到点云对齐的流程记录我在这台UR5上的整个标定流程从准备到出结果大约是40分钟。如果你按照本文的步骤走第一次可能需要两三个小时但多来几次熟练后时间会大幅缩短。整个流程梳理如下表步骤操作预计耗时关键检查点1固定相机和标定板检查相机支架刚性10分钟相机不能有任何晃动2相机内参自标定10分钟重投影误差小于0.5像素3采集30组数据10分钟每组要静止1秒以上再采集4检测角点检查检测成功率2分钟检测率100%除个别极端角度5calibrateHandEye求解1秒得到T_cam_base6重投影验证1分钟误差小于1像素7空间点验证5分钟用尖锥触碰标定板角点误差5mm内8点云变换检查1分钟点云能否落在机器人坐标系中正确位置实际上我在做的时候经常会跳过第6步直接做第7步因为空间点验证更直观。但是空间点验证依赖一个前提你要能精确控制机械臂的TCP去碰角点。对UR5来说用示教器手动移动TCP到一个已知点有可能存在人为误差更精确的做法是用运动学正解计算。自己做验证时我发现一个比较实用的技巧在机器人基座附近贴一张小ArUco码然后用相机拍这张ArUco码通过它来反推相机到基座的变换再和标定结果对比。这样做虽然精度不如TCP触碰法但速度快、不需要示教器操作适合日常巡检。9. 踩坑记录三次典型标定失败案例全复盘这里整理几个我在过去项目里踩过的坑有些隐蔽到排查了两三天。9.1 那次让我怀疑人生的问题相机内参是错的有一段时期我的标定重投影误差一直稳定在一个偏大的值2到3像素怎么调采集姿势都没用。后来我仔细检查了一下发现用的是RealSense出厂内参但相机出厂内参标定时是在特定温度和环境光下测的和我现场环境差异较大。用自标定内参后重投影误差从2.5像素降到了0.6像素。这个案例告诉我内参自标定不是可选项是必选项特别是RealSense这种受温度影响较大的设备。9.2 UR和相机的时钟不同步问题有一次我换了台电脑跑采集程序结果标定出来平移向量比之前偏大了将近两厘米。排查了很久最后发现问题出在采集同步上——新电脑跑起来之后RTDE读位姿的线程和相机采图的线程调度时序变了导致位姿滞后于图像约几百毫秒。修复方案把采集代码改成单线程串行模式机械臂完全静止后再依次采集位姿和图像不要用多线程并发。降低采集频率但保证每一帧数据都是可靠的。这比采集速度快但数据一致性差强得多。9.3 用了工具坐标系导致的手眼关系错乱前面5.6提到过如果机械臂装了夹爪但RTDE读的还是法兰盘位姿而你在其他地方用了工具位姿两者混用会让整个变换链对不上。我当时是给UR配置了Tool0下的一个自定义工具坐标系RTDE返回TCP位姿时返回的是工具位姿而不是法兰盘位姿但后续代码里基于法兰盘的假设做变换导致标定结果差出一个工具偏移量。血的教训代码里所有变换的参考坐标系必须统一RTDE读取的到底是法兰盘还是要工具坐标系的位姿在写代码前就要明确。10. 本系列的衔接与后续计划手眼标定这一篇搞定后整个避障系统的感知链路就打通了机械臂能够通过深度相机“看见”自己周围的障碍物并且知道障碍物在机器人坐标系下的精确位置。基于标定好的T_cam_base下一步就可以做这些事情点云建模并发布到Moveit的Planning Scene让OMPL等规划器在真实的障碍物环境中搜索无碰撞路径。把RGB图像结合点云做目标检测和姿态估计喂给机械臂做抓取或喷涂等任务。在点云中加入动态环境的实时检测比如人的位置做动态避障。下一篇我会写基于Moveit的点云构建与动态避障实战重点讲怎么把深度相机获取的点云高效地整合进Moveit的规划场景如何处理机械臂自遮挡以及实测中RRT-Connect和PRM在动态环境下的表现差异。如果你正在做UR5避障或者类似的机械臂项目建议把这一篇的手眼标定步骤先跑通后面内容都建立在这个坐标变换基础上。分享一个我自己的习惯每次标定完我会在工控机上保留一份标定日志记录日期、环境温度、重投影误差、空间点验证误差、操作人等信息。这样如果日后出现标定失效我可以快速定位是环境变化还是硬件位移导致的问题。这个习惯帮我节省了大量排查时间推荐你也试试。 UR5机械臂手眼标定实战：从深度相机到机器人坐标系的点云对齐全流程
开头部分我想从实际场景切入——很多做机械臂避障的人卡住的第一关往往不是路径规划算法本身而是手眼标定。Moveit里的避障算法再先进给你的是一个建立在错误坐标系下的点云那一切规划都是空中楼阁。这篇文章就聚焦UR5机械臂配合深度相机做避障时绕不开的手眼标定环节完整走一遍从标定板准备、数据采集、数学求解到最终把点云对齐到机器人坐标系的整个流程。这套方法我在项目里反复用过多次也踩过不少坑。这篇东西既是给系列文章补上“感知坐标系的最后一块拼图”也可以单独拿出来当一份实操手册看。适合正在做机械臂抓取、避障、无序分拣或者任何需要把相机点云和机器人运动学关联起来的工程师参考。1. 手眼标定先搞清楚你这套系统是眼在手上还是眼在手外先说一个我见过太多人搞反的概念。手眼标定虽然叫“手眼”但手眼关系的核心不是相机和手之间物理距离多远而是相机坐标系和机械臂末端或基座坐标系之间的位姿变换关系。在做标定之前必须明确自己的系统属于哪种构型因为两种构型的数学模型和求解方式完全不同。1.1 两种经典构型的本质区别手眼标定分两种眼在手上eye-in-hand和眼在手外eye-to-hand。构型相机安装位置待求矩阵方程形式眼在手上固定在机械臂末端相机到末端的变换 (T_{cam}^{tool})(AX XB)眼在手外固定在工作空间外部相机到机械臂基座的变换 (T_{cam}^{base})(AX ZB)眼在手上的典型应用是近距离精细抓取或视觉伺服——相机跟着机械臂走靠近工件后才能看清细节泛光干扰少精度较高。但它有个天然问题相机视野随着机械臂运动而变化如果你做的是全工作空间的避障规划眼在手上的点云只覆盖局部很难为全局避障提供完整的环境信息。眼在手外则是把深度相机固定在工作空间上方或侧方类似监控视角一次能看到机械臂、工件和大部分障碍物。这个构型下待求的是相机坐标系到机械臂基座坐标系的固定变换矩阵只要标定一次后续相机不动、机器人不动变换关系就始终成立。1.2 避障场景下我为什么选眼在手外UR5配合Moveit做避障尤其是动态环境避障我强烈建议用眼在手外。原因有三第一全局感知完整性。避障算法的输入必须包含机械臂本身、目标物、障碍物三者的空间关系。眼在手外的俯视或斜视视角天然提供了整个场景的全局点云Moveit的规划场景Planning Scene可以直接用这份点云构建碰撞世界。第二标定之后稳定性高。眼在手外一次标定只要相机和机械臂底座没有相对位移变换矩阵一劳永逸。不像眼在手上每次更换末端工具都可能影响手眼关系需要重新标定。第三点云对齐逻辑直观。眼在手外时点云从相机坐标系变换到机器人基座坐标系是一个固定的4x4齐次变换矩阵。点云中的每个点直接表达了在机器人基座系下的三维坐标无论是喂给Moveit做碰撞检测还是自己写RRT、PRM的避障算法数据天然是“对齐好”的。当然眼在手外也有代价——机械臂自身会遮挡部分视野尤其是UR5这种六轴臂运动到某些构型时手臂本身会挡住目标点。这个问题我在后面点云处理部分会专门讲怎么用模型过滤来规避。1.3 本系列方案的硬件安装参考我自己的实机方案是这样的深度相机我用的是Intel RealSense D435这个系列深度图质量稳定SDK也成熟安装在UR5底座斜上方约1.5米处向下倾斜30度左右保证整个机械臂工作空间都在视野内。安装时有一点特别重要相机支架必须有足够的刚性。别用那种细长的万向臂或者塑料支架相机哪怕有1毫米的位移在1.5米工作距离下反映到点云上就是几个毫米的误差对标定结果的影响是灾难性的。建议用铝型材或者CNC加工的固定支架装好之后用记号笔在底座上做个标记方便日后检查相机是否被误碰移位。2. 数学原理不讲虚的AXXB与AXZB到底在解什么噪声从哪来很多人拿到OpenCV的calibrateHandEye函数就直接调但完全不清楚背后的数学模型导致出错了也不知道怎么排查。这一节把数学讲透。2.1 坐标变换链从标定板到机器人基座的闭环眼在手外的场景下我们每次采集数据时实际上建立了这样一条坐标链[ T_{board}^{cam} \rightarrow T_{cam}^{base} \rightarrow T_{end}^{base} \rightarrow T_{board}^{end} ]其中(T_{board}^{cam})标定板坐标系到相机坐标系的变换通过检测标定板角点并用solvePnP求得(T_{cam}^{base})就是我们要求的矩阵相机到机器人基座的固定变换(T_{end}^{base})机械臂末端到基座的变换直接从UR5的控制接口读取(T_{board}^{end})标定板坐标系到机械臂末端的变换这个值在标定过程中是常量——因为标定板固定不动机械臂末端无论如何运动标定板相对于末端的关系始终不变。这个“常量”约束就是我们能够列方程求解的核心。2.2 从推导看为什么要多采集位姿把上面的变换链从两个方向展开。第一次采集时机械臂末端在姿态1有[ T_{board}^{cam1} \cdot T_{cam}^{base} T_{end1}^{base} \cdot T_{board}^{end} ]第二次采集时机械臂末端在姿态2有[ T_{board}^{cam2} \cdot T_{cam}^{base} T_{end2}^{base} \cdot T_{board}^{end} ]两次等式右边都有同一个未知量 (T_{board}^{end})把它消掉[ (T_{end2}^{base})^{-1} \cdot T_{board}^{cam2} \cdot T_{cam}^{base} (T_{end1}^{base})^{-1} \cdot T_{board}^{cam1} \cdot T_{cam}^{base} ]整理成经典形式[ A \cdot X X \cdot B ]其中 (A (T_{end1}^{base})^{-1} \cdot T_{end2}^{base}) 是机械臂两次运动之间的变换(B T_{board}^{cam1} \cdot (T_{board}^{cam2})^{-1}) 是标定板两次在相机系下的变换(X T_{cam}^{base}) 是待求的手眼矩阵。单组方程有无穷多解所以必须采集多组不同位姿的数据构成超定方程组用最小二乘法求出最优解。这也是为什么网上所有手眼标定的教程都强调“至少采集15到20组位姿”。2.3 旋转和平移的噪声特性为什么平移更难标准手眼标定的求解通常分两步先解旋转部分再解平移部分。旋转部分的约束关系是 (R_A \cdot R_X R_X \cdot R_B)这个方程对旋转轴的约束比较强即使有噪声解出来的旋转矩阵也相对稳定。平移部分则是一个线性方程组对噪声极其敏感尤其是当机械臂两次运动的旋转角度太小时平移部分的病态程度会急剧上升。所以你在实际标定时会看到一个现象标定结果里旋转矩阵的误差可能只有零点几度但平移向量的误差可能达到一两厘米。这不一定是你代码写错了很可能是数据采集时旋转角度变化不够大、位姿太相似导致的。2.4 求解算法的选型参考OpenCV的cv2.calibrateHandEye提供了四种方法分别是Tsai-Lenz、Park、Horaud和Andreff。实际测试下来我推荐用Park方法methodcv2.CALIB_HAND_EYE_PARK它在旋转和平移的联合优化上表现比较均衡对噪声的鲁棒性比Tsai-Lenz好。如果追求速度且数据质量高Tsai-Lenz也够用。但别用Andreff——它虽然一次性联合求解旋转和平移但实际工程中遇到退化位姿时稳定性较差。3. 标定板和采集工作这一步做不好后面全是白费数学再漂亮数据采集不严谨结果照样一塌糊涂。这个环节我栽过好几次跟头把经验教训都整理出来。3.1 标定板选择ChArUco板为什么比棋盘格好用很多人直接打印一张棋盘格就开始标定然后发现图像边缘区域的角点检测经常失败。我的建议是直接用ChArUco板。ChArUco板结合了棋盘格和ArUco码的优点每个角点周围都有唯一的编码信息带来三个核心优势第一支持部分遮挡检测。棋盘格只要画面里有一个角点被机械臂遮挡整张板的角点提取就可能失败。ChArUco板只要能看到足够多的ArUco码就能重建所有角点的位置这在机械臂工作空间内采集数据时非常实用——因为机械臂本身就很容易挡到标定板。第二角点方向唯一。棋盘格的角点图案是对称的检测时可能出现方向歧义而ChArUco板每个角点有独特的编码上下文solvePnP的初始姿态估计更稳定。第三支持多板同屏。如果你工作空间大可以放多张ChArUco板同时检测扩展有效标定区域。打印ChArUco板时用cv2.aruco.generateImageMarker配合自定义字典生成然后使用激光打印别用喷墨。喷墨打印的黑白边界有晕染角点检测的亚像素精度会受影响。3.2 标定板的物理准备打印出来的板子不要直接拿在手里晃要贴在刚性平板上。我用的是3mm铝板表面贴哑光相纸再覆一层哑光膜防反光。如果板子有翘曲角点的世界坐标和实际物理位置就会有偏差直接引入系统误差。尺寸选择上标定板边长占图像对角线长度的1/3到1/2比较合适。我用的板子是8x6格子格子边长30mm配合D435在1.5米工作距离下使用角点清晰可辨。3.3 采集规则和数量我在采集数据时有一套固定的流程确认标定板固定不动——我直接用胶带把它贴在墙上或者用三脚架架住保证在整个采集过程中标定板纹丝不动。控制UR5末端不一定要夹爪用法兰盘上的参考尖点就行移动到标定板附近的多个位姿确保相机能看到标定板且标定板尽量出现在图像的不同区域。每个位姿下记录当前机械臂末端位姿从UR的RTDE接口读四元数xyz 当前深度相机的彩色图。至少采集20组数据每组之间机械臂的姿态变化要足够大——旋转角度差至少在30度以上最好混合不同高度、不同俯仰角。让标定板出现在图像的边缘区域也采集几组这能显著提高标定结果在全视野范围内的适用性。3.4 最容易忽略的不同步问题采集数据时相机图像和机械臂位姿必须是同一时刻的。如果你先拍10张图再让机械臂动10个位置然后去读位姿这数据就是废的。正确做法让机械臂在每个位姿停顿1到2秒然后同时触发相机采集和RTDE位姿读取。我用Python写了一个简单的同步脚本机械臂移动到目标位姿后发送一个ready信号相机和RTDE同时采样然后才运动到下一位姿。如果机械臂运动过程中有抖动或者还在微调读到的位姿和图像就不匹配。UR5的重复定位精度虽然很高但运动过程中的轨迹插值会导致位姿数据变化所以一定要等机械臂完全静止再采。3.5 相机内参手眼标定的隐藏前提做手眼标定之前必须先完成相机内参标定尤其是畸变系数否则solvePnP求出来的 (T_{board}^{cam}) 会带有系统性偏差手眼标定再怎么做精度也上不去。RealSense D435出厂自带内参但也要自己再标一遍更稳妥因为温度变化、镜片松动都可能导致内参漂移。用cv2.calibrateCamera对同一个ChArUco板采集20到30张不同角度的图像求出新的内参矩阵和畸变系数然后在后续所有代码里用这个自标定的内参别用出厂内置的。注意RealSense的深度图和彩色图有视差偏移采集时要用rs.align把深度图对齐到彩色图坐标系上否则后面点云和图像对不上手眼标定做了也白做。4. 完整的标定与点云对齐代码一步步照抄就能跑代码部分我直接给出完整流程每一步都有注释。用的环境是Ubuntu 20.04 Python 3.8 OpenCV 4.5 RealSense SDK 2.0 UR RTDE。4.1 数据采集脚本首先是一段采集数据的代码骨架它负责采集彩色图、对齐深度图、同步读取UR位姿。import cv2 import numpy as np import pyrealsense2 as rs from ur_rtde import rtde_receive # --- 初始化RealSense --- pipeline rs.pipeline() config rs.config() config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) profile pipeline.start(config) # 深度图对齐到彩色图 align rs.align(rs.stream.color) # --- 初始化UR RTDE --- rtde_r rtde_receive.RTDEReceive(192.168.1.10) # UR5 IP # --- 相机内参自标定结果 --- camera_matrix np.array([[930.0, 0, 640.0], [0, 930.0, 360.0], [0, 0, 1.0]]) dist_coeffs np.array([0.1, -0.05, 0.0, 0.0, 0.0]) # --- 生成ChArUco板 --- aruco_dict cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) board cv2.aruco.CharucoBoard((8, 6), 0.03, 0.02, aruco_dict) board_ids board.ids # 循环采集 save_rgb [] save_poses [] for i in range(30): input(f移动到第{i1}个位姿后按回车采集...) # 等机械臂静止 time.sleep(1.0) # 采集图像 frames pipeline.wait_for_frames() aligned_frames align.process(frames) color_frame aligned_frames.get_color_frame() color_image np.asanyarray(color_frame.get_data()) # 读取UR位姿TCP在法兰盘中心 tcp_pose rtde_r.getActualTCPPose() # [x, y, z, rx, ry, rz] 旋转向量形式 # 保存 save_rgb.append(color_image.copy()) save_poses.append(tcp_pose) print(f第{i1}组: tcp {tcp_pose}) np.save(rgb_images.npy, np.array(save_rgb)) np.save(tcp_poses.npy, np.array(save_poses))这一步要特别注意UR的RTDE返回的是旋转向量rx,ry,rz不是四元数也不是欧拉角。后面要么把旋转向量转旋转矩阵要么转四元数要看OpenCV的calibrateHandEye需要的格式。4.2 角点检测与solvePnP对每一张彩色图检测ChArUco角点然后用solvePnP求标定板坐标系到相机坐标系的变换。def detect_charuco_pose(color_image, camera_matrix, dist_coeffs, board): # 灰度化 gray cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) # 检测ArUco marker detector_params cv2.aruco.DetectorParameters() detector cv2.aruco.ArucoDetector(board.dictionary, detector_params) corners, ids, rejected detector.detectMarkers(gray) if ids is None or len(ids) 4: return None, None # 插值ChArUco角点 ret, charuco_corners, charuco_ids cv2.aruco.interpolateCornersCharuco( corners, ids, gray, board) if not ret or charuco_corners is None: return None, None # 如果角点太少放弃这一帧 if len(charuco_corners) 10: return None, None # 用PnP求标定板到相机的变换 obj_points board.getChessboardCorners() # 世界坐标系标定板系下的角点坐标 obj_points obj_points[charuco_ids.flatten()] retval, rvec, tvec cv2.solvePnP(obj_points, charuco_corners, camera_matrix, dist_coeffs) # 构建4x4变换矩阵 T_board_cam R, _ cv2.Rodrigues(rvec) T_board_cam np.eye(4) T_board_cam[:3, :3] R T_board_cam[:3, 3] tvec.flatten() return T_board_cam, charuco_corners小坑提醒ChArUco板的obj_points用的是角点索引来索引的charuco_ids是角点在板上的唯一编号用obj_points[charuco_ids.flatten()]才能对齐到正确的世界坐标。我一开始直接用了整个obj_points数组导致结果完全错误排查了快两小时。4.3 读取UR位姿并组装数据UR的RTDE返回的是[tcp_x, tcp_y, tcp_z, rx, ry, rz]其中旋转向量表示TCP相对于基座的旋转。转成4x4变换矩阵 (T_{end}^{base})。def to_transformation_matrix(tcp_pose): x, y, z, rx, ry, rz tcp_pose R, _ cv2.Rodrigues(np.array([rx, ry, rz])) T np.eye(4) T[:3, :3] R T[:3, 3] [x, y, z] return T # 遍历所有采集的数据 T_board_cam_list [] T_end_base_list [] for i in range(len(save_rgb)): T_bc, corners detect_charuco_pose(save_rgb[i], camera_matrix, dist_coeffs, board) if T_bc is None: continue T_eb to_transformation_matrix(save_poses[i]) T_board_cam_list.append(T_bc) T_end_base_list.append(T_eb) print(f成功检测到标定板的帧数: {len(T_board_cam_list)})4.4 调用calibrateHandEye求解手眼矩阵眼在手外构型下OpenCV的cv2.calibrateHandEye函数要求输入的是机器人末端到基座的变换从基座看末端和标定板到相机的变换从相机看标定板然后直接输出T_cam_base。等等这里有个容易搞混的关键点。cv2.calibrateHandEye的官方用法是针对眼在手上的参数是R_gripper2base和t_gripper2base末端到基座以及R_target2cam和t_target2cam标定板到相机输出是R_cam2gripper和t_cam2gripper即相机到末端的变换。对于眼在手外我们需要的是T_cam2base也就是相机到基座的变换。OpenCV没有直接提供eye-to-hand的专用接口但是数学变换是等价的只需要把参数做些调整。眼在手外时方程改为 (AX ZB)。但如果你把“标定板固定在环境中”这个事实和“相机固定在环境中”这个事实互换角色实际上可以把眼在手外转换成一个眼在手上的求逆问题。具体做法把标定板想象成“假机械臂末端”把相机想象成“假标定板”。这时候待求矩阵 (X T_{base}^{cam} (T_{cam}^{base})^{-1})。也就是说把R_gripper2base传标定板的旋转矩阵因为是标定板在动——“假末端”R_target2cam传相机在基座系的旋转——但相机在基座系下是固定的我们不知道。这是行不通的。换个思路。变换链 (T_{board}^{cam} \cdot T_{cam}^{base} T_{end}^{base} \cdot T_{board}^{end})我把它改写成[ T_{cam}^{base} (T_{board}^{cam})^{-1} \cdot T_{end}^{base} \cdot T_{board}^{end} ]或者把未知的 (T_{board}^{end}) 消掉变成[ (T_{board}^{cam2})^{-1} \cdot T_{board}^{cam1} \cdot T_{cam}^{base} T_{end2}^{base} \cdot (T_{end1}^{base})^{-1} \cdot T_{cam}^{base} ]整理成 (A \cdot X X \cdot B) 的形式后可以直接复用[ A_i (T_{end_{i1}}^{base})^{-1} \cdot T_{end_i}^{base} ] [ B_i (T_{board_{i1}}^{cam})^{-1} \cdot T_{board_i}^{cam} ]注意这里的自变量是 (T_{cam}^{base})和之前推导时定义的 (A)、(B) 正好互换了位置但形式仍然是 (AX XB)。OpenCV的calibrateHandEye照样可以解但参数传入要小心# 组装连续两帧的相对运动 A_list [] # 机械臂末端的相对运动从第i1帧到第i帧 B_list [] # 标定板的相对运动从第i1帧到第i帧 for i in range(len(T_end_base_list) - 1): # 机械臂末端从位姿i1到位姿i的相对变换从base系下看 A_i np.linalg.inv(T_end_base_list[i1]) T_end_base_list[i] # 标定板从位姿i1到位姿i的相对变换从cam系下看 B_i np.linalg.inv(T_board_cam_list[i1]) T_board_cam_list[i] A_list.append(A_i) B_list.append(B_i) # 分离旋转和平移 R_A [A[:3, :3] for A in A_list] t_A [A[:3, 3] for A in A_list] R_B [B[:3, :3] for B in B_list] t_B [B[:3, 3] for B in B_list] # 求解 R_X, t_X cv2.calibrateHandEye( R_A, t_A, R_B, t_B, methodcv2.CALIB_HAND_EYE_PARK ) T_cam_base np.eye(4) T_cam_base[:3, :3] R_X T_cam_base[:3, 3] t_X.flatten() print(相机到基座的变换矩阵) print(T_cam_base)我还是把完整的更稳做法写一遍以避免直接对calibrateHandEye的输入输出约定产生歧义。在OpenCV的资料里函数输出的是R_cam2gripper和t_cam2gripper。对眼在手外等价变换后可以求得T_gripper2cam最后再对整体取逆即可。代码如下# 注意这里R_gripper2base传的是T_end_base末端到基座 # R_target2cam传的是T_board_cam标定板到相机 R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( [T_end_base_list[i][:3, :3] for i in range(len(T_end_base_list))], [T_end_base_list[i][:3, 3] for i in range(len(T_end_base_list))], [T_board_cam_list[i][:3, :3] for i in range(len(T_board_cam_list))], [T_board_cam_list[i][:3, 3] for i in range(len(T_board_cam_list))], methodcv2.CALIB_HAND_EYE_PARK ) # 直接用这个结果就是T_cam2gripper但我们要的是T_cam2base # 利用公式 T_cam_base T_gripper_base T_cam_gripper T_gripper_base T_end_base_list[0] # 用任意一帧的末端位姿 T_cam_gripper np.eye(4) T_cam_gripper[:3, :3] R_cam2gripper T_cam_gripper[:3, 3] t_cam2gripper.flatten() # T_cam_base T_gripper_base T_cam_gripper T_cam_base T_gripper_base T_cam_gripper等等这里要重新捋一下。T_cam_gripper是相机坐标系下末端的位置——不对R_cam2gripper表示的是“在gripper坐标系下表达的cam坐标系的旋转”所以T_cam_gripper实际是“cam在gripper系下的位姿”即 (T_{cam}^{gripper})。那 (T_{cam}^{base} T_{end}^{base} \cdot T_{cam}^{end}) 对任意一帧都成立因为cam和base都是固定的end是变化的但等式右边的两个矩阵相乘结果应该一致。所以T_base_gripper T_end_base_list[0] # 即T_end_base # T_cam_base T_end_base T_cam_end T_cam_end T_cam_gripper # 注意这里T_cam_gripper T_cam_end T_cam_base T_end_base_list[0] T_cam_end但T_cam_end这个名字在OpenCV输出是T_cam2gripper含义是“cam在gripper系下的位姿”与T_cam2gripper符号一致。OK这样写应该没问题。我建议在做完求解后用一个独立的验证流程反向检查一遍见第5章。4.5 点云变换到机器人基座坐标系拿到T_cam_base后点云的变换就是一个齐次坐标乘法的问题。def transform_pointcloud(points_cam, T_cam_base): points_cam: (N, 3) 相机坐标系下的点云 T_cam_base: (4, 4) 相机到机器人基座的变换矩阵 # 转齐次坐标 ones np.ones((points_cam.shape[0], 1)) points_hom np.hstack([points_cam, ones]) # (N, 4) # 变换 points_base (T_cam_base points_hom.T).T # (N, 4) # 转回3维 points_base points_base[:, :3] return points_base从RealSense获取点云时注意深度图的坐标系是相机光学坐标系Z轴朝前而OpenCV的坐标系是图像坐标系Z轴朝前但Y轴朝下。如果你直接用rs2::pointcloud生成的点云xyz都在相机坐标系下直接用上面的变换即可。如果是自己从深度图生成点云要特别注意坐标轴方向的约定否则会出现点云上下颠倒或者前后翻转的问题。RealSense SDK的rs2.deproject_pixel_to_point返回的点已经是相机坐标系下的3D坐标直接用。4.6 在Moveit中显示对齐后的点云点云变换到基座坐标系后就可以直接发布到Moveit的Planning Scene中用于避障。import moveit_commander from moveit_msgs.msg import CollisionObject, PlanningScene import rospy def publish_pointcloud_to_moveit(points_base): rospy.init_node(publish_cloud_to_scene) planning_scene PlanningScene() # 构造CollisionObject用点云表示障碍物 co CollisionObject() co.id obstacle_cloud co.header.frame_id base_link co.header.stamp rospy.Time.now() # 用octomap或者直接mesh表示 # 这里简化直接把点云发布为PointCloud2消息 pub rospy.Publisher(/pointcloud_in_base, PointCloud2, queue_size1) # ... 发布逻辑更推荐的做法是把点云转成OctoMap再加载到Planning Scene中因为Moveit的碰撞检测对原始点云需要构建FCL网格计算量较大。OctoMap体素化之后既能保留障碍物轮廓又能大幅降低碰撞检测的计算开销。下一篇文章我会专门讲这部分。5. 标定验证与翻车现场别只盯着重投影误差标定完成只是第一步验证标定结果是否可靠才是真正拉开差距的地方。很多人在这一步草草了事直接用标定结果去跑避障结果机械臂撞了才发现标定有问题回头还得排查是标定错还是避障算法错。我分享一套我自己常用的验证链路。5.1 第一关重投影误差检验把求解出的T_cam_base代回到变换链中对每一帧数据重新计算标定板角点的重投影位置看误差有多大。def check_reprojection_error(T_cam_base, T_end_base_list, T_board_cam_list): errors [] for i in range(len(T_end_base_list)): # 从T_cam_base和T_end_base反推T_board_end T_board_end_est np.linalg.inv(T_end_base_list[i]) T_cam_base T_board_cam_list[i] # T_board_end应该是一个常量因为板和末端都固定 # 这里用所有帧的平均值作为真值 # ... # 或者换个更直接的方法 # 用T_end_base和T_cam_base预测标定板在相机系下的位姿 T_board_cam_pred np.linalg.inv(T_cam_base) T_end_base_list[i] T_board_end_mean # 然后把T_board_cam_pred投影到图像平面和检测到的角点做比对 return np.mean(errors)通常重投影误差在1像素以内说明标定解算没问题如果超过3像素说明数据采集中存在较大不一致要回头检查是否有不同步帧、标定板是否发生位移、UR位姿读数是否准确等。5.2 第二关空间点验证重投影只能验证标定结果在“图像平面”上自洽但无法暴露深度方向Z轴的误差。更严格的办法是用机械臂TCP去触碰点云中的空间点。具体操作把标定板放在工作空间某个位置让UR5末端装一个尖锥工具去触碰标定板上的某个角点。从机器人示教器读到这个角点在机器人基座坐标系下的坐标 (P_{base}^{true})。同时用深度相机拍下这个角点通过相机内参和深度值得到它在相机坐标系下的坐标 (P_{cam}^{meas})。用标定得到的 (T_{cam}^{base}) 把 (P_{cam}^{meas}) 变换到基座系和 (P_{base}^{true}) 比对。误差在5毫米以内基本可接受10毫米以上就需要重新标定。这个测试最好在工作空间的多个区域做因为手眼标定结果在不同区域精度会有差异。5.3 翻车现场一位姿不同步导致的整体偏移症状标定出来的旋转矩阵看起来合理但平移向量明显偏大或者偏小整体点云位置偏移。原因排查相机图像和UR位姿不是在同一时刻采集的机械臂运动中微小的位姿变化被当成了静态度量。特征就是误差方向随机械臂运动方向变化——机械臂向X运动时误差偏向-X运动时误差偏-。修复确保机械臂在每个位姿完全静止后再采集或者使用更可靠的同步机制比如RTDE和相机在同一线程里用time.time()打时间戳对齐。5.4 翻车现场二标定板太小导致角点检测不稳定症状标定板检测帧率低检测到的角点数忽多忽少标定结果跳动大。原因工作距离太远标定板在图像中占比过小ArUco码解码不稳定。修复换大标定板或者把相机靠近一点重新安装。保证标定板在图像中占对角线长度的1/3到1/2ArUco码在图像中至少要覆盖20x20像素。5.5 翻车现场三退化位姿组合导致解算崩溃症状标定结果误差很大而且每次运行结果都不一样。原因采集时机械臂位姿变化太小特别是旋转角度变化不够。想象一下如果机械臂末端每次都只是平移了一点点所有数据几乎都在描述同一个视角这时候矩阵方程的约束条件不够会出现退化现象。修复采集数据时确保机械臂的俯仰角和偏航角有足够大的变化范围至少30度以上。最好设计一套标准采集路径——机械臂在标定板前方画一个球面轨迹每次指向标定板的角度都有明显差异。5.6 翻车现场四UR的TCP和实际末端工具不匹配症状如果机械臂末端装了夹爪或其他工具直接读TCP位姿法兰盘中心可能和你期望的“手眼”参考点不一致。修复标定时有两种选择——要么把工具坐标系的TCP设到法兰盘中心保证RTDE读出来的就是法兰盘位姿要么在UR上配置好工具坐标系让RTDE读出来的TCP包含工具偏移。关键是手眼标定公式里的(T_{end}^{base})必须和实际使用的坐标系一致。如果你要算的是“相机到基座”的关系那么(T_{end}^{base})直接用法兰盘位姿即可后续做点云对齐也不涉及末端工具。但如果你要算“相机到工具”的关系就需要把工具坐标系的值传入。5.7 翻车现场五深度图和彩色图没对齐症状点云在物体边缘有严重的“飞点”或者点云位置和彩色图看起来对不上。原因RealSense的深度传感器和RGB传感器物理位置不同视角有视差。如果不做rs.align直接用深度图生成的点云会和彩色图的坐标系有偏差导致PnP算出来的位姿不准。修复在采集数据之前就做深度图对齐并且对齐后的深度图在尺寸上和彩色图保持一致。标定板角点的像素坐标用于PnP深度值用于空间定位两者必须对应同一个坐标系。6. 进阶优化把标定精度从“能跑”提升到“好用”完成基本标定后如果精度还满足不了避障需求还可以从几个方向继续优化。6.1 多点平均法不要只做一次标定就完事可以采集两到三次独立的数据集分别标定得到多个T_cam_base然后对旋转矩阵求平均把旋转矩阵转四元数后球面平均或者直接对数映射平均对平移向量求加权平均。这样做的好处是可以规避单次采集中偶发的异常数据帧。我实测中三次标定结果如果差异很大旋转超过1度或平移超过5毫米说明数据采集过程有问题需要先排查再继续。6.2 非线性优化精修OpenCV的calibrateHandEye只做了两步线性求解可以在此基础上再用非线性优化比如Levenberg-Marquardt把重投影误差作为代价函数进一步精修。from scipy.optimize import least_squares def cost_function(params, T_end_base_list, T_board_cam_list): # 把T_cam_base参数化旋转向量平移 rvec params[:3] tvec params[3:] R, _ cv2.Rodrigues(rvec) T_cam_base np.eye(4) T_cam_base[:3, :3] R T_cam_base[:3, 3] tvec # 用T_board_end的一致性构造误差 errors [] T_board_end_list [] for i in range(len(T_end_base_list)): T_board_end np.linalg.inv(T_end_base_list[i]) T_cam_base T_board_cam_list[i] T_board_end_list.append(T_board_end) mean_T_board_end np.mean(T_board_end_list, axis0) for T_board_end in T_board_end_list: err (T_board_end[:3, :3] - mean_T_board_end[:3, :3]).flatten() err np.append(err, T_board_end[:3, 3] - mean_T_board_end[:3, 3]) errors.extend(err) return np.array(errors) # 初始值用OpenCV的结果 init_params np.hstack([cv2.Rodrigues(T_cam_base[:3, :3])[0].flatten(), T_cam_base[:3, 3]]) result least_squares(cost_function, init_params, args(T_end_base_list, T_board_cam_list))这套优化代码不需要单独安装什么库scipy就够了。优化后我一般能看到重投影误差下降20%到30%。但要注意非线性优化只能在数据本身一致性好时发挥作用如果采集时有几帧不同步的数据优化反而会把整体结果带偏。6.3 基于标定结果的动态检查标定完成后我习惯在工作空间固定几个标记点比如贴几个ArUco码在桌面上每次开机后让机械臂末端去触碰这些标记点快速验证标定结果是否仍然有效。如果发现误差突然变大优先检查相机支架是否被碰歪用水平尺和激光笔检查相机螺丝是否松动RealSense的镜头外壳很容易因热胀冷缩松动工作环境温度是否变化过大温度变化会导致相机内参漂移这一套检查流程只要3分钟但能省下后面排查了一整天才发现标定失效的时间。7. 避障链路中的点云预处理标定完了不等于能直接用标定结果正确是点云能到机器人坐标系的前提但避障算法真正需要的是干净、降噪、结构化的场景数据。直接拿原始点云去跑避障效果会惨不忍睹。7.1 直通滤波把采集范围缩小到工作空间相机视野里通常包含大量无关信息远处的墙壁、地面、各种杂物。这些点云如果不过滤掉不仅会增加Moveit碰撞检测的计算负担还可能被误识别为障碍物导致规划失败。def passthrough_filter(points_base, x_range, y_range, z_range): mask ( (points_base[:, 0] x_range[0]) (points_base[:, 0] x_range[1]) (points_base[:, 1] y_range[0]) (points_base[:, 1] y_range[1]) (points_base[:, 2] z_range[0]) (points_base[:, 2] z_range[1]) ) return points_base[mask]UR5的工作范围是一个半径约850mm的球体我会把直通滤波的范围设置成比这个稍大一圈的立方体区域既保证机械臂可达范围内的障碍物全部被保留又过滤掉远处无用点云。7.2 体素滤波降采样RealSense D435在1280x720分辨率下每帧产生约92万个点。全量点云直接用于碰撞检测FCL的碰撞检测开销会非常大Moveit的规划周期会被拖到几秒甚至更久。降采样是必须的。import open3d as o3d def voxel_downsample(points_base, voxel_size0.01): pcd o3d.geometry.PointCloud() pcd.points o3d.utility.Vector3dVector(points_base) pcd pcd.voxel_down_sample(voxel_size) return np.asarray(pcd.points)voxel_size取**0.01m1厘米**对避障来说完全够用机械臂末端碰不到障碍物的精度要求在厘米级就够了。体素滤波后点云数量能降到2到5万点碰撞检测速度大幅提升。7.3 去噪和动目标处理深度相机最烦人的问题是飞点和边缘拖影。RealSense可以用rs.temporal_filter和rs.hole_filling_filter做预处理能有效减少空洞和飞点。对于动态障碍物比如人或者移动的料车我建议让避障系统以低频周期性更新点云比如2Hz而不是每帧都更新。这样既能跟上动态环境变化又不会因为点云抖动导致Moveit反复重新规划。7.4 机械臂自遮挡的处理眼在手外构型下机械臂本身会出现在点云中。如果直接把包含机械臂的点云传给Moveit做避障Moveit会认为自己和自己碰撞导致规划失败。解决方案有两种检测并删除机械臂自身点云可以通过Moveit的Planning Scene拿到当前机械臂各连杆的包围盒或网格模型然后把落在这些区域内的点云删除。用PassThrough或裁剪区域手动抠除机械臂区域在固定安装下这个区域是确定的可以直接写死。第一种方法更通用复杂度也更高我在下一篇避障实战系列里会专门写这部分。这里先提个醒看到机械臂点云出现在Moveit障碍物里不要惊慌这是正常现象需要过滤。8. 一次完整的标定实战从数据采集到点云对齐的流程记录我在这台UR5上的整个标定流程从准备到出结果大约是40分钟。如果你按照本文的步骤走第一次可能需要两三个小时但多来几次熟练后时间会大幅缩短。整个流程梳理如下表步骤操作预计耗时关键检查点1固定相机和标定板检查相机支架刚性10分钟相机不能有任何晃动2相机内参自标定10分钟重投影误差小于0.5像素3采集30组数据10分钟每组要静止1秒以上再采集4检测角点检查检测成功率2分钟检测率100%除个别极端角度5calibrateHandEye求解1秒得到T_cam_base6重投影验证1分钟误差小于1像素7空间点验证5分钟用尖锥触碰标定板角点误差5mm内8点云变换检查1分钟点云能否落在机器人坐标系中正确位置实际上我在做的时候经常会跳过第6步直接做第7步因为空间点验证更直观。但是空间点验证依赖一个前提你要能精确控制机械臂的TCP去碰角点。对UR5来说用示教器手动移动TCP到一个已知点有可能存在人为误差更精确的做法是用运动学正解计算。自己做验证时我发现一个比较实用的技巧在机器人基座附近贴一张小ArUco码然后用相机拍这张ArUco码通过它来反推相机到基座的变换再和标定结果对比。这样做虽然精度不如TCP触碰法但速度快、不需要示教器操作适合日常巡检。9. 踩坑记录三次典型标定失败案例全复盘这里整理几个我在过去项目里踩过的坑有些隐蔽到排查了两三天。9.1 那次让我怀疑人生的问题相机内参是错的有一段时期我的标定重投影误差一直稳定在一个偏大的值2到3像素怎么调采集姿势都没用。后来我仔细检查了一下发现用的是RealSense出厂内参但相机出厂内参标定时是在特定温度和环境光下测的和我现场环境差异较大。用自标定内参后重投影误差从2.5像素降到了0.6像素。这个案例告诉我内参自标定不是可选项是必选项特别是RealSense这种受温度影响较大的设备。9.2 UR和相机的时钟不同步问题有一次我换了台电脑跑采集程序结果标定出来平移向量比之前偏大了将近两厘米。排查了很久最后发现问题出在采集同步上——新电脑跑起来之后RTDE读位姿的线程和相机采图的线程调度时序变了导致位姿滞后于图像约几百毫秒。修复方案把采集代码改成单线程串行模式机械臂完全静止后再依次采集位姿和图像不要用多线程并发。降低采集频率但保证每一帧数据都是可靠的。这比采集速度快但数据一致性差强得多。9.3 用了工具坐标系导致的手眼关系错乱前面5.6提到过如果机械臂装了夹爪但RTDE读的还是法兰盘位姿而你在其他地方用了工具位姿两者混用会让整个变换链对不上。我当时是给UR配置了Tool0下的一个自定义工具坐标系RTDE返回TCP位姿时返回的是工具位姿而不是法兰盘位姿但后续代码里基于法兰盘的假设做变换导致标定结果差出一个工具偏移量。血的教训代码里所有变换的参考坐标系必须统一RTDE读取的到底是法兰盘还是要工具坐标系的位姿在写代码前就要明确。10. 本系列的衔接与后续计划手眼标定这一篇搞定后整个避障系统的感知链路就打通了机械臂能够通过深度相机“看见”自己周围的障碍物并且知道障碍物在机器人坐标系下的精确位置。基于标定好的T_cam_base下一步就可以做这些事情点云建模并发布到Moveit的Planning Scene让OMPL等规划器在真实的障碍物环境中搜索无碰撞路径。把RGB图像结合点云做目标检测和姿态估计喂给机械臂做抓取或喷涂等任务。在点云中加入动态环境的实时检测比如人的位置做动态避障。下一篇我会写基于Moveit的点云构建与动态避障实战重点讲怎么把深度相机获取的点云高效地整合进Moveit的规划场景如何处理机械臂自遮挡以及实测中RRT-Connect和PRM在动态环境下的表现差异。如果你正在做UR5避障或者类似的机械臂项目建议把这一篇的手眼标定步骤先跑通后面内容都建立在这个坐标变换基础上。分享一个我自己的习惯每次标定完我会在工控机上保留一份标定日志记录日期、环境温度、重投影误差、空间点验证误差、操作人等信息。这样如果日后出现标定失效我可以快速定位是环境变化还是硬件位移导致的问题。这个习惯帮我节省了大量排查时间推荐你也试试。