3D手眼标定转换详细实施步骤及原理概述

3D手眼标定转换详细实施步骤及原理概述 一、手眼标定的核心目标二、3D手眼标定的原理概述一、基本概念与坐标系定义**二、数学建模与方程推导****1. 坐标变换的齐次矩阵表示****2. 手眼标定方程推导** **三、方程求解方法****1. 分离旋转与平移****2. 旋转矩阵求解****3. 平移向量求解** **四、解的存在性与唯一性****1. 理论条件****2. 实际应用** **五、算法扩展与优化****1. 多传感器标定****2. 非线性优化****3. 鲁棒性增强** **六、数学推导示例（Eye-in-Hand场景）****七、误差分析与验证****1. 重投影误差****2. 运动一致性验证** **三、实施步骤****1. 标定准备****2. 数据采集****(1) 棋盘格标定板****(2) 圆形标定板****(3) ArUco/Charuco标定板****(1) 输入数据****(2) PnP算法选择****(3) 代码实现****(1) 多特征点优化****(2) 亚像素优化****(3) 畸变校正****(4) 异常值剔除****(5) 多帧平均****(1) 重投影误差****(2) 物理验证** **3. 位姿数据转换****4. 手眼标定计算****5. 标定结果验证** **四、常见问题与解决**

一、手眼标定的核心目标

通过数学方法确定传感器（如相机）与机械臂之间的空间变换关系，实现以下两种场景的精准坐标转换：

Eye-in-Hand（眼在手上）：相机安装在机械臂末端，标定相机到机械臂末端的变换矩阵 ( X )。 T cam cal T_{\text{cam}}^{\text{cal}} Tcamcal​

Eye-to-Hand（眼在手外）：相机固定在工作空间外，标定相机到机械臂基座的变换矩阵 ( Y )。

二、3D手眼标定的原理概述

---

一、基本概念与坐标系定义

手眼标定的核心目标是求解传感器（如相机）与机械臂之间的刚性变换矩阵，即确定两者的相对位置和姿态。根据传感器安装位置的不同，分为两种场景：

Eye-in-Hand（眼在手上）

相机安装在机械臂末端，标定相机到末端的变换矩阵 ( X )。方程形式：( A X = X B )。

Eye-to-Hand（眼在手外）

相机固定在工作空间外，标定相机到机械臂基座的变换矩阵 ( Y )。方程形式：( A X = Y B )。

坐标系定义：

基座坐标系（Base）：机械臂的固定参考系。末端坐标系（Tool）：机械臂末端执行器的坐标系。相机坐标系（Camera）：相机的光学中心坐标系。标定板坐标系（Calibration Board）：标定板上的局部坐标系。

---

二、数学建模与方程推导 1. 坐标变换的齐次矩阵表示 齐次变换矩阵 ( T ) 包含旋转矩阵 ( R ) 和平移向量 ( t )：

T = [ R t 0 1 ] ∈ R 4 × 4 T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} T=[R0​t1​]∈R4×4

变换链示例（Eye-in-Hand）： 基座 → 末端 → 相机 → 标定板，对应的变换关系为：

T base board = T base tool ⋅ X ⋅ T cam board T_{\text{base}}^{\text{board}} = T_{\text{base}}^{\text{tool}} \cdot X \cdot T_{\text{cam}}^{\text{board}} Tbaseboard​=Tbasetool​⋅X⋅Tcamboard​

2. 手眼标定方程推导

数据采集过程： 机械臂移动到两个不同位姿 ( i ) 和 ( j )，分别记录：

末端到基座的变换：

T base tool , i 和 T base tool , j T_{\text{base}}^{\text{tool},i} 和 T_{\text{base}}^{\text{tool},j} Tbasetool,i​和Tbasetool,j​

标定板到相机的变换： T cam board , i 和 T cam board , j T_{\text{cam}}^{\text{board},i} 和 T_{\text{cam}}^{\text{board},j} Tcamboard,i​和Tcamboard,j​

相对运动关系： 机械臂末端从位姿 ( i ) 到 ( j ) 的相对运动为：

