从深度图到机械臂动作，一条龙搞定！
- *来源**：第四届3D视觉创新应用竞赛*
- *参赛者**：白勇*
- *单位**：河北工业大学*
一、项目亮点速览 🎯
想象一下这个场景：一堆杂乱无章的物体摆在桌上，机器人走过去，看一眼，伸出手，稳稳地抓起目标物体——整个过程不需要任何人工干预，也不需要预先建模。
这就是本项目的目标：让机器人拥有"自主抓取"的能力！ 🦾
核心能力一览
| 核心能力 | 技术实现 | 亮点 |
|---|---|---|
| 📷 RGB-D 感知 | Orbbec Gemini2 双目结构光相机 | 眼在手外标定，支持深度+RGB同步采集 |
| 🧠 端到端推理 | 稀疏卷积网络（MinkowskiEngine） | 原始点云 → 抓取位姿，一步到位 |
| ✋ 多自由度抓取 | 6-DOF 位姿估计 + 夹爪宽度预测 | 支持任意角度、任意朝向的抓取 |
| 💨 高速推理 | TensorRT + Jetson Orin Nano | 毫秒级响应，满足实时抓取需求 |
| 🤖 真机执行 | 大族 Elfin-E05 + 气动夹爪 | 重复定位精度 ±0.02mm，稳定可靠 |
| 🛡️ 碰撞规避 | 基于体素的碰撞检测算法 | 自动过滤不可行抓取，安全可靠 |
二、背景与意义 🌍
2.1 为什么要做这个？
机器人抓取是智能制造、物流分拣、服务机器人等领域的核心基础能力。然而，传统的抓取方案存在诸多痛点：
| 传统方案 | 痛点 | 本方案的思路 |
|---|---|---|
| 基于规则的抓取策略 | 需要专家手工设计，泛化性差 | 🧠 数据驱动的端到端学习 |
| 两阶段方案（先检测再抓取） | 误差累积，速度慢 | ⚡ 单阶段直接输出抓取位姿 |
| 稠密卷积网络 | 点云稀疏性导致计算浪费 | 🔧 稀疏卷积（MinkowskiEngine）精准发力 |
| 仅支持平行夹爪 | 自由度受限，无法处理复杂场景 | 🎯 6-DOF 全自由度抓取 |
2.2 应用场景
| 场景 | 说明 |
|---|---|
| 🏭 工业分拣 | 流水线上的无序工件抓取 |
| 📦 物流仓储 | 包裹分拣、码垛拆垛 |
| 🏠 服务机器人 | 家庭/餐厅中的物品抓取 |
| 🔬 科研教学 | 机器人感知与操作研究平台 |
三、系统硬件架构 🛠️
3.1 硬件全家福
| 设备 | 型号/规格 | 功能 |
|---|---|---|
| 🤖 机械臂 | 大族 Elfin-E05 | 5kg 负载，±0.02mm 重复定位精度，6 自由度 |
| 📷 深度相机 | Orbbec Gemini2 | 双目结构光，RGB-D 同步输出 |
| 💻 边缘计算 | NVIDIA Jetson Orin Nano 8GB | 40 TOPS AI 算力，CUDA + TensorRT 加速 |
| ✋ 末端执行器 | 气动夹爪 | 快速响应，适应不同尺寸物体 |
| ⚡ 电控柜 | 定制配电柜 | 机器人 + 夹爪 + 气泵统一供电与控制 |
| 🌬️ 气泵 | 微型气泵 + 电磁阀 | 为气动夹爪提供稳定气源 |
3.2 系统拓扑
3.3 坐标系标定
系统采用 "眼在手外"（Eye-to-Hand） 的标定方式，需要建立以下坐标系转换链：
世界坐标系 (World) → 相机坐标系 (Camera) → 机器人基坐标系 (Base) → 末端执行器坐标系 (End-Effector)
标定步骤：
- 相机内参标定：获取 Gemini2 的焦距、主点、畸变参数
- 手眼标定（Eye-to-Hand）：求解相机 → 机器人基座的变换矩阵
- 工具标定：确定末端执行器（夹爪）相对于法兰中心的偏移
- 工作空间标定：定义机器人可达的抓取区域
💡 为什么选"眼在手外"？ 相比"眼在手上"，眼在手外方案视野更大、标定更稳定，且相机不会随机械臂运动而产生振动模糊，特别适合固定视角的抓取场景。
四、系统整体流程 🎬
4.1 从"看到"到"抓到"的完整链路
4.2 数据流详解
| 阶段 | 输入 | 输出 | 关键操作 |
|---|---|---|---|
| 采集 | — | RGB 图像 (1280×720) + 深度图 | Gemini2 SDK |
| 点云生成 | 深度图 + 相机内参 | 3D 点云 (N×3) | 反投影公式 |
| 预处理 | 原始点云 | 采样点云 (20000 点) | 体素下采样 + 随机采样 |
| 网络推理 | 点云坐标 + 特征 | 抓取位姿 (M×6) + 宽度 + 深度 | 稀疏卷积网络 |
| 碰撞检测 | 抓取候选 + 场景点云 | 可行抓取掩码 | 体素 IOU 计算 |
| 坐标变换 | 相机坐标系位姿 | 机器人基坐标系位姿 | 手眼标定矩阵 |
| 执行 | 目标位姿 | — | 运动学逆解 + 轨迹规划 |
五、核心算法详解 🔬
5.1 网络架构总览
本项目的核心网络基于 GraspNet-1Billion 方案改进，使用 MinkowskiEngine 的稀疏卷积重构 PointNet++ 的编解码结构，大幅提升了推理效率。
5.2 稀疏卷积：为什么不用普通卷积？
传统卷积在处理点云时面临一个根本性问题：点云是稀疏的！
想象一张 1280×720 的深度图，有效点可能只有 30%~50%，如果对整张图做稠密卷积，大量计算浪费在"空白"上。
稀疏卷积（Sparse Convolution） 的核心思想：
只在有数据的位置进行卷积运算，跳过空白区域，计算量与有效点数成正比，而非与图像尺寸成正比。
| 对比项 | 稠密卷积 | 稀疏卷积（MinkowskiEngine） |
|---|---|---|
| 计算量 | O(H×W×C) | O(N×C)，N 为有效点数 |
| 内存占用 | 与分辨率成正比 | 与有效点数成正比 |
| 适用场景 | 稠密图像 | 点云、稀疏体素 |
| 推理速度 | 较慢 | 快 3~10 倍 |
5.3 编码器-解码器结构（Backbone）
项目的骨干网络 ByBackFpt 是一个经典的 U-Net 型编码器-解码器结构，使用 MinkowskiEngine 的稀疏卷积实现：
编码器路径（下采样）：
# 输入: 原始点云 (N, 3) → 体素化 → SparseTensor
# Stride 1 → 2
conv0p1s1 → att0 → BN → ReLU # (N, 32)
conv1p1s2 → att1 → BN → ReLU # stride=2 下采样
block1 (BasicBlock ×1) # (N/2, 32)
# Stride 2 → 4
conv2p2s2 → att2 → BN → ReLU # stride=2 下采样
block2 (BasicBlock ×1) # (N/4, 64)
# Stride 4 → 8
conv3p4s2 → att3 → BN → ReLU # stride=2 下采样
block3 (BasicBlock ×1) # (N/8, 128)
# Stride 8 → 16 (瓶颈层)
conv4p8s2 → att4 → BN → ReLU # stride=2 下采样
block4 (BasicBlock ×1) # (N/16, 256)解码器路径（上采样 + 跳跃连接）：
# Stride 16 → 8
convtr4p16s2 → BN → ReLU # 转置卷积上采样
concat(upsampled, out_b3p8) # 跳跃连接！
block5 (BasicBlock ×1) # (N/8, 192)
# Stride 8 → 4
convtr5p8s2 → BN → ReLU
concat(upsampled, out_b2p4) # 跳跃连接！
block6 (BasicBlock ×1) # (N/4, 192)
# Stride 4 → 2
convtr6p4s2 → BN → ReLU
concat(upsampled, out_b1p2) # 跳跃连接！
block7 (BasicBlock ×1) # (N/2, 192)
# Stride 2 → 1
convtr7p2s2 → BN → ReLU
concat(upsampled, out_p1) # 跳跃连接！
block8 (BasicBlock ×1) # (N, 192)
# 最终输出
final = Conv1x1(block8, out_channels) # → 抓取参数关键点解读：
| 设计要素 | 作用 |
|---|---|
| 🔽 下采样（Stride 2 Conv） | 逐层降低分辨率，扩大感受野，提取全局特征 |
| 🔼 上采样（Transpose Conv） | 恢复空间分辨率，保留细节信息 |
| 🔗 跳跃连接（Skip Connection） | 融合低层细节和高层语义，避免信息丢失 |
| 🎯 自注意力机制（att0~att4） | 增强关键区域的特征响应，提升抓取质量 |
| 📐 BasicBlock | ResNet 基础块（Conv→BN→ReLU×2），保证梯度流畅传播 |
5.4 自注意力机制
在网络的关键位置插入了自注意力模块（BySelfAttentionLayer），让网络能够关注到对抓取决策最重要的点：
# 自注意力核心思想（伪代码）
Q = Linear(features) # Query: 我要找什么？
K = Linear(features) # Key: 我有什么？
V = Linear(features) # Value: 实际信息
# 计算注意力权重
scores = Q @ K.T / sqrt(d_k) # 相似度打分
weights = Softmax(scores) # 归一化为权重
output = weights @ V # 加权聚合信息💡 直观理解： 想象你在人群中找人——你的"Query"是目标特征（比如"穿红衣服"），每个路人的"Key"是他们的外貌特征，你计算相似度后，重点关注最匹配的那几个人。自注意力机制就是让网络学会这种"重点关注"的能力。
5.5 抓取表示：6-DOF 位姿
一个完整的抓取由以下参数定义：
| 参数 | 维度 | 含义 |
|---|---|---|
| 平移 T | (3,) | 夹爪中心点坐标 (x, y, z) |
| 旋转 R | (3, 3) | 夹爪朝向（3×3 旋转矩阵） |
| 宽度 W | (1,) | 夹爪张开宽度 |
| 深度 D | (1,) | 夹爪插入深度 |
旋转矩阵 → 抓取方向的解码：
R = [r11 r12 r13] gripper_x = [r11, r21, r31]ᵀ ← 夹爪闭合方向（两指连线方向）
[r21 r22 r23] gripper_y = [r12, r22, r32]ᵀ ← 夹爪 approaching 方向
[r31 r32 r33] gripper_z = [r13, r23, r33]ᵀ ← 夹爪法线方向5.6 损失函数设计
网络的训练使用多任务损失：
# 伪代码：多任务损失函数
L_total = L_graspness + λ₁ · L_objectness + λ₂ · L_width + λ₃ · L_depth
# 其中：
# L_graspness: 抓取质量评分损失（回归）→ 预测每个点的"可抓取程度"
# L_objectness: 物体二分类损失（交叉熵）→ 区分前景物体和背景
# L_width: 夹爪宽度回归损失（Smooth L1）→ 预测最佳张开宽度
# L_depth: 抓取深度回归损失（Smooth L1）→ 预测最佳插入深度六、碰撞检测模块 🛡️
6.1 为什么需要碰撞检测？
神经网络输出的抓取候选可能有成百上千个，但其中很多在物理上不可行——比如夹爪会撞到桌面、夹到无关物体等。碰撞检测模块的作用就是：自动过滤掉这些"危险"的抓取姿势。
6.2 算法原理
项目使用 ModelFreeCollisionDetector，将夹爪建模为 5 个几何区域，逐一检测每个区域与场景点云的碰撞情况：
6.3 核心代码解读
class ModelFreeCollisionDetector():
def __init__(self, scene_points, voxel_size=0.005):
self.finger_width = 0.01 # 手指厚度
self.finger_length = 0.06 # 手指长度
self.voxel_size = voxel_size
# 对场景点云进行体素下采样，加速碰撞检测
scene_cloud = o3d.geometry.PointCloud()
scene_cloud.points = o3d.utility.Vector3dVector(scene_points)
scene_cloud = scene_cloud.voxel_down_sample(voxel_size)
self.scene_points = np.array(scene_cloud.points)
def detect(self, grasp_group, collision_thresh=0.05):
# 将场景点变换到每个抓取的夹爪坐标系下
targets = self.scene_points[np.newaxis,:,:] - T[:,np.newaxis,:]
targets = np.matmul(targets, R)
# 分别对 5 个区域做碰撞检测
left_mask = mask1 & mask2 & mask3 & mask4 # 左手指区域
right_mask = mask1 & mask2 & mask5 & mask6 # 右手指区域
bottom_mask = mask1 & mask3 & mask5 & mask7 # 底部区域
shifting_mask = mask1 & mask3 & mask5 & mask8 # 接近方向区域
global_mask = left_mask | right_mask | bottom_mask | shifting_mask
# 计算 IOU = 碰撞点数 / 夹爪区域体素总量
global_iou = global_mask.sum(axis=1) / (volume + 1e-6)
# IOU 超过阈值 → 判定为碰撞
collision_mask = (global_iou > collision_thresh)
return collision_mask检测逻辑表格：
| 检测区域 | 判定条件 | 物理含义 |
|---|---|---|
| 左手指 | 点在左手指的长方体空间内 | 夹爪左侧会撞到物体 |
| 右手指 | 点在右手指的长方体空间内 | 夹爪右侧会撞到物体 |
| 底部 | 点在底部连接片空间内 | 夹爪底部会碰触表面 |
| 接近方向 | 点在接近方向预留空间内 | 夹爪接近路径上有障碍物 |
| 全局 IOU | 以上任一为真且 IOU > 阈值 | 判定为碰撞，丢弃该抓取 |
📌 阈值选择：
collision_thresh=0.05意味着夹爪区域内如果有超过 5% 的点与场景重叠，就认为发生了碰撞。这个阈值是在"安全性"和"抓取候选保留率"之间的权衡。
七、数据集与训练策略 📊
7.1 GraspNet-1Billion 数据集
项目基于 GraspNet-1Billion 数据集进行训练，这是目前最大的机器人抓取基准数据集：
| 属性 | 数值 |
|---|---|
| 场景数 | 190 个（100 训练 + 90 测试） |
| 物体种类 | 88 类（含日常物品、工业零件等） |
| 每场景图像数 | 256 张（多视角覆盖） |
| 总抓取标注 | ~1.17 亿个 |
| 相机 | Kinect + RealSense（双相机） |
7.2 数据加载与增强
关键数据处理步骤：
# GraspNetDataset 核心流程（伪代码）
def get_data_label(index):
# Step 1: 读取深度图 + 分割标签 + 相机参数
depth = Image.open(depthpath[index])
seg = Image.open(labelpath[index])
meta = scio.loadmat(metapath[index])
# Step 2: 从深度图生成 3D 点云（反投影）
camera = CameraInfo(intrinsic, factor_depth)
cloud = create_point_cloud_from_depth_image(depth, camera)
# Step 3: 工作空间掩码（去除桌面外/异常点）
workspace_mask = get_workspace_mask(cloud, seg, trans)
mask = depth_mask & workspace_mask
cloud_masked = cloud[mask]
# Step 4: 随机采样至固定点数
idxs = np.random.choice(len(cloud_masked), 20000, replace=False)
cloud_sampled = cloud_masked[idxs]
# Step 5: 体素量化 → 生成稀疏张量
coors = cloud_sampled / voxel_size # 连续坐标 → 体素坐标
feats = np.ones_like(cloud_sampled) # 初始特征（全 1）
return {
'point_clouds': cloud_sampled, # (20000, 3) 浮点坐标
'coors': coors, # (20000, 3) 体素坐标（整数）
'feats': feats, # (20000, 3) 点特征
'graspness_label': ..., # 每个点的抓取质量标签
'objectness_label': ..., # 每个点的物体/背景标签
'grasp_points_list': ..., # 抓取点集合
'grasp_widths_list': ..., # 对应抓取宽度
'grasp_scores_list': ..., # 对应抓取评分
}数据增强策略：
| 增强方式 | 具体操作 | 作用 |
|---|---|---|
| 🔄 YZ 平面翻转 | flip_mat = diag([-1, 1, 1]) | 增加左右对称性样本 |
| 🔄 Z 轴旋转 | 随机旋转 ±30° | 增强旋转不变性 |
| 🎲 随机采样 | 每次随机选取 20000 点 | 增加训练多样性 |
7.3 抓取标签格式
# 每个物体的抓取标签（.npz 文件）
{
'points': (N, 3) → 抓取点坐标（物体坐标系）
'width': (N,) → 推荐夹爪宽度
'scores': (N,) → 抓取质量评分 [0, 1]
}八、部署与运行 🚀
8.1 开发环境
| 组件 | 版本 |
|---|---|
| 操作系统 | Ubuntu 22.04 LTS |
| CUDA | 12.x |
| cuDNN | 8.x |
| Python | 3.8+ |
| PyTorch | 2.0+ |
| MinkowskiEngine | 0.5.x |
| TensorRT | 8.x |
| Open3D | 0.17+ |
8.2 系统部署架构
8.3 运行流程
# Step 1: 启动相机驱动
python test_camera.py
# → 初始化 Gemini2，开始采集 RGB-D 数据
# Step 2: 运行抓取检测 + 机器人控制
python dazu_real_grasp.py
# → 推理 → 碰撞检测 → 坐标变换 → 机器人执行真实抓取主循环（伪代码）：
# dazu_real_grasp.py 核心逻辑
while True:
# 1. 采集一帧 RGB-D
rgb, depth = camera.capture()
# 2. 生成点云
cloud = create_point_cloud_from_depth(rgb, depth, camera_intrinsic)
# 3. 网络推理 → 获取抓取候选
grasp_group = model.infer(cloud)
# 4. 碰撞检测 → 过滤不可行抓取
collision_mask = collision_detector.detect(grasp_group)
grasp_group = grasp_group[~collision_mask]
# 5. 选取最佳抓取（评分最高）
best_grasp = grasp_group.top_k(1)
# 6. 坐标变换：相机坐标系 → 机器人基坐标系
robot_pose = transform_camera_to_base(best_grasp.pose)
# 7. 发送给机器人执行
robot.move_to_pose(robot_pose)
gripper.close()九、技术栈总览 🛠️
| 类别 | 技术选型 | 用途 |
|---|---|---|
| 📷 相机 SDK | Orbbec Gemini2 SDK | RGB-D 数据采集 |
| 🧠 深度学习框架 | PyTorch 2.0+ | 模型训练与推理 |
| 🔧 稀疏卷积库 | MinkowskiEngine | 高效点云卷积运算 |
| ⚡ 推理加速 | TensorRT | 部署端模型优化 |
| 🤖 机器人控制 | 大族 Elfin SDK / ROS | 机械臂运动控制 |
| 📐 点云处理 | Open3D | 点云读写、可视化、体素化 |
| 🖼️ 图像处理 | OpenCV | 图像预处理 |
| 📊 数据格式 | NumPy / SciPy | 数值计算与数据加载 |
| 💻 开发板 | Jetson Orin Nano | 边缘端部署平台 |
十、总结与展望 🌟
核心贡献
| 贡献 | 说明 |
|---|---|
| 🎯 端到端架构 | 从原始点云到抓取位姿，单阶段直接输出，无中间环节 |
| ⚡ 稀疏卷积加速 | MinkowskiEngine 替代稠密卷积，推理速度提升数倍 |
| 🛡️ 碰撞感知 | 基于体素的碰撞检测，自动过滤不可行抓取 |
| 🤖 真机验证 | 大族 Elfin-E05 + 气动夹爪，完整抓取闭环 |
| 📱 边缘部署 | Jetson Orin Nano 上实现实时推理 |
未来方向
| 方向 | 目标 |
|---|---|
| 🧠 多物体场景 | 支持杂乱场景中多物体同时抓取规划 |
| 🎨 语义感知 | 结合实例分割，实现"抓取指定的红色杯子" |
| 📱 移动端优化 | 模型量化 + TensorRT 进一步降低延迟 |
| 🔄 Sim2Real | 仿真训练 + 域适应，减少真实数据依赖 |
| 🏭 工业落地 | 对接产线 PLC，实现实际工业分拣应用 |
🌟 写在最后： 这个项目展示了一条完整的"感知 → 决策 → 执行"技术链路——从 RGB-D 相机采集、稀疏卷积网络推理、碰撞检测过滤，到机械臂运动执行，每个环节都经过精心设计。特别值得一提的是，团队选择了 MinkowskiEngine 稀疏卷积这一"利器"，在点云处理效率和精度之间找到了绝佳平衡点。如果你也在做机器人抓取相关的工作，这套方案绝对值得参考！🏆
相关资源
| 资源 | 链接 |
|---|---|
| 📖 GraspNet 论文 | Fang et al., "GraspNet-1Billion: A Large-Scale Benchmark for General Object Grasping", CVPR 2020 |
| 📖 MinkowskiEngine | GitHub – NVIDIA/MinkowskiEngine |
| 📖 GraspNet 数据集 | http://graspnet.net |
| 📖 GraspNetAPI | GitHub – graspnet/graspnetAPI |