引言

机械臂视觉抓取（Visual Grasping）是机器人领域的核心技术之一，其目标是赋予机械臂”看懂”三维世界并准确完成物体拾取的能力。从工业流水线的精密装配，到仓储物流的柔性拣选，再到医疗辅助机器人的精细操作，视觉引导的机械臂抓取正以前所未有的速度走进现实应用。

一套完整的视觉抓取系统涉及多个高度耦合的技术模块：首先从摄像头图像中检测并定位目标物体；进一步精确估计物体在三维空间中的六自由度姿态，即物体相对于相机的旋转矩阵 $\mathbf{R} \in SO(3)$ 和平移向量 $\mathbf{t} \in \mathbb{R}^3$；然后通过手眼标定建立相机坐标系与机械臂基座坐标系之间的变换关系；最后利用运动规划为机械臂规划一条安全、无碰撞的运动轨迹以完成抓取。

下图展示了整个视觉抓取流水线的技术框架：

---

一、传感器输入：RGB 与 RGB-D

1.1 纯 RGB 相机

RGB 相机提供丰富的纹理与颜色信息，但缺乏直接的深度信息。相机的针孔投影模型将三维点 $\mathbf{P}_c = (X_c, Y_c, Z_c)^\top$ 映射到图像像素坐标 $(u, v)$：

\[\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z_c} \mathbf{K} \mathbf{P}_c, \quad \mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\]

其中 $f_x, f_y$ 为焦距（像素单位），$(c_x, c_y)$ 为主点坐标。实际相机还存在径向畸变和切向畸变，需通过棋盘格标定估计畸变系数 $\mathbf{d} = (k_1, k_2, p_1, p_2, k_3)$。

1.2 RGB-D 相机（深度相机）

RGB-D 相机在提供彩色图像的同时，还直接输出逐像素深度值 $d(u,v)$，极大地简化了三维感知。常见的 RGB-D 传感器包括：

• Intel RealSense D系列（D435, D455）：结构光 + 主动红外立体视觉，适合室内近距离（0.2m–10m）；
• Microsoft Azure Kinect DK：ToF（飞行时间）深度传感器，精度高；
• Orbbec Gemini / Femto：国产 RGB-D 方案，性价比高。

给定深度值 $d(u,v)$，可将像素反投影为相机坐标系下的三维点：

\[X_c = \frac{(u - c_x) \cdot d}{f_x}, \quad Y_c = \frac{(v - c_y) \cdot d}{f_y}, \quad Z_c = d\]

将所有像素反投影后得到点云（Point Cloud），是后续6D姿态估计和运动规划的重要输入。

---

二、目标检测与定位

目标检测的任务是在图像中找到目标物体的位置并给出类别标签，为后续姿态估计提供感兴趣区域（Region of Interest, ROI）。

2.1 YOLO 系列：实时目标检测的演进

YOLO（You Only Look Once）系列是机器人视觉中使用最广泛的实时目标检测框架，其核心思想是将检测任务转化为一次前向推理的回归问题：将图像划分为 $S \times S$ 的网格，每个格子直接预测 $B$ 个边界框及其置信度，同时预测类别概率。

YOLOv1–v2：单阶段检测的起点（2016–2017）

YOLOv1（CVPR 2016）提出统一检测框架，24层卷积网络 + 2层全连接层，在 Pascal VOC 上实现 45 FPS 实时检测。核心损失函数为：

\[\mathcal{L} = \lambda_{coord} \sum_{i=0}^{S^2}\sum_{j=0}^{B} \mathbb{1}_{ij}^{obj} \left[(x_i - \hat{x}_i)^2 + (y_i - \hat{y}_i)^2\right] + \lambda_{coord} \sum \mathbb{1}_{ij}^{obj}\left[(\sqrt{w_i} - \sqrt{\hat{w}_i})^2 + (\sqrt{h_i} - \sqrt{\hat{h}_i})^2\right] + \mathcal{L}_{conf} + \mathcal{L}_{cls}\]

YOLOv2（YOLO9000，2017）引入 Batch Normalization、Anchor Boxes 和多尺度训练，mAP 提升至 78.6%（VOC 2007）。

YOLOv3–v4：多尺度与跨阶段特征（2018–2020）

YOLOv3（2018）引入 Darknet-53 骨干（含残差连接的53层网络）和多尺度预测（三个不同分辨率的检测头），显著提升小目标检测能力。损失函数改用二元交叉熵（BCE）替代平方误差。

YOLOv4（2020）整合了大量 tricks：CSPDarknet53 骨干、PANet 特征融合、Mish 激活函数、Mosaic 数据增强、CIoU 损失等，在 COCO 上达到 43.5% AP @ 65 FPS（Tesla V100），被称为工程化的巅峰之作。

YOLOv5–v8：Ultralytics 时代（2020–2023）

YOLOv5（Ultralytics，2020）虽无正式论文，但凭借优秀的工程实现（PyTorch 原生、TensorRT 导出、ONNX 支持）迅速成为工业界事实标准，提供 n/s/m/l/x 五种规格满足不同算力需求。

