📂 项目地址
GitHub：Xbotics-Embodied-AI-club/Xbotics-Embodied-AI-Handbook/tree/main
1 从“看得到”到“抓得到”：位姿估计在机器人操作中的定位
机器人操作任务中的视觉输出，不能直接等同于控制输入。 目标检测给出的是图像区域，实例分割给出的是前景像素，点云恢复给出的是相机坐标系下的几何观测；而机械臂最终需要的是基座坐标系下、满足夹爪结构与运动学约束的抓取目标。位姿估计的功能，正是在这些不同表征之间建立可计算、可验证、可执行的转换关系。
可以先从一个最普通的桌面抓取场景说起：相机看到桌面上有一个杯子，检测器给出了一个框，分割模型也把杯身轮廓描了出来。到这一步，人很容易觉得“机器人已经看见杯子了”。但对机械臂来说，问题才刚刚开始：杯子离相机多远？杯口朝上还是倾斜？夹爪应该抓杯身、杯口，还是杯柄附近？这些问题都不在检测框里，也不在二维 mask 里。位姿估计正是在这里进入链路，把“图像里有一个目标”继续推进为“空间中有一个可操作的刚体对象”。
换句话说，本讲关心的不是单个视觉模型能否在图像上画出漂亮结果，而是机器人如何把一次观测转化为动作决策。这个过程可以分成三类表征：图像域的观测表征、三维空间中的物体状态表征，以及机器人控制器可消费的动作表征。位姿估计位于中间层，既承接前端视觉，又约束后端抓取。
图 6.1 可以看作本讲的主线：左侧是机器人获得的 RGB-D 观测，右侧是机械臂真实执行的抓取动作，中间则依次经过检测、分割、点云、位姿、抓取映射和坐标转换。每一步都在回答一个更具体的问题：先问“目标在哪个图像区域”，再问“这个区域对应哪些三维点”，再问“这些点组成的物体在空间里如何摆放”，最后才问“夹爪应该以什么姿态去接触它”。
1.1 背景与动机：为什么机器人不能只靠检测抓取
1.1.1 检测框的本质：二维投影，信息严重缺失
目标检测与实例分割解决的是图像域中的语义归属问题；抓取控制解决的是三维空间中的接触与约束问题。二者的关键差异不是“精度高低”，而是输出空间不同：检测框属于二维投影表征，抓取位姿属于刚体位姿与动作表征。
这也是初学者最容易形成的误解：把“检测到了”误认为“可以抓了”。检测框只告诉我们物体在画面里的大致范围，它没有深度、没有旋转，也不知道夹爪从哪一侧接近更安全。对人来说，这些信息可以凭常识补全；对机器人来说，它们必须被显式计算出来。
若系统仅输出 bbox，抓取点可能落在背景、桌面或物体边缘；若仅输出 mask，目标虽然已与背景分离，但尚未进入米制三维空间；若仅输出三维中心，机械臂可以到达目标附近，却无法确定夹爪应如何旋转。即使获得 object pose，也仍需结合物体几何、夹爪结构和任务约束，进一步生成 grasp candidate。因此，位姿估计必须放在“感知表征逐步转化为操作表征”的链路中理解。
1.1.2 从 2D 到 3D：维度跃迁带来的根本性挑战
从信息闭合的角度看，抓取任务至少要求系统回答四类问题：目标是否存在，目标在三维空间中的位置，目标相对于夹爪的朝向，以及夹爪能否沿某条无碰撞路径完成接触。检测框只回答第一类和一小部分第二类问题；mask 进一步改善前景分离，但仍停留在图像域；object pose 才开始描述物体的刚体状态；grasp candidate 则把物体状态与末端执行器结构、接触参数和任务约束联系起来。也就是说，位姿估计不是为了“多输出几个自由度”，而是为了补齐从视觉判断到物理执行之间缺失的空间约束。
从控制系统视角看，这一步也是变量空间的切换：像素坐标 没有物理单位，深度点 进入相机坐标系但缺少刚体朝向，6D 位姿 才同时包含位置和旋转。机器人抓取所需的不是“图像上哪里亮”，而是“某个刚体相对于机器人在哪里、如何摆放、能否被当前夹爪稳定接触”。这正是 2D 感知和 3D 操作之间最本质的差距。
1.1.3 位姿估计何时是关键中间环节
对于位姿驱动抓取，一条最基本的变换链可写为：
其中， 表示物体坐标系相对于相机坐标系的 6D 位姿， 表示在物体坐标系中定义的抓取模板， 来自手眼标定， 则是提交给运动规划器的基座系抓取目标。由此可见，6D 位姿并不是链路终点，而是连接视觉几何与抓取动作的中间状态。
一个实用判断是：如果任务只要求“指向一个区域”，bbox 或 3D 中心可能已经足够；如果任务要求“沿某个方向接近、避开桌面、夹住局部结构”，位姿、抓取模板和坐标转换通常会成为更可靠的中间约束。但显式 object pose 并不是所有抓取系统的必经环节：直接 6-DoF 抓取检测、场景点云到抓取候选生成、端到端视觉运动策略，都可能绕过物体坐标系位姿，直接预测 grasp pose、抓取候选或动作。
本节小结： 检测框和实例掩膜让机器人“看见”目标，但它们仍然停留在图像投影层面，无法直接回答目标的深度、朝向、接触位置和执行坐标系问题。对于基于已知物体模型或抓取模板的位姿驱动抓取，6D object pose 是常用且可验证的中间表示；对于直接抓取检测或端到端策略，系统也可以直接预测 grasp pose、抓取候选或动作。理解这一点之后，6D 位姿就不再是“多预测几个自由度”的必然环节，而是一种在可解释性、几何约束和工程调试上很有价值的中间表征。
1.2 机器人感知的三层目标：识别、定位与定姿
机器人操作中的视觉感知可归纳为三个递进目标：识别（Recognition）—定位（Localization）—定姿（Pose Estimation）。三者分别回答“是什么”“在哪里”“朝向如何”，共同构成从语义理解到几何恢复再到刚体状态估计的基本链条。
这三个问题可以按机器人逐步接近动作执行的顺序理解。识别阶段确定目标身份：场景里哪个是杯子，哪个是盒子。定位阶段开始测量空间位置：目标在相机前方多少米、相对机器人基座处于什么位置。定姿阶段则进入操作细节：杯子是否倾斜、盒子的长边朝向哪里、夹爪开合方向是否应与物体长轴对齐。只有走完这三步，视觉结果才逐渐接近控制系统能够使用的形式。
1.2.1 第一层：识别（Recognition）——解决"是什么"的问题
识别层负责确定任务目标和语义类别，是整条链路的入口。它通常输出类别、检测框或初始 mask，帮助系统从复杂场景中选出需要继续处理的对象。
在机器人抓取中，识别并不只是给图像打标签，而是决定后续几何计算应该围绕哪个目标展开。桌面上可能同时存在杯子、盒子、工具和杂物，如果系统没有先确认任务对象，后续的点云提取和位姿估计就可能处理到错误物体。对于“拿起红色杯子”这类任务，识别层还要结合类别、颜色、位置或语言指令，在多个候选目标之间完成选择。
识别结果通常不直接进入控制器，而是作为空间裁剪和语义过滤的依据。检测框可以限制分割模型的搜索范围，类别信息可以决定是否调用某个 CAD 模型或抓取模板，置信度则可以作为是否继续执行的门槛。若识别阶段漏检，整条链路无法启动；若误检，把背景或相邻物体当成目标，后续几何算法即使计算得很精确，也只是在精确地处理错误对象。
1.2.2 第二层：定位（Localization）——解决"大致在哪里"的问题
定位层把二维候选区域与深度观测结合起来，恢复目标在三维空间中的位置。它不一定要求完整旋转姿态，但至少要给出可用于接近的空间坐标或目标点云。
这一层完成的是从图像平面到物理空间的关键过渡。检测框和 mask 只说明目标投影在哪些像素上，定位层需要利用深度图、相机内参和前景掩膜，把这些像素反投影成相机坐标系下的三维点。此时系统开始获得米制尺度的信息，例如物体中心离相机多远、位于桌面左侧还是右侧、是否在机械臂可达范围内。
定位结果可以有不同粒度。最简单的是一个 3D 中心点，适合顶部抓取、点击、靠近观察等对姿态要求较低的动作；更完整的是目标点云，它保留了物体局部形状和表面分布，可作为后续 6D 位姿估计、主轴提取或抓取候选生成的输入。定位误差通常会表现为抓空、偏抓或接近路径偏离目标，因此真机调试时要重点检查深度尺度、相机内参、mask 是否混入背景点，以及手眼标定是否带来系统偏移。
1.2.3 第三层：定姿（Pose Estimation）——解决"朝哪个方向"的问题
定姿层进一步恢复物体坐标系的朝向，使夹爪能够根据物体长轴、局部平面、孔洞或把手等几何结构调整自身姿态。对侧向抓取、插拔、装配等任务而言，这一层往往决定操作能否稳定完成。
与定位只关心“到哪里”不同，定姿关心的是物体坐标系相对于相机或机器人坐标系如何旋转。对于规则盒子，夹爪可能需要与长边对齐；对于杯子，杯柄方向会影响可抓区域；对于插拔和装配任务，几度的姿态偏差就可能导致顶撞或无法插入。因此，6D 位姿中的旋转部分并不是附加信息，而是把视觉几何转化为可执行接触动作的核心约束。
定姿方法通常会利用 RGB 纹理、点云几何、关键点、CAD 模型或学习得到的特征来估计刚体变换。工程上还要处理对称物体带来的多解问题：圆柱体绕自身轴旋转可能在视觉上等价，但夹爪、标签朝向或任务约束未必等价。定姿失败时，机器人往往不是够不到目标，而是以错误角度接近目标，表现为夹爪斜撞、夹偏、碰到桌面或无法沿预期方向插入。因而，定姿层是从“空间定位”走向“动作对齐”的关键一步。
这三个层级在工程实现中并不一定严格分步。例如，部分 6D 位姿网络会同时输出平移和旋转，部分抓取检测网络会绕过显式 object pose 直接输出 grasp pose。但在系统分析和调试中，将三者区分仍然重要：定位误差主要影响“能否到达目标附近”，定姿误差主要影响“夹爪能否以正确方向接触物体”。前者通常表现为抓空或偏位，后者更容易表现为夹偏、顶撞或滑脱。
这种区分在真机调试中尤其重要。若机器人每次都偏向同一侧，问题往往在深度尺度、手眼外参或 TCP 偏置；若机器人位置基本正确但夹爪总是斜着撞向物体，问题更可能来自旋转估计、物体对称性处理或抓取模板定义。把“识别、定位、定姿”拆开分析，可以避免把所有失败都笼统归因于“模型不准”。
本节小结： 识别、定位和定姿分别回答“是什么”“在哪里”和“朝向如何”，三者不是并列的孤立任务，而是一条逐步逼近操作执行的感知阶梯。识别层决定系统应该处理哪个对象，定位层把目标从像素区域推进到米制三维空间，定姿层则进一步恢复物体坐标系，使夹爪可以按正确方向接触目标。工程调试中，把这三层拆开分析非常重要：漏检、深度偏移和姿态翻转会导致完全不同的失败表现。后续的点云提取、6D 位姿估计和抓取生成，正是在这三层信息逐渐闭合的基础上展开。
1.3 两条技术路线：模块化感知 vs 端到端感知
按照是否显式构造中间表征，机器人抓取系统可分为两类：模块化感知与端到端感知。前者将检测、分割、点云、位姿、抓取规划拆成可检查的模块；后者尝试由原始观测直接生成抓取动作或动作评分。
如果把抓取系统看成一条生产线，模块化方法会在每个工位留下可检查的半成品：bbox、mask、点云、位姿矩阵、抓取候选。端到端方法则更像一个整体策略网络，输入图像和状态，直接给出动作。前者调试成本低、链路更长；后者形式简洁、数据要求更高。两者没有绝对优劣，关键取决于任务是否需要可解释、可验证和可逐步排错。
1.3.1 模块化感知：经典的分层流水线架构
模块化路线强调显式中间结果。每一步输出都可以被保存、可视化和单独评估，因此特别适合教学实验、工业部署和需要追责排错的真实机器人系统。
1.3.2 端到端感知：直接映射的数据驱动范式
端到端路线试图减少人工设计的中间步骤，让模型直接从图像、点云或状态输入中预测动作。它的优势是形式统一、可吸收大规模数据；难点在于数据成本、泛化边界和失败解释。
1.3.3 深度对比与综合权衡
本讲采用模块化路线作为主线。原因并不是端到端路线不重要，而是模块化路线更适合建立清晰的机器人感知知识结构：每一级表征都有明确的数据类型、坐标系和误差来源，便于将视觉算法与运动规划、手眼标定和真机调试连接起来。
实际系统中，两条路线也常常是混合的。例如，前端可以使用开放词汇检测和通用分割模型获得目标 mask，后端仍然使用点云滤波、ICP、手眼标定和运动规划保证几何一致性；也可以用学习方法生成多个抓取候选，再用 IK 与碰撞检测筛掉不可执行的结果。这样的混合架构既利用了深度模型的泛化能力，又保留了机器人系统中必要的几何校验。
本节小结： 模块化路线把检测、分割、点云、位姿和抓取拆成可观察的中间环节，优势是数据类型清晰、误差来源可追踪、单模块验证方便；端到端路线则试图用数据直接学习从观测到动作的整体映射，优势是接口简洁、表达能力强、适合开放场景策略学习。两条路线的差别，本质上是可靠性、可解释性、数据成本和泛化能力之间的工程权衡。真实机器人系统通常不会极端地选择其中一边，而是在前端吸收大模型的开放能力，在后端保留点云几何、手眼标定、IK 和碰撞检测等硬约束。这样的折中也解释了本讲为什么以模块化链路为主线，同时保留对学习式方法的讨论。
1.4 本讲核心内容与学习路径
本讲后续内容按照“二维感知 → 三维点云 → 6D 位姿 → 抓取执行”的顺序展开。核心问题不是单个算法如何刷高指标，而是前一阶段输出如何成为后一阶段的输入约束。
因此，阅读本讲时可以始终抓住一条线：机器人不是为了“看懂图像”而看图像，而是为了最后能安全地动手。第 3 节关注如何把二维前景变成干净点云；第 4 节关注如何从点云和图像中恢复物体姿态；第 5 节关注如何把物体姿态进一步翻译成夹爪动作。这样看，后续每个技术模块就不是孤立知识点，而是同一条抓取链路上的连续环节。
1.4.1 本讲定位：服务机器人场景下的全链路视角
本讲不把位姿估计孤立为一个视觉算法，而是把它放在机器人操作闭环中讨论。这样处理的好处是，读者可以同时看到模型输出、坐标变换、抓取模板和执行校验之间的依赖关系。
1.4.2 从输入到执行：七步闭环学习主线
本节小结： 本讲的核心不是孤立讲一个位姿估计算法，而是建立从 RGB-D 观测到机器人执行目标的完整链路视角。位姿估计的价值不在于单独输出一个 6D 数值，而在于把图像域的语义线索与深度几何观测转化为机器人可继续使用的中间状态。沿着这条主线，第 3 节先构造高质量目标点云，第 4 节恢复物体刚体状态，第 5 节再把物体状态映射为满足坐标、机构和安全约束的抓取位姿。读后续内容时，应始终追问“这个输出如何被下一环使用、它的误差会如何传导、它能否被真机验证”，这样整条链路才会从概念变成可部署的工程流程。
2 机器人感知任务体系与标准抓取流水线
2.1 机器人感知任务分类图谱
机器人视觉感知是一个庞大的任务集合，不同任务对应不同的输出形式、技术路线与应用场景。从最基础的图像分类到复杂的多传感器融合感知，各任务之间既有独立的适用范围，又存在明确的依赖与递进关系。建立清晰的分类体系，有助于精准定位位姿估计在整个领域中的位置，理解各任务之间的衔接逻辑，也为后续章节中流水线设计、算法选型与系统调试提供概念基础。
2.1.1 按任务目标划分：语义、几何与属性三大类
按照任务的核心输出目标，机器人感知可分为语义类、几何类、属性类三大分支。三者从"看懂是什么"到"量清在哪里"再到"知道怎么用"逐级深化，构成了从环境理解到物理操作的完整认知链路。
1.语义类任务：核心解决"是什么"的问题，输出语义标签与区域信息。
语义感知是机器人进入场景后的第一步认知，其目标是将原始像素映射为有意义的概念标签。根据输出粒度的不同，语义类任务可进一步细分为四个层次。图像分类是最基础的语义任务，它判断整张图像的主体类别，适用于场景级粗分类，如判断当前相机视野中是否包含待抓取物体。目标检测在分类的基础上叠加了定位能力，输出图像中每个目标的类别与矩形边界框，兼顾语义理解与粗定位，是当前工业机器人抓取系统中使用最广泛的语义模块。实例分割将输出粒度推进到像素级别，为每个独立物体生成精确的轮廓掩膜，从而实现目标与背景的精确分离，是连接语义感知与几何计算的关键桥梁。语义分割则关注场景中不同类别的区域划分，如桌面、地面、墙壁、传送带等，用于机器人对整体环境的语义理解与导航决策。
2.几何类任务：核心解决"在哪里、朝哪"的问题，输出空间几何信息。
几何感知是机器人从"看见"走向"操作"的关键跃迁。语义感知告诉机器人场景中有哪些物体，而几何感知则告诉机器人这些物体在三维空间中的精确位置与姿态。按照输出信息的完整度，几何类任务可划分为四个层级。3D 中心点估计输出物体的三维质心坐标，是最低粒度的几何输出，常用于粗略的目标定位与抓取点引导。6D 物体位姿估计输出物体在相机坐标系下的完整三维位置与旋转姿态，包含 3 个平移自由度和 3 个旋转自由度，是抓取操作的核心中间表示，也是绝大多数位姿驱动抓取系统的关键环节。抓取位姿估计在物体位姿的基础上进一步结合夹爪模型与抓取策略，直接输出夹爪的可执行位姿，是几何感知链条的最终操作目标。深度估计则从单张 RGB 图像预测场景的稠密深度图，为单目视觉系统提供从二维到三维的映射能力，是缺乏主动深度传感器时的替代方案。
3.属性与 Affordance 类任务：核心解决"能怎么用"的问题，输出物体的操作属性。
语义感知告诉机器人"这是什么"，几何感知告诉机器人"它在哪里"，但机器人要完成实际抓取操作，还需要知道"它能怎么用"。属性与 Affordance 类任务正是为此而生。物理属性估计关注物体的尺寸、重量、材质、重心分布等固有物理特性，这些信息直接影响抓取策略的选择——例如，易碎物体需要轻柔夹持，重心偏移的物体需要调整夹取位置。Affordance 感知直接面向可操作性，判断物体的可操作区域与可执行动作，如杯柄适合提拉、瓶盖适合旋转、按钮适合按压，这种"功能导向"的感知方式使机器人能够理解物体与操作动作之间的映射关系。力觉触觉感知则通过接触传感器在抓取过程中实时感知交互状态，如滑移检测、接触力反馈等，用于抓取过程中的闭环修正与自适应调整，是连接感知与执行的关键反馈通道。
三类任务在机器人操作系统中各司其职：语义类任务构建场景认知，几何类任务提供空间坐标，属性类任务指导操作策略。三者协同工作，共同支撑起机器人在复杂环境中的自主操作能力。
2.1.2 按输入模态划分：从单目到多传感器融合
输入模态决定了感知系统的信息上限。不同的传感器配置带来不同的信息丰富度、系统复杂度和适用场景，按照传感器配置的递进关系，可将感知系统分为四个层级。
1.单 RGB 模态：仅依靠彩色相机输入，硬件成本最低，但缺乏直接的几何深度信息。
单 RGB 模态是视觉感知的最简配置，仅使用一台彩色相机获取场景的二维纹理信息。其最大优势在于硬件成本低、系统部署简单、数据采集方便。然而，由于缺乏直接的深度测量，三维信息完全依赖算法从单张或连续多帧 RGB 图像中推理得到，如通过单目深度估计网络预测深度图，或通过运动恢复结构从多视图重建三维点云。这种依赖算法推理的几何获取方式精度有限，对纹理稀疏、光照变化、反射表面等场景的鲁棒性较差。单 RGB 模态适用于对深度精度要求不高、场景结构简单且纹理丰富的抓取任务，或在深度传感器无法使用的特殊场景中作为替代方案。
2.RGB-D 模态：彩色图像与深度图同步输入，同时提供语义纹理与几何距离信息。
RGB-D 模态是当前机器人抓取感知的主流标准配置。深度相机通过结构光、飞行时间或双目立体视觉等原理，在获取彩色图像的同时同步输出像素对齐的深度图，使每个像素点同时具备颜色信息与距离信息。这种配置从根本上解决了单目视觉缺乏直接深度测量的核心瓶颈，使得三维点云的获取变得快捷可靠。RGB-D 模态的典型传感器包括 Intel RealSense 系列、Microsoft Kinect 系列、Orbbec 系列等，它们体积小巧、成本适中、精度满足多数抓取场景需求，已成为学术研究与工业部署中的事实标准。RGB-D 模态的局限性在于：深度传感器存在测量范围限制，对透明物体、反光表面、黑色吸光物体等特殊材质的深度测量效果不佳，且强光环境下红外结构光容易受到干扰。
3.多相机模态：多台相机从不同视角协同观测，减少遮挡盲区，提升位姿估计精度。
单台 RGB-D 相机虽然能提供完整的三维信息，但受限于单一视角，不可避免地存在遮挡盲区——物体被遮挡的部分无法被观测到，导致点云数据不完整，进而影响位姿估计的精度与鲁棒性。多相机模态通过部署两台或更多相机从不同角度协同观测，有效弥补了单视角的遮挡缺陷。多相机系统需要解决的关键技术问题包括：多相机之间的外参标定（将不同相机的坐标系统一到同一参考系下）、多视角数据的融合策略（如何将不同视角的点云和图像信息有效融合）、以及计算资源的协同分配。多相机模态显著提升了位姿估计的精度上限，尤其适用于精密操作和复杂装配场景，但系统复杂度和硬件成本也随之增加。
4.多传感器融合：融合视觉、激光雷达、力觉、IMU 等多种传感器信息。
多传感器融合是感知系统的最高配置形态，通过融合多种异质传感器的信息，弥补单一传感器模式的固有缺陷，实现优势互补。典型的融合配置包括：视觉传感器提供丰富的语义纹理信息，激光雷达提供大范围高精度的三维几何信息，惯性测量单元提供运动状态与加速度信息，力觉传感器提供接触交互信息。多传感器融合的关键技术在于异质数据的时间同步与空间对齐、不同置信度信息的融合策略、以及融合后的状态估计与不确定性量化。这种配置适用于动态、非结构化、光照条件复杂的真实场景，如户外移动操作、人机协作、灾难救援等，是高阶人形机器人与移动操作机器人的发展方向。
本节小结：机器人感知可从任务目标与输入模态两个维度建立分类体系。从任务目标看，语义、几何、属性三类任务逐级深化，分别回答"是什么""在哪里""怎么用"三个核心问题，共同支撑机器人的操作能力。从输入模态看，从单 RGB 到多传感器融合的逐步升级，不断提升感知系统的信息上限与环境适应性。位姿估计与抓取位姿生成作为几何类任务的核心环节，是连接语义感知与物理操作的关键桥梁，也是后续章节深入讨论的主题。
2.2 位姿驱动抓取的完整流水线
位姿驱动是当前工业与服务机器人抓取领域最成熟、落地最广泛的技术范式。它以 6D 物体位姿为核心中间表示，将物体在三维空间中的精确位置与姿态作为后续操作的依据，串联起从图像输入到机械臂执行的完整流程。理解这条流水线的每个环节及其依赖关系，是工程实践中进行系统设计、算法选型与故障排查的核心基础。
2.2.1 标准 8 步流程：从图像采集到机械臂执行
完整的位姿驱动抓取包含 8 个串行环节，前序环节的输出是后序环节的输入，形成严格的依赖链路。任一环节的失效都会阻断后续流程，因此理解每个环节的输入输出与功能边界至关重要。
1.RGB-D 图像采集
通过深度相机同步获取时间对齐、像素对齐的彩色图像与深度图，是整个感知系统的数据源头。彩色图像提供丰富的纹理与语义信息，深度图提供每个像素对应的空间距离信息。采集环节的关键技术指标包括：图像分辨率与帧率、深度测量范围与精度、彩色与深度图像之间的像素对齐精度、以及数据传输的实时性。采集质量直接决定了后续所有环节的性能上限——如果深度图存在大范围空洞或噪点，后续的点云提取与位姿估计将难以获得可靠结果。实践中，采集环节需要根据场景特点选择合适的相机型号与参数配置，如曝光时间、增益、白平衡等，并确保传感器处于稳定的工作状态。
2.目标检测
在 RGB 图像中运行目标检测器，输出目标物体的类别、边界框与置信度。目标检测的作用是快速缩小后续处理的空间范围，将感知注意力从整幅图像聚焦到目标物体所在的局部区域。当前主流的检测器包括 YOLO 系列、Faster R-CNN、DETR 等，在工业场景中通常以 YOLO 系列部署最为广泛，因其在检测速度与精度之间取得了良好的平衡。检测环节需要关注的关键指标包括：检测召回率（是否漏检了目标物体）和检测精度（是否出现了误检或边界框偏移），两者都会对后续流程产生直接影响。
3.实例分割
基于检测框或文本提示生成像素级实例掩膜，精确分离目标物体与背景像素。实例分割的输出是一张与 RGB 图像同尺寸的二值掩膜，其中目标物体对应的像素值为 1，背景像素值为 0。这一环节的核心价值在于消除背景噪声对后续几何计算的干扰——如果未做精确分割，背景点云混入目标点云，将导致位姿估计出现系统性偏移。当前可用的实例分割方法包括基于深度学习的经典方法（如 Mask R-CNN）和基于视觉基础模型的方法（如 SAM），后者通过文本提示或点在提示实现零样本分割，在开放类别场景中具有显著优势。
4.目标点云提取
用实例掩膜过滤深度图，通过像素反投影生成目标物体的三维点云，并完成滤波、去噪、降采样等预处理。具体而言，对于掩膜内每个像素，利用相机内参将其二维像素坐标与深度值反投影为相机坐标系下的三维点坐标，从而得到目标物体的局部点云。提取后的点云通常需要经过一系列预处理操作：统计滤波去除离群噪声点、体素降采样减少点云密度以降低计算开销、法向量估计为后续配准提供几何特征。点云质量的好坏直接决定了位姿估计的成败——空洞过多、噪声过大或点云过于稀疏都会导致位姿估计收敛失败或输出错误结果。
5.6D 位姿估计
基于目标点云与 RGB 特征，结合物体先验模型，计算物体在相机坐标系下的完整 6D 位姿矩阵。6D 位姿包含 3 个平移参数（x, y, z）和 3 个旋转参数（通常用旋转矩阵或四元数表示），构成一个 4×4 的齐次变换矩阵。当前主流的 6D 位姿估计方法可分为三类：基于对应点匹配的方法（如 PnP 求解）、基于点云配准的方法（如 ICP 迭代最近点算法）、以及基于端到端深度学习的方法（如 PoseCNN、GDR-Net 等）。实际工程中，通常采用"粗估计 + 精配准"的两阶段策略：先用深度学习网络获得初始位姿，再用 ICP 进行精细化迭代配准，兼顾速度与精度。
6.抓取位姿生成
结合物体位姿、几何尺寸与抓取策略，计算夹爪的接近方向、开合尺度、安全偏移与姿态匹配关系，生成可执行的抓取位姿与预抓取位姿。抓取位姿生成并非简单的位姿复制，而是需要考虑多个约束条件：夹爪的几何尺寸与运动学限制、物体的局部几何特征（如抓取表面是否平坦、是否存在可夹持的凸起结构）、物体与周围环境的碰撞可能性、以及抓取策略的选择（如顶抓、侧抓、两指夹持、吸盘吸取等）。预抓取位姿是夹爪在接近物体之前的准备位置，通常位于物体正前方一定偏移距离处，确保接近路径的安全性与可执行性。
7.基座坐标转换
通过手眼标定得到的外参矩阵，将相机坐标系下的抓取位姿转换为机器人基座标系下的位姿，完成视觉空间到机器人空间的映射。手眼标定是连接感知系统与执行系统的关键环节，通常分为"眼在手上"和"眼在手外"两种配置，分别对应不同的标定方法与计算流程。标定结果的精度直接影响抓取位姿在机器人空间中的准确性——一个微小的标定误差将在远距离处被放大为可观的位姿偏移。基座坐标转换的本质是一个刚体变换，即通过左乘一个 4×4 的齐次变换矩阵将相机坐标系下的位姿变换到基座坐标系。
8.机械臂执行
运动规划器基于目标抓取位姿生成无碰撞运动轨迹，驱动机械臂完成接近、抓取、抬升的完整动作。运动规划需要考虑机械臂的正逆运动学、关节限位、自碰撞避免、环境碰撞避免、以及运动轨迹的光滑性与速度连续性。常用的运动规划算法包括 RRT 系列（快速随机搜索树）、CHOMP（协变哈密顿优化运动规划）、STOMP（随机轨迹优化运动规划）等。执行阶段还包括力控与柔顺控制策略，在夹爪接触物体时通过力觉反馈进行自适应调整，以防止夹持力过大损坏物体或夹持力过小导致滑落。
2.2.2 各环节失效的误差传导效应
位姿驱动抓取流水线的核心特征是严格的串行依赖关系，误差具有逐级传导与放大的特性。理解各环节失效的典型表现与误差传导规律，是工程调试时快速定位故障根源的核心基础。
1.检测漏检与误检是流水线中最致命的失效模式。如果目标检测器未能检测到场景中的目标物体，后续所有环节将无法正确启动，抓取直接失败。如果检测器误将背景物体识别为目标，流水线将定位到错误目标，产生"抓取成功但拿错了物体"的隐蔽故障。检测环节的误差通常来自训练数据分布与部署场景的差异、极端光照条件、严重遮挡等情况。
2.分割轮廓偏差的影响具有传导性。实例分割输出的掩膜如果存在边界偏差——过大则混入背景像素，过小则截断目标物体的边缘部分——都会导致后续提取的目标点云不完整或包含噪声。这种偏差会进一步传导到位姿估计环节：背景点云的混入会使位姿出现系统性偏移，而目标点云被截断则可能使位姿估计在局部最优中收敛失败。
3.点云质量差是工程实践中最为常见的故障来源之一。深度传感器在透明、反光、吸光等特殊材质表面会产生显著的空洞与噪点，导致目标点云稀疏、残缺或包含大量离群点。点云质量差会直接导致位姿估计环节的收敛失败——ICP 算法在点云过于稀疏时无法找到正确的对应关系，深度学习方法的位姿输出也会因输入信息不足而出现较大偏差。
4.位姿估计偏差对抓取结果的影响最为直接。平移误差会使夹爪抓取位置偏离物体中心，导致夹偏或抓空；旋转误差则会使夹爪姿态与物体表面不匹配，导致夹爪与物体边缘碰撞、物体被推离原位或夹持不稳。位姿估计的精度要求与物体尺寸和夹爪类型密切相关：小型物体对平移误差更敏感，细长物体对旋转误差更敏感。
5.标定误差具有全局性影响特征。不同于其他环节的随机误差或局部误差，手眼标定误差会在整个工作空间中产生系统性的均匀偏移——所有抓取结果都出现固定方向和固定大小的位姿偏差。这种系统性误差在流水线调试中具有独特的诊断价值：如果机器人抓取物体时总是偏向同一方向，应当优先排查标定参数而非其他环节的算法性能。
理解上述误差传导规律，有助于在实操中建立系统化的故障排查思路：首先确认标定参数是否正确，再检查点云采集质量，然后依次验证检测、分割、位姿估计各环节的输出，最后定位到具体失效环节。
本节小结：位姿驱动抓取包含 8 步标准流水线，从数据采集到机械臂执行形成完整的前馈依赖链路。各环节的误差具有逐级传导放大的特性，前端感知数据的质量决定了系统的性能上限，标定误差则会带来系统性的全局偏差。在工程实践中，理解误差传导规律比掌握单个算法的原理更为重要，因为它是快速定位故障根源、系统性优化系统性能的核心能力。
2.3 核心概念辨析：bbox /mask/center point /object pose /grasp pose
初学者在入门机器人视觉抓取时，最容易混淆不同层级的感知输出概念。这些概念虽然都描述物体在空间中的某种"位置"，但它们在定义形式、维度信息、用途场景和精度要求上存在本质差异。清晰区分五类核心输出的定义与内涵，是建立正确认知体系的前提。
2.3.1 五类感知输出的定义与维度对比
五类感知输出按照信息粒度从低到高排列，逐级逼近机器人操作所需的完整空间描述。
边界框（Bounding Box, bbox） 是最粗略的感知输出形式，在二维图像平面上用一个带方向的矩形框标记目标物体的位置。其数学表示为 (x, y, w, h)，其中 (x, y) 为矩形框左上角或中心点的像素坐标，w 和 h 分别为框的宽度和高度。边界框仅提供二维平面上的粗略定位，不包含任何三维深度信息，也不包含物体的精确轮廓和旋转姿态。它适合作为后续精处理的空间范围引导，但无法直接用于机器人操作。
掩膜（Mask） 是像素级别的语义输出，在图像平面上为每个像素分配一个二进制标签，指示该像素是否属于目标物体。相比于边界框，掩膜提供了物体的精确轮廓，能够区分物体与背景的边界，从而消除背景像素对后续几何计算的干扰。掩膜仍然是在二维图像平面上的描述，不包含三维空间信息，但其像素级精度使其成为连接二维感知与三维几何计算的关键桥梁。
中心点（Center Point） 是三维空间中的最低粒度几何输出，通常表示为物体质心在相机坐标系下的三维坐标 (x, y, z)。中心点提供了物体在三维空间中的位置信息，但完全不包含物体的旋转姿态和尺寸信息。对于对称物体或球形物体，中心点可能足以引导抓取，但对于大多数非对称物体，仅凭中心点无法确定夹爪的接近方向与夹持姿态。
物体位姿（Object Pose） 是完整的 6D 空间描述，包含物体在相机坐标系下的三维位置 (x, y, z) 和三维旋转姿态 (rx, ry, rz)，通常以 4×4 齐次变换矩阵表示。物体位姿提供了物体在空间中的完整刚体变换信息，是连接视觉感知与机器人操作的核心中间表示。从物体位姿出发，可以计算出物体上任意一点在空间中的位置，也可以确定物体的朝向和对称性。
抓取位姿（Grasp Pose） 是机器人的最终操作目标，表示夹爪在机器人基座标系下的可执行位姿。与物体位姿不同，抓取位姿不仅包含空间位置与姿态，还隐含了夹爪的开合状态、接近方向、安全偏移等操作信息。抓取位姿是直接发送给机器人运动规划器的指令，其坐标系通常以机器人基座为参考，而非相机坐标系。从物体位姿到抓取位姿的转换，需要经过基座坐标变换和抓取策略计算两个关键步骤。
下表从六个维度对这五类输出进行系统对比：
五类输出的信息粒度逐级提升：从最粗糙的矩形区域，到像素级轮廓，再到三维点、完整物体姿态，最终落脚到可驱动机械臂的抓取指令。
2.3.2 用途场景与精度要求差异
不同输出对应不同的任务阶段，对精度的要求差异显著，越靠近执行端精度要求越高。
边界框主要用于目标检测与粗定位阶段，其精度要求相对宽松。在典型的抓取场景中，只要边界框完整包围目标物体且能够正确引导后续的分割与点云提取，即可满足要求。边界框的精度通常以交并比（IoU）衡量，在抓取场景中 IoU 达到 0.7 以上即可接受，因为边界框本身并不直接参与几何计算，其误差会在后续环节中被进一步修正。
掩膜的精度要求高于边界框，因为掩膜直接决定了目标点云的质量。掩膜如果过小，会截断物体边缘，导致点云不完整；如果过大，会混入背景像素，导致位姿估计出现系统性偏移。掩膜精度通常以边界 F1 分数或平均 IoU 衡量，在抓取场景中要求掩膜边界误差不超过数个像素。对于细小部件或精密操作场景，掩膜精度要求更高，需要亚像素级别的分割精度。
中心点的精度要求取决于物体尺寸与夹爪类型。对于大型物体（如箱子、盒子），中心点误差在厘米级别仍可接受，因为夹爪的容错空间较大。对于小型物体（如螺丝、电子元件），中心点误差需要控制在毫米级别，否则夹爪可能完全错过目标。中心点精度通常以绝对位置误差（APE）衡量。
物体位姿的精度要求与抓取任务类型密切相关。对于普通抓取任务，平移误差在物体尺寸的 10% 以内、旋转误差在 10° 以内通常可以接受，因为夹爪的柔顺机构与力控反馈可以补偿一定程度的位姿偏差。对于精密装配任务，如插拔连接器、拧螺丝，位姿误差要求达到亚毫米级和 1° 以内。物体位姿精度通常以平移误差（cm/mm）和旋转误差（°）分别衡量，也可以使用 ADD（平均距离度量）或 ADD-S（对称物体平均距离度量）等综合指标评估。
抓取位姿作为最终的可执行指令，其精度要求最为严格。抓取位姿的误差直接决定了抓取能否成功，而不像前端感知误差那样可以通过后续环节补偿。在基座坐标转换中，标定误差会与位姿估计误差叠加，因此抓取位姿的实际精度要求通常比物体位姿高出 1-2 个数量级。抓取位姿的精度评估没有统一的度量标准，最终以抓取成功率这一物理指标作为评判依据。
核心结论：检测和分割都是中间手段，生成可执行的 grasp pose 才是机器人感知的最终目标。所有前端感知算法的性能优劣，最终都要以抓取成功率这一物理指标作为评判标准，而不是以各自输出层面的精度指标作为独立评价依据。
本节小结：bbox、mask、center point、object pose、grasp pose 五类输出按信息粒度逐级提升，分别对应不同的任务阶段与精度要求。从二维到三维、从粗定位到精操作，每一类输出都在流水线中承担着不可替代的角色。感知的终点不是物体识别，也不是位姿估计，而是生成机械臂可直接执行的抓取位姿。所有前端算法的价值，最终都要通过抓取成功率来验证——这是贯穿整个机器人感知系统的核心评价逻辑。
2.4 不同硬件平台的感知需求差异
不同形态的机器人硬件，在自由度、安装方式、操作场景与工作空间上存在显著差异，对感知系统的需求侧重点也各不相同。不存在放之四海而皆准的感知方案，必须结合具体硬件特性与目标应用场景进行针对性设计。本节以四种典型机器人平台为例，分析其感知需求差异与选型思路。
2.4.1 SO101 桌面机械臂：固定视角下的精准定位
SO101 类小型桌面机械臂通常为 6 自由度固定基座配置，搭配单台固定深度相机，面向桌面分拣、简单抓取等轻量级工业与教育场景。
核心需求：该类平台的核心场景是单目标、固定视角下的高重复抓取，因此首要诉求是高重复定位精度与运行稳定性，而非感知的泛化能力。由于相机与机械臂基座的相对位置固定不变，手眼标定可一次性完成并在长期使用中保持有效。
技术特点：场景结构化程度高，工作空间有限，物体种类相对固定，光照条件可控。这些特点使得感知系统可以采用简化设计——不需要复杂的动态重标定，不需要多视角融合，也不需要强泛化能力。感知系统的主要挑战在于如何在有限算力条件下实现稳定可靠的实时检测与位姿估计。
选型建议：优先采用模块化流水线架构，使用 YOLOv8 等轻量级检测器完成目标检测，使用基于深度学习的位姿估计网络（如 GDR-Net）进行粗估计，再通过 ICP 算法进行精配准。这种组合在固定场景下能够兼顾速度与精度，且各模块可独立替换与优化。对于算力受限的嵌入式平台，可进一步精简流水线，如直接使用基于模板匹配的位姿估计方法，放弃深度学习的灵活性以换取更高的运行效率。
2.4.2 xLeRobot 移动操作：多坐标系与动态场景适配
xLeRobot 类移动操作机器人由移动底盘与机械臂组成，相机分布在头部（眼在手外）与腕部（眼在手上），面向动态室内环境的移动抓取任务。
核心需求：该类平台面临的核心挑战是多坐标系之间的统一与动态场景下的目标跟踪。移动底盘的运动使相机视角不断变化，机器人在导航过程中需要同时处理全局场景理解与局部精细操作，对感知系统提出了更高的集成度要求。
技术特点：需要同时维护眼在手外与眼在手上两套标定关系。头部相机负责大范围场景感知与目标搜索，腕部相机负责近距离精细观测与抓取引导。底盘运动带来的视角变化要求感知系统具备目标跟踪与重识别能力，避免因视角变化导致的目标丢失。此外，底盘在运动过程中的振动与晃动也需要通过滤波算法进行补偿，以保证感知输出的稳定性。
选型建议：采用"全局粗定位 + 局部精定位"的两级感知架构。全局层级使用头部相机进行场景级目标检测，输出目标在全局地图中的粗略位置；局部层级在机器人接近目标后切换至腕部相机，进行精细的位姿估计与抓取位姿生成。这种架构兼顾了搜索效率与抓取精度，是移动操作场景中主流的工程方案。在视觉基础模型成熟后，可引入开放词汇检测与分割模型，增强对未知物体的泛化能力。
2.4.3 Imeta-Y1 科研机械臂：精密操作的高位姿精度要求
Imeta-Y1 类高精度科研机械臂面向精密插拔、微装配、电子元件焊接等复杂操作任务，对位姿精度要求极高，通常需要亚毫米级的定位精度与亚度级的旋转精度。
核心需求：精密操作的核心诉求是极致的 6D 位姿精度与鲁棒性。这类任务中，物体通常尺寸较小、结构复杂，对位姿误差的容忍度极低——一个微小的旋转角度偏差就可能导致插拔失败或零件损坏。因此，感知系统需要能够在各种干扰条件下稳定输出高精度位姿。
技术特点：精密操作场景对感知系统的要求体现在多个维度。首先，需要高分辨率深度传感器或高精度结构光扫描仪，以获取足够精细的目标点云。其次，需要多视角融合以消除单视角的遮挡盲区，通过多相机阵列从不同角度同时观测目标，融合得到完整的三维信息。此外，小部件的精细分割也是关键挑战——物体尺寸小、边缘模糊，通用的实例分割模型难以达到所需精度，通常需要针对特定物体进行精细调优或使用高分辨率定制模型。
选型建议：采用多相机阵列结合高精度 ICP 配准的方案。多相机阵列提供多视角观测数据，通过点云融合得到完整的高密度目标点云；高精度 ICP 配准基于物体 CAD 模型进行迭代优化，实现亚毫米级的位姿精度。同时，配合力控末端执行器，在接触过程中通过力觉反馈进行接触式位姿补偿，进一步弥补视觉感知的残余误差。
2.4.4 Unitree G1 人形机器人：全身感知与多视角协同
Unitree G1 类人形机器人具备全身自由度，多相机分布在头部、手部、躯干等位置，面向非结构化家庭与服务场景，是当前对感知系统要求最为全面的机器人平台。
核心需求：人形机器人的感知需求具有多层次、多模态、高实时的特点。首先是全身感知空间的统一——头部相机、手部相机、躯干传感器各自有独立的坐标系，需要统一到机器人本体坐标系下。其次是多视角的快速切换与协同——机器人在执行任务过程中需要根据当前动作状态动态切换主视角，如行走时以头部相机为主，抓取时以手部相机为主。再次是目标与双手的相对位姿匹配——双手操作任务中，需要同时感知左右手与被操作物体之间的相对位置关系。最后是开放类别泛化能力——家庭场景中的物体种类繁多且不可预知，感知系统必须具备零样本或少样本的泛化能力。
技术特点：人形机器人的感知场景高度非结构化，物体类别多变，光照条件复杂，遮挡严重。这些特点使得传统的固定类别、固定场景的感知方案难以胜任。视觉基础模型（如 CLIP、SAM、DINOv2 等）的引入为开放类别感知提供了新的技术路径，使机器人能够在不需要特定训练的情况下识别和操作未知物体。
选型建议：采用模块化与端到端混合架构。基础通用任务（如物体检测、场景分割、目标搜索）使用开放词汇的视觉基础模型，利用其在海量数据上预训练的泛化能力；精密操作任务（如精细抓取、装配）则切换回基于几何精配准的传统方法，利用 CAD 模型或高精度点云配准获取亚毫米级精度。这种混合架构在人形机器人的感知系统中兼具泛化性与精度，是当前技术条件下的最优权衡。
本节小结：不同硬件平台的感知需求差异显著，不存在通用的"万能感知方案"。桌面机械臂追求固定场景下的稳定精度与重复定位能力，移动操作机器人侧重多坐标系统一与动态场景适配，科研机械臂要求极致的位姿精度与鲁棒性，人形机器人则更看重泛化能力与全身协同感知。感知方案的设计必须结合硬件特性与目标场景进行针对性选型，在精度、泛化性、实时性、系统复杂度之间寻求最优权衡。
3 视觉感知前端：目标检测分割与点云提取
视觉感知前端的任务，是将原始 RGB-D 观测转化为可供 6D 位姿估计使用的目标点云表示。这个过程不是简单的图像裁剪，而是一个从二维语义约束到三维几何恢复的表征转换过程：检测建立候选区域，分割约束前景像素，深度图提供尺度，内参模型完成反投影，点云滤波则将原始观测整理为稳定的几何输入。
可以把这一节理解为机器人“把一张图像观测转化为一簇空间点”的过程。起初，机器人只有一张 RGB 图和一张深度图，里面既有目标物体，也有桌面、背景和其他干扰物。检测器先把处理范围集中到目标附近，分割模型再提取目标前景，深度图提供每个前景像素到相机的距离，最后反投影和滤波把这些像素变成三维空间中的目标点云。这个过程稳定以后，后面的位姿估计才有可靠的几何输入。
从系统角度看，第 3 节输出的不是“好看的分割图”，而是后续位姿估计可以使用的 target point cloud。因此，前端质量应以点云纯净度、完整性、尺度一致性和实时性来评价，而不仅是检测 mAP 或分割 IoU。
3.1 输入模态：RGB-D 数据与多相机配置
3.1.1 RGB 相机：语义纹理的提供者
RGB-D 观测同时包含语义纹理与几何测距信息。RGB 图像适合完成类别识别、边缘提取和实例分割；深度图为每个有效像素提供距离观测；相机内参 则定义了像素坐标到相机坐标的几何映射。对机器人而言，这三者必须作为一个整体理解：RGB 决定“哪些像素属于目标”，Depth 决定“这些像素离相机多远”，内参决定“这些像素如何回到三维空间”。
图 6.2 展示的是一个典型 RGB-D 相机的硬件形态。对机器人抓取而言，RGB 与 Depth 的关系不是“谁替代谁”，而是互补：RGB 更擅长区分类别和边界，Depth 更擅长提供距离和尺度。只有二者完成时间同步、像素对齐和内参标定，后续的“mask 筛深度”和“深度反投影”才有意义。否则，即使分割结果看起来很准，也可能因为 RGB 与深度没有对齐，把错误位置的深度值拿来生成点云。
实际部署时，需要优先确认三个基础条件：第一，RGB 与 Depth 是否已经注册到同一像素坐标系；第二，深度值是否有明确单位，例如 16-bit 深度图常以毫米存储，而 Open3D 等库通常以米为几何单位；第三，相机内参是否与当前分辨率一致。很多看似“位姿估计不稳定”的问题，最后都能追溯到这三项基础配置。
3.1.2 深度相机：三维几何的感知核心
深度相机本身也有不同技术路线。双目立体视觉依赖左右图像的视差，适合纹理丰富、光照可控或户外场景；结构光通过投射编码图案获得深度，近距离精度较高，但容易受强光和反光表面影响；ToF 相机通过测量光飞行时间估计距离，覆盖范围较大，但在边缘、反光和多径反射场景中仍会产生系统性噪声。选型时不应只看分辨率，还要同时考虑工作距离、目标材质、安装方式和允许的标定复杂度。
3.1.3 多相机配置：从单一视角到系统级视觉
常见相机拓扑可分为三类。Eye-to-Hand 相机固定在工作空间外，视野稳定，适合桌面分拣和工位监控；Eye-in-Hand 相机安装在末端执行器附近，近距离精度高，适合精细对位和二次复检；多相机协同则通过顶视、头部、腕部等视角互补，缓解遮挡并扩展可观测空间。拓扑选择会直接影响后续手眼标定形式和抓取闭环策略。
3.1.4 关键参数：选型的量化依据
相机选型最终要落到可量化指标上：工作距离决定深度有效范围，分辨率和视场角决定目标在图像中的像素占比，帧率影响闭环响应，标定稳定性决定点云尺度和坐标转换是否可信。若这些底层条件不足，后续模型精度很难弥补硬件观测质量的缺口。
本节小结： RGB-D 输入为抓取感知提供了语义与几何两类互补信息：RGB 帮助系统识别目标类别、纹理和边界，Depth 给出尺度、距离和局部几何，内参和外参则把这些像素观测放回真实空间。相机拓扑、工作距离、分辨率、深度噪声和标定稳定性并不是外围配置，而是直接决定点云质量和坐标转换可靠性的基础条件。多相机配置可以减少遮挡、扩大视野，但也会引入同步、外参维护和数据融合成本。只有输入侧足够可信，后续检测、分割、点云提取和位姿估计才有稳定发挥的可能。
3.2 目标检测：从边界框到目标粗定位
3.2.1 核心作用：缩小搜索空间，为后续处理"划重点"
目标检测的作用，是在整幅图像中建立受约束的候选区域，减少后续分割、深度关联和点云处理的搜索空间。检测结果并不直接给出抓取点，而是提供语义先验和区域先验。
在实际系统中，检测通常是整条链路的第一道候选生成环节。没有检测框，后续分割模型可能要面对整幅图像；没有类别和置信度，任务层也很难判断当前目标是否就是用户想抓的对象。检测的价值不在于一步到位解决抓取，而在于把问题从“整张图里有什么”缩小为“这个局部区域里如何恢复目标几何”。
3.2.2 输出形式：边界框、类别与置信度
class | ||
bbox | ||
score |
3.2.3 YOLO 系列：实时检测的工业标准
闭集检测器的代表是 YOLO 系列。它的优势是推理快、部署成熟，适合固定类别和实时性要求高的场景。对桌面机械臂来说，如果任务只涉及少量已知物体，训练一个轻量 YOLO 或 YOLO-seg 模型往往比直接使用大型通用模型更稳定。原因很直接：闭集任务的类别边界清晰，标注成本可控，模型输出也更容易和后端抓取策略库绑定。
3.2.4 Grounded-SAM：开放词汇检测的新范式
开放词汇检测器解决的是另一类问题：任务目标不一定来自固定类别表，而可能来自语言指令，例如“拿起那个蓝色瓶子”。Grounding DINO 与 SAM 组合后，可以先根据文本找到候选目标，再由分割模型给出前景 mask，适合服务机器人和开放环境。它的优势是泛化，代价是链路更长、延迟更高，而且文本提示的歧义会传递到后续 mask 和点云。
3.2.5 核心局限：二维信息不足以支撑抓取
检测阈值不应只根据 mAP 设定。阈值过低会引入误检，导致背景深度进入点云；阈值过高会漏掉目标，使整条感知链无法启动。对于抓取系统，检测器的输出应和后续点云质量一起评估。
在工程实现中，检测结果通常还要经过任务层过滤。例如，同一帧图像中检测到多个杯子时，系统需要根据语言指令、距离、可达性或抓取优先级选择其中一个；当检测框连续多帧跳动时，应引入跟踪或时序平滑，避免后续点云在帧间抖动。检测器不是独立终点，而是整个抓取链路的候选生成器。
本节小结： 目标检测是抓取感知链路中的第一道候选生成环节，它把全图搜索问题缩小到目标相关区域，也为任务层提供类别和置信度线索。闭集 YOLO 适合已知类别、实时性要求高的场景，开放词汇检测则适合服务机器人面对长尾物体和语言指令的任务。无论采用哪类检测器，检测结果本身都仍只是二维入口，不能直接给出深度、姿态或可执行接触点。要让这个入口真正服务抓取，还必须继续经过实例分割、深度关联、点云处理和坐标转换。
3.3 实例分割：从像素级掩码到目标区域提取
3.3.1 核心作用：从"矩形框"到"精确轮廓"
若直接用 bbox 裁剪深度图，矩形区域通常会同时包含目标、桌面和邻近物体。这些非目标像素在反投影后会变成背景点云，污染质心估计、法向量估计和点云配准。因此，实例分割在本链路中的功能，是为三维恢复提供目标级前景约束。
这里可以把 bbox 和 mask 的差别理解得更直观一些：bbox 在图像上给出一个“搜索范围”，mask 则进一步给出目标前景像素集合。对于人眼来说，框里混入少量桌面并不影响理解；但对点云算法来说，桌面点和物体点没有语义标签，一旦一起进入配准或主轴估计，就会被当成同一个几何对象处理。很多抓取偏差并不是位姿算法本身的问题，而是从这一步开始，目标点云已经被背景点污染了。
3.3.2 经典实例分割模型：Mask R-CNN 与 YOLACT
Mask R-CNN 代表了经典两阶段实例分割范式 [1]。它的思路可以拆成两步看：第一步先让 Region Proposal Network（RPN）在特征图上找出一批可能包含目标的候选区域；第二步对每个候选区域做更精细的判断，包括类别分类、边界框修正和像素级 mask 预测。图 6.3 中的 mask branch 就是 Mask R-CNN 相比 Faster R-CNN 增加的关键分支，它让模型不只知道“这里有一个目标”，还能知道“目标在这个框里的哪些像素上”。
这种两阶段结构的优势是稳。候选区域先把问题缩小，再在局部区域内做细分割；RoIAlign 等对齐机制减少了特征量化误差，因此边界质量通常较好，适合小零件分拣、工件装配前定位、桌面物体点云提取等对轮廓敏感的场景。它的代价是链路较长，推理速度和部署复杂度不如单阶段方法。对于教学和离线实验，Mask R-CNN 很适合用来解释“检测框为什么不够、mask 为什么有价值”；对于实时抓取，则需要结合算力和帧率要求权衡。
YOLACT 则走了另一条路线：不再先生成候选区域再逐个精修，而是在一次前向传播中同时预测两类东西：一组全图共享的 prototype masks，以及每个检测实例对应的 mask coefficients [2]。最终每个实例的 mask 由这些 prototype 按系数组合得到。图 6.4 右侧的 “Prototypes mask 与 mask coefficients 线性组合” 展示的就是这个过程。它把“每个实例单独做精细分割”的问题，改写成“全图原型 + 实例系数”的组合问题。
这类方法的关键取舍是用“可组合的原型”换取速度。它不需要对每个 RoI 单独运行复杂的 mask 分支，因此更适合视频流和边缘设备。但也因为 mask 是由若干原型组合出来的，在边界特别复杂、小物体密集或严重遮挡时，分割质量可能不如两阶段方法稳定。对机器人抓取来说，如果场景是传送带、桌面分拣或固定类别物体，YOLACT 类方法可以提供较高帧率；如果任务要求精密插拔或小零件位姿估计，则需要重点验证 mask 边缘误差是否会放大为点云误差。
3.3.3 通用分割基础模型：SAM / SAM2
SAM2 的价值不只在于“能分割”，更在于它把分割从单张图像扩展到了视频序列 [3]。图 6.5 中的 memory attention 和 memory bank 可以理解为模型的短期记忆：第一帧由点、框或 mask 提示确定目标以后，后续帧不必每次从零开始找目标，而是参考过去帧中的目标表示继续跟踪。对于机器人来说，这一点很重要，因为机械臂一旦开始运动，视角、遮挡和光照都会变化。如果每一帧的 mask 都独立预测，目标前景可能抖动；如果能利用时序记忆，点云和位姿估计会更稳定。
在抓取系统中，SAM2 更适合作为“交互式目标锁定 + 时序前景保持”的工具，而不是替代几何模块。第一次提示可以来自人工点击、语言检测框或任务规划器；后续帧则依赖记忆机制维持同一目标身份。这样做的直接收益是降低 mask 在连续帧中的跳变，从而减少点云质心和位姿估计的抖动。
图 6.6 展示的流程进一步把语言提示、视觉提示和时序跟踪结合起来。用户可以说“抓那个红色杯子”，开放词汇检测器先根据文本找到候选目标，分割模型再生成 mask，跟踪模块负责在后续帧维持目标一致性。对于服务机器人和移动操作平台，这种能力比固定类别检测更接近真实需求：家庭、办公室、仓储环境中的物体种类很难提前穷尽，机器人必须能根据语言描述临时确定目标。
不过，SAM 系列和 Grounded-SAM 类方法不能被理解为完整抓取前端。它们输出的仍然是二维 mask，并不直接解决深度空洞、透明物体、手眼标定或夹爪碰撞问题。更准确的定位是：它们把开放类别目标从图像中分离出来，为后续 RGB-D 几何恢复提供更强的前景约束。要让它真正服务抓取，还需要继续接上深度筛选、反投影、点云滤波和坐标转换。
3.3.4 核心局限：仍为 2D 感知，需与深度图配合
实例分割误差通常以三种方式向下游传播：边界漏分会低估物体尺寸；背景混入会偏移点云质心和主方向；遮挡粘连会把多个物体错误合并，使位姿估计建立在错误几何对象上。
本节小结： 实例分割把粗糙的 bbox 进一步收缩为目标前景像素，是二维视觉进入三维点云之前的关键筛选步骤。对机器人而言，mask 的价值不只是让图像结果更精细，而是决定哪些深度像素会被带入三维空间；一旦背景、桌面或邻物混入，后续质心估计、主轴估计和点云配准都会受到影响。Mask R-CNN、YOLACT、SAM/SAM2 等方法各有适用边界，选型时要同时考虑实时性、开放类别能力、边缘精度和与深度图的对齐质量。因此，实例分割虽然仍停留在 2D 层面，却直接决定了下一步目标点云是否“干净”、位姿估计是否有可靠输入。
3.4 点云生成与滤波：从深度图到三维目标点云
目标点云生成是从图像域进入欧氏空间的关键步骤。给定实例 mask、深度图和相机内参，系统先筛选目标前景深度，再将有效像素反投影到相机坐标系，最后通过滤波和降采样形成位姿估计所需的几何输入。
到这里，机器人完成了从“看见一块前景区域”到“量出一簇空间点”的转换。这一步看似只是公式计算，但它决定了后续所有几何算法的输入基础是否可靠。深度尺度错了，点云会整体变大或变小；mask 边缘错了，点云会混入桌面；滤波参数过强，目标局部结构会被抹掉。位姿估计如果建立在这样的点云上，再复杂的模型也很难稳定输出正确结果。
3.4.1 像素反投影：从深度图到三维点云的核心公式
设相机内参矩阵为：
对于深度像素 ，若深度图原始值需要通过尺度因子 转换为米制深度，则先令：
其在相机坐标系下的三维点 为：
该公式恢复的是相机坐标系下的观测点，不是机器人基座坐标系下的控制目标。因此，第 3 节输出仍属于视觉几何表征，后续必须经过手眼外参和抓取映射才能用于执行。
反投影公式还隐含两个容易被忽略的前提。第一，深度值 必须与 RGB 或 mask 所在像素对齐；若 RGB 与 Depth 分辨率不同或外参未对齐，mask 中的前景像素可能会取到背景深度。第二，深度单位必须明确：许多 RGB-D 相机输出 16-bit 深度图，数值单位可能是毫米，也可能需要乘以 depth_scale 才能转为米。单位错误不会让程序报错，但会让点云尺度整体错误，后续位姿估计和手眼转换都会被污染。
3.4.2 点云预处理：从"原始点云"到"干净点云"
mask > 0 | |||
K, depth_scale | |||
z_min, z_max | |||
nb_neighbors, std_ratio | |||
voxel_size | |||
.pcd / .ply |
图 6.7 中使用的是经典 Stanford Bunny 点云 / 网格模型，来源于 Stanford 3D Scanning Repository [5]。它是计算机图形学、三维重建、点云处理和 Open3D / PCL 教学中非常常见的基准模型，并不是 OpenCV 专属案例；本讲使用它只是为了直观展示点云滤波、降采样和离群点去除的效果。
点云预处理的目标不是“尽可能保留所有点”，而是在保留主要几何结构的前提下去除噪声并降低计算量。过弱的滤波会使 ICP 或位姿回归受到离群点干扰；过强的滤波则会破坏边缘、孔洞、把手等对姿态判断有价值的局部结构。Open3D 中常用的体素降采样和统计离群点滤波，正好对应这两个目标：前者控制点数，后者根据邻域距离剔除孤立点 [4]。
三类滤波可以按功能区分。直通滤波用于删除工作空间之外的点，例如桌面以下、相机量程之外或机器人不可达区域；统计离群点滤波根据每个点到邻近点的平均距离剔除孤立飞点，适合处理深度边缘和反光噪声；体素降采样把空间划分成固定大小的立方体网格，每个体素保留一个代表点，用来降低点数并提高配准速度。常见经验是先保证目标点云语义纯净，再做适度去噪，最后按下游算法实时性选择体素大小。
z_min, z_max | |||
nb_neighbors, std_ratio | |||
voxel_size |
参数调节时要先判断下游需求。如果下一步是 ICP 精配准，应保留足够的边缘、平面和局部结构；如果下一步只是计算 3D 中心或粗略包围盒，可以使用更强的降采样来换取速度。对真实机器人来说，滤波参数不是越强越好，而是要服务位姿估计和抓取规划所需的几何信息量。
3.4.3 目标点云提取与误差修正路径
f_x, f_y, c_x, c_y 与 depth_scale | ||
z_min / z_max | ||
voxel_size，但避免过度降采样 | ||
这些现象分别对应不同层级的假设失配。尺度异常通常源于相机参数或深度单位错误；飞点和空洞对应测距噪声与边界不连续；粘连则说明二维前景约束失败。调试时应先定位表征层级，再调整对应模块。
3.4.4 全链路回顾与实验索引
本节的实验代码位于 Visual_Perception 目录。其中，pipeline_from_syllabus.py 是与 3.4 节最直接对应的课堂脚本：它不依赖交互式分割界面，而是假设已经有 depth.png、mask.png 和 intrinsics.json，然后严格按照“mask 筛深度 → 反投影 → 直通滤波 → 统计滤波与体素降采样 → 导出点云”的顺序执行。示例输入可从 University of Washington RGB-D Object Dataset 下载一帧 full 640×480 RGB-D 数据，并按代码目录 README 中的说明整理为 data/rgbd_object_demo/。
运行方式如下：
python pipeline_from_syllabus.py --demo-dir data/rgbd_object_demo --no-vis
若希望同时弹出最终点云窗口，可去掉 --no-vis。脚本运行后会在示例目录的 output/ 下保存 masked depth、各阶段点云、阶段截图和 JSON 摘要，便于将终端统计、点云文件和正文图示逐一对应起来。
step1_mask_filtering() | syllabus_masked_depth.png | |||
step2_backprojection() | syllabus_target_raw.pcd | |||
step3_passthrough() | syllabus_target_passthrough.pcd | |||
step4_denoise_and_downsample() | syllabus_target_statistical.pcd | |||
step4_denoise_and_downsample() | ||||
step5_export() | syllabus_target_clean.pcd |
这段代码的价值在于它把 3.4 节的每个概念都变成了可检查的中间结果。syllabus_masked_depth.png 对应二维前景约束，说明 mask 是否真正限制了深度区域；syllabus_target_raw.pcd 对应反投影后的原始目标点云，用来检查深度尺度和相机内参；syllabus_target_passthrough.pcd 对应工作空间裁剪，用来判断 Z 向阈值是否误删目标；syllabus_target_statistical.pcd 和 syllabus_target_clean.pcd 则分别对应去噪与降采样后的几何表示。脚本还会保存 syllabus_pipeline_summary.json，记录输入路径、参数、像素统计、各阶段点数和 XYZ 范围。
图 6.8 对应第 1 步。这里最值得观察的是“掩膜前景像素数”和“掩膜后有效深度像素数”并不一定相等：前者来自二维 mask，后者还要求对应像素有有效深度。目标边缘、反光区域或透明区域常常会出现 mask 有前景、Depth 无有效值的情况，这也是点云空洞的直接来源。
图 6.9 对应第 2 步。反投影点数通常等于 masked depth 中的有效深度像素数。若点数异常少，应优先检查 mask 是否过小、深度图是否存在大面积空洞；若点云尺度明显异常，则应检查 intrinsics.json 中的 fx, fy, cx, cy 与 depth_scale。
图 6.10 对应第 3 步。直通滤波的作用不是精修点云，而是快速删除明显不属于工作空间的点。若过滤后点数变化很小，说明当前目标本来就在有效 Z 范围内；若点数突然大幅下降，则需要确认 z_min / z_max 是否过窄，避免把目标本体截断。
图 6.11 对应第 4 步。统计滤波先根据邻域距离剔除孤立点，体素降采样再按空间网格合并近邻点。前者主要改善点云纯净度，后者主要降低计算量。若统计滤波剔除比例过高，可能是 nb_neighbors 过大或 std_ratio 过小；若体素降采样后点数过低，则可能丢失物体边缘、孔洞、把手等对位姿估计有用的局部结构。
图 6.12 对应第 5 步。最终输出的 syllabus_target_clean.pcd 是第 4 节位姿估计可以直接读取的目标点云。这里的 XYZ 范围不仅是日志信息，也是一项必要的合理性检查：如果物体尺寸明显不符合常识，问题通常不在位姿估计，而在前面的深度尺度、内参、mask 或滤波参数。
课堂讨论可以围绕三个问题展开：为什么反投影必须依赖相机内参；为什么深度空洞常出现在物体边缘、反光面和透明材质上；为什么体素降采样需要在“计算效率”和“几何细节保留”之间折中。
本节小结： 点云生成完成了从二维像素到三维几何的关键跃迁：mask 负责选出目标前景，深度与内参负责反投影，滤波与降采样负责把原始观测整理成可计算的目标点云。这里最重要的不是某个函数调用，而是每一步都留下可检查的中间结果：masked depth、raw point cloud、passthrough point cloud 和 clean point cloud。直通滤波、统计滤波和体素降采样分别解决空间裁剪、离群点抑制和计算规模控制问题，但参数过强也可能削掉真实几何细节。这样一来，位姿估计前端的错误就不再是模糊的“模型不准”，而可以被定位到 mask、深度尺度、内参、外参或滤波参数等具体环节。
3.5 常用开源工具与模型选型
3.5.1 检测分割工具
3.5.2 点云处理工具
3.5.3 选型建议：三维度决策框架
选型时不应使用“固定类别就用 YOLO、开放词汇就用 Grounded-SAM、精度优先就用 SAM”这类无条件口诀。更可靠的决策表应同时检查：类别是否封闭，是否需要实例 mask，是否有深度图 / CAD / 相机内参，时延预算和算力是否允许，模型许可与维护成本是否可接受，以及目标数据上的检测、分割和任务成功率是否达标。对于教学实验，YOLO / Grounding DINO → SAM → Open3D 是一条容易观察中间结果的链路；对于工业部署，则应根据目标类别、节拍、算力、失败代价和维护能力做更保守的组合。
本节小结： 第 3 节的核心输出不是 bbox，也不是 mask，而是经过前景约束、深度反投影与几何整形后形成的目标点云表征。检测与分割负责回答“处理哪个目标”，深度和内参负责把目标送入三维空间，滤波和工具链则负责让这份点云足够干净、足够轻量、足够可调试。开源工具的选择应围绕链路需求展开：检测分割看类别开放性和实时性，点云处理看几何算子和可视化能力，整体系统还要考虑部署复杂度。接下来，第 4 节将在这份点云或 RGB-D 观测基础上讨论 6D 位姿估计，第 5 节则进一步把位姿结果转化为夹爪可以执行的抓取目标。
4 6D 物体位姿估计：原理与主流技术方案
4.1 什么是 6D 位姿：平移与旋转的数学表示
6D 位姿（6 Degree-of-Freedom Pose）是描述刚体在三维空间中完整状态的标准表示，包含 3 个平移自由度与 3 个旋转自由度，是连接视觉感知与机器人操作的核心中间量。清晰理解其数学表示，是掌握位姿估计算法的基础。
在机器人抓取场景中，当我们说"估计物体的 6D 位姿"时，本质上是要求解一个刚体变换，它将物体从自身的模型坐标系映射到当前观测到的相机坐标系中。这个变换由平移和旋转两部分组成，分别对应物体在空间中的"位置"和"朝向"。
展示相机坐标系下物体的 3 个平移自由度与 3 个旋转自由度，符合机器人视觉的标准坐标系约定
图 4-1 6D 位姿自由度示意图
4.1.1 平移自由度：物体三维空间位置的量化描述
平移描述物体坐标系原点在参考坐标系（通常为相机坐标系或机器人基座坐标系）中的三维空间位置，用三维列向量表示：
单位通常为米（m）或毫米（mm）。在机器人视觉的标准相机坐标系（OpenCV 约定）下：
x 对应物体在成像平面内的水平位置，向右为正方向；
y 对应物体在成像平面内的垂直位置，向下为正方向；
z 对应物体沿相机光轴的深度距离，向前为正方向。
平移分量决定了物体"在空间哪里"，是机械臂能够到达目标区域的基础前提。在工程中，平移量通常通过像素反投影结合深度图初步获得——给定物体某个像素点在图像上的坐标 (u, v) 及其对应的深度值 d，通过相机内参矩阵 K 即可反投影出该点在相机坐标系下的三维坐标,这一过程是连接二维图像与三维空间的关键桥梁，也是后续所有几何计算的起点。平移误差会直接导致抓取位置出现左右或前后偏差，在工程中通常要求平移精度达到毫米级别。
4.1.2 旋转自由度：三种主流旋转表示方法对比
旋转描述物体坐标系相对于参考坐标系的空间朝向，共有 3 个独立自由度。工程中常用三种表示方式，各有优劣与适用场景。
1. 欧拉角
表示方法：通过三次绕坐标轴的旋转角度描述刚体朝向，工业界最常用 ZYX 外旋顺规，即依次绕参考系 Z 轴旋转 yaw、绕 Y 轴旋转 pitch、绕 X 轴旋转 roll，对应偏航、俯仰、翻滚三个角度。
优势：直观易懂，三个角度具有明确的物理意义，便于人类理解与现场调试。
劣势：存在万向锁（Gimbal Lock）问题 —— 当 pitch 角为 ±90° 时，第一次旋转的 Z 轴与第三次旋转的 X 轴重合，系统损失一个旋转自由度，导致数值奇异；此外欧拉角插值不连续，求导复杂，不适合作为算法的直接优化目标。
2. 四元数
表示方法：用四维单位向量描述旋转，形式为： 其中w为实部，为虚部，编码了旋转轴与旋转角度信息。
优势：无万向锁奇异问题；计算高效（旋转复合仅需四元数乘法）；插值平滑（SLERP 球面线性插值）；存储空间小，是算法优化与机器人控制系统的首选表示。
劣势：直观性差，不便于人工直接解读与调试。
工程地位：是机器人控制系统与位姿估计算法中最常用的旋转内部表示形式。
3. 旋转矩阵
表示方法：用 3×3 的正交矩阵描述旋转，矩阵的三个列向量分别对应物体坐标系 X、Y、Z 轴在参考坐标系下的单位方向向量： 满足正交性约束：，且行列式（保证右手系旋转变换）。
优势：数学性质严谨，可直接用于三维点的坐标变换，是几何推导与坐标计算的标准形式。
劣势：参数冗余（9 个参数描述 3 个自由度），存在强约束，不适合直接作为神经网络的回归目标，训练中难以保证正交性。
4.1.3 齐次变换矩阵：位姿的统一工程表达形式
工程实践中，通常将旋转矩阵与平移向量整合为 4×4 的齐次变换矩阵，作为位姿的标准输出与系统间交换格式：
其中左上角 3×3 为旋转矩阵，右上角 3×1 为平移向量，最后一行用于齐次坐标补全。
该矩阵可直接完成不同坐标系之间的点坐标变换：对于任意一点在物体坐标系下的齐次坐标，其在相机坐标系下的坐标为：
齐次变换矩阵的核心工程价值在于：
1. 级联变换便捷：多级坐标系的串联变换可通过矩阵乘法直接实现，如，是手眼标定、坐标转换链路的标准数据结构；
2. 逆变换易求：变换矩阵的逆可通过旋转矩阵转置快速计算，无需复杂的矩阵求逆运算：
本节小结：6D 位姿由 3 个平移自由度与 3 个旋转自由度组成，分别描述物体的空间位置与朝向。旋转有欧拉角、四元数、旋转矩阵三种主流表示方式，其中四元数适合算法计算与控制，旋转矩阵适合几何推导与坐标变换，欧拉角适合人工调试。齐次变换矩阵是工程中位姿的统一表达形式，支持级联坐标变换，是全链路数据交互的标准格式。
4.2 位姿估计的三类技术路线
6D 位姿估计经过数十年发展，形成了三类技术路线清晰、适用场景各异的方法体系：传统几何法、模板匹配法与深度学习法。三者并非简单的迭代替代关系，而是在不同场景、不同硬件条件下各有最优解，工程落地中常采用混合架构取长补短。
图 4-2 6D 位姿估计技术路线谱系
4.2.1 传统几何法：基于点云配准的 ICP 系列
传统几何法是位姿估计的经典技术路径，核心依赖刚体变换的几何约束，无需数据训练，可解释性强。
核心思想
将观测得到的目标点云与物体的 CAD 模型点云进行迭代配准，寻找使两组点云最优对齐的刚体变换矩阵（旋转 + 平移）。代表算法为ICP（Iterative Closest Point，迭代最近点）及其众多变体，包括点到点 ICP、点到平面 ICP、截断 ICP 等。
算法流程（点到点 ICP）
1. 输入初始化：给定初始位姿估计、观测目标点云、模型参考点云；
2. 对应点匹配：为每个观测点在模型点云中寻找欧氏距离最近的对应点，形成对应点对；
3. 最优变换求解：构造最小二乘目标函数，求解使所有对应点距离平方和最小的刚体变换： 其中为第i个观测点对应的模型点索引。该问题可通过 SVD（奇异值分解）求得闭式解：
4. 迭代收敛：用求解得到的变换更新观测点云，重复步骤 2-3，直到误差变化小于阈值或达到最大迭代次数。
算法变体与优化
点到平面 ICP：目标函数改为最小化观测点到模型点切平面的距离，收敛速度显著快于点到点版本，是工程中最常用的变体；
截断 ICP（Truncated ICP）：对距离过大的对应点进行截断，提升对离群点与遮挡的鲁棒性。
优势与局限
优势：精度极高，收敛后可达亚毫米级；无需训练，纯几何驱动，可解释性强；有成熟的开源实现（如 PCL、Open3D）。
劣势：对初始位姿要求高，初始偏差过大易陷入局部最优；对严重遮挡、大量离群点敏感；无纹理、高度对称物体易匹配错误。
适用场景：已知物体精确三维模型、初始位姿偏差较小的工业场景，常用作深度学习粗估计后的位姿精化环节。
图 4-3 ICP 点云配准迭代过程示意图
4.2.2 模板匹配法：基于三维模型的姿态对齐
模板匹配法是工业场景中应用较早的实时位姿估计方案，核心是预先生成姿态模板库，在线通过特征匹配快速检索位姿。
核心思想
离线阶段预先渲染物体在不同视角、不同姿态下的二维 / 三维特征模板，构建完整的模板库；在线阶段将输入图像 / 点云与模板库进行特征匹配，找到相似度最高的模板，其对应的姿态即为估计结果。代表方案为经典LINEMOD算法及其深度学习增强变体。
算法流程
1. 离线建库：对物体 CAD 模型进行全视角采样（通常按 5°-10° 步长采样方位角与俯仰角），渲染生成大量视角图像；提取每幅图像的梯度方向、深度法线等鲁棒特征，与对应姿态绑定存入模板库；
2. 在线匹配：对输入场景图像提取相同特征，通过滑动窗口与模板库进行快速卷积匹配，输出相似度最高的模板对应的 6D 位姿；
3. 后处理：对匹配结果进行非极大值抑制，去除重复检测，输出最终位姿。
优势与局限
优势：推理速度极快，适合高实时性场景；对低纹理、无纹理金属 / 塑料物体友好；对硬件算力要求低，可在嵌入式平台部署。
劣势：姿态精度有限，通常仅作为粗估计；模板库存储空间随视角粒度指数增长；新物体需要重新建库，零样本泛化能力差。
适用场景：结构化工业分拣场景、固定物体模型、对实时性要求高但精度要求适中的任务。
4.2.3 深度学习法：从两阶段方案到端到端范式
深度学习方法利用神经网络从大规模数据中学习位姿特征，凭借强泛化性与端到端能力成为当前研究主流，根据架构设计可分为两阶段与端到端两大类。
1. 两阶段方案
核心逻辑：第一阶段完成目标检测与实例分割，裁剪出目标区域；第二阶段单独输入位姿回归网络，输出 6D 位姿。
典型代表：SAM-6D，以 SAM 大模型的零样本分割结果为基础，后续接轻量级位姿回归头。
技术特点：任务解耦清晰，便于分模块调试与优化；分割精度直接决定位姿估计的上限；可复用成熟的检测分割模型，降低开发成本。
2. 端到端方案
端到端方案可进一步分为直接回归型与检测驱动型两类：
直接回归型：网络输入原始 RGB-D 图像，直接输出完整的旋转与平移参数。代表为 FoundationPose，通过隐式几何重构与可微优化实现强泛化性。
优势：零样本泛化能力强，对遮挡、截断场景鲁棒，可适配未知类别物体；
劣势：模型参数量大，推理速度慢，训练数据与算力需求高。
检测驱动型：基于 YOLO 等目标检测架构扩展，单次前向传播同步预测类别、检测框与物体 3D 包围盒角点的 2D 投影，再通过PnP（Perspective-n-Point）几何算法求解 6D 位姿。代表为 YOLO-6D。
核心原理：PnP 算法已知物体 3D 关键点的模型坐标与对应的 2D 像素投影，结合相机内参即可闭式求解相机到物体的位姿，是连接 2D 检测与 3D 位姿的经典几何桥梁。
优势：推理速度极快，模型轻量化，支持实时部署；工程落地成熟，便于与现有检测系统集成；
劣势：泛化能力有限，依赖已知物体 3D 模型，严重遮挡与对称场景下精度下降明显。
三类技术路线对比：
本节小结 6D 位姿估计形成了三类成熟的技术路线：传统几何法精度最高但依赖初始值，模板匹配法速度最快但泛化性差，深度学习法泛化性强但算力需求高。工程实践中常采用 “深度学习粗估计 + ICP 精配准” 的混合方案，兼顾速度、泛化性与精度，是当前落地最广泛的组合模式。
4.3 典型开源方案对比：FoundationPose / SAM-6D / YOLO-6D
FoundationPose、SAM-6D 与 YOLO-6D 是当前工程与科研领域最具代表性的三类开源位姿估计方案，分别对应端到端通用型、两阶段融合型、检测驱动实时型三条技术路径，适配不同的场景需求。
图 4-4 三类开源方案架构对比
图 4-4 三类开源方案架构对比
4.3.1 三类方案的核心维度横向对比
三类方案的定位差异显著：FoundationPose 偏向科研前沿与通用场景，SAM-6D 偏向服务机器人开放场景，YOLO-6D 偏向工业结构化实时场景。
4.3.2 不同场景下的选型决策建议
位姿估计方案不存在绝对的优劣，选型的核心是匹配场景需求，在精度、速度、泛化性、部署成本之间寻求最优权衡。
1. 工业结构化场景：固定物体、追求实时性 优先选择 YOLO-6D + ICP 精化 的方案：前端用 YOLO-6D 快速输出粗位姿，后端用 ICP 基于 CAD 模型做几何精配准，兼顾实时性与高精度，是工业分拣、装配场景的最优解。该方案部署成熟、调试简单，且能充分利用工业场景物体固定、光照可控的特点。
2. 服务机器人场景：物体类别多变、追求泛化性 优先选择 SAM-6D 方案：依托 SAM 的零样本分割能力，适配家庭、办公等开放场景下的多种常见物体，无需针对每个物体单独训练，平衡泛化性与推理速度。配合点云预处理与简单几何约束，可满足多数服务抓取任务的精度要求。
3. 科研前沿场景：未知物体、追求通用能力 优先选择 FoundationPose 方案：其强零样本泛化能力适合探索未知物体操作、通用抓取等前沿方向，是当前通用位姿估计的标杆方案。该方案无需物体 CAD 模型，在遮挡、截断场景下仍能输出稳定位姿，适合基础研究与非结构化场景探索。
选型核心原则：优先匹配场景需求，不盲目追求 SOTA。在满足任务要求的前提下，优先选择部署简单、维护成本低、调试方便的方案；混合架构往往能通过模块组合取得比单一方案更优的工程性价比。
本节小结：三类主流开源方案各有定位：YOLO-6D 胜在速度与轻量化，适合工业实时场景；SAM-6D 胜在开放泛化，适合服务机器人场景；FoundationPose 胜在通用能力，适合科研前沿探索。选型需结合场景的物体种类、精度要求、硬件条件综合判断，“粗估计 + 精配准” 的混合架构是工程落地的主流思路。
4.4 位姿估计的精度评价指标
位姿估计的性能需要量化指标进行客观评估，不同指标从不同维度衡量精度。掌握评价指标是算法选型、效果评测与故障排查的基础。
4.4.1 ADD 与 ADD-S：位姿精度的核心综合指标
ADD 与 ADD-S 是位姿估计领域最通用的综合评价指标，从点云整体对齐程度衡量位姿偏差，也是 LineMOD、YCB-Video 等主流数据集的标准评测指标。
图 4-5 ADD 与 ADD-S 评价指标原理对比
1. ADD（Average Distance）指标
计算方式：将物体模型上的所有采样点，分别通过估计位姿与真值位姿进行变换，计算两组变换后点集中对应点的欧氏距离，取所有点距离的平均值： 其中M为物体模型的采样点数，为物体坐标系下的第i个模型点，为估计位姿，为真值位姿。
物理含义：衡量估计位姿与真值位姿之间的整体空间偏差，距离越小表示位姿精度越高。
合格阈值：通常以物体直径的 10%作为合格判定阈值，若 ADD 小于该阈值则判定为本次位姿估计正确。
2. ADD-S 指标
提出背景：ADD 指标对于对称物体存在评估歧义 —— 对称物体旋转后点的一一对应关系不唯一，即使位姿正确，直接按索引计算也会得到较大距离，导致误判为错误。
计算方式：对于每个估计位姿变换后的点，不再匹配固定索引的对应点，而是在真值变换后的模型点云中寻找最近的点，再计算所有最小距离的平均值：
适用场景：专门用于旋转对称物体的位姿精度评估，是位姿估计数据集的标准补充指标。对于非对称物体，ADD-S 与 ADD 的计算结果基本一致。
3. AUC（Area Under Curve）
计算方式：以误差阈值为横轴、位姿准确率为纵轴，绘制不同误差阈值下的准确率曲线，计算曲线下的面积。
优势：比单一阈值更全面地反映算法在不同精度要求下的整体性能，避免单一阈值的偶然性，是学术论文中最常用的综合对比指标。通常取 0 到物体直径 10% 的误差范围计算 AUC，取值范围为 0 到 1，数值越高位姿性能越好。
4.4.2 分项误差指标：平移误差与旋转误差
综合指标便于整体性能对比，但在工程调试与故障排查时，需要分项指标定位误差来源，区分是位置偏差还是朝向偏差。
1. 平移误差
计算方式：估计位姿的平移向量与真值平移向量之间的欧氏距离，单位通常为毫米（mm）：
工程意义：直接反映物体位置估计的准确程度，对应抓取操作的左右、上下、前后偏差。
典型合格标准：桌面级抓取场景通常要求平移误差小于 5mm；精密装配场景要求亚毫米级。
误差溯源：平移误差过大通常源于深度图质量差、相机内参不准或手眼标定存在系统偏差。
2. 旋转误差
计算方式：估计旋转与真值旋转之间的测地线角度差，单位为度（°）。可通过旋转矩阵的迹快速计算： 其中为矩阵的迹，即主对角线元素之和。
工程意义：反映物体朝向估计的准确程度，旋转偏差会在长臂展处被放大，是抓取姿态错误、夹爪碰撞物体的主要诱因。
典型合格标准：桌面抓取场景通常要求旋转误差小于 5°；精密装配场景要求小于 1°。
误差溯源：旋转误差过大通常源于点云特征不足、物体对称歧义或严重遮挡导致的朝向模糊。
工程调试经验：分项误差可以帮助快速定位问题根源 —— 若平移误差大而旋转误差正常，优先排查深度质量与标定；若旋转误差大而平移正常，优先排查点云完整性、物体对称性与位姿优化算法。
本节小结：位姿估计的核心评价指标包括综合指标与分项指标两类。ADD/ADD-S 与 AUC 是综合评估位姿精度的标准指标，用于算法整体性能对比与学术排名；平移误差与旋转误差分项指标则用于误差溯源与工程调试。实际评测中需结合使用，既看整体准确率，也分析分项误差的分布规律，才能全面评估算法的工程可用性。
往期回顾
-END-
点击下方卡片，关注【Xbotics具身智能实验室】公众号
你想要的这里都有~~
Ask Me Anything｜提问箱
对文章有疑惑，或想聊更深？欢迎把你的问题丢给我们：技术方案、实操踩坑、课程与资料、项目合作、职业发展，都可以问。
怎么问：在评论区留言，或私信公众号
我们会做什么：每周集中整理高质量问题并公开回复，重点问题邀请作者或嘉宾深度解答；典型问题会加入知识库并持续更新。
提问小提示：尽量说明「你的目标—当前做法—期望产出」，附上必要信息（硬件/软件版本、数据规模等），能更快获得有用答案。
一起把问题变成知识，推动社区进步 🚀
