404 lines
11 KiB
Markdown
404 lines
11 KiB
Markdown
# HandEyeCalib 手眼标定模块
|
||
|
||
## 概述
|
||
|
||
HandEyeCalib 是一个基于 Eigen 库实现的手眼标定模块，支持 Eye-To-Hand（眼在手外）和 Eye-In-Hand（眼在手上）两种标定模式。
|
||
|
||
## 依赖
|
||
|
||
- Eigen 3.3.9+（仅头文件）
|
||
- C++11 或更高版本
|
||
|
||
## 两种标定模式
|
||
|
||
### Eye-To-Hand（眼在手外）
|
||
|
||
相机固定安装在外部（如支架上），不随机器人运动。
|
||
|
||
```
|
||
┌─────────┐
|
||
│ 相机 │ ← 固定位置
|
||
└────┬────┘
|
||
│ 观测
|
||
▼
|
||
┌─────────┐ ┌─────────┐
|
||
│ 标定点 │ ←──→ │ 机器人 │
|
||
└─────────┘ └─────────┘
|
||
```
|
||
|
||
**求解目标**：相机坐标系到机器人基座坐标系的变换矩阵
|
||
|
||
**数学模型**：
|
||
```
|
||
P_robot = R * P_camera + T
|
||
```
|
||
|
||
### Eye-In-Hand（眼在手上）
|
||
|
||
相机安装在机器人末端，随机器人运动。
|
||
|
||
```
|
||
┌─────────┐
|
||
│ 机器人 │
|
||
└────┬────┘
|
||
│
|
||
┌────┴────┐
|
||
│ 末端 │
|
||
├─────────┤
|
||
│ 相机 │ ← 随末端运动
|
||
└────┬────┘
|
||
│ 观测
|
||
▼
|
||
┌─────────┐
|
||
│ 标定点 │ ← 固定位置
|
||
└─────────┘
|
||
```
|
||
|
||
**求解目标**：相机坐标系到末端坐标系的变换矩阵
|
||
|
||
**数学模型**（AX=XB 问题）：
|
||
```
|
||
T_end * T_cam * P_camera = P_base
|
||
```
|
||
|
||
## 算法原理
|
||
|
||
### SVD 分解法（Eye-To-Hand）
|
||
|
||
1. 计算两组点的质心
|
||
2. 去中心化处理
|
||
3. 构建协方差矩阵 W = Σ(q1 * q2^T)
|
||
4. SVD 分解：W = U * S * V^T
|
||
5. 计算旋转矩阵：R = V * M * U^T
|
||
6. 计算平移向量：T = p2 - R * p1
|
||
|
||
### Tsai-Lenz 方法（Eye-In-Hand）
|
||
|
||
1. 构建相邻位姿间的相对变换 A = T_end_i^(-1) * T_end_j
|
||
2. 构建超定方程组
|
||
3. SVD 求解旋转矩阵
|
||
4. 最小二乘求解平移向量
|
||
|
||
## 欧拉角旋转顺序
|
||
|
||
支持6种常用旋转顺序：
|
||
|
||
| 枚举值 | 旋转顺序 | 说明 |
|
||
|--------|----------|------|
|
||
| `HECEulerOrder::XYZ` | X→Y→Z | Roll-Pitch-Yaw |
|
||
| `HECEulerOrder::XZY` | X→Z→Y | |
|
||
| `HECEulerOrder::YXZ` | Y→X→Z | |
|
||
| `HECEulerOrder::YZX` | Y→Z→X | |
|
||
| `HECEulerOrder::ZXY` | Z→X→Y | |
|
||
| `HECEulerOrder::ZYX` | Z→Y→X | Yaw-Pitch-Roll，机器人常用 |
|
||
|
||
## API 参考
|
||
|
||
### 创建/销毁实例
|
||
|
||
```cpp
|
||
// 创建实例
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
|
||
// 使用完毕后销毁
|
||
DestroyHandEyeCalibInstance(calib);
|
||
```
|
||
|
||
### Eye-To-Hand 标定
|
||
|
||
```cpp
|
||
int CalculateRT(
|
||
const std::vector<HECPoint3D>& eyePoints, // 相机坐标系下的点
|
||
const std::vector<HECPoint3D>& robotPoints, // 机器人坐标系下的点
|
||
HECCalibResult& result); // 输出结果
|
||
```
|
||
|
||
### Eye-In-Hand 标定
|
||
|
||
```cpp
|
||
// 标定点位置未知
|
||
int CalculateEyeInHand(
|
||
const std::vector<HECEyeInHandData>& calibData, // 标定数据
|
||
HECCalibResult& result); // 输出结果
|
||
|
||
// 标定点位置已知
|
||
int CalculateEyeInHandWithTarget(
|
||
const std::vector<HECEyeInHandData>& calibData, // 标定数据
|
||
const HECPoint3D& targetInBase, // 标定点在基座下的坐标
|
||
HECCalibResult& result); // 输出结果
|
||
```
|
||
|
||
### 坐标变换
|
||
|
||
```cpp
|
||
// 完整变换: dst = R * src + T
|
||
void TransformPoint(
|
||
const HECRotationMatrix& R,
|
||
const HECTranslationVector& T,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint);
|
||
|
||
// 仅旋转: dst = R * src
|
||
void RotatePoint(
|
||
const HECRotationMatrix& R,
|
||
const HECPoint3D& srcPoint,
|
||
HECPoint3D& dstPoint);
|
||
```
|
||
|
||
### 欧拉角转换
|
||
|
||
```cpp
|
||
// 旋转矩阵 → 欧拉角
|
||
void RotationMatrixToEuler(
|
||
const HECRotationMatrix& R,
|
||
HECEulerOrder order,
|
||
HECEulerAngles& angles);
|
||
|
||
// 欧拉角 → 旋转矩阵
|
||
void EulerToRotationMatrix(
|
||
const HECEulerAngles& angles,
|
||
HECEulerOrder order,
|
||
HECRotationMatrix& R);
|
||
```
|
||
|
||
## 使用示例
|
||
|
||
### 示例1：Eye-To-Hand 标定
|
||
|
||
```cpp
|
||
#include "IHandEyeCalib.h"
|
||
|
||
void EyeToHandCalibration()
|
||
{
|
||
// 创建标定实例
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
|
||
// 准备标定数据（至少3组对应点）
|
||
std::vector<HECPoint3D> cameraPoints; // 相机坐标系下的点
|
||
std::vector<HECPoint3D> robotPoints; // 机器人坐标系下的点
|
||
|
||
// 添加标定点（示例数据）
|
||
cameraPoints.push_back(HECPoint3D(100.0, 50.0, 200.0));
|
||
robotPoints.push_back(HECPoint3D(500.0, 300.0, 100.0));
|
||
|
||
cameraPoints.push_back(HECPoint3D(150.0, 80.0, 210.0));
|
||
robotPoints.push_back(HECPoint3D(550.0, 330.0, 110.0));
|
||
|
||
cameraPoints.push_back(HECPoint3D(120.0, 60.0, 195.0));
|
||
robotPoints.push_back(HECPoint3D(520.0, 310.0, 95.0));
|
||
|
||
// 更多点...（建议10组以上）
|
||
|
||
// 执行标定
|
||
HECCalibResult result;
|
||
int ret = calib->CalculateRT(cameraPoints, robotPoints, result);
|
||
|
||
if (ret == 0) {
|
||
// 标定成功，输出结果
|
||
printf("标定误差: %.3f mm\n", result.error);
|
||
|
||
// 使用标定结果进行坐标转换
|
||
HECPoint3D cameraPoint(130.0, 70.0, 205.0);
|
||
HECPoint3D robotPoint;
|
||
calib->TransformPoint(result.R, result.T, cameraPoint, robotPoint);
|
||
|
||
printf("相机点 (%.2f, %.2f, %.2f) -> 机器人点 (%.2f, %.2f, %.2f)\n",
|
||
cameraPoint.x, cameraPoint.y, cameraPoint.z,
|
||
robotPoint.x, robotPoint.y, robotPoint.z);
|
||
}
|
||
|
||
// 销毁实例
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
```
|
||
|
||
### 示例2：Eye-In-Hand 标定（标定点位置未知）
|
||
|
||
```cpp
|
||
#include "IHandEyeCalib.h"
|
||
|
||
void EyeInHandCalibration()
|
||
{
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
|
||
// 准备标定数据
|
||
std::vector<HECEyeInHandData> calibData;
|
||
|
||
// 在不同位姿下采集数据
|
||
for (int i = 0; i < 10; i++) {
|
||
HECEyeInHandData data;
|
||
|
||
// 获取当前末端位姿（从机器人控制器读取）
|
||
// data.endPose = getRobotEndPose();
|
||
|
||
// 示例：构建末端位姿矩阵
|
||
HECRotationMatrix R_end;
|
||
HECTranslationVector T_end(500.0 + i * 10, 200.0, 300.0 - i * 5);
|
||
data.endPose = HECHomogeneousMatrix(R_end, T_end);
|
||
|
||
// 获取相机观测到的标定点坐标
|
||
// data.targetInCamera = detectTargetInCamera();
|
||
data.targetInCamera = HECPoint3D(100.0 - i * 2, 50.0 + i, 200.0);
|
||
|
||
calibData.push_back(data);
|
||
}
|
||
|
||
// 执行标定
|
||
HECCalibResult result;
|
||
int ret = calib->CalculateEyeInHand(calibData, result);
|
||
|
||
if (ret == 0) {
|
||
printf("Eye-In-Hand 标定成功\n");
|
||
printf("标定误差: %.3f mm\n", result.error);
|
||
|
||
// 输出相机到末端的变换矩阵
|
||
printf("旋转矩阵 R:\n");
|
||
for (int i = 0; i < 3; i++) {
|
||
printf(" [%.6f, %.6f, %.6f]\n",
|
||
result.R.at(i, 0), result.R.at(i, 1), result.R.at(i, 2));
|
||
}
|
||
printf("平移向量 T: [%.3f, %.3f, %.3f]\n",
|
||
result.T.at(0), result.T.at(1), result.T.at(2));
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
```
|
||
|
||
### 示例3：Eye-In-Hand 标定（标定点位置已知）
|
||
|
||
```cpp
|
||
#include "IHandEyeCalib.h"
|
||
|
||
void EyeInHandCalibrationWithTarget()
|
||
{
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
|
||
std::vector<HECEyeInHandData> calibData;
|
||
|
||
// 采集多组数据（同示例2）
|
||
// ...
|
||
|
||
// 标定点在基座坐标系下的已知位置
|
||
HECPoint3D targetInBase(600.0, 400.0, 50.0);
|
||
|
||
// 执行标定
|
||
HECCalibResult result;
|
||
int ret = calib->CalculateEyeInHandWithTarget(calibData, targetInBase, result);
|
||
|
||
if (ret == 0) {
|
||
printf("标定成功，误差: %.3f mm\n", result.error);
|
||
}
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
```
|
||
|
||
### 示例4：欧拉角转换
|
||
|
||
```cpp
|
||
#include "IHandEyeCalib.h"
|
||
|
||
void EulerAngleConversion()
|
||
{
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
|
||
// 从欧拉角创建旋转矩阵
|
||
HECEulerAngles angles;
|
||
angles.roll = 0.1; // 弧度
|
||
angles.pitch = 0.2;
|
||
angles.yaw = 0.3;
|
||
|
||
HECRotationMatrix R;
|
||
|
||
// 使用 ZYX 顺序（机器人常用）
|
||
calib->EulerToRotationMatrix(angles, HECEulerOrder::ZYX, R);
|
||
|
||
// 使用 XYZ 顺序
|
||
calib->EulerToRotationMatrix(angles, HECEulerOrder::XYZ, R);
|
||
|
||
// 从旋转矩阵提取欧拉角
|
||
HECEulerAngles extractedAngles;
|
||
calib->RotationMatrixToEuler(R, HECEulerOrder::ZYX, extractedAngles);
|
||
|
||
// 角度制转换
|
||
double roll_deg, pitch_deg, yaw_deg;
|
||
extractedAngles.toDegrees(roll_deg, pitch_deg, yaw_deg);
|
||
printf("欧拉角 (度): Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
|
||
roll_deg, pitch_deg, yaw_deg);
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
```
|
||
|
||
### 示例5：完整的位姿转换
|
||
|
||
```cpp
|
||
#include "IHandEyeCalib.h"
|
||
|
||
void PoseTransformation()
|
||
{
|
||
IHandEyeCalib* calib = CreateHandEyeCalibInstance();
|
||
|
||
// 假设已完成标定
|
||
HECCalibResult calibResult;
|
||
// ... 标定过程 ...
|
||
|
||
// 相机检测到的目标位置
|
||
HECPoint3D eyePoint(100.0, 50.0, 200.0);
|
||
|
||
// 相机检测到的目标姿态（X/Y/Z轴方向向量）
|
||
std::vector<HECPoint3D> eyeDirVectors;
|
||
eyeDirVectors.push_back(HECPoint3D(1.0, 0.0, 0.0)); // X轴
|
||
eyeDirVectors.push_back(HECPoint3D(0.0, 1.0, 0.0)); // Y轴
|
||
eyeDirVectors.push_back(HECPoint3D(0.0, 0.0, 1.0)); // Z轴
|
||
|
||
// 转换到机器人坐标系
|
||
HECPoseResult poseResult;
|
||
calib->TransformPose(calibResult, eyePoint, eyeDirVectors, false, poseResult);
|
||
|
||
// 输出机器人位姿
|
||
printf("机器人位置: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||
poseResult.position.x, poseResult.position.y, poseResult.position.z);
|
||
|
||
double roll_deg, pitch_deg, yaw_deg;
|
||
poseResult.angles.toDegrees(roll_deg, pitch_deg, yaw_deg);
|
||
printf("机器人姿态: Roll=%.2f°, Pitch=%.2f°, Yaw=%.2f°\n",
|
||
roll_deg, pitch_deg, yaw_deg);
|
||
|
||
DestroyHandEyeCalibInstance(calib);
|
||
}
|
||
```
|
||
|
||
## 标定建议
|
||
|
||
1. **采集点数**：建议采集 10-20 组标定点
|
||
2. **点分布**：标定点应均匀分布在工作空间内
|
||
3. **避免共面**：标定点不应全部位于同一平面
|
||
4. **精度要求**：机器人示教精度和相机检测精度直接影响标定结果
|
||
5. **验证**：标定完成后，使用新的测试点验证标定精度
|
||
|
||
## 错误码
|
||
|
||
| 错误码 | 说明 |
|
||
|--------|------|
|
||
| 0 (SUCCESS) | 成功 |
|
||
| APP_ERR_PARAM | 参数错误（点数不足或不匹配） |
|
||
|
||
## 文件结构
|
||
|
||
```
|
||
Module/HandEyeCalib/
|
||
├── Inc/
|
||
│ ├── HandEyeCalib_global.h # 导出宏定义
|
||
│ ├── HandEyeCalibTypes.h # 数据类型定义
|
||
│ └── IHandEyeCalib.h # 接口定义
|
||
├── _Inc/
|
||
│ └── HandEyeCalib.h # 实现类声明
|
||
├── Src/
|
||
│ └── HandEyeCalib.cpp # 实现
|
||
├── Doc/
|
||
│ └── HandEyeCalib.md # 本文档
|
||
└── HandEyeCalib.pro # Qt 工程文件
|
||
```
|