A = ( T base tool , i ) − 1 ⋅ T base tool , j A = (T_{\text{base}}^{\text{tool},i})^{-1} \cdot T_{\text{base}}^{\text{tool},j} A=(Tbasetool,i​)−1⋅Tbasetool,j​

标定板从位姿 ( i ) 到 ( j ) 的相对运动为： B = ( T cam board , i ) − 1 ⋅ T cam board , j B = (T_{\text{cam}}^{\text{board},i})^{-1} \cdot T_{\text{cam}}^{\text{board},j} B=(Tcamboard,i​)−1⋅Tcamboard,j​

手眼方程： 由于基座到标定板的变换应一致，可得：

T base tool , i ⋅ X ⋅ T cam board , i = T base tool , j ⋅ X ⋅ T cam board , j T_{\text{base}}^{\text{tool},i} \cdot X \cdot T_{\text{cam}}^{\text{board},i} = T_{\text{base}}^{\text{tool},j} \cdot X \cdot T_{\text{cam}}^{\text{board},j} Tbasetool,i​⋅X⋅Tcamboard,i​=Tbasetool,j​⋅X⋅Tcamboard,j​

化简后得到核心方程： A X = X B (Eye-in-Hand) A X = X B \quad \text{(Eye-in-Hand)} AX=XB(Eye-in-Hand)

---

三、方程求解方法 1. 分离旋转与平移

