一、手眼标定的核心目标

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

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

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

3. 

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

---

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

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

1. Eye-in-Hand（眼在手上）

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

2. Eye-to-Hand（眼在手外）

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

坐标系定义：

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

---

二、数学建模与方程推导

1. 坐标变换的齐次矩阵表示

• 齐次变换矩阵 ( T ) 包含旋转矩阵 ( R ) 和平移向量 ( t )：

$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

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

$$T_{\text{base}}^{\text{board}}=T_{\text{base}}^{\text{tool}}\cdot X\cdot T_{\text{cam}}^{\text{board}}$$

2. 手眼标定方程推导

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

  • 末端到基座的变换：

  $$T_{\text{base}}^{\text{tool},i}和T_{\text{base}}^{\text{tool},j}$$

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

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

$$A=(T_{\text{base}}^{\text{tool},i}{)}^{-1}\cdot T_{\text{base}}^{\text{tool},j}$$

标定板从位姿 ( i ) 到 ( j ) 的相对运动为：
$$B=(T_{\text{cam}}^{\text{board},i}{)}^{-1}\cdot T_{\text{cam}}^{\text{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}$$

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

---

三、方程求解方法

1. 分离旋转与平移

将齐次矩阵分解为旋转矩阵 ( R ) 和平移向量 ( t )，方程拆分为：
$$\{\begin{array}{l}{R}_A{R}_X=R_X{R}_B\\ (R_A-I)t_X=R_X{t}_B-t_A\end{array}$$

• 旋转部分：求解 ( R_A R_X = R_X R_B )。
• 平移部分：在已知 ( R_X ) 后，通过线性方程求解 ( t_X )。

2. 旋转矩阵求解

• 四元数法：将旋转矩阵转换为四元数，利用四元数乘法性质求解。
• 轴角法：假设旋转轴 ω 和角度θ，通过特征值分解求解。
• SVD分解：对旋转方程构造矩阵，通过奇异值分解求最小二乘解。

示例（Tsai算法）：

1. 对每对
   $$(A_i,B_i)$$

   构造方程
   $$R_A{R}_X-R_X{R}_B=0$$

2. 将所有方程堆叠成超定方程组，通过SVD求解
   $$R_X$$

3. 平移向量求解

• 将已知

$$R_X$$

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

$$(R_A-I)t_X=R_X{t}_B-t_A$$

• 使用最小二乘法求解
  $$t_X$$

---

四、解的存在性与唯一性

1. 理论条件

• 旋转部分：至少需2组非共轴旋转数据（即绕不同轴的旋转）。
• 平移部分：平移方向需与旋转轴不共线，避免方程秩不足。

2. 实际应用

• 数据量要求：推荐至少10组不同位姿数据，覆盖机械臂工作空间。
• 噪声处理：使用RANSAC或加权最小二乘法抑制异常值影响。

---

五、算法扩展与优化

1. 多传感器标定

• LiDAR-相机标定：将点云与图像特征对齐，扩展方程为 ( A X = Y B )。
• 时间同步：加入时间戳对齐，处理动态场景下的标定。

2. 非线性优化

• 目标函数：最小化重投影误差或运动一致性误差：

$$\underset{X}{\min }\sum _{i=1}^N\Vert A_{i}X-X{B}_i{\Vert }_F^2$$

• 求解器：使用Levenberg-Marquardt算法或李群优化（如Manopt库）。

3. 鲁棒性增强

• 加权最小二乘：根据数据置信度分配权重。
• 抗遮挡策略：在部分特征点丢失时，仍能保持标定稳定性。

---

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

步骤1：构造旋转方程
对每对
$$(A_i,B_i)$$
，提取旋转矩阵
$$R_A,R_B$$
方程写作：
$$R_A{R}_X=R_X{R}_B$$
转换为四元数形式：

https://www.bilibili.com/video/BV1VQ4y1H7F9/?spm_id_from=333.337.search-card.all.click
$$q_A\otimes q_X=q_X\otimes q_B$$
展开后可得线性约束，堆叠所有约束形成超定方程组。

步骤2：SVD求解旋转矩阵

构造矩阵
$$M=\sum _{i=1}^N(R_A^{(i)}\otimes R_B^{(i)})$$
，对 M 进行SVD分解：
$$M=U\Sigma V^T$$
取 V 的最后一列作为
$$R_X$$
的解。

步骤3：求解平移向量
代入 Rx 至平移方程，构建线性方程组：
$$\left[\begin{array}{c}{R}_A^{(1)}-I\\ ⋮\\ R_A^{(N)}-I\end{array}\right]t_X=\left[\begin{array}{c}{R}_X{t}_B^{(1)}-t_A^{(1)}\\ ⋮\\ R_X{t}_B^{(N)}-t_A^{(N)}\end{array}\right]$$

通过伪逆求解
$$t_X=(A^{T}A{)}^{-1}{A}^{T}b$$

---

七、误差分析与验证

1. 重投影误差

将标定板角点通过标定矩阵投影到机械臂坐标系，计算与理论位置的偏差：
$$\text{Error}=\frac{1}{N}\sum _{i=1}^N\Vert P_{\text{base},i}-T_{\text{base}}^{\text{tool},i}\cdot X\cdot P_{\text{cam},i}\Vert$$

2. 运动一致性验证

检查方程 ( A X = X B ) 的残差范数：
$$\text{Residual}=\Vert AX-XB{\Vert }_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. 数据采集

• 标定板位姿采集：

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

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

  2. 在每个位姿下：

     • 记录机械臂末端的位姿

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

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

       在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. 常见问题与解决

       问题	原因	解决方案
       重投影误差大	相机内参不准/畸变未校正	重新校准相机
       位姿估计不稳定	特征点检测抖动	使用亚像素优化或RANSAC
       Z轴方向误差明显	标定板非平面/特征点共面	使用非共面点或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_{\text{base}}^{\text{tool}}和T_{\text{cam}}^{\text{cal}}$$

  • 公式：

  $$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right],R=e^{[\omega {]}_{\times }}$$

  $$其中\omega 为旋转向量，[\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. 标定结果验证

• 重投影误差：
  将标定板特征点通过标定矩阵投影到机械臂坐标系，计算误差：

$$\text{Error}=\frac{1}{N}\sum _{i=1}^N\Vert P_{\text{base},i}-T_{\text{base}}^{\text{tool},i}\cdot X\cdot P_{\text{cam},i}\Vert$$

• 合格标准：误差 < 1 mm。

• 物理抓取测试：

  1. 机械臂根据标定结果抓取标定块，重复10次。
  2. 统计实际抓取位置与理论位置的偏差。
  • 合格标准：抓取成功率 > 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);           // 合并为齐次矩阵
}

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