YOLOv7（Wang et al., CVPR 2022 Workshop）引入 E-ELAN（Extended Efficient Layer Aggregation Network）和辅助训练头（Auxiliary Head），在 COCO 上以 51.4% mAP @ 161 FPS 创造新纪录。

YOLOv8（Ultralytics，2023）彻底重构为锚框无关（Anchor-free）的解耦检测头设计，支持检测、分割、关键点估计、OBB等多任务，COCO mAP 达到 53.9%（YOLOv8x），在机器人视觉中被广泛采用。

YOLOv9–v12：走向无 NMS 与注意力机制（2024–2025）

YOLOv9（2024）提出可编程梯度信息（PGI）和通用高效层聚合网络（GELAN），通过梯度路径规划减少信息瓶颈，在 COCO 上以更少参数实现更高精度。

YOLOv10（2024）引入一致性双重分配（Consistent Dual Assignments）彻底消除 NMS 后处理，端到端训练大幅降低推理延迟，是首个真正无 NMS 的 YOLO。

YOLO11（2024 年 9 月发布）引入 C3k2 模块（CSP Bottleneck，kernel=2）和 C2PSA（Cross-Stage Partial with Spatial Attention）注意力模块，在参数量更少的情况下 mAP 反超 YOLOv8，YOLO11x 在 COCO 上达到 54.7% mAP（13ms，A100）。

YOLOv12（2025）将多头自注意力机制（MHSA）引入 YOLO 框架，以 R-ELAN（Residual Efficient Layer Aggregation Network）替代传统 CSP，实现更强的全局建模能力，是 YOLO 系列走向 Transformer 融合的重要里程碑。

YOLO26：边缘优先的极简端到端重设计（2025）

YOLO26 是 Ultralytics 于 2025 年 9 月在伦敦 YOLO Vision 2025 活动上发布的最新一代 YOLO，于 2026 年 1 月 14 日正式开放模型权重下载。与命名上的跨越（从 v12 跳至 26）相对应的，是架构设计理念上的一次根本性转变——边缘优先（Edge-first）的极简主义重设计。

两大核心架构改动：

① 移除 DFL（Distribution Focal Loss）：传统检测头将边界框坐标预测建模为离散分布（每条边预测 reg_max 个 bin 的概率再积分），虽精度高但图计算复杂、跨编译器导出（ONNX/TensorRT/CoreML）时算子兼容性差、量化困难。YOLO26 改回轻量的直接坐标回归，大幅降低图复杂度，量化损失极小。

② 原生 NMS-free 端到端推理：延续 YOLOv10 开创的一致性双重分配思路，YOLO26 的检测头直接输出紧凑、无冗余的预测集合，不再依赖 NMS 后处理。消除了部署时需手动调节 IoU/Score 阈值的问题，推理延迟进一步降低。

训练侧创新：

• ProgLoss（渐进式损失平衡）：在训练过程中动态调整分类损失与回归损失的权重比例，早期稳定分类学习，后期强化定位精度，收敛更平稳；
• STAL（小目标感知标签分配，Small-Target-Aware Label Assignment）：传统 TAL（Task-Aligned Assignment）对小目标分配锚点时存在正样本稀疏问题，STAL 通过额外的几何感知策略为小目标分配更多正样本，在 IoT、航拍、机器人等场景下小目标检测精度显著提升；
• MuSGD 优化器：将 LLM 训练中 Moonshot AI（Kimi K2）所用的 Muon 优化器与 SGD 结合，引入二阶动量近似，训练收敛更快、更稳定——这是 LLM 训练方法向计算机视觉迁移的一次有趣尝试。

性能：YOLO26n 在 COCO 上 mAP 约 39.8–40.3%，CPU 推理速度比 YOLO11n 快约 43%；YOLO26l 可达约 53.0–53.4% mAP。模型提供 n/s/m/l/x 五种规格，并衍生出支持开放词汇检测的 YOLOE-26 变体。

下表总结了 YOLO 各版本的关键演进：

版本	年份	骨干	关键创新	COCO mAP
YOLOv1	2016	GoogLeNet-style	单阶段回归检测	63.4 (VOC)
YOLOv2	2017	Darknet-19	Anchor Boxes, BatchNorm	78.6 (VOC)
YOLOv3	2018	Darknet-53	多尺度检测，残差连接	33.0
YOLOv4	2020	CSPDarknet53	PANet, CIoU, Mosaic	43.5
YOLOv5	2020	CSP	工程化标准，TensorRT导出	50.7
YOLOv7	2022	E-ELAN	Aux Head，模型缩放	51.4
YOLOv8	2023	CSP-Darknet	Anchor-free，解耦头	53.9
YOLOv9	2024	GELAN	PGI，可编程梯度	55.6
YOLOv10	2024	CSP+PSA	NMS-free，一致性双重分配	54.4
YOLO11	2024	C3k2+C2PSA	注意力增强，高效小目标	54.7
YOLOv12	2025	R-ELAN+MHSA	全局注意力，Flash Attention	55.2+
YOLO26	2025	简化CSP	NMS-free + 无DFL，MuSGD，ProgLoss，STAL	~53.0（l规格），CPU快43%

2.2 SAM 系列：分割基础模型的演进