将齐次矩阵分解为旋转矩阵 ( R ) 和平移向量 ( t )，方程拆分为： { R A R X = R X R B ( R A − I ) t X = R X t B − t A \begin{cases} R_A R_X = R_X R_B \\ (R_A - I) t_X = R_X t_B - t_A \end{cases} {RA​RX​=RX​RB​(RA​−I)tX​=RX​tB​−tA​​

旋转部分：求解 ( R_A R_X = R_X R_B )。平移部分：在已知 ( R_X ) 后，通过线性方程求解 ( t_X )。 2. 旋转矩阵求解 四元数法：将旋转矩阵转换为四元数，利用四元数乘法性质求解。轴角法：假设旋转轴 ω 和角度θ，通过特征值分解求解。SVD分解：对旋转方程构造矩阵，通过奇异值分解求最小二乘解。

示例（Tsai算法）：

对每对 ( A i , B i ) (A_i, B_i) (Ai​,Bi​)

构造方程 R A R X − R X R B = 0 R_A R_X - R_X R_B = 0 RA​RX​−RX​RB​=0

将所有方程堆叠成超定方程组，通过SVD求解 R X R_X RX​

3. 平移向量求解 将已知

R X R_X RX​

代入平移方程，构造线性方程组：

( R A − I ) t X = R X t B − t A (R_A - I) t_X = R_X t_B - t_A (RA​−I)tX​=RX​tB​−tA​

使用最小二乘法求解 t X t_X tX​

---

四、解的存在性与唯一性 1. 理论条件 旋转部分：至少需2组非共轴旋转数据（即绕不同轴的旋转）。平移部分：平移方向需与旋转轴不共线，避免方程秩不足。 2. 实际应用 数据量要求：推荐至少10组不同位姿数据，覆盖机械臂工作空间。噪声处理：使用RANSAC或加权最小二乘法抑制异常值影响。

---

五、算法扩展与优化 1. 多传感器标定 LiDAR-相机标定：将点云与图像特征对齐，扩展方程为 ( A X = Y B )。时间同步：加入时间戳对齐，处理动态场景下的标定。 2. 非线性优化 目标函数：最小化重投影误差或运动一致性误差：

min ⁡ X ∑ i = 1 N ∥ A i X − X B i ∥ F 2 \min_X \sum_{i=1}^{N} \| A_i X - X B_i \|_F^2 Xmin​i=1∑N​∥Ai​X−XBi​∥F2​

求解器：使用Levenberg-Marquardt算法或李群优化（如Manopt库）。 3. 鲁棒性增强 加权最小二乘：根据数据置信度分配权重。抗遮挡策略：在部分特征点丢失时，仍能保持标定稳定性。

---

六、数学推导示例（Eye-in-Hand场景）

步骤1：构造旋转方程 对每对 ( A i , B i ) (A_i, B_i) (Ai​,Bi​) ，提取旋转矩阵 R A , R B R_A, R_B RA​,RB​ 方程写作： R A R X = R X R B R_A R_X = R_X R_B RA​RX​=RX​RB​ 转换为四元数形式：

.bilibili /video/BV1VQ4y1H7F9/?spm_id_from=333.337.search-card.all.click q A ⊗ q X = q X ⊗ q B q_A \otimes q_X = q_X \otimes q_B qA​⊗qX​=qX​⊗qB​ 展开后可得线性约束，堆叠所有约束形成超定方程组。

步骤2：SVD求解旋转矩阵

构造矩阵 M = ∑ i = 1 N ( R A ( i ) ⊗ R B ( i ) ) M = \sum_{i=1}^{N} (R_A^{(i)} \otimes R_B^{(i)}) M=i=1∑N​(RA(i)​⊗RB(i)​) ，对 M 进行SVD分解： M = U Σ V T M = U \Sigma V^T M=UΣVT 取 V 的最后一列作为 R X R_X RX​ 的解。

步骤3：求解平移向量 代入 Rx 至平移方程，构建线性方程组： [ R A ( 1 ) − I ⋮ R A ( N ) − I ] t X = [ R X t B ( 1 ) − t A ( 1 ) ⋮ R X t B ( N ) − t A ( N ) ] \begin{bmatrix} R_A^{(1)} - I \\ \vdots \\ R_A^{(N)} - I \end{bmatrix} t_X = \begin{bmatrix} R_X t_B^{(1)} - t_A^{(1)} \\ \vdots \\ R_X t_B^{(N)} - t_A^{(N)} \end{bmatrix} ​RA(1)​−I⋮RA(N)​−I​ ​tX​= ​RX​tB(1)​−tA(1)​⋮RX​tB(N)​−tA(N)​​ ​

通过伪逆求解 t X = ( A T A ) − 1 A T b t_X = (A^T A)^{-1} A^T b tX​=(ATA)−1ATb

---

七、误差分析与验证 1. 重投影误差

将标定板角点通过标定矩阵投影到机械臂坐标系，计算与理论位置的偏差： Error = 1 N ∑ i = 1 N ∥ P base , i − T base tool , i ⋅ X ⋅ P cam , i ∥ \text{Error} = \frac{1}{N} \sum_{i=1}^{N} \| P_{\text{base},i} - T_{\text{base}}^{\text{tool},i} \cdot X \cdot P_{\text{cam},i} \| Error=N1​i=1∑N​∥Pbase,i​−Tbasetool,i​⋅X⋅Pcam,i​∥

2. 运动一致性验证

检查方程 ( A X = X B ) 的残差范数： Residual = ∥ A X − X B ∥ F \text{Residual} = \| A X - X B \|_F Residual=∥AX−XB∥F​

三、实施步骤 1. 标定准备

标定块设计：

选择高精度标定板（如棋盘格、Charuco板、ArUco标记板）。

ArUco部分用于插值棋盘角的位置，因此它具有标记板的多功能性，因为它允许遮挡或部分视图。此外，由于插值的角属于棋盘，因此在亚像素精度方面非常准确。 在需要高精度的情况下，例如在相机校准中，Charuco 棋盘比标准的 Aruco 棋盘更好。

标定块需具有明确几何特征（角点、圆形标记等），且非对称设计(4*4,6*6)以避免误识别。

传感器与机械臂配置：

Eye-in-Hand：将相机刚性固定在机械臂末端。Eye-to-Hand：将相机固定于工作台顶部，覆盖机械臂工作范围。确保标定块在相机视野内，且机械臂可达。

数据采集设备：

图像：华睿3D相机

点来源：2D找点转成3D，

每个高度数据的XY是如何获取或者计算的

举例来说，深度图中第r行c列的像素点，其深度值为d，深度图对应的相机内参为（fx,cx,fy,cy）（如果深度图对齐到彩色图上，则采用彩色图的相机内参），则可通过如下公式计算该像素点对应的三维坐标(x,y,z)

机械臂：信源。

2. 数据采集

标定板位姿采集：

机械臂抓取标定块，移动至不同位姿（至少15组），覆盖以下运动：

旋转：绕X/Y/Z轴旋转（俯仰、偏航、横滚）。平移：沿X/Y/Z轴移动，覆盖工作空间不同区域。

在每个位姿下：

记录机械臂末端的位姿

T base tool （通过关节编码器计算）。 T_{\text{base}}^{\text{tool}} （通过关节编码器计算）。 Tbasetool​（通过关节编码器计算）。

通过相机拍摄标定板图像，找标定板上的多个位点来计算标定板位姿

在3D标定中，标定板的位姿（位置和方向）是通过检测其上的多个特征点（如角点、圆心、ArUco标记等）计算得到的。以下是详细的步骤和方法：

1. 标定板设计与特征点选择

标定板需设计为具有明确且可重复检测的特征点，常见类型包括：

棋盘格标定板：黑白相间方格，角点为特征点。圆形标定板：排列规则的圆点，圆心为特征点。ArUco/Charuco标定板：结合棋盘格和二维码标记，提供唯一ID和角点。自定义标记：非对称几何图案，如L形、T形等。

关键设计原则：

特征点唯一性：确保每个特征点可被唯一识别（如ArUco的ID或非对称布局）。高对比度：特征点与背景对比明显（如黑白棋盘格）。已知几何尺寸：标定板的物理尺寸（如方格边长、圆点间距）需预先测量并输入算法。

2. 特征点检测方法

(1) 棋盘格标定板

检测原理：通过图像处理寻找黑白方格的交点（角点）。

OpenCV实现：

import cv2 # 读取图像并检测角点 image = cv2.imread("chessboard.png") gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, (rows, cols), None) # 亚像素优化 if ret: criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) (2) 圆形标定板

检测原理：通过霍夫变换检测圆心的亚像素位置。

OpenCV实现：

circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, dp=1, minDist=20, param1=50, param2=30, minRadius=10, maxRadius=50) (3) ArUco/Charuco标定板

检测原理：识别二维码标记并提取角点。

OpenCV实现：

aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250) charuco_board = cv2.aruco.CharucoBoard_create(5, 7, 0.04, 0.02, aruco_dict) corners, ids, _ = cv2.aruco.detectMarkers(image, aruco_dict)

