528 lines
19 KiB
Markdown
528 lines
19 KiB
Markdown
# 业务流程详细说明 - 补充文档
|
||
|
||
## 2️⃣ WorkpiecePositionApp - 工件定位抓取应用
|
||
|
||
**位置**: `App/WorkpieceProject/WorkpiecePosition/WorkpiecePositionApp/`
|
||
|
||
### 业务目标
|
||
实现**机器人精确抓取无序堆放的工件**
|
||
|
||
### 双传感器融合架构
|
||
|
||
```
|
||
┌─────────────────────────────────────────────────────────────┐
|
||
│ WorkpiecePositionApp 传感器配置 │
|
||
└─────────────────────────────────────────────────────────────┘
|
||
|
||
传感器1: BinocularMark (远程 - 通过网络)
|
||
├─ 类型: 双目相机系统 (GalaxyDevice)
|
||
├─ 功能: 提供Mark 3D坐标
|
||
├─ 接入: BinocularMarkReceiver TCP客户端
|
||
└─ 数据: std::vector<SWD_charuco3DMark>
|
||
|
||
传感器2: EpicEyeDevice (本地 - 直接连接)
|
||
├─ 类型: VisionPro相机 (激光线扫描/结构光)
|
||
├─ 功能: 获取工件3D点云
|
||
├─ 接口: Device/EpicEyeDevice/
|
||
└─ 数据: PointCloud (x,y,z点集)
|
||
```
|
||
|
||
### 核心业务流程
|
||
|
||
```
|
||
步骤1: 初始化阶段
|
||
├─ 创建BinocularMarkReceiver实例
|
||
├─ 连接到BinocularMarkApp (IP + 端口5901)
|
||
├─ 注册Mark结果回调: OnMarkResult()
|
||
├─ 初始化EpicEyeDevice相机
|
||
├─ 加载配置: 工件尺寸、抓取点偏移、坐标系标定
|
||
└─ 启动心跳保活 (30秒)
|
||
|
||
步骤2: Mark数据接收 (实时)
|
||
├─ BinocularMarkReceiver::OnMarkResult() 触发
|
||
├─ 接收数据: std::vector<SWD_charuco3DMark>
|
||
│ └─ 每个Mark包含: markID + 3D坐标(x,y,z)
|
||
├─ 数据验证: 检查Mark数量、ID有效性
|
||
├─ 缓存Mark位置
|
||
└─ 触发点云采集
|
||
|
||
步骤3: 点云数据采集
|
||
├─ 调用 EpicEyeDevice::TriggerCapture()
|
||
├─ 等待点云数据回调
|
||
├─ 接收点云: std::vector<SVzNL3DPoint>
|
||
├─ 点云预处理:
|
||
│ ├─ 离群点过滤
|
||
│ ├─ 降采样 (减少计算量)
|
||
│ └─ 坐标系统一 (转到双目坐标系)
|
||
└─ 数据准备完成
|
||
|
||
步骤4: 工件中心定位算法 ⭐核心⭐
|
||
┌─────────────────────────────────────────────────┐
|
||
│ 输入: │
|
||
│ • Mark 3D坐标: marks[] │
|
||
│ • 工件点云: pointCloud │
|
||
│ │
|
||
│ 处理流程: │
|
||
│ 1. 建立Mark坐标系 │
|
||
│ - 使用多个Mark确定工件平面 │
|
||
│ - 计算工件方向向量 │
|
||
│ - 生成坐标变换矩阵 T_mark_to_world │
|
||
│ │
|
||
│ 2. 点云配准与裁剪 │
|
||
│ - 将点云转换到Mark坐标系 │
|
||
│ - ROI裁剪 (基于Mark位置+工件尺寸) │
|
||
│ - 提取目标工件区域点云 │
|
||
│ │
|
||
│ 3. 工件分割 │
|
||
│ - 平面分割 (RANSAC) │
|
||
│ - 点云聚类 (欧式聚类) │
|
||
│ - 选择最大簇作为目标工件 │
|
||
│ │
|
||
│ 4. 中心位置计算 │
|
||
│ - 计算点云质心 (x, y, z) │
|
||
│ - PCA主成分分析 → 姿态角 │
|
||
│ ├─ 第1主成分: 工件长轴方向 │
|
||
│ ├─ 第2主成分: 工件短轴方向 │
|
||
│ └─ 第3主成分: 工件法向量 │
|
||
│ - 转换为欧拉角 (rx, ry, rz) │
|
||
│ │
|
||
│ 5. 抓取点优化 │
|
||
│ - 应用抓取点偏移 (从配置读取) │
|
||
│ - 碰撞检测 (避开周围物体) │
|
||
│ - 抓取稳定性评估 (重心位置) │
|
||
│ │
|
||
│ 输出: │
|
||
│ SSG_6DOF graspPose { │
|
||
│ x, y, z, // 抓取位置 │
|
||
│ x_roll, // 绕X轴旋转 │
|
||
│ y_pitch, // 绕Y轴旋转 │
|
||
│ z_yaw // 绕Z轴旋转 │
|
||
│ } │
|
||
└─────────────────────────────────────────────────┘
|
||
|
||
步骤5: 发送抓取指令
|
||
├─ 坐标转换: Mark坐标系 → 机器人基座标系
|
||
├─ 格式化数据 (符合机器人协议)
|
||
├─ 发送TCP/Serial → 机器人控制器
|
||
├─ 等待执行反馈
|
||
└─ 记录抓取结果
|
||
```
|
||
|
||
### 关键算法实现
|
||
|
||
```cpp
|
||
SSG_6DOF WorkpiecePositionPresenter::calculateWorkpieceCenter(
|
||
const std::vector<SWD_charuco3DMark>& marks,
|
||
const PointCloud& workpieceCloud
|
||
) {
|
||
// 1. 建立Mark坐标系
|
||
Eigen::Matrix4d T_mark = buildMarkCoordinateSystem(marks);
|
||
|
||
// 2. 点云变换与裁剪
|
||
PointCloud transformedCloud = transformPointCloud(workpieceCloud, T_mark);
|
||
PointCloud croppedCloud = cropROI(transformedCloud, marks, workpieceDimension);
|
||
|
||
// 3. 工件分割
|
||
PointCloud workpiece = segmentWorkpiece(croppedCloud);
|
||
|
||
// 4. 中心计算
|
||
Eigen::Vector3d centroid = computeCentroid(workpiece);
|
||
Eigen::Matrix3d covariance = computeCovariance(workpiece, centroid);
|
||
|
||
// PCA特征分解
|
||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covariance);
|
||
Eigen::Matrix3d eigenvectors = solver.eigenvectors();
|
||
|
||
// 转欧拉角
|
||
Eigen::Vector3d eulerAngles = rotationMatrixToEulerAngles(eigenvectors);
|
||
|
||
// 5. 应用抓取偏移
|
||
SSG_6DOF graspPose;
|
||
graspPose.x = centroid.x() + graspOffset.x;
|
||
graspPose.y = centroid.y() + graspOffset.y;
|
||
graspPose.z = centroid.z() + graspOffset.z;
|
||
graspPose.x_roll = eulerAngles.x();
|
||
graspPose.y_pitch = eulerAngles.y();
|
||
graspPose.z_yaw = eulerAngles.z();
|
||
|
||
return graspPose;
|
||
}
|
||
```
|
||
|
||
### 数据流图
|
||
|
||
```
|
||
BinocularMarkApp
|
||
↓ (TCP 5901)
|
||
BinocularMarkReceiver
|
||
↓ (Callback)
|
||
WorkpiecePositionPresenter::OnMarkResult(marks)
|
||
↓
|
||
缓存Mark数据
|
||
↓
|
||
触发: EpicEyeDevice::TriggerCapture()
|
||
↓ (Callback)
|
||
WorkpiecePositionPresenter::OnPointCloudReady(cloud)
|
||
↓
|
||
调用: calculateWorkpieceCenter(marks, cloud)
|
||
↓
|
||
生成: SSG_6DOF graspPose
|
||
↓
|
||
坐标转换 → 机器人坐标系
|
||
↓
|
||
发送TCP/Serial → 机器人控制器
|
||
↓
|
||
执行抓取
|
||
```
|
||
|
||
---
|
||
|
||
## 3️⃣ WorkpieceSpliceApp - 工件拼接装配应用
|
||
|
||
**位置**: `App/WorkpieceProject/WorkpieceSplice/WorkpieceSpliceApp/`
|
||
|
||
### 业务目标
|
||
实现**多工件精确拼接定位**（精度要求：±0.1mm, ±0.5°）
|
||
|
||
### 单传感器架构
|
||
|
||
```
|
||
┌─────────────────────────────────────────────────────────────┐
|
||
│ WorkpieceSpliceApp 传感器配置 │
|
||
└─────────────────────────────────────────────────────────────┘
|
||
|
||
传感器: BinocularMark (远程 - 通过网络)
|
||
├─ 类型: 双目相机系统 (GalaxyDevice)
|
||
├─ 功能: 提供Mark 3D坐标
|
||
├─ 接入: BinocularMarkReceiver TCP客户端
|
||
├─ 数据: std::vector<SWD_charuco3DMark>
|
||
└─ 作用: 闭环反馈，实时监控工件位姿
|
||
|
||
执行机构: 6轴工业机械臂
|
||
├─ 功能: 调整工件位姿
|
||
├─ 通信: TCP/Modbus
|
||
└─ 控制: 位置控制 + 姿态控制
|
||
```
|
||
|
||
### 核心业务流程
|
||
|
||
```
|
||
步骤1: 初始化阶段
|
||
├─ 创建BinocularMarkReceiver实例
|
||
├─ 连接到BinocularMarkApp
|
||
├─ 注册Mark结果回调: OnMarkResult()
|
||
├─ 初始化机械臂通信
|
||
├─ 加载拼接配置:
|
||
│ ├─ 目标Mark位置 (设计位置)
|
||
│ ├─ 拼接公差 (±0.1mm, ±0.5°)
|
||
│ ├─ 最大调整次数
|
||
│ └─ 调整步长参数
|
||
└─ 启动心跳
|
||
|
||
步骤2: Mark数据接收与分析
|
||
├─ BinocularMarkReceiver::OnMarkResult() 触发
|
||
├─ 接收多个工件的Mark数据
|
||
├─ 根据markID识别不同工件:
|
||
│ ├─ 工件A: Mark ID 0-7
|
||
│ ├─ 工件B: Mark ID 8-15
|
||
│ └─ 工件C: Mark ID 16-23
|
||
├─ 分组并缓存各工件Mark坐标
|
||
└─ 触发拼接计算
|
||
|
||
步骤3: 拼接位姿计算 ⭐核心⭐
|
||
┌─────────────────────────────────────────────────┐
|
||
│ 输入: │
|
||
│ • 当前Mark坐标: currentMarks[] │
|
||
│ • 目标Mark坐标: targetMarks[] (from config) │
|
||
│ │
|
||
│ 处理流程: │
|
||
│ 1. 计算当前工件位姿 │
|
||
│ - 基于多个Mark拟合位姿 │
|
||
│ - 使用SVD最小二乘法 │
|
||
│ → currentPose (x,y,z,rx,ry,rz) │
|
||
│ │
|
||
│ 2. 计算目标工件位姿 │
|
||
│ - 读取设计位置的Mark坐标 │
|
||
│ - 拟合目标位姿 │
|
||
│ → targetPose (x,y,z,rx,ry,rz) │
|
||
│ │
|
||
│ 3. 计算位姿差异 │
|
||
│ ΔPose = targetPose - currentPose │
|
||
│ ├─ Δx, Δy, Δz (平移偏差) │
|
||
│ └─ Δrx, Δry, Δrz (旋转偏差) │
|
||
│ │
|
||
│ 4. 公差检查 │
|
||
│ if (|Δx|<0.1mm && |Δy|<0.1mm && │
|
||
│ |Δz|<0.1mm && |Δrx|<0.5° && │
|
||
│ |Δry|<0.5° && |Δrz|<0.5°) { │
|
||
│ withinTolerance = true; │
|
||
│ } │
|
||
│ │
|
||
│ 5. 生成调整指令 │
|
||
│ - 应用阻尼系数 (避免超调) │
|
||
│ - 限制单次调整量 │
|
||
│ → adjustment (x,y,z,rx,ry,rz) │
|
||
│ │
|
||
│ 输出: │
|
||
│ SpliceAdjustment { │
|
||
│ currentPose, // 当前位姿 │
|
||
│ targetPose, // 目标位姿 │
|
||
│ adjustment, // 调整量 │
|
||
│ withinTolerance // 是否满足公差 │
|
||
│ } │
|
||
└─────────────────────────────────────────────────┘
|
||
|
||
步骤4: 闭环控制调整
|
||
├─ 循环: while (!withinTolerance && iterations < maxIterations)
|
||
│ ├─ 发送调整指令 → 机械臂
|
||
│ ├─ 等待机械臂执行完成
|
||
│ ├─ 获取最新Mark数据 (实时反馈)
|
||
│ ├─ 重新计算位姿偏差
|
||
│ ├─ 检查公差
|
||
│ └─ iterations++
|
||
│
|
||
├─ 成功: withinTolerance == true
|
||
│ └─ 执行拼接操作 (压合/焊接/固定)
|
||
│
|
||
└─ 失败: iterations >= maxIterations
|
||
└─ 报警,记录失败数据
|
||
|
||
步骤5: 拼接质量验证
|
||
├─ 拼接完成后再次检测Mark
|
||
├─ 计算最终位姿偏差
|
||
├─ 记录拼接质量数据
|
||
└─ 合格判定 / 不合格报警
|
||
```
|
||
|
||
### 关键算法实现
|
||
|
||
```cpp
|
||
SpliceAdjustment WorkpieceSplicePresenter::calculateSpliceAdjustment(
|
||
const std::vector<SWD_charuco3DMark>& currentMarks,
|
||
const std::vector<SWD_charuco3DMark>& targetMarks
|
||
) {
|
||
SpliceAdjustment result;
|
||
|
||
// 1. SVD拟合当前位姿
|
||
Eigen::Matrix4d T_current = fitPoseSVD(currentMarks);
|
||
result.currentPose = matrixToPose(T_current);
|
||
|
||
// 2. SVD拟合目标位姿
|
||
Eigen::Matrix4d T_target = fitPoseSVD(targetMarks);
|
||
result.targetPose = matrixToPose(T_target);
|
||
|
||
// 3. 计算位姿差异
|
||
Eigen::Matrix4d T_delta = T_target * T_current.inverse();
|
||
result.adjustment = matrixToPose(T_delta);
|
||
|
||
// 应用阻尼系数 (避免振荡)
|
||
double dampingFactor = 0.8;
|
||
result.adjustment.x *= dampingFactor;
|
||
result.adjustment.y *= dampingFactor;
|
||
result.adjustment.z *= dampingFactor;
|
||
result.adjustment.x_roll *= dampingFactor;
|
||
result.adjustment.y_pitch *= dampingFactor;
|
||
result.adjustment.z_yaw *= dampingFactor;
|
||
|
||
// 4. 公差检查
|
||
result.withinTolerance = (
|
||
fabs(result.adjustment.x) < 0.1 &&
|
||
fabs(result.adjustment.y) < 0.1 &&
|
||
fabs(result.adjustment.z) < 0.1 &&
|
||
fabs(result.adjustment.x_roll) < 0.5 &&
|
||
fabs(result.adjustment.y_pitch) < 0.5 &&
|
||
fabs(result.adjustment.z_yaw) < 0.5
|
||
);
|
||
|
||
return result;
|
||
}
|
||
```
|
||
|
||
### 闭环控制流程图
|
||
|
||
```
|
||
BinocularMarkApp (持续检测Mark)
|
||
↓ (TCP实时)
|
||
BinocularMarkReceiver
|
||
↓ (Callback)
|
||
WorkpieceSplicePresenter::OnMarkResult(currentMarks)
|
||
↓
|
||
calculateSpliceAdjustment(currentMarks, targetMarks)
|
||
↓
|
||
生成: adjustment (Δx,Δy,Δz,Δrx,Δry,Δrz)
|
||
↓
|
||
检查: withinTolerance?
|
||
├─ YES → 拼接操作
|
||
└─ NO → 继续调整
|
||
↓
|
||
发送调整指令 → 机械臂
|
||
↓
|
||
等待执行完成 (200ms)
|
||
↓
|
||
获取新Mark数据 (实时反馈)
|
||
↓
|
||
重新计算 (闭环) → 返回检查公差
|
||
↓
|
||
达到公差或超过最大次数 → 结束
|
||
```
|
||
|
||
---
|
||
|
||
## 📊 关键数据结构
|
||
|
||
### Mark 3D数据 (从BinocularMarkApp输出)
|
||
```cpp
|
||
typedef struct {
|
||
int markID; // Mark唯一标识符
|
||
SVzNL3DPoint mark3D; // 3D坐标
|
||
// SVzNL3DPoint定义:
|
||
// struct { double x, y, z; }
|
||
} SWD_charuco3DMark;
|
||
|
||
// 示例数据:
|
||
// Mark 0: (100.5, 200.3, 50.1)
|
||
// Mark 1: (150.2, 200.5, 50.0)
|
||
// Mark 2: (100.3, 250.1, 50.2)
|
||
```
|
||
|
||
### 工件抓取位姿 (WorkpiecePosition输出)
|
||
```cpp
|
||
typedef struct {
|
||
double x, y, z; // 位置 (mm)
|
||
double x_roll; // 绕X轴旋转 (°)
|
||
double y_pitch; // 绕Y轴旋转 (°)
|
||
double z_yaw; // 绕Z轴旋转 (°)
|
||
} SSG_6DOF;
|
||
|
||
// 示例:
|
||
// graspPose = {125.5, 225.0, 50.0, 0.5, -1.2, 90.0}
|
||
```
|
||
|
||
### 拼接调整结果 (WorkpieceSplice输出)
|
||
```cpp
|
||
struct SpliceAdjustment {
|
||
SSG_6DOF currentPose; // 当前位姿
|
||
SSG_6DOF targetPose; // 目标位姿
|
||
SSG_6DOF adjustment; // 调整量
|
||
bool withinTolerance; // 是否满足公差
|
||
|
||
// 示例:
|
||
// adjustment = {-0.05, 0.03, 0.01, -0.2, 0.1, -0.3}
|
||
// withinTolerance = true
|
||
};
|
||
```
|
||
|
||
---
|
||
|
||
## ⚙️ 系统性能指标
|
||
|
||
| 指标项 | BinocularMarkApp | WorkpiecePosition | WorkpieceSplice |
|
||
|--------|------------------|-------------------|-----------------|
|
||
| **精度** | ±0.1mm | ±0.2mm | ±0.1mm, ±0.5° |
|
||
| **速度** | 10-30 FPS | <500ms/次 | <1s/次调整 |
|
||
| **可靠性** | >99%识别率 | 95%成功率 | 2-3次收敛 |
|
||
| **延迟** | <10ms (局域网) | <50ms反馈 | <100ms反馈 |
|
||
|
||
---
|
||
|
||
## 🛠️ 典型应用场景
|
||
|
||
### 场景1: 料箱无序抓取
|
||
```
|
||
1. BinocularMarkApp检测料箱中的多个工件Mark
|
||
2. WorkpiecePositionApp逐个计算抓取位姿
|
||
3. 机器人按顺序抓取工件放到传送带
|
||
4. 循环直到料箱清空
|
||
```
|
||
|
||
### 场景2: 精密装配
|
||
```
|
||
1. BinocularMarkApp同时检测底座和盖板的Mark
|
||
2. WorkpieceSpliceApp计算相对位置偏差
|
||
3. 机械臂调整盖板位姿 (闭环控制)
|
||
4. 满足公差后执行压合/螺栓固定
|
||
5. 质检验证拼接精度
|
||
```
|
||
|
||
### 场景3: 柔性生产线
|
||
```
|
||
1. BinocularMarkApp识别不同型号工件 (通过Mark ID)
|
||
2. WorkpiecePositionApp根据型号选择不同抓取策略
|
||
3. 机器人自适应抓取不同工件
|
||
4. 实现混线生产
|
||
```
|
||
|
||
---
|
||
|
||
## 📁 配置文件位置
|
||
|
||
```
|
||
GrabBag/
|
||
├─ App/BinocularMark/BinocularMarkApp/
|
||
│ └─ config.json # Mark检测服务端配置
|
||
│
|
||
├─ App/WorkpieceProject/
|
||
│ ├─ WorkpiecePositionApp/
|
||
│ │ └─ config.json # 工件定位配置
|
||
│ ├─ WorkpieceSpliceApp/
|
||
│ │ └─ config.json # 工件拼接配置
|
||
│ ├─ BUSINESS_FLOW.md # 业务流程总览
|
||
│ └─ BUSINESS_FLOW_DETAILS.md # 本文档（详细说明）
|
||
```
|
||
|
||
---
|
||
|
||
## 🔗 模块依赖关系
|
||
|
||
```
|
||
BinocularMarkApp
|
||
├─ Device/GalaxyDevice # 大恒相机驱动
|
||
├─ SDK/binocularMark # Mark检测算法
|
||
├─ SDK/OpenCV320 # 图像处理
|
||
└─ VrNets/VrTCPServer # TCP通信
|
||
|
||
BinocularMarkReceiver
|
||
├─ SDK/binocularMark # 数据类型定义
|
||
├─ Qt Network # TCP客户端
|
||
└─ 被以下模块使用:
|
||
├─ WorkpiecePositionApp
|
||
└─ WorkpieceSpliceApp
|
||
|
||
WorkpiecePositionApp
|
||
├─ Module/BinocularMarkReceiver # Mark数据接收
|
||
├─ Device/EpicEyeDevice # 点云相机
|
||
├─ SDK/workpieceCornerExtraction # 工件算法
|
||
└─ Eigen (第三方) # 矩阵运算
|
||
|
||
WorkpieceSpliceApp
|
||
├─ Module/BinocularMarkReceiver # Mark数据接收
|
||
├─ AppUtils/UICommon # UI组件
|
||
├─ Eigen (第三方) # 位姿计算
|
||
└─ 机械臂通信模块 (待开发)
|
||
```
|
||
|
||
---
|
||
|
||
## ✅ 总结
|
||
|
||
这是一个**模块化、高精度、实时闭环**的工业视觉引导系统：
|
||
|
||
### 核心优势
|
||
1. **模块化设计**: BinocularMarkReceiver公共模块，易于复用和扩展
|
||
2. **多传感器融合**: Mark定位 + 点云检测，提高鲁棒性
|
||
3. **闭环控制**: 实时Mark反馈，动态调整工件位姿
|
||
4. **高精度**: ±0.1mm定位精度，满足精密装配要求
|
||
5. **实时性**: TCP通信<10ms延迟，支持在线检测
|
||
|
||
### 技术亮点
|
||
- 双目立体视觉 + Charuco Mark检测
|
||
- 点云配准与工件分割
|
||
- PCA主成分分析姿态估计
|
||
- SVD最小二乘位姿拟合
|
||
- 闭环PID控制策略
|
||
|
||
### 应用价值
|
||
- **提高效率**: 自动化抓取，减少人工干预
|
||
- **保证质量**: 高精度拼接，降低次品率
|
||
- **柔性生产**: 支持混线生产，快速换型
|