企业官网建设流程全解析

开头部分，我想从实际场景切入——很多做机械臂避障的人，卡住的第一关往往不是路径规划算法本身，而是手眼标定。Moveit里的避障算法再先进，给你的是一个建立在错误坐标系下的点云，那一切规划都是空中楼阁。这篇文章就聚焦UR5机械臂配合深度相机做避障时，绕不开的手眼标定环节，完整走一遍从标定板准备、数据采集、数学求解到最终把点云对齐到机器人坐标系的整个流程。

这套方法我在项目里反复用过多次，也踩过不少坑。这篇东西既是给系列文章补上“感知坐标系的最后一块拼图”，也可以单独拿出来当一份实操手册看。适合正在做机械臂抓取、避障、无序分拣，或者任何需要把相机点云和机器人运动学关联起来的工程师参考。

1. 手眼标定先搞清楚：你这套系统是眼在手上还是眼在手外

先说一个我见过太多人搞反的概念。手眼标定虽然叫“手眼”，但手眼关系的核心不是相机和手之间物理距离多远，而是相机坐标系和机械臂末端（或基座）坐标系之间的位姿变换关系。在做标定之前，必须明确自己的系统属于哪种构型，因为两种构型的数学模型和求解方式完全不同。

1.1 两种经典构型的本质区别

手眼标定分两种：眼在手上（eye-in-hand）和眼在手外（eye-to-hand）。

眼在手上的典型应用是近距离精细抓取或视觉伺服——相机跟着机械臂走，靠近工件后才能看清细节，泛光干扰少，精度较高。但它有个天然问题：相机视野随着机械臂运动而变化，如果你做的是全工作空间的避障规划，眼在手上的点云只覆盖局部，很难为全局避障提供完整的环境信息。

眼在手外则是把深度相机固定在工作空间上方或侧方，类似监控视角，一次能看到机械臂、工件和大部分障碍物。这个构型下，待求的是相机坐标系到机械臂基座坐标系的固定变换矩阵，只要标定一次，后续相机不动、机器人不动，变换关系就始终成立。

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. 数学原理不讲虚的：AX=XB与AX=ZB到底在解什么，噪声从哪来

很多人拿到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方法（method=cv2.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 采集规则和数量

我在采集数据时有一套固定的流程：

1. 确认标定板固定不动——我直接用胶带把它贴在墙上或者用三脚架架住，保证在整个采集过程中标定板纹丝不动。
2. 控制UR5末端（不一定要夹爪，用法兰盘上的参考尖点就行）移动到标定板附近的多个位姿，确保相机能看到标定板，且标定板尽量出现在图像的不同区域。
3. 每个位姿下记录：当前机械臂末端位姿（从UR的RTDE接口读，四元数+xyz）+ 当前深度相机的彩色图。
4. 至少采集20组数据，每组之间机械臂的姿态变化要足够大——旋转角度差至少在30度以上，最好混合不同高度、不同俯仰角。
5. 让标定板出现在图像的边缘区域也采集几组，这能显著提高标定结果在全视野范围内的适用性。

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"移动到第{i+1}个位姿后按回车采集...") # 等机械臂静止 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"第{i+1}组: 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_{i+1}}^{base})^{-1} \cdot T_{end_i}^{base} ] [ B_i = (T_{board_{i+1}}^{cam})^{-1} \cdot T_{board_i}^{cam} ]

注意这里的自变量是 (T_{cam}^{base})，和之前推导时定义的 (A)、(B) 正好互换了位置，但形式仍然是 (AX = XB)。OpenCV的calibrateHandEye照样可以解，但参数传入要小心：