3. 位姿计算（PnP算法）

通过检测到的特征点的图像坐标和其对应的3D世界坐标，使用 Perspective-n-Point (PnP) 算法计算标定板的6自由度位姿。

(1) 输入数据 图像坐标：检测到的特征点像素坐标（2D点）。世界坐标：标定板上特征点的物理坐标（3D点），需根据标定板设计预先定义（如棋盘格原点在左上角，Z=0）。 (2) PnP算法选择 OpenCV函数：cv2.solvePnP算法类型： 迭代法（ITERATIVE）：基于Levenberg-Marquardt优化，需初始猜测。EPnP：无需初始值，适合实时应用。RANSAC-based PnP：抗噪声能力强，适用于部分遮挡场景。 (3) 代码实现 # 定义标定板的3D世界坐标（假设Z=0） obj_pts = np.array([[0,0,0], [0.1,0,0], [0.1,0.1,0], [0,0.1,0]], dtype=np.float32) # 单位：米 # 检测到的图像坐标（假设已通过特征检测获得） img_pts = np.array([[320, 240], [400, 240], [400, 320], [320, 320]], dtype=np.float32) # 相机内参（需预先标定） camera_matrix = np.array([[800, 0, 320], [0, 800, 240], [0, 0, 1]], dtype=np.float32) dist_coeffs = np.zeros((5, 1)) # 假设无畸变 # 计算位姿 ret, rvec, tvec = cv2.solvePnP(obj_pts, img_pts, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE) # 将旋转向量转换为旋转矩阵 R, _ = cv2.Rodrigues(rvec) print("旋转矩阵:\n", R) print("平移向量:\n", tvec)

4. 提高位姿估计精度的关键技巧