Segment Anything Model（SAM） 系列是 Meta AI 发布的基础分割模型族，以零样本可提示分割（Zero-shot Promptable Segmentation）为核心能力，在机器人视觉中尤其适合新物体的实例分割。

SAM 1（2023 年 4 月）

架构：ViT-H 图像编码器（约 632M 参数） + 轻量级提示编码器 + 掩码解码器（双向 Transformer）。编码器一次运行即可生成图像嵌入，解码器可在 ~50ms（CPU）内响应任意提示，支持：

• 点提示（正/负点击）；
• 框提示（BBox）；
• 掩码提示（粗略掩码输入精化）。

训练数据：SA-1B——11M 图像、1.1B 分割掩码，通过模型辅助的三阶段数据引擎（人工辅助 → 半自动 → 全自动）构建。

能力：强大的零样本泛化，但仅支持图像（不支持视频）、无文本提示能力、高分辨率下计算代价高。

轻量化变体：

• FastSAM（2023）：将 SAM 的分割任务解耦为全实例分割（YOLOv8-Seg 骨干）+ 提示引导选择，仅用 SA-1B 的 2% 数据训练，速度提升 ~50×；
• MobileSAM（2023）：通过知识蒸馏将 ViT-H 编码器替换为轻量 TinyViT，速度比 FastSAM 快 5×，模型小 7×；
• EfficientSAM（2024）：利用 SAM-leveraged Masked Image Pre-training（SAMI）训练轻量编码器，在保持精度的同时实现高效推理。

SAM 2（2024 年 8 月）

SAM 2 将分割能力从图像扩展到视频，引入可提示视觉分割（Promptable Visual Segmentation, PVS）任务。

架构核心改进：

• 图像编码器：将 SAM 1 的 ViT 替换为 Hiera（分层 MAE 预训练编码器），支持多尺度特征，计算效率更高；
• 记忆模块（Memory Module）：存储历史帧的空间特征图（近期帧记忆）和语义信息（跨帧掩码信息），以 FIFO 队列管理；
• 记忆注意力（Memory Attention）：当前帧特征与记忆库中的历史特征做交叉注意力，实现跨帧时序一致性。

这使得 SAM 2 可以在任意帧给出提示，模型自动将分割结果传播到整个视频——即使目标被短暂遮挡（消失后重新出现），分割仍能保持连续。

性能对比：

• 图像分割精度优于 SAM 1（58.9 mIoU → 61.4 mIoU，单点提示）；
• 推理速度约 44 FPS；
• 人工标注速度提升 8×（相比在每帧运行 SAM 1）。

SAM 2.1（2024 年 9 月）

SAM 2.1 是 SAM 2 的改进版本，重点优化：

• 相似外观目标和小目标的分割精度（通过额外数据增强训练）；
• 遮挡处理：训练时使用更长的视频序列，提供更多上下文；
• 开发者友好：首次开源训练代码，提供 Web Demo 前后端代码，支持社区微调。

可用模型规格：sam2.1_t（Tiny）/ sam2.1_s（Small）/ sam2.1_b+（Base+）/ sam2.1_l（Large）。

SAM 3（2025 年，最新）

SAM 3 是 SAM 系列的重大跃升，引入开放词汇理解（Open-Vocabulary Understanding）：

• 统一骨干（Perception Encoder）：对齐视觉与语言特征，理解”黄色校车”这样的语义概念，而非仅凭形状匹配；
• 文本提示支持：可接受文本短语（如 “all dogs”、”red car”）进行分割，不再局限于点/框提示；
• 训练数据：SA-Co（Segment Anything with Concepts）数据集，包含数百万张图像关联到名词短语概念和掩码，LLM 作为”AI 标注员”辅助生成；
• 性能：在概念分割基准上比先前开放词汇方法提升约 2×。

SAM 3 进一步扩展到 SAM 3D，支持从单张图像生成三维物体 Mesh（用于 AR/VR 和机器人仿真）。

---

三、6D 物体姿态估计

对于物体姿态估计，几种分类：

1. DOPE：2D关键点+PNP
2. SAM6D：分割模型+姿态估计网络
3. FoundationPose：基于逐步refinement，属于通用模型范式，且结合了神经渲染以及大数据训练

3.1 问题定义与数学表述

6D 物体姿态估计的目标是：给定场景图像（RGB 或 RGB-D）和目标物体的 CAD 模型，求解物体坐标系相对于相机坐标系的刚体变换矩阵 $\mathbf{T} \in SE(3)$：

\[\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad \mathbf{R} \in SO(3),\; \mathbf{t} \in \mathbb{R}^3\]

$\mathbf{R}$ 为旋转矩阵（$\mathbf{R}^\top \mathbf{R} = \mathbf{I}$，$\det(\mathbf{R}) = 1$），$\mathbf{t}$ 为三维平移向量。物体坐标系中的点 $\mathbf{p}_{obj}$ 通过此变换投影到相机坐标系：

\[\mathbf{p}_{cam} = \mathbf{R} \mathbf{p}_{obj} + \mathbf{t}\]

常见旋转参数化方式：