# 组装连续两帧的相对运动 A_list = [] # 机械臂末端的相对运动（从第i+1帧到第i帧） B_list = [] # 标定板的相对运动（从第i+1帧到第i帧） for i in range(len(T_end_base_list) - 1): # 机械臂末端从位姿i+1到位姿i的相对变换（从base系下看） A_i = np.linalg.inv(T_end_base_list[i+1]) @ T_end_base_list[i] # 标定板从位姿i+1到位姿i的相对变换（从cam系下看） B_i = np.linalg.inv(T_board_cam_list[i+1]) @ 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, method=cv2.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))], method=cv2.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_size=1) # ... 发布逻辑

更推荐的做法是把点云转成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去触碰点云中的空间点。

具体操作：

1. 把标定板放在工作空间某个位置，让UR5末端（装一个尖锥工具）去触碰标定板上的某个角点。
2. 从机器人示教器读到这个角点在机器人基座坐标系下的坐标 (P_{base}^{true})。
3. 同时用深度相机拍下这个角点，通过相机内参和深度值得到它在相机坐标系下的坐标 (P_{cam}^{meas})。
4. 用标定得到的 (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/2，ArUco码在图像中至少要覆盖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, axis=0) 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_size=0.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.01m（1厘米）**对避障来说完全够用，机械臂末端碰不到障碍物的精度要求在厘米级就够了。体素滤波后点云数量能降到2到5万点，碰撞检测速度大幅提升。

7.3 去噪和动目标处理

深度相机最烦人的问题是飞点和边缘拖影。RealSense可以用rs.temporal_filter和rs.hole_filling_filter做预处理，能有效减少空洞和飞点。

对于动态障碍物（比如人或者移动的料车），我建议让避障系统以低频周期性更新点云（比如2Hz），而不是每帧都更新。这样既能跟上动态环境变化，又不会因为点云抖动导致Moveit反复重新规划。

7.4 机械臂自遮挡的处理

眼在手外构型下，机械臂本身会出现在点云中。如果直接把包含机械臂的点云传给Moveit做避障，Moveit会认为自己和自己碰撞，导致规划失败。

解决方案有两种：

1. 检测并删除机械臂自身点云，可以通过Moveit的Planning Scene拿到当前机械臂各连杆的包围盒或网格模型，然后把落在这些区域内的点云删除。
2. 用PassThrough或裁剪区域手动抠除机械臂区域，在固定安装下这个区域是确定的，可以直接写死。

第一种方法更通用，复杂度也更高，我在下一篇避障实战系列里会专门写这部分。这里先提个醒：看到机械臂点云出现在Moveit障碍物里，不要惊慌，这是正常现象，需要过滤。

8. 一次完整的标定实战：从数据采集到点云对齐的流程记录

我在这台UR5上的整个标定流程，从准备到出结果大约是40分钟。如果你按照本文的步骤走，第一次可能需要两三个小时，但多来几次熟练后时间会大幅缩短。

整个流程梳理如下表：

实际上我在做的时候，经常会跳过第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，下一步就可以做这些事情：

1. 点云建模并发布到Moveit的Planning Scene，让OMPL等规划器在真实的障碍物环境中搜索无碰撞路径。
2. 把RGB图像结合点云做目标检测和姿态估计，喂给机械臂做抓取或喷涂等任务。
3. 在点云中加入动态环境的实时检测（比如人的位置），做动态避障。

下一篇我会写基于Moveit的点云构建与动态避障实战，重点讲怎么把深度相机获取的点云高效地整合进Moveit的规划场景，如何处理机械臂自遮挡，以及实测中RRT-Connect和PRM在动态环境下的表现差异。如果你正在做UR5避障或者类似的机械臂项目，建议把这一篇的手眼标定步骤先跑通，后面内容都建立在这个坐标变换基础上。

分享一个我自己的习惯：每次标定完，我会在工控机上保留一份标定日志，记录日期、环境温度、重投影误差、空间点验证误差、操作人等信息。这样如果日后出现标定失效，我可以快速定位是环境变化还是硬件位移导致的问题。这个习惯帮我节省了大量排查时间，推荐你也试试。