(1) 多特征点优化 最少点数：PnP算法至少需要4个非共面点，但更多点可提升精度（推荐≥10个点）。点分布：特征点应覆盖标定板的不同区域，避免集中在一侧。 (2) 亚像素优化

对检测到的角点或圆心进行亚像素级优化，减少图像量化误差：

cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria) (3) 畸变校正

使用预先标定的相机畸变系数（dist_coeffs）校正图像坐标：

undistorted_corners = cv2.undistortPoints(corners, camera_matrix, dist_coeffs) (4) 异常值剔除

使用RANSAC或LMeds算法过滤错误匹配点：

ret, rvec, tvec, inliers = cv2.solvePnPRansac(obj_pts, img_pts, camera_matrix, dist_coeffs) (5) 多帧平均

采集多帧数据计算位姿，取平均值或中位数抑制瞬时噪声。

5. 验证标定板位姿

(1) 重投影误差

将计算出的位姿反投影到图像，计算与检测点的误差：

projected_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, camera_matrix, dist_coeffs) error = np.linalg.norm(projected_pts - img_pts, axis=2).mean() print("重投影误差（像素）:", error) # 应 < 0.5 像素 (2) 物理验证 机械臂触碰验证：控制机械臂末端触碰标定板上的物理点，对比理论坐标与实际坐标。多视角一致性：在不同视角下计算标定板位姿，检查其空间一致性。

6. 常见问题与解决

问题原因解决方案重投影误差大相机内参不准/畸变未校正重新校准相机位姿估计不稳定特征点检测抖动使用亚像素优化或RANSACZ轴方向误差明显标定板非平面/特征点共面使用非共面点或3D标定板部分特征点无法检测遮挡/光照不均优化光照条件或使用抗遮挡算法（如ArUco）

数据格式示例：

// 机械臂末端位姿（X, Y, Z, Rx, Ry, Rz） Mat ToolPose = (Mat_<double>(6,1) << 100.0, 200.0, 300.0, 0.1, -0.2, 1.5); // 标定板在相机中的位姿（X, Y, Z, Rx, Ry, Rz） Mat CalPose = (Mat_<double>(6,1) << 50.0, -30.0, 400.0, -0.3, 0.5, 2.0); 3. 位姿数据转换

欧拉角/旋转向量 → 旋转矩阵： 将机械臂和标定板的位姿转换为齐次变换矩阵 T base tool 和 T cam cal T_{\text{base}}^{\text{tool}} 和 T_{\text{cam}}^{\text{cal}} Tbasetool​和Tcamcal​

公式：

T = [ R t 0 1 ] , R = e [ ω ] × T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}, \quad R = e^{[\omega]_\times} T=[R0​t1​],R=e[ω]×​

其中 ω 为旋转向量， [ ω ] × 为反对称矩阵。 其中 ω 为旋转向量，{[\omega]_\times} 为反对称矩阵。 其中ω为旋转向量，[ω]×​为反对称矩阵。

这是一个包含角速度符号 ω 的公式。

代码示例：

// 将位姿向量转换为齐次矩阵 Mat attitudeVectorToMatrix(Mat& pose) { Mat R, T; Rodrigues(pose({3,0,3,1}), R); // 旋转向量转旋转矩阵 T = pose({0,0,3,1}).clone(); // 提取平移向量 return R_T2RT(R, T); // 合并为齐次矩阵 } 4. 手眼标定计算

数学模型： 手眼标定方程为 ( A X = X B )（Eye-in-Hand）或 ( A X = Y B )（Eye-to-Hand），其中：

( A )：机械臂末端位姿变化（通过关节编码器计算）。( B )：标定板位姿变化（通过相机检测）。( X )：待求的相机到机械臂末端的变换矩阵（Eye-in-Hand）。

标定方法选择：

方法原理适用场景CALIB_HAND_EYE_TSAI基于旋转和平移分步求解快速、中等精度CALIB_HAND_EYE_PARK最小化旋转和平移联合误差高精度、计算量大CALIB_HAND_EYE_HORAUD基于四元数的闭式解抗噪声能力强

代码实现：

