机器人眼在手外与 TCP Pivot 标定：误差溯源、独立验证与工程化实践

摘要本文围绕机械臂手眼标定残差从 9.69 mm 降至 0.76 mm 的完整排障过程展开核心根因是机器人关节零位与 FK 模型定义不一致。文章依次涵盖坐标系契约的形式化定义、视觉观测质量与标定精度的非对等性分析、四元数分量顺序歧义陷阱、基于对照实验的系统性误差溯源、关节零点校准后的精度跃迁与泛化性验证、TCP Pivot 标定以及将一次性脚本收敛为工程工具包的实践。目录一、坐标系契约的形式化定义二、视觉观测质量与标定精度的非对等性分析三、四元数分量顺序歧义对运动学一致性验证的干扰四、基于对照实验的系统性误差溯源五、关节零点校准后的精度跃迁与泛化性验证六、TCP Pivot固定一个点改变尽可能多的方向七、把一次性脚本收敛成工程工具包关键词解析手眼标定求解相机与机器人基座之间固定变换外参的过程本文采用眼在手外相机固定于外部模式。FK正向运动学由关节角计算机器人末端位姿的模型本文以robot.xml定义。关节零位各关节角度的零点基准若与 FK 模型不一致会引入系统性位姿偏差。四元数顺序四元数分量排列约定厂家协议为wxyzSciPy 接收xyzw顺序错位会导致旋转解析错误。重投影误差棋盘格角点经相机模型投影回图像后与检测点的像素偏差反映视觉链内部一致性不反映机器人位姿正确性。RMS均方根残差平方均值的平方根用于衡量整体误差水平对离群点敏感度低于最大值。TCP Pivot固定工具尖端于同一物理点、改变姿态通过线性最小二乘求解尖端相对末端偏置的方法。留出验证用未参与求解的新样本评估模型泛化能力比拟合残差更能反映真实精度。条件数设计矩阵奇异值之比反映问题对噪声的敏感程度数值越小越稳定。一、坐标系契约的形式化定义标定之前第一件事不是调用某个 OpenCV API而是明确每个变换的方向。本文统一使用记号 ATB表示“把 B 坐标系中的点变换到 A 坐标系”。定义B机器人基座base_LinkF右腕末端wrist_roll_R_LinkC相机光学坐标系vsensor_rgb_optical_frameP棋盘格坐标系biaoding_R_Link。每个采样姿态都满足BTF,i FTP BTC CTP,i其中BTF,i 由 7 个右臂关节角和robot.xml正向运动学得到CTP,i 由相机图像、内参、棋盘格角点和 PnP 得到BTC 是需要部署的相机外参FTP 是棋盘格相对末端的实际安装变换也是未知量。这套建模有一个很实用的好处棋盘格不需要被“完美地”安装在设计位置。求解器会同时估计 BTC 和 FTP所以安装板厚度、棋盘格原点和方向等偏置已经包含在结果中部署时不能再手工叠加一次。从多个姿态构造相对运动后可以得到经典的AijX XBij工具先用线性 AXXB 得到初值再以所有原始样本的 BTF,iX Y CTP,i 一致性做鲁棒非线性优化其中 XFTPYBTC。线性解解决初始化非线性优化则避免只依赖两两组合并降低偶发坏样本的影响。下面给出基于 SciPy 的线性初始化与鲁棒非线性优化的核心实现import numpy as np from scipy.spatial.transform import Rotation from scipy.optimize import least_squares def solve_hand_eye(poses_BT, poses_CT): poses_BT: 机器人基座到末端的变换列表 (4x4) poses_CT: 相机到棋盘格的变换列表 (4x4) # 构造 AX XB 的线性方程 A_list, B_list [], [] for i in range(len(poses_BT) - 1): A np.linalg.inv(poses_BT[i]) poses_BT[i 1] B np.linalg.inv(poses_CT[i]) poses_CT[i 1] A_list.append(A) B_list.append(B) # 线性初值旋转部分 M np.zeros((3, 3)) for A, B in zip(A_list, B_list): Ra, Rb A[:3, :3], B[:3, :3] M Ra - Rb # 通过 SVD 求旋转初值简化示意 U, _, Vt np.linalg.svd(M) R_init U Vt # 平移初值简化示意 t_init np.zeros(3) # 鲁棒非线性优化最小化所有样本的一致性残差 def residuals(params): rx, ry, rz, tx, ty, tz params R Rotation.from_euler(xyz, [rx, ry, rz]).as_matrix() t np.array([tx, ty, tz]) res [] for BT, CT in zip(poses_BT, poses_CT): # BTF,i X Y CTP,i - X FTP, Y BTC pred BT np.eye(4) pred[:3, :3] R pred[:3, 3] t err pred CT - BT res.append(err[:3, 3]) return np.concatenate(res) x0 np.concatenate([Rotation.from_matrix(R_init).as_euler(xyz), t_init]) result least_squares(residuals, x0, losssoft_l1, f_scale0.01) rx, ry, rz, tx, ty, tz result.x R Rotation.from_euler(xyz, [rx, ry, rz]).as_matrix() T np.eye(4) T[:3, :3] R T[:3, 3] [tx, ty, tz] return T二、视觉观测质量与标定精度的非对等性分析第一轮session_01的结果是会话平移 RMS平移最大值旋转 RMS旋转最大值session_019.69 mm17.49 mm2.52°3.92°重新固定棋盘格后的session_025.43 mm11.31 mm1.96°3.65°重新固定棋盘格确实让结果变好却没有从根本上解决问题。更关键的是其他观测量都很健康单张图像重投影误差大约为0.040.13 px曝光前后机器人运动小于0.07 mm固定姿态重复采集 5 次平移重复性 RMS 为0.0189 mm旋转重复性 RMS 为0.0049°棋盘格角点稳定相机和棋盘格没有可观测的松动相机 SDK 的 CXP 初始化告警没有影响实际使用的 RGB 数据流。这组证据非常重要重投影误差只能说明“图像中的角点能被当前相机模型解释”并不能证明机器人位姿正确。手眼标定同时依赖视觉链和运动学链像素级误差很小而三维闭环误差很大反而应当把注意力转向 FK、关节反馈和坐标系协议。三、四元数分量顺序歧义对运动学一致性验证的干扰为验证机器人侧契约我们编写了只读诊断程序在多个姿态下同时读取厂家接口返回的 7 个右臂关节角request_get_move_pose返回的厂家 END 位姿robot.xml根据相同关节角计算出的 FK 位姿。然后检查下面的相对变换是否在不同姿态下保持固定FTE (BTF)-1 BTE早期诊断一度出现几十度的旋转漂移看起来像是 FK 方向或厂家 END 定义彻底不一致。真正的原因却是协议字段right_quat的顺序为wxyz而 SciPy 等常用库接收的是xyzw正确的边界转换应当是w, x, y, z right_quat rotation Rotation.from_quat([x, y, z, w])为验证四元数顺序是否被正确解析可运行如下只读诊断from scipy.spatial.transform import Rotation def parse_right_quat(right_quat): 厂家协议为 wxyzSciPy 接收 xyzw w, x, y, z right_quat return Rotation.from_quat([x, y, z, w]) def check_fk_consistency(joint_angles, vendor_end_pose, robot_xml_fk): 对比厂家 END 位姿与 robot.xml FK 位姿的相对变换是否固定 rel np.linalg.inv(robot_xml_fk) vendor_end_pose return rel修正解析后FK 与厂家 END 的固定关系稳定到约0.036 mm / 0.003° RMS。最终 TCP 数据中的复核值也达到0.031 mm / 0.003° RMS。这里要特别区分两个结论四元数顺序错误解释了“为什么早期 FK/厂家 END 诊断出现几十度误差”它并没有解释手眼标定中仍然存在的 510 mm 残差因为手眼主流程使用的是关节角和 FK而不是这组错误解析的 END 旋转。换句话说这是一个真实 bug但不是最终根因。工程排障中修掉一个明显错误后仍要重新验证原始症状不能因为找到了 bug 就提前结束调查。四、基于对照实验的系统性误差溯源接下来对几个常见怀疑点做了离线实验将无编码棋盘格角点顺序旋转 180°调换手眼方程中的变换方向重新拟合相机内参更换样本子集和组合方式对机器人各关节加入固定偏置并观察残差变化。前三类修改都无法解释原始误差。如果变换方向真的写反通常不会表现为几个毫米的稳定残差而会出现数量级更大的错误如果相机内参或角点检测是主因也很难同时解释极低的重投影误差和极好的静止重复性。真正出现决定性证据的是关节固定偏置拟合session_01elbow_R_Joint 约 5.5° session_02elbow_R_Joint 约 5.3°在两次独立安装、两套采样中同一个肘关节都出现方向一致、数值接近的偏置。加入该偏置后手眼残差可以从约510 mm降到0.51.2 mm。于是根因逐渐清晰机器人实际反馈的关节角零位与robot.xml中 FK 模型使用的关节零位定义不一致。这里的偏置拟合只是一种诊断手段不应直接当作生产修复。因为联合优化关节偏置和外参存在参数耦合算法有可能用一个“看起来合理”的关节补偿吸收其他误差。更可靠的证据链是偏置在两个会话中重复出现机器人重新校零后该偏置消失用全新数据重做标定后误差显著下降最后再用未参与拟合的姿态验证。关节固定偏置拟合的核心代码如下import numpy as np from scipy.optimize import least_squares def fit_joint_offset(poses_BT, poses_CT, joint_angles, joint_names, target_joint): 对指定关节加入固定偏置观察手眼残差变化 idx joint_names.index(target_joint) def residuals(offset): angles joint_angles.copy() angles[:, idx] offset # 用修正后的关节角重新计算 FK再求解手眼残差 poses_BT_corrected compute_fk(angles) # 需按 robot.xml 实现 return handeye_residual(poses_BT_corrected, poses_CT) result least_squares(residuals, 0.0) return result.x[0] # 返回拟合出的关节偏置度五、关节零点校准后的精度跃迁与泛化性验证完成 TRON2 关节零点校准后没有继续使用旧样本而是新建session_03重新采集 20 个姿态。结果如下指标session_01session_02重新校零后的session_03平移 RMS9.691 mm5.428 mm0.760 mm平移最大值17.495 mm11.310 mm1.421 mm旋转 RMS2.524°1.955°0.269°旋转最大值3.916°3.653°0.470°最终的 BTC 平移为[0.2023027, 0.0136507, -0.0602669] m四元数采用xyzw顺序[-0.4933546, 0.5036122, -0.4976263, 0.5053158]同时求得的 FTP 平移为[0.1615320, 0.0024369, -0.0853258] m这不是需要再次叠加的“额外修正”而是标定板实际安装关系的估计结果。独立留出验证比拟合残差更有说服力仅报告参与优化的 20 个姿态仍然无法排除过拟合或采样分布过于相似。为此又采集了 5 个完全不参与求解的新姿态使用已固定的 BTC 和 FTP 预测标定板位姿留出集指标结果平移 RMS0.654 mm平移最大值0.830 mm旋转 RMS0.354°旋转最大值0.537°留出误差与训练残差处在同一量级说明session_03学到的是可推广的坐标关系而不是只把已有 20 组数据拟合得好看。六、TCP Pivot固定一个点改变尽可能多的方向手眼标定解决相机和机器人基座之间的关系TCP 标定解决厂家 END 到真实工具尖端的偏置两者不能互相替代。设第 i 个姿态下厂家 END 位姿为BTE,i [Ri, ti]工具尖端在 END 坐标系中的未知位置为 p固定支点在基座坐标系中的位置为 c。只要每次都让尖端接触同一个物理点就有Ri p ti c把多个姿态堆叠起来即可形成线性最小二乘问题[R1 -I; R2 -I; ...] [p; c] -[t1; t2; ...]工具先用线性最小二乘初始化再用soft_l1鲁棒损失优化。最终 15 个姿态得到TCP Pivot 的线性最小二乘初始化与鲁棒优化实现如下import numpy as np from scipy.optimize import least_squares def solve_tcp_pivot(poses_BTE): poses_BTE: 厂家 END 位姿列表 (4x4)所有姿态尖端接触同一固定点 n len(poses_BTE) A np.zeros((3 * n, 6)) b np.zeros(3 * n) for i, T in enumerate(poses_BTE): R, t T[:3, :3], T[:3, 3] A[3 * i:3 * i 3, :3] R A[3 * i:3 * i 3, 3:] -np.eye(3) b[3 * i:3 * i 3] -t # 线性最小二乘初始化 x0, _, _, _ np.linalg.lstsq(A, b, rcondNone) p_init, c_init x0[:3], x0[3:] # soft_l1 鲁棒优化 def residuals(x): p, c x[:3], x[3:] res [] for T in poses_BTE: R, t T[:3, :3], T[:3, 3] res.append(R p t - c) return np.concatenate(res) result least_squares(residuals, np.concatenate([p_init, c_init]), losssoft_l1) p result.x[:3] return p # 工具尖端在 END 坐标系中的位置vendor_right_end - screwdriver_tcp [74.13, -5.18, -47.78] mm质量指标为指标结果Pivot 残差 RMS1.099 mmPivot 最大残差2.141 mm相对旋转跨度161.6°设计矩阵秩6满秩设计矩阵条件数11.42旋转条件数6.05采 TCP 数据时姿态数量并不是唯一目标。只绕工具自身轴旋转或者所有姿态都朝向相近方向即使收集很多样本问题仍可能病态。更重要的是保持同一个支点、覆盖多个俯仰和偏航方向并关注旋转跨度、矩阵秩和条件数。还必须明确一个限制Pivot 方法只约束尖端位置不能唯一确定完整的工具姿态。因此结果文件有意写成parent_frame: vendor_right_end child_frame: screwdriver_tcp translation_m: [0.0741278, -0.0051795, -0.0477772] quaternion_xyzw: null position_only: true orientation_status: unspecified_position_only这里的null不是缺失数据而是对可观测性的诚实表达。在工具轴方向通过机械测量或厂家定义独立确认以前不能为了凑齐 6D 位姿而填入单位四元数。七、把一次性脚本收敛成工程工具包排障完成后项目没有把设备路径、机器人地址和输出目录继续散落在脚本中而是增加了一层配置驱动的工具包。Calibration/ ├── config/tron2_right_arm.yaml # 相机、机器人、工具、标定板和阈值 ├── config.py # 配置加载、路径解析、契约校验 ├── preflight.py # 离线预检不连接真机 ├── validate_config.py # 兼容预检入口 ├── run_handeye.py # 眼在手外统一入口 ├── run_tcp.py # TCP Pivot 统一入口 └── environment.txt # 环境与换设备说明这层封装主要解决了五个工程问题。1. 用一个 YAML 管理设备差异相机内参、SDK 路径、FK 模型、7 个关节的顺序、厂家四元数协议、棋盘格规格、采样数量和输出目录都进入配置文件。更换相机、机器人或工具时优先替换配置而不是复制并修改一套新脚本。配置校验还会主动检查右臂关节列表是否恰好包含 7 个关节厂家right_quat是否明确声明为wxyz棋盘格是否为当前求解器要求的 11×8 内角点、15 mm 方格FK 模型、相机内参、SDK 和工具描述文件是否存在工具描述中是否包含预期的右臂工具链。2. 预检与真机采集分离在连接机器人之前先运行python preflight.py --config config/tron2_right_arm预检脚本的核心逻辑如下import yaml from pathlib import Path def load_config(config_path): with open(config_path, r, encodingutf-8) as f: cfg yaml.safe_load(f) return cfg def validate_config(cfg): errors [] # 右臂关节列表必须恰好包含 7 个关节 if len(cfg[robot][right_arm_joints]) ! 7: errors.append(右臂关节列表必须恰好包含 7 个关节) # 厂家四元数协议必须明确声明为 wxyz if cfg[vendor][quat_order] ! wxyz: errors.append(厂家 right_quat 必须声明为 wxyz 顺序) # 棋盘格规格校验 if cfg[board][inner_corners] ! [11, 8] or cfg[board][square_mm] ! 15: errors.append(棋盘格必须为 11x8 内角点、15 mm 方格) # 关键文件存在性校验 for key in [fk_model, camera_intrinsics, sdk_path, tool_desc]: if not Path(cfg[paths][key]).exists(): errors.append(f缺少文件: {cfg[paths][key]}) return errors def preflight(config_path): cfg load_config(config_path) errors validate_config(cfg) if errors: for e in errors: print(f[FAIL] {e}) return False print([PASS] 预检通过可以连接真机) return True if __name__ __main__: import sys preflight(sys.argv[1])