手眼标定与 3D 抓取：眼手协同实战
产线上机械臂抓不准的锅，90% 要扣在标定头上——不是相机不行，是"手"（机械臂）和"眼"（相机）的关系没量准。相机看到的是"目标在相机坐标系下的坐标"，机械臂要的是"目标在基座坐标系下的坐标"，中间隔着的那个变换没标准，视觉坐标换算到机器人坐标就会整体跑偏，重复精度 0.02 mm 的机械臂照样一把抓空。
这篇的主线只有一句话：**相机装在手上还是装在旁边，决定标定方程的两种解法，也决定误差往哪走。**想通这一句，标定流程、TF 结构、排查思路就全都顺理成章了。
一、先分清两种眼睛
手眼标定（Hand-Eye Calibration）解的是经典的 AX = XB 方程：已知若干组"机器人自己动了多少"和"相机看到标定板动了多少"，求出相机与机械臂之间那个固定的未知变换 X。数学上它是同一个方程，但两种安装方式会让 A 和 B 取完全不同的东西：
- Eye-in-Hand（眼在手上）：相机固定在法兰/工具上，跟着机械臂一起动。标定板静止在桌面上，动的是机械臂。此时
A = T_toolA_toolB⁻¹（两次末端位姿之差），B = T_base_board（基座到标定板），解出的X是T_tool0_camera（tool0 到 camera_link）。手动机、眼随手动。 - Eye-to-Hand（眼在旁边）：相机固定在三脚架或支架上俯视工作区，机械臂带着标定板动。此时
A = T_base_toolA_toolB_base（基座系下末端位移），B = T_boardA_boardB（标定板位移），解出的X是T_base_camera（基座到相机）。手动机、眼旁观。
不用管公式怎么推，记住"谁动机理"就够了：动的是相机，解"手→眼"；动的是标定板，解"地→眼"。
| 维度 | Eye-in-Hand | Eye-to-Hand |
|---|---|---|
| 视野 | 随臂移动，越伸越看得清 | 固定视野，看得远 |
| 精度 | 工作距离短，放大误差小 | 靠分辨率撑，远距易糊 |
| 标定难度 | 高，需同步采集位姿 | 中，支架易保持稳定 |
| 典型场景 | 无序抓取、多工位转移 | 上料下料、固定工位 |
**安装方式还决定误差往哪走：**Eye-in-Hand 的相机离目标近，深度误差被工作距离放大的比例小，但机械臂关节误差直接混进相机位姿；Eye-to-Hand 固定不动、无关节耦合，可视野是死的，距离一远像素误差换算成毫米就放大。所以近距离精细抓取优先眼在手上，大场景监管优先眼在旁边——没有更好的安装，只有更合适的安装。
安装环节还有两个常被忽略的物理细节，踩过的人都懂：一是相机支架的刚性——悬臂太长或钣金太薄，机械臂急停时的振动会让相机相对法兰微移，标定结果悄悄失效，表现为"设备运行一周后抓取精度缓慢劣化"，重启又能好一阵，其实是支架松了；二是线缆的走线——拖链里未固定的网线反复弯折拉扯相机接头，同样会造成微米到毫米级的位移。标定精度的前提是"手眼关系刚性不变"，这个前提是结构件给的，不是算法给的。
二、标定流程：采集是九成功夫
有了方程，剩下的全是让方程"可解且解得准"的活。以 easy_handeye2 的流程为例（MoveIt2 手眼标定插件是同一套底层逻辑，只是包在 planning 里）：
- 相机内参标定（前置）：棋盘格内参，重投影误差控制在
0.5 px以内才算合格，内参歪了后面全歪； - 采样 15–20 个姿态：机械臂带着相机（或标定板）摆不同位姿，每个位姿记录
T_base_tool和检测到的棋盘角点； - 求解
AX = XB：easy_handeye2 内置 Tsai-Lenz、Park、Daniilidis 等经典算法，推荐先用 Tsai-Lenz 跑通； - 残差验证：看每个采样点的重投影误差散点，剔除离群点后重解。
-
采样姿态的讲究比算法选择重要得多：
-
角度必须拉开：三个旋转轴各至少 ±20°，全是小角度微调会让
AX = XB的旋转部分接近零，方程病态、解不稳定； -
不能全共面：所有姿态的棋盘格中心不能落在同一平面附近，否则平移分量欠约束，Z 方向会漂；
-
棋盘格充满视野 1/3 以上：角点检测亚像素精度才有保证，太远时角点成像只有几个像素，误差直接翻倍。
摆姿态的实操口诀：想象棋盘格法线指向一个球面的不同经纬度，15 个姿态尽量均匀撒在球面上，同时机械臂保持在舒适工作区间内、棋盘格不出视野不脱焦。easy_handeye2 的采样界面会实时显示已采姿态的分布，盯着球面点撒匀即可，比背参数表管用。
**重投影误差是及格线：**求解完成后用标出的 X 把所有位姿的棋盘格投影回图像，平均误差 < 2 px 及格，< 1 px 良好；单点 > 4 px 的采样直接踢掉重摆。别迷信求解器——垃圾进垃圾出，15 个好姿态胜过 40 个烂姿态。
求解算法本身怎么选？Tsai-Lenz 1989 年的经典做法是先把旋转和平移分开解：用旋转部分的性质解出 X 的旋转，再回代解平移，两步走、速度快，对姿态分布要求高；Park 和 Daniilidis 是后续改进，用李群/对偶四元数把旋转平移联合求解，姿态分布稍差时更稳。**工程上的诚实建议：姿态采得好，三个算法结果差在 0.1 mm 以内；姿态采得差，换哪个算法都救不回来。**所以调算法不如多摆五个姿态。
标定频率也常被问：多久重标一次？判断标准不是日历而是事件——发生碰撞、换过相机/镜头、支架受过冲击、抓取偏差出现"随位姿变化"的模式，四者任一命中就重标。产线例行公事的月度重标，多数时候是在给心理安全感打工。
三、TF 树固化：把标定结果焊死
标定出的 X 是一个静态变换，写进 TF 树的方式就是 static_transform_publisher。TF 树的结构取决于安装方式：
Eye-in-Hand 的 TF 链：
world
└── base_link （机械臂基座）
└── link_1 ... link_6
└── tool0 （法兰）
└── camera_link ← 标定解 X = T_tool0_camera
└── camera_optical_frame
Eye-to-Hand 的 TF 链：
world
├── base_link （机械臂基座）
│ └── link_1 ... link_6
│ └── tool0
└── camera_link ← 标定解 X = T_base_camera
└── camera_optical_framelaunch 文件里固化（数值替换成标定输出）：
<!-- Eye-in-Hand：tool0 -> camera_link -->
<node pkg="tf2_ros" exec="static_transform_publisher"
args="--x 0.052 --y -0.031 --z 0.118
--qx 0.707 --qy 0.0 --qz 0.0 --qw 0.707
--frame-id tool0 --child-frame-id camera_link" /># Python 写法（lifecycle 场景更灵活）
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
msg = TransformStamped()
msg.header.frame_id = "tool0"
msg.child_frame_id = "camera_link"
msg.transform.translation.x = 0.052
msg.transform.translation.y = -0.031
msg.transform.translation.z = 0.118
# 旋转同理填 quaternion，来自标定结果的旋转矩阵转四元数
StaticTransformBroadcaster(node).sendTransform(msg)两个工程细节：四元数用 scipy.spatial.transform.Rotation.from_matrix(T[:3, :3]).as_quat() 转换；camera_optical_frame 的光学坐标系朝向是 Z 向前、X 向右、Y 向下，与 ROS 的 X 前 Y 左 Z 上相反，漏了这层旋转，点云会整体镜像，表现为"抓取点总在真实位置对称侧"——这是新手排查半天的经典坑。
TF 树的正确性验证也有懒人办法：RViz 里把所有 frame 打开，肉眼检查三条铁律——camera_link 必须长在 tool0（或 base_link）上不能悬空；机械臂动的时候点云必须"钉"在场景里不动（动的是相机，点云世界里物体不动）；用 TF2 的 lookup_transform 算一遍 tool0 到 camera_link 的距离，和卷尺量的支架尺寸对得上。三条全过，TF 这关才算焊死。
TF 树固化之后，还有一件小事顺手做掉：把标定日期、重投影误差、采样数写进 launch 文件注释或一个 calibration_info.yaml。半年后抓取精度飘了，第一件事就是翻出这份档案——上次标定误差是多少、什么时候标的，心里立刻有底。工业系统的可维护性，一半靠日志和档案，不靠老师傅的记忆。
四、3D 感知抓取：从点云到抓取位姿
深度相机的选型直接决定点云质量的上限，三条主流路线各有脾气：
- 结构光（RealSense D435/D455）：近距
0.3–3 m精度毫米级，但红外结构光在阳光下或多台同用时互相干扰； - ToF：测距范围大、帧率高，多机互不干扰，但角落处的多路径反射会出鬼影点；
- 主动红外双目：折中方案，弱纹理物体上靠红外散斑补特征。
室内产线无日照直射、工作距离 1 m 以内，结构光是性价比默认解；户外物流场景换 ToF。点云管线的滤波再好，也救不回传感器物理上限之外的噪声。
标定焊死之后，视觉管线才能跑起来。深度相机输出的原始点云几十万点，直接喂给位姿估计既慢又脆，标准管线是"滤波 → 法线 → 位姿"三步：
import pclpy
from pclpy import pcl
# 1. 直通滤波：只留工作台上方 0.2~0.8 m 的点云
cloud = pcl.io.loadPCDFile("scene.pcd")
crop = cloud.make_cropbox()
crop.set_min(-1.0, -1.0, 0.2)
crop.set_max(1.0, 1.0, 0.8)
work_cloud = crop.filter()
# 2. 体素降采样：5 mm 格子，点数砍一个数量级
vox = work_cloud.make_voxel_grid_filter()
vox.set_leaf_size(0.005, 0.005, 0.005)
down = vox.filter()
# 3. 统计离群点去除：邻域均值滤波踢掉飞点
sor = down.make_statistical_outlier_filter()
sor.set_mean_k(50)
sor.set_std_dev_mul_thresh(1.0)
clean = sor.filter()
# 4. 表面法线：pcl_normal_estimation
ne = clean.make_normal_estimation()
ne.set_k_search(30)
normals = ne.compute()
# 5. 估计抓取位姿：法线反向即接近方向，SACSegmentation 拟合平面
seg = clean.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
seg.set_distance_threshold(0.01)
inliers, coefficients, _ = seg.segment()得到目标位姿后，把环境喂给 MoveIt2 的 planning scene，让规划器知道哪里是桌面、哪里是障碍：
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState
moveit = MoveItPy("moveit_py")
panda = moveit.get_planning_component("panda_arm")
scene = moveit.get_planning_scene_monitor()
# 生成 OctoMap 碰撞环境：点云投影成占据栅格
scene.apply_planning_scene_msg_with_octomap_from_point_cloud(
point_cloud_msg, frame_id="camera_optical_frame"
)
# 设置抓取目标位姿（来自位姿估计）
panda.set_start_state_to_current_state()
panda.set_goal_state(
pose_goal={
"panda_hand": target_pose_stamped, # 视觉估出的抓取位姿
}
)
plan = panda.plan()OctoMap 的价值在于"实时"：每帧点云更新占据栅格，工件被挪走、新料箱进场，碰撞环境跟着变，规划器不会按五分钟前的旧地图撞上去。滤波管线的顺序也有讲究——先直通裁剪再降采样，把算力花在工作区内的点上；统计滤波放最后，因为离群点在降采样后更容易被识别成孤立小簇。
位姿估计的输出也要做一层校验再交给机械臂，裸信视觉是产线大忌。三个实用闸门：抓取位姿与重力方向夹角——法线估计出的接近方向若偏离竖直 > 30°，多半是点云噪声或遮挡导致的假平面，丢弃；点云支撑数——拟合出平面的内点数少于阈值（比如 200 点），说明目标太小或太远，结果不可信；与上一帧一致性——工件理论上静止，位姿帧间跳变 > 10 mm 必有其一出错，触发重测。三个闸门把"视觉偶尔抽风"挡在机械臂动起来之前，代价只是几行判断代码。
五、抗遮挡与柔顺抓取：抓偏了抓歪了怎么办
实际产线不会像实验室那样摆得端端正正。遮挡的对策是提前把托盘边缘、料箱壁写进碰撞环境（planning scene 里的 collision object），规划时自然绕开；柔顺的对策是笛卡尔空间路径规划——不走关节空间插值，而是直线/圆弧轨迹精确控制末端姿态。选笛卡尔的根本原因：关节空间插值出来的末端轨迹是条看不出规律的曲线，伸进料箱抓工件时这条曲线可能扫到箱壁；笛卡尔直线则保证下探路径全程竖直，行为可预期。代价是计算量大（逆运动学逐点求解）且可能碰到关节极限，所以只在最后十几厘米的下探段用笛卡尔，长距离移动仍走关节空间规划，各取所长。
from pymoveit2 import MoveIt2Servo
from moveit.planning import cartesian_path
# 笛卡尔直线插值：竖直下探抓取，避免斜插碰翻相邻工件
(cart_plan, fraction) = panda.compute_cartesian_path(
position_target=grasp_position,
orientation_target=grasp_orientation,
max_step=0.01, # 1 cm 步长
jump_threshold=0.0,
)
if fraction < 0.95:
node.get_logger().warn(f"只完成 {fraction:.0%}，路径被遮挡截断")最后是绕不开的排查环节——抓取偏差三个嫌疑人，按排查成本从低到高：
- 标定嫌疑：抓取点偏移量随机械臂位姿变化而变化（换个工作位置偏差方向就变）→ 手眼标定漂了，重跑第二节流程；
- 手眼延迟嫌疑：偏差方向和传送带/工件运动方向一致，工件速度一快偏差就大 → 相机触发到图像可用之间延迟没补，参考多传感器硬件时间同步里的硬件触发方案；
- 夹爪回差嫌疑：每次闭合姿态都差一点，但偏差量固定不随位姿变 → 齿轮回差/气压不稳，机械问题，换标定排查思路。
实际排查可以更快：拿一个工件放在视野中心正下方，让机械臂竖直下去抓，再移到视野边缘重复一次。中心准、边缘偏 → 标定嫌疑（视野边缘畸变放大）；两个位置都偏同一个方向和量 → 夹爪嫌疑；工件静止时准、放在动平台上偏 → 延迟嫌疑。两分钟三次抓取，嫌疑人基本锁定，再针对性深挖。
**抓歪了抓空了，先看偏差的模式，再动标定。**三个嫌疑人的"作案特征"完全不同，无脑重标定只会浪费半天还找不着真凶。另一个隐性事实是夹爪本身也有重复精度：气动平行夹爪 0.1–0.5 mm、电动伺服夹爪 < 0.05 mm，预算允许时上电动夹爪，能把第三嫌疑人直接从排查名单里划掉。
六、软硬联动
手眼标定是"眼"这头的地基，机器人系统的另外两块拼图在：
- 眼要看得准，先得拍得齐：多相机系统的硬件触发与编码器对时，见多传感器硬件时间同步：触发电路与编码器接口——手眼延迟排查的底层原理就在这篇；
- 眼手协同写完，控制回路怎么闭环验证：ros2_control 硬件在环：仿真与实机无缝切换——抓取策略先在 HIL 里磨，再上真机。
参考文献
- Tsai, R. Y., & Lenz, R. K. "A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration." IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345–358, 1989.（
AX = XB闭式解经典） - Hörmmann, C., et al. "easy_handeye2: Simple, Straight-Forward Hand-Eye Calibration for ROS 2." GitHub Repository, 2023.（本篇采样与求解流程参考）
- Rusu, R. B., & Cousins, S. "3D is Here: Point Cloud Library (PCL)." IEEE International Conference on Robotics and Automation (ICRA), 2011.（滤波/法线/分割模块）
- MoveIt2 Documentation. "MoveIt Py Tutorial and Planning Scene Tutorial." MoveIt.ros.org, 2024.（planning scene 与 OctoMap 接口）
- Chen, H., et al. "A Survey on Hand-Eye Calibration for Robot Vision." Robotics and Computer-Integrated Manufacturing, 2021.（两种安装方式的误差分析对比）
本文采用 署名-非商业性使用-相同方式共享 4.0 国际 许可协议，转载请注明出处。
