3D手眼标定转换详细实施步骤及原理概述
- 一、手眼标定的核心目标
- 二、3D手眼标定的原理概述
-
-
-
- 一、基本概念与坐标系定义
- **二、数学建模与方程推导**
-
- [**1. 坐标变换的齐次矩阵表示**](#1. 坐标变换的齐次矩阵表示)
- [**2. 手眼标定方程推导**](#2. 手眼标定方程推导)
- **三、方程求解方法**
-
- [**1. 分离旋转与平移**](#1. 分离旋转与平移)
- [**2. 旋转矩阵求解**](#2. 旋转矩阵求解)
- [**3. 平移向量求解**](#3. 平移向量求解)
- **四、解的存在性与唯一性**
-
- [**1. 理论条件**](#1. 理论条件)
- [**2. 实际应用**](#2. 实际应用)
- **五、算法扩展与优化**
-
- [**1. 多传感器标定**](#1. 多传感器标定)
- [**2. 非线性优化**](#2. 非线性优化)
- [**3. 鲁棒性增强**](#3. 鲁棒性增强)
- **六、数学推导示例（Eye-in-Hand场景）**
- **七、误差分析与验证**
-
- [**1. 重投影误差**](#1. 重投影误差)
- [**2. 运动一致性验证**](#2. 运动一致性验证)
-
-
- **三、实施步骤**
-
- [**1. 标定准备**](#1. 标定准备)
- [**2. 数据采集**](#2. 数据采集)
-
-
-
- [**(1) 棋盘格标定板**](#(1) 棋盘格标定板)
- [**(2) 圆形标定板**](#(2) 圆形标定板)
- [**(3) ArUco/Charuco标定板**](#(3) ArUco/Charuco标定板)
- [**(1) 输入数据**](#(1) 输入数据)
- [**(2) PnP算法选择**](#(2) PnP算法选择)
- [**(3) 代码实现**](#(3) 代码实现)
- [**(1) 多特征点优化**](#(1) 多特征点优化)
- [**(2) 亚像素优化**](#(2) 亚像素优化)
- [**(3) 畸变校正**](#(3) 畸变校正)
- [**(4) 异常值剔除**](#(4) 异常值剔除)
- [**(5) 多帧平均**](#(5) 多帧平均)
- [**(1) 重投影误差**](#(1) 重投影误差)
- [**(2) 物理验证**](#(2) 物理验证)
-
-
- [**3. 位姿数据转换**](#3. 位姿数据转换)
- [**4. 手眼标定计算**](#4. 手眼标定计算)
- [**5. 标定结果验证**](#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=[R0t1]∈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} {RARX=RXRB(RA−I)tX=RXtB−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 RARX−RXRB=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=RXtB−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 Xmini=1∑N∥AiX−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 RARX=RXRB
转换为四元数形式：
https://www.bilibili.com/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= RXtB(1)−tA(1)⋮RXtB(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=N1i=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部分用于插值棋盘角的位置，因此它具有标记板的多功能性，因为它允许遮挡或部分视图。此外，由于插值的角属于棋盘，因此在[亚像素](https://so.csdn.net/so/search?q=%E4%BA%9A%E5%83%8F%E7%B4%A0&spm=1001.2101.3001.7020)精度方面非常准确。
在需要高精度的情况下，例如在相机校准中，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 base tool （通过关节编码器计算）。 T_{\\text{base}}\^{\\text{tool}} （通过关节编码器计算）。 Tbasetool（通过关节编码器计算）。
* 通过相机拍摄标定板图像，找标定板上的多个位点来计算标定板位姿
在3D标定中，标定板的位姿（位置和方向）是通过检测其上的多个特征点（如角点、圆心、ArUco标记等）计算得到的。以下是详细的步骤和方法：
**1. 标定板设计与特征点选择**
标定板需设计为具有明确且可重复检测的特征点，常见类型包括：
* **棋盘格标定板**：黑白相间方格，角点为特征点。
* **圆形标定板**：排列规则的圆点，圆心为特征点。
* **ArUco/Charuco标定板**：结合棋盘格和二维码标记，提供唯一ID和角点。
* **自定义标记**：非对称几何图案，如L形、T形等。
**关键设计原则**：
* **特征点唯一性**：确保每个特征点可被唯一识别（如ArUco的ID或非对称布局）。
* **高对比度**：特征点与背景对比明显（如黑白棋盘格）。
* **已知几何尺寸**：标定板的物理尺寸（如方格边长、圆点间距）需预先测量并输入算法。
**2. 特征点检测方法**
###### **(1) 棋盘格标定板**
* **检测原理**：通过图像处理寻找黑白方格的交点（角点）。
* **OpenCV实现**：
```python
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实现**：
```python
circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, dp=1, minDist=20,
param1=50, param2=30, minRadius=10, maxRadius=50)
```
###### **(3) ArUco/Charuco标定板**
* **检测原理**：识别二维码标记并提取角点。
* **OpenCV实现**：
```python
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) 代码实现**
```python
# 定义标定板的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) 亚像素优化**
对检测到的角点或圆心进行亚像素级优化，减少图像量化误差：
```python
cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
```
###### **(3) 畸变校正**
使用预先标定的相机畸变系数（`dist_coeffs`）校正图像坐标：
```python
undistorted_corners = cv2.undistortPoints(corners, camera_matrix, dist_coeffs)
```
###### **(4) 异常值剔除**
使用RANSAC或LMeds算法过滤错误匹配点：
```python
ret, rvec, tvec, inliers = cv2.solvePnPRansac(obj_pts, img_pts, camera_matrix, dist_coeffs)
```
###### **(5) 多帧平均**
采集多帧数据计算位姿，取平均值或中位数抑制瞬时噪声。
**5. 验证标定板位姿**
###### **(1) 重投影误差**
将计算出的位姿反投影到图像，计算与检测点的误差：
```python
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） |
* **数据格式示例**：
```cpp
// 机械臂末端位姿（X, Y, Z, Rx, Ry, Rz）
Mat ToolPose = (Mat_