// 标定Eye-to-Hand场景（相机固定） calibrateHandEye(R_gripper2base, T_gripper2base, R_target2cam, T_target2cam, R_cam2gripper, T_cam2gripper, CALIB_HAND_EYE_TSAI); 5. 标定结果验证 重投影误差： 将标定板特征点通过标定矩阵投影到机械臂坐标系，计算误差：

Error = 1 N ∑ i = 1 N ∥ P base , i − T base tool , i ⋅ X ⋅ P cam , i ∥ \text{Error} = \frac{1}{N} \sum_{i=1}^{N} \| P_{\text{base},i} - T_{\text{base}}^{\text{tool},i} \cdot X \cdot P_{\text{cam},i} \| Error=N1​i=1∑N​∥Pbase,i​−Tbasetool,i​⋅X⋅Pcam,i​∥

合格标准：误差 < 1 mm。

物理抓取测试：

机械臂根据标定结果抓取标定块，重复10次。统计实际抓取位置与理论位置的偏差。 合格标准：抓取成功率 > 95%（无堆叠场景）。 四、常见问题与解决 问题原因解决方案标定误差 > 2 mm标定板特征点检测不准使用高精度标定板，优化视觉算法抓取点Z轴偏差大深度相机标定误差重新校准深度相机，加入Z轴补偿旋转矩阵非正交机械臂欧拉角定义错误检查欧拉角顺序（内旋/外旋、XYZ/ZYX）标定结果不稳定数据量不足或位姿单一增加数据量，确保覆盖全工作空间

1.标定时找到的3D点是如何找的？ （1）是利用原有的3D原始数据图有对应的算法搜索出的3D点， （2）还是相机输出的2D图通过2D算法搜索特征点最终转换计算得到的3D点， （3）还是有什么其他的什么找点方式

标定时采用棋盘格标定板，通过彩色相机拍摄标定板，提取标定板角点进行标定，标定算法完成找点，不需要其他工具参与。目前本司软件找点可能需要依赖其他工具，按照上述的第二种方式

2.进行3D手眼标定时，标定板不动，移动机械臂，拍摄15+张图片，覆盖多个位姿，并记录每张图片拍摄时机械臂的位姿，位姿数据包括3D点坐标（位置）与姿态（方向），3D点可以通过搜索得出X,Y,Z。但是姿态以什么表示，欧拉角还是四元数还是其他什么格式。 根据相机安装方式不同存在有两种情况 眼在手上： 相机安装在机械臂末端。标定板放置在固定位置，控制机械臂移动相机拍照 眼在手外： 相机安装在机械臂外的固定位置。标定板固定在机械臂末端，控制机械臂移动标定板拍照 机械臂的位姿通常由6个量表示(x,y,z,rx,ry,rz)，前3个为平移量，后3个为旋转量，旋转量一般为欧拉角（也有采用旋转向量、四元数，具体以机械臂厂家为准）

3.机械臂抓取标定块进行标定，搜索的3D点与抓取点不一致标定得出的转换矩阵，进行抓取坐标转换时如何保证搜索到的3D点与抓取点一致 眼在手上：标定的是相机坐标系和机械臂末端坐标系的旋转平移关系 眼在手外：标定的是相机坐标系和机械臂基底坐标系的旋转平移关系 机械臂出厂默认的末端坐标系原点在法兰盘中心，需要根据使用的夹具，按照实际使用需求在机械臂上设置好工具坐标系。标定完成后，得到一个特定的转换矩阵

4.关于坐标转换工具的输入与输出为坐标与位姿角度，坐标为3D坐标，位姿需要相应转换

代码示例：

// 输入时将位姿向量转换为齐次矩阵 Mat attitudeVectorToMatrix(Mat& pose) { Mat R, T; Rodrigues(pose({3,0,3,1}), R); // 旋转向量转旋转矩阵 T = pose({0,0,3,1}).clone(); // 提取平移向量 return R_T2RT(R, T); // 合并为齐次矩阵 }

输出时需要将在通过转换矩阵得到的结果，转换成需要的位姿向量（欧拉角，四元数等）