• 四元数 $\mathbf{q} = (q_w, q_x, q_y, q_z)$，$|\mathbf{q}|=1$：无奇异，神经网络常用；
• 轴角（Rodrigues公式）： \(\mathbf{R} = \mathbf{I} + \sin\theta [\hat{\mathbf{k}}]_\times + (1 - \cos\theta) [\hat{\mathbf{k}}]_\times^2\)；
• 6D 旋转表示（Zhou et al., CVPR 2019）：取旋转矩阵前两列，通过 Gram-Schmidt 正交化恢复 $\mathbf{R}$，连续可微，被众多深度网络采用。

3.2 方法分类

维度	类别	代表方法
输入模态	RGB	MegaPose, GDR-Net
	RGB-D	SAM-6D, DenseFusion
泛化性	Instance-level	PoseCNN, DeepIM
	Novel Object (Zero-shot)	MegaPose, SAM-6D, FoundationPose
核心思路	Render & Compare	MegaPose, FoundationPose
	Point Matching	SAM-6D (PEM)
	Direct Regression	SSD-6D, EfficientPose

3.3 SAM-6D（CVPR 2024）

3.3.1 方法概述

SAM-6D （CVPR 2024）。核心思想：借助 SAM 的零样本实例分割能力，以 RGB-D 图像为输入，实现新物体的零样本6D姿态估计。整体由 ISM（实例分割模型） 和 PEM（姿态估计模型） 两个子网络组成。

3.3.2 实例分割模型（ISM）

ISM 工作分三步：

Step 1 — 生成所有掩码候选：以 SAM 的 ViT-H 编码器提取图像特征，通过 SAM 掩码解码器生成所有可能的类别无关掩码候选 ${\mathcal{M}_k}$。

Step 2 — 三重对象匹配评分：对每个候选掩码计算与目标物体的匹配得分：

\[S_k = \lambda_1 \cdot S_{sem} + \lambda_2 \cdot S_{app} + \lambda_3 \cdot S_{geo}\]

• 语义分数 $S_{sem}$：利用 DINOv2 提取目标物体的多视角渲染特征与候选区域特征的余弦相似度；
• 外观分数 $S_{app}$：细粒度纹理和颜色分布对比；
• 几何分数 $S_{geo}$：掩码对应的深度点云与 CAD 模型点云的几何相似度。

Step 3 — NMS 过滤：保留得分最高的有效实例掩码。

3.3.3 姿态估计模型（PEM）

PEM 将姿态估计建模为部分到部分的点云匹配，核心创新是背景令牌（Background Tokens）。

数据准备：从 RGB-D 图像利用 ISM 掩码裁出观测点云 \(\mathcal{P}_{obs}\)（部分可见）；从 CAD 模型均匀采样得完整点云 \(\mathcal{P}_{cad}\)。

两阶段匹配：

粗粒度点匹配（CPM）：

在 Transformer 的注意力计算中引入背景令牌 $\mathbf{B} \in \mathbb{R}^{N_b \times d}$：

\[\text{Attention}(\mathbf{Q}, [\mathbf{K}_{pts} \| \mathbf{K}_{bg}], [\mathbf{V}_{pts} \| \mathbf{V}_{bg}])\]

背景令牌充当软性”垃圾桶”，吸收因遮挡等原因找不到对应的点，防止错误对应关系污染匹配矩阵。通过 Sinkhorn 算法求解最优部分匹配，再用 Kabsch-Umeyama 算法估计粗略姿态 $\mathbf{T}_{coarse}$。

精细点匹配（FPM）：以 \(\mathbf{T}_{coarse}\) 初始化，Sparse-to-Dense Point Transformer 在更细粒度点上建立密集对应，精化为 \(\mathbf{T}_{final}\)。

训练损失为：

\[\mathcal{L}_{PEM} = \underbrace{\sum_{(i,j) \in \mathcal{C}^*} \text{BCE}(\hat{a}_{ij}, 1) + \sum_{(i,j) \notin \mathcal{C}^*} \text{BCE}(\hat{a}_{ij}, 0)}_{\text{匹配矩阵监督}} + \alpha \mathcal{L}_{geo}\]

3.3.4 优缺点分析

维度	优点	缺点
泛化性	零样本，无需目标物体参与训练	对无纹理物体性能下降
分割质量	SAM 提供高质量零样本掩码	分割+匹配串行，整体约几秒/帧
几何鲁棒性	Background Tokens 显著提升遮挡场景精度	严重截断点云仍有挑战
精度	BOP 七数据集 mean AR 70.4%	无实时追踪模式
部署	官方 Docker 镜像	依赖 RGB-D（需深度相机）

---

3.4 FoundationPose（CVPR 2024 Highlight，NVIDIA）

3.4.1 方法概述

FoundationPose CVPR 2024 Highlight）是一个统一基础模型，同时支持基于 CAD 模型（Model-based）和基于少量参考图像（Model-free）的6D姿态估计与追踪，两种模式共享完全相同的下游网络。

PS：本质上，这篇工作将新物体的 6D pose estimation 统一建模为一个 render-and-compare 的 pose hypothesis search 问题。 在 model-free 场景下，它通过学习 object-centric neural implicit representation 来替代 CAD 模型，从而实现对新物体的可渲染表示。 在此基础上，方法采用 多假设初始化 + iterative pose refinement + hierarchical pose ranking 的框架来优化位姿。 此外，为了提升泛化能力，作者构建了一个 scalable synthetic data pipeline，其中利用 LLM 生成纹理描述并结合 diffusion 模型扩展外观多样性。

3.4.2 神经隐式表示作为统一桥梁

为统一两种输入模式，FoundationPose 引入物体中心的神经场：

• Model-based：直接以 CAD 模型渲染任意新视角，无需学习；
• Model-free：以约 16 张参考图像训练轻量 NeRF，学习物体隐式三维表示，再通过 NeRF 合成任意新视角的 RGBD 图像；

两种方式最终都得到可用于 Render & Compare 的合成 RGBD 渲染图像，使下游姿态估计网络完全不感知输入模式差异。

3.4.3 大规模合成训练数据

FoundationPose 利用 NVIDIA Isaac Sim（基于 Omniverse）进行物理正确光追渲染，并借助 LLM 自动生成场景描述、材质配置和物体摆放方案，以极低人工成本生成数以百万计的带标注训练样本。

基于neural object field / SDF 的对象表示，实现在没有 CAD 模型时也能进行高质量新视角 RGBD 渲染。

3.4.4 姿态假设生成与精化

姿态假设池：均匀采样 $SO(3)$ 空间生成约 1000 个候选姿态 $\mathcal{H} = {(\mathbf{R}_i, \mathbf{t}_i)}$。

精化网络：对每个候选 $\mathbf{T}_i$，将 CAD 模型在此姿态下渲染后与观测图像拼接，送入基于 ViT 的跨注意力 Transformer：

\[\Delta \mathbf{T}_i = f_\theta\bigl([I_{obs} \oplus I_{synth}(\mathbf{T}_i)]\bigr), \quad \mathbf{T}_i' = \Delta \mathbf{T}_i \cdot \mathbf{T}_i\]

网络通过对比学习训练，拉近真值姿态渲染与观测的特征距离，推远错误姿态的特征距离：

\[\mathcal{L}_{contrastive} = -\log \frac{\exp(\mathbf{z}_{obs} \cdot \mathbf{z}^+_{synth} / \tau)}{\sum_j \exp(\mathbf{z}_{obs} \cdot \mathbf{z}^{(j)}_{synth} / \tau)}\]

姿态评分与选择：评分网络对所有精化后候选评分，选出 $\mathbf{T}^* = \mathbf{T}’_{\arg\max_i s_i}$。

3.4.5 实时追踪模式

初始化后系统切换为追踪模式：无需重新生成假设池，仅对上一帧姿态做单步精化，推理速度约 32 Hz，满足实时操作需求。

3.4.6 优缺点分析

维度	优点	缺点
统一性	一个模型同时解决 model-based 和 model-free	初始化依赖外部 2D 检测器
精度	BOP model-based 排行榜 #1	初始化阶段约 1.3s
追踪	32 Hz 实时追踪	严重遮挡时仍可能失败
生态	支持 Isaac ROS / TensorRT 加速	NVIDIA 专有许可证

---

3.5 MegaPose（CoRL 2022A）

3.5.1 方法概述

MegaPose（CoRL 2022）是”百万级合成数据 + Render & Compare“范式的先驱，同时也是 FoundationPose 等后续方法的重要基础。核心架构由粗略估计器（Coarse Estimator）和精化器（Refiner）组成。

3.5.2 大规模合成训练集

MegaPose 在以下数据上训练：

• Google Scanned Objects（GSO）：约 1000 种真实扫描物体，纹理丰富；
• ShapeNet：约 50,000 种 CAD 模型，形状多样；

结合随机背景、光照和 Physically-Based Rendering（PBR），生成 ShapeNet-1M 和 GSO-1M 两个百万帧合成数据集，训练后无需重新训练即可直接推理任意新物体。

3.5.3 粗略估计器（Coarse Estimator）

多视角渲染候选池：对旋转空间 $SO(3)$ 均匀采样 $N$ 个姿态 ${\mathbf{T}j}$，利用 CAD 模型渲染得合成图像集合 ${I{synth}(\mathbf{T}_j)}$。

双流特征提取：真实 ROI 图像和所有合成视图通过权重共享的骨干网络提取特征：

\[\mathbf{f}_{obs} = \phi(I_{obs}), \quad \mathbf{f}_j = \phi(I_{synth}(\mathbf{T}_j))\]

可修正性分类器：对每个候选姿态，预测该候选与真实姿态的误差是否在精化器可修正的范围内（二分类问题），选 Top-K 送入精化器。这一设计将粗估问题转化为分类问题，大幅降低学习难度。

3.5.4 精化器（Refiner）

以粗略姿态为初始化，通过迭代 Render & Compare 精化：

\[\mathbf{T}_{k+1} = \begin{bmatrix} \Delta \mathbf{R}_k & \Delta \mathbf{t}_k \\ \mathbf{0}^\top & 1 \end{bmatrix} \cdot \mathbf{T}_k\]

$\Delta \mathbf{T}_k$ 由跨注意力 Transformer 预测（对比真实 ROI 与当前姿态渲染图），迭代约5步至收敛。训练使用对称感知损失：

\[\mathcal{L}_{refine} = \min_{\mathbf{s} \in Sym(\mathcal{O})} \left\| \mathbf{R}_{gt} \mathbf{s} - \hat{\mathbf{R}} \right\|_F + \lambda \|\mathbf{t}_{gt} - \hat{\mathbf{t}}\|_2\]

其中 $Sym(\mathcal{O})$ 为物体的对称群，避免对称物体的姿态歧义。

3.5.5 优缺点分析

维度	优点	缺点
先驱意义	开创百万级合成数据+新物体泛化范式	速度较慢（约数秒/帧）
数据集	完全开源（MIT 协议）ShapeNet-1M / GSO-1M	精度被 FoundationPose 超越
泛化性	无需目标物体参与训练	需 ROI 预先给定

3.6 其他补充

DOPE

• Deep Object Pose Estimation for Semantic Robotic Grasping of Household Objects
• 基于ROS与D435的DOPE-ROS-D435
• DOPE源码Deep_Object_Pose

用合成的图片来训练模型，也可以让机械臂在真实世界中识别并抓取物体。通过Domain Randomization来实现各种各样的合成数据。

DOPE属于典型的基于关键点回归（Keypoint-based）的 6D 姿态估计方法。它的核心思路是将 3D 姿态估计分解为：2D 关键点检测 + PnP 求解：

• 预测物体 3D 边界框的 8 个顶点以及 1 个质心在 2D 图像上的投影位置（共 9 个点）。
• 获得 2D 关键点坐标后，利用已知的物体 3D 模型尺寸，通过 PnP (Perspective-n-Point) 算法直接解算出相机坐标系下的 $R$（旋转）和 $t$（平移）。

---

四、手眼标定

4.1 问题背景与两种安装配置

手眼标定（Hand-Eye Calibration） 的目的是精确确定相机坐标系与机械臂坐标系之间的刚性变换关系，使机械臂能够根据相机观测到的物体位姿，准确计算运动指令。根据相机的安装位置，分为两种经典配置：

4.2 Eye-in-Hand（随动式）：

求解 AX = XB

配置说明

相机固定安装在机械臂末端执行器（End-Effector）上，随末端运动。标定目标（棋盘格/ArUco 板）固定在工作台上不动。

已知量：

• $\mathbf{T}_{WE}^{(i)}$：由机械臂正运动学（FK）给出的第 $i$ 个位姿下末端相对于基座（世界坐标系 $W$）的变换；
• $\mathbf{T}_{CT}^{(i)}$：相机通过图像检测标定板得到的相机到标定板的变换（PnP 求解）。

未知量：$\mathbf{X} = \mathbf{T}_{EC}$（末端坐标系到相机坐标系的变换）。

方程推导

由于标定板在世界坐标系中静止不动，任意两次位姿 $i$ 和 $j$ 下有约束：

\[\mathbf{T}_{WT} = \mathbf{T}_{WE}^{(i)} \cdot \mathbf{T}_{EC} \cdot \mathbf{T}_{CT}^{(i)} = \mathbf{T}_{WE}^{(j)} \cdot \mathbf{T}_{EC} \cdot \mathbf{T}_{CT}^{(j)}\]

整理得：

\[\underbrace{\left(\mathbf{T}_{WE}^{(j)}\right)^{-1} \mathbf{T}_{WE}^{(i)}}_{\mathbf{A}} \cdot \mathbf{X} = \mathbf{X} \cdot \underbrace{\mathbf{T}_{CT}^{(i)} \left(\mathbf{T}_{CT}^{(j)}\right)^{-1}}_{\mathbf{B}}\]

即经典 AX = XB 方程。其中 $\mathbf{A}$ 是机器人末端的相对运动，$\mathbf{B}$ 是相机观测到的标定板相对运动，$\mathbf{X} = \mathbf{T}_{EC}$ 为唯一未知量（$\mathbf{X} = \mathbf{Z}$ 的特殊情况）。

注意：单组方程有无穷多解，需至少 n ≥ 3 次不同的机械臂运动（且运动轴各不相同）才能唯一确定 $\mathbf{X}$。

4.3 Eye-to-Hand（固定式）

求解 AX = ZB

配置说明

相机固定安装在机器人工作空间的外部支架上，不随末端运动。标定目标（棋盘格）安装在末端执行器上，随末端运动。

已知量：

• $\mathbf{T}_{WE}^{(i)}$：FK 给出的末端相对于基座（世界坐标系 $W$）的变换；
• $\mathbf{T}_{CT}^{(i)}$：相机观测到末端标定板的变换（PnP 求解）。

未知量有两个：

• $\mathbf{X} = \mathbf{T}_{EW}$（末端坐标系到世界坐标系的变换，即基座在相机视角下的外参，其实是机器人世界-基座变换）；
• $\mathbf{Z} = \mathbf{T}_{CW}$（相机坐标系到世界坐标系的变换，即固定相机的外参）。

方程推导

由于固定相机对末端标定板的观测满足：

\[\mathbf{T}_{CT}^{(i)} = \mathbf{T}_{CW} \cdot \mathbf{T}_{WE}^{(i)} \cdot \mathbf{T}_{ET}\]

整理（消去末端到标定板的固定变换 $\mathbf{T}_{ET}$）后得：

\[\underbrace{\mathbf{T}_{WE}^{(i)}}_{\mathbf{A}} \cdot \mathbf{X} = \mathbf{Z} \cdot \underbrace{\mathbf{T}_{CT}^{(i)}}_{\mathbf{B}}\]

即 AX = ZB 方程，需同时求解 $\mathbf{X}$ 和 $\mathbf{Z}$ 两个未知变换矩阵（Zhuang et al., 1994）。

与 AX=XB 的本质区别：AX=XB 是 AX=ZB 在 X=Z 时的特殊情况（即相机-末端和世界-基座变换可以合并）。AX=ZB 的通用形式需要联立求解两个未知量，但信息量也因此更丰富——即使只有单次运动数据也有约束（虽仍需多次运动保证唯一解）。

4.4 求解方法

分离式闭合解：Tsai-Lenz 方

Tsai-Lenz 是最经典的 AX=XB 求解方法，分两步求解：

旋转求解：将旋转矩阵转为旋转向量 $\mathbf{a} = \log(\mathbf{R}_A)$，$\mathbf{b} = \log(\mathbf{R}_B)$，旋转方程退化为线性系统：

\[(\mathbf{R}_A - \mathbf{I})\,\mathbf{x} = \mathbf{b} - \mathbf{R}_A \mathbf{a}\]

多次运动叠加为超定系统，最小二乘求旋转向量 $\mathbf{x}$，归一化后恢复 $\mathbf{R}_X$。

平移求解：代入旋转结果后，平移满足：

\[(\mathbf{R}_A - \mathbf{I})\,\mathbf{t}_X = \mathbf{R}_X \mathbf{t}_B - \mathbf{t}_A\]

再次最小二乘求解 $\mathbf{t}_X$。

缺点：旋转和平移分步求解会导致误差传播——旋转误差直接影响平移估计精度。

同时求解：对偶四元数法

将旋转和平移统一编码为对偶四元数 $\hat{\mathbf{q}} = \mathbf{q} + \epsilon \mathbf{q}’$，将 AX=XB 转化为关于 $\hat{\mathbf{x}}$ 的线性方程，同时求解旋转和平移，有效降低误差传播，在测量噪声下比分离式方法更鲁棒。

Kronecker 积法（AX=ZB，Zhuang et al., 1994）

专门针对 AX=ZB 的闭合解：利用 Kronecker 积将矩阵方程向量化，转化为线性方程组 $\mathbf{M}\text{vec}(\mathbf{X}, \mathbf{Z}) = \mathbf{0}$，通过 SVD 同时求解 $\mathbf{X}$ 和 $\mathbf{Z}$。

非线性迭代优化（最优）

在测量噪声下，基于 $SE(3)$ 的非线性最小二乘效果最佳：

\[\min_{\mathbf{X}} \sum_{(i,j)} \left\| \mathbf{A}_{ij} \mathbf{X} - \mathbf{X} \mathbf{B}_{ij} \right\|_F^2\]

使用 Levenberg-Marquardt 算法在李群 $SE(3)$ 上迭代优化，是目前精度最高的实用方法（如 OpenCV cv2.calibrateHandEye 的 ROBOT_WORLD_HAND_EYE 模式）。

4.5 标定流程与精度

实际操作：

1. 将棋盘格固定在桌面（Eye-in-Hand）或安装在末端（Eye-to-Hand）；
2. 控制机械臂运动到 15–20 个不同位姿，尽量覆盖三轴大角度旋转（>30°）；
3. 每个位姿：记录相机检测标定板的 $\mathbf{T}{CT}$ 和 FK 给出的 $\mathbf{T}{WE}$；
4. 求解对应方程组得手眼变换。

精心标定通常可达 ±1–5 mm 位置误差（如幻灯片所示系统已实现 ±5 mm）。

ROS 工具：easy_handeye、visp_hand2eye_calibration、OpenCV cv2.calibrateHandEye。

4.6 完整坐标变换链

完成手眼标定后，对相机中观测到的物体位姿 $\mathbf{T}_{C \to obj}$，通过完整变换链推算物体在基座坐标系中的位置：

Eye-in-Hand：

\[\mathbf{T}_{W \to obj} = \mathbf{T}_{W \to E} \cdot \mathbf{T}_{E \to C} \cdot \mathbf{T}_{C \to obj}\]

Eye-to-Hand：

\[\mathbf{T}_{W \to obj} = \mathbf{T}_{W \to C}^{-1} \cdot \mathbf{T}_{C \to obj} = \mathbf{Z}^{-1} \cdot \mathbf{T}_{C \to obj}\]

---

五、运动规划与执行

5.1 MoveIt 2 简介

MoveIt 2（适配 ROS 2）是机器人操作领域最广泛使用的开源运动规划框架，提供了统一接口整合运动规划、碰撞检测、逆运动学求解和轨迹执行等功能。

MoveIt 的核心节点是 move_group，通过插件机制与各种规划库（OMPL、CHOMP、Pilz）交互，并维护规划场景（Planning Scene）来表示机器人和环境的完整状态。

5.2 逆运动学（IK）

在执行抓取前，需要将末端目标笛卡尔位姿 $\mathbf{T}_{target}$ 转换为关节空间角度 $\boldsymbol{\theta} = (\theta_1, \ldots, \theta_n)$。对于 6-DOF 机械臂（如 UR5, Franka Panda），解析 IK 最多有 16 个解，需结合配置约束选择最优解。MoveIt 中常用 KDL 和 TRAC-IK 插件。

5.3 运动规划算法

OMPL：采样规划

RRT（Rapidly-exploring Random Trees）：在关节空间随机采样延伸树结构：

\[q_{rand} \sim \text{Uniform}(\mathcal{C}), \quad q_{new} = \text{Extend}(q_{near}, q_{rand})\]

RRTConnect：双向 RRT，从起点和终点同时生长两棵树并尝试连接，是 MoveIt 的推荐默认规划器，规划效率显著提升。

RRT*：引入 rewire 操作，渐近收敛到最优路径，适合对路径质量要求严格的场景。

PRM（Probabilistic Roadmap Method）：预先构建概率路图，适合在固定工作空间内重复规划。

CHOMP：轨迹优化

CHOMP 通过梯度下降同时最小化轨迹平滑代价和碰撞代价：

\[\mathcal{U}[\xi] = \mathcal{F}_{obs}[\xi] + \lambda \mathcal{F}_{smooth}[\xi]\] \[\mathcal{F}_{obs}[\xi] = \int_0^1 \sum_{b \in \mathcal{B}} |\dot{\xi}| \cdot c(\mathbf{x}_b(\xi)) \, dt, \quad \mathcal{F}_{smooth}[\xi] = \frac{1}{2} \int_0^1 \|\ddot{\xi}\|^2 \, dt\]

其中 $c(\mathbf{x})$ 为障碍物的符号距离场代价，$\mathcal{B}$ 为机器人上的控制点集合。

Pilz：工业运动规划器

适合规整运动类型：LIN（笛卡尔直线运动）、CIRC（圆弧运动）、PTP（点到点关节运动）。

5.4 碰撞检测

MoveIt 使用 FCL（Flexible Collision Library） 进行高效碰撞检测，支持自碰撞（通过 SRDF 配置忽略对）和与环境障碍物的碰撞。Octomap 将深度相机点云实时转化为三维体素占据栅格，提供动态环境表示。

5.5 抓取姿态生成

在视觉抓取中需生成末端执行器的抓取姿态（Grasp Pose）：

• GPD（Grasp Pose Detection）：点云输入卷积网络，直接预测抓取候选得分和位姿；
• GraspNet-1Billion（Fang et al., CVPR 2020）：在 10 亿量级抓取标注数据上训练，实现通用密集抓取预测；
• AnyGrasp / Contact-GraspNet：新一代端到端抓取方法，泛化性更强。

5.6 完整抓取执行流程

下图以 Mermaid 流程图展示完整的视觉抓取执行流程：

图7：完整视觉抓取执行流程图

流程关键说明：

• 感知（A→B→D）：传感器采集 → 检测定位 → 6D 姿态估计，整体约 0.5–2s（首帧）；
• 变换（E）：利用手眼标定结果将物体位姿从相机坐标系转到机械臂基座坐标系；
• 规划（F→H）：生成抓取位姿后，通过 IK 求解关节角目标，再由 MoveIt 规划无碰撞轨迹；
• 执行（K）：ros2_control 驱动机械臂沿规划轨迹运动；
• 反馈（L）：检测抓取成功/失败，必要时回到检测步骤重新规划。

---

六、总结与展望

主要技术要点总结

模块	核心方法	关键指标
目标检测	YOLO11 / YOLOv10	mAP, FPS
实例分割	SAM 2.1 / SAM 3	mIoU, 零样本泛化
6D 姿态估计	FoundationPose, SAM-6D, MegaPose	BOP AR, ADD(-S)
手眼标定	AX=XB (Eye-in-Hand) / AX=ZB (Eye-to-Hand)	±mm 级位置误差
运动规划	MoveIt + OMPL/RRT* + CHOMP	规划成功率
抓取生成	GraspNet-1B / AnyGrasp	Grasp Success Rate

当前挑战与未来方向

无纹理/对称物体：现有 RGB-based 方法对无纹理或高对称性物体的姿态歧义问题仍未完全解决，需结合几何约束或对称群显式建模。

遮挡与堆叠场景：Bin Picking 中高度遮挡的部分可见点云对6D估计提出更高要求，SAM-6D 的 Background Tokens 是一个方向，但仍有较大提升空间。

端到端学习：将检测、姿态估计、规划、执行整合为端到端可微系统（如 Diffusion Policy + 6D Pose 联合训练）是学术界热点。

基础模型的进一步应用：SAM 3、Grounded-SAM 等新型基础模型与机器人操作的结合正在快速发展；扩散模型（Diffusion Models）在抓取姿态生成中的应用也方兴未艾。

实时性与精度的权衡：FoundationPose 的 32 Hz 追踪模式是当前最佳实践之一，但在低算力平台（如 Jetson Orin）上的轻量化部署仍是挑战。

随着硬件算力的持续提升和基础模型范式的深化，视觉引导的机械臂抓取正在从受限场景走向开放世界——这正是当代机器人研究最令人兴奋的前沿之一。