📂 项目地址
GitHub：Xbotics-Embodied-AI-club/Xbotics-Embodied-AI-Handbook/tree/main
5 抓取位姿生成：从物体位姿到机器人可执行目标
第 4 节得到的 object pose 描述物体在空间中的刚体状态；机器人控制系统真正需要的，是末端执行器在机器人基座坐标系下的可执行目标。因此，抓取位姿生成不是在物体中心“放一个点”，而是将物体状态、夹爪几何、任务约束和坐标变换统一到同一个操作表征中。
如果说第 4 节解决的是“物体摆在哪里”，第 5 节解决的就是“末端执行器应如何接近并约束物体”。这两个问题看起来相近，实际差别很大。一个盒子的物体坐标原点由 CAD 或数据集约定，并不必然是几何中心或质心；即便它恰好位于中心，夹爪也不一定要去中心点。一个杯子的坐标系可以朝向杯口，但夹爪可能更适合从杯身侧面接近；一个工具的姿态估计很准确，也不代表机械臂一定能以那个方向绕过桌面和邻近物体。抓取候选生成就是把这些操作层面的因素逐一补上。
可以将该层理解为如下映射：
其中， 是相机系下的物体位姿， 是目标点云或几何模型， 表示夹爪结构参数， 表示环境、任务和可达性约束；输出 是抓取候选集合。严格说，grasp pose 指夹爪 TCP 在抓取时刻的 目标 ；夹爪开合宽度、接近方向、预抓取位姿、接触点和质量分数是抓取候选的附加参数。这个映射看起来像一个简单的坐标变换，实际上包含了任务层、几何层和运动学层的共同约束。
5.1 为什么 object pose 不等于 grasp pose
object pose 和 grasp pose 描述的是两个不同对象。前者描述物体坐标系相对于参考坐标系的刚体变换；后者描述夹爪工具中心点（TCP）相对于机器人基座坐标系的目标变换。二者相关，但不等价。
工程上可以把 object pose 看作“感知模块的状态估计”，把 grasp pose 看作“规划模块的动作候选”。前者尽量客观地描述物体，后者必须带有任务意图：是搬起、递送、倒水、插入，还是只把物体推到一侧。任务不同，即使同一个物体位姿也会对应不同抓取方式。
5.1.1 抓取点选择：物体中心不等于最佳接触点
一个直观例子是螺丝刀：object pose 可以告诉我们螺丝刀的长轴方向和空间位置，但抓取时通常不会去夹刀尖，也不会盲目夹几何中心，而要选择靠近手柄、受力稳定且不影响后续使用的位置。再比如杯子，杯子的物体坐标系可能定义在杯身中心，但抓取策略可能选择从侧面夹杯身，也可能选择抓杯柄附近。也就是说，object pose 是对“物”的描述，grasp pose 是对“手”的描述，中间必须经过抓取策略映射。
{pose, width, approach, pregrasp, score, contacts...} |
5.1.2 接近方向：物体朝向不等于夹爪进入方向
即使物体姿态估计正确，夹爪也不一定沿物体坐标轴直接接近。接近方向要同时考虑桌面、邻物、夹爪形状和任务意图：顶部抓取更关心桌面法线，侧向抓取更关心局部表面法线，插拔任务则可能要求沿孔轴或工具轴运动。
5.1.3 夹爪开合尺度：物体尺寸不等于夹爪开口宽度
夹爪开口宽度来自被夹持局部的几何尺度，而不是物体整体尺寸。对杯子、工具、带把手物体等非均匀目标而言，物体 bbox 尺寸只能提供粗略上界，真正可用的开合尺度需要结合局部点云、抓取模板或学习模型估计。
5.1.4 安全余量：物体表面不等于夹爪目标位置
抓取位姿还必须包含安全偏移和预抓取姿态。机械臂通常先到达目标外侧的 pre-grasp，再沿接近方向低速进入 grasp 位姿，以减少标定误差、深度噪声和规划误差带来的碰撞风险。
若在物体坐标系下预定义抓取模板 ，Eye-to-Hand 情形下可写为：
Eye-in-Hand 情形下，由于相机随末端运动，还需引入当前末端位姿：
这两个公式说明：从 object pose 到 grasp pose 的转换，本质上是从物体状态表征到动作构型表征的映射。该映射同时受物体几何、夹爪结构、环境约束和坐标链路方向的共同影响。
本节小结： object pose 描述物体坐标系的刚体状态，grasp pose 描述夹爪 TCP 的目标刚体变换；完整抓取候选还应包含开合宽度、接近方向、预抓取位姿、评分和接触约束等附加参数。一个物体位姿可以对应多个抓取候选，不同候选又会受到夹爪结构、任务意图、周围障碍物和可达性约束的筛选。位姿估计提供的是操作所需的重要中间量，而不是机械臂可以直接执行的最终指令。真正的抓取生成，需要把物体状态、夹爪几何、运动规划和安全策略放在同一条链路中共同考虑。
5.2 抓取候选的核心要素：抓取点、接近方向、开合尺度、安全偏移与姿态匹配
一个可执行抓取候选通常包含抓取点、接近方向、姿态对齐、开合宽度和安全偏移等要素。它们共同决定夹爪如何接近目标、在哪个位置形成接触、以何种方向闭合，以及如何在执行前预留安全缓冲。MoveIt 等运动规划框架中的 grasp_pose、pre_grasp_approach、post_grasp_retreat、pre_grasp_posture 和 grasp_posture，本质上就是把 pose 与这些附加动作参数结构化成可提交给规划器的接口字段 [6][7]。
可以按一次真实抓取来理解这些参数：夹爪先要知道目标点在哪里，这是抓取点；接着要知道从哪个方向靠近，这是接近方向；靠近时手指该怎么转，这是姿态对齐；手指张多大，这是开合宽度；最后，不能直接高速冲到接触点，而要先停在目标外侧一个安全位置，这就是安全偏移和预抓取位姿。少掉这些附加参数，单独的 grasp_pose 只是一个空间刚体变换，抓取很容易从“可执行动作”退化成“空间里一个孤立的目标姿态”。
5.2.1 抓取点（Grasp Point）：夹爪中心到哪里
抓取点是夹爪 TCP 或两指中心在接触时刻应该到达的位置。它可以来自物体质心、点云包围盒中心、局部平面中心、CAD 模型关键点，也可以来自学习模型预测的接触区域。抓取点并不一定等于物体几何中心：拿杯子时，抓取点可能在杯身侧壁；拿盒子时，抓取点可能接近上表面中心；拿带柄工具时，抓取点常常落在把手或细长轴上。
工程上应区分三类点：目标中心点、接触中心点和 TCP 目标点。目标中心点只表示物体大致在哪里；接触中心点表示夹爪希望夹住哪个局部结构；TCP 目标点还要考虑夹爪几何偏置。若把这三者混为一谈，就会出现“视觉上对准了物体中心，但夹爪指尖实际碰到边缘”的问题。
抓取点的常见检查方式包括：看点是否落在目标点云内部或可接触表面附近；看该点到桌面、邻物和遮挡物的距离是否足够；看以该点为中心的夹爪模型是否能容纳目标局部宽度。对于点云稀疏或遮挡严重的目标，不建议只用单帧质心直接抓取，应结合包围盒、局部法线或多候选排序做保护。
5.2.2 接近方向（Approach Direction）：夹爪从哪个方向来
接近方向决定末端执行器进入目标附近的路径。它通常由桌面法线、物体侧面法线、物体主轴、局部表面法线或任务约束确定，是避碰和姿态规划中最敏感的参数之一。对桌面顶部抓取而言，接近方向常取世界系或基座系的竖直向下；对侧向抓取而言，接近方向通常沿物体侧面法线或夹爪闭合平面的法向进入；对插拔、挂取、按压等任务，接近方向还必须服从孔轴、把手方向或接触面的功能约束。
接近方向不是随便选一条看起来最短的路径。它至少要同时满足三类条件：第一，沿该方向移动时夹爪不会先碰桌面、相机支架、邻近物体或机器人本体；第二，方向要与夹爪闭合后的受力方向匹配，避免把物体推走而不是夹住；第三，pre-grasp 到 grasp 的最后一小段应尽量短、直、低速，便于把视觉误差限制在接触前的局部区域。
一个实用检查是把接近方向画成箭头，并从 pre-grasp 点到 grasp 点连成线段。如果这条线穿过桌面、穿过物体主体之外的遮挡物，或者与夹爪手指开合方向混淆，就说明候选还不能执行。许多抓取失败并不是抓取点错了，而是接近方向把夹爪带到了一个必然碰撞的位置。
5.2.3 开合尺度（Gripper Opening Width）：夹爪张多大
开合尺度决定夹爪闭合前的初始宽度。它需要覆盖被夹持局部的几何尺寸，并加入安全余量；过小会导致手指在到达抓取点前提前碰撞目标，过大则会增加碰到邻物、桌沿或容器壁的风险。对两指夹爪而言，开合尺度通常与夹爪闭合方向上的局部宽度相关，而不是简单等于整个物体的最大宽度。
可以用下面的关系理解：
其中 是抓取局部在夹爪开合方向上的尺寸， 是给定位误差、点云噪声和夹爪重复定位误差预留的余量。这个余量不能无限增大，因为夹爪张得越开，进入狭窄空间时越容易碰撞邻物；也不能太小，因为真实深度、标定和物体摆放都会有误差。
开合尺度还必须受夹爪硬件限制约束。报告中应说明夹爪最小闭合宽度、最大开口宽度和本次候选使用的宽度。如果候选宽度超过夹爪行程，即使位姿完全正确也无法执行；如果宽度接近极限，闭合力和接触稳定性也可能下降。
5.2.4 安全偏移（Safety Offset）：夹爪停在哪儿
安全偏移用于构造 pre-grasp，使夹爪在真正接触前先停在目标外侧。它把长距离运动规划和短距离接触动作分开，是降低执行风险的基本设计。没有 pre-grasp 的抓取，相当于让规划器直接把 TCP 送到接触位姿；一旦深度、标定或路径略有误差，就可能在高速接近阶段撞到物体或桌面。
安全偏移通常沿接近方向反向设置：
其中 是抓取点， 是从 pre-grasp 指向 grasp 的单位接近方向， 是预留距离。这个距离要大于预期定位误差和夹爪外形厚度带来的缓冲需求，但也不能过大；过大会让最后一段直线接近跨越更多障碍物，反而增加碰撞风险。
实际系统中，pre-grasp 还承担一个调试功能：机械臂可以先运动到 pre-grasp，暂停或低速观察，确认夹爪、目标和桌面的相对位置，再执行最后的 approach。对教学实验，即使没有真机，也应在可视化中画出 pre-grasp 点和 grasp 点，让读者看到“安全停靠点”和“实际接触点”的区别。
5.2.5 姿态匹配（Orientation Alignment）：夹爪方向与物体姿态对齐
姿态匹配决定夹爪开合方向、接近方向和 TCP 坐标系之间的关系。规则物体可以依赖主轴或抓取模板，复杂物体则常需要学习模型生成多个候选，再由几何与运动学约束筛选。
一个夹爪姿态通常可以由三根互相正交的轴构造：接近轴、开合轴和手掌法向轴。接近轴决定夹爪从哪里进入，开合轴决定两根手指从哪两个方向夹住物体，第三根轴由右手系约束确定。仅有接近方向还不够，因为夹爪可以绕接近方向旋转；这个绕轴旋转角会决定手指是否对准物体长轴、短轴、把手、孔洞或可接触平面。
对盒子、书本、工具柄等有明显主轴的物体，夹爪开合方向通常要与局部宽度方向对齐，而不是与物体最长方向对齐。对圆柱、杯子这类存在旋转对称的物体，多个姿态可能在几何上等价，但会因为把手、遮挡或邻物而在执行上不同。对插拔和装配任务，姿态匹配比抓取点更敏感，因为几度旋转误差就可能导致插入失败或侧向卡死。
调试姿态匹配时，应同时画出物体坐标系和夹爪 TCP 坐标系，并说明每根轴的物理含义。若只显示一个 4×4 矩阵，读者很难判断夹爪到底是从上方来、从侧面来，还是绕接近方向转错了 90 度。工程报告中最好明确写出：哪根轴是 approach，哪根轴是 finger closing direction，哪根轴是 palm normal。
5.2.6 总结：五个要素的协同关系
从接口角度看，一个抓取候选通常不是只保存 pose，而是一组随动作阶段展开的参数：
grasp_pose | ||
pre_grasp_approach | ||
post_grasp_retreat | ||
pre_grasp_posture | ||
grasp_posture | ||
grasp_quality |
在代码中，建议把候选保存成结构化对象，而不是只保存一个矩阵。例如：
{
"grasp_point": [0.12, -0.04, 0.63],
"approach_direction": [0.0, 0.0, -1.0],
"closing_direction": [1.0, 0.0, 0.0],
"gripper_width": 0.065,
"pre_grasp_distance": 0.08,
"grasp_pose": "4x4 matrix in base frame",
"score": 0.82,
"validity_checks": {
"within_workspace": true,
"width_in_range": true,
"ik_solved": true,
"collision_free": false
}
}
这个结构的好处是失败时能快速归因：width_in_range=false 说明候选超出夹爪行程；ik_solved=false 说明空间位姿不可达；collision_free=false 说明几何候选存在但路径不可执行。相比只输出 T_base_grasp，这种表示更接近真实规划器和抓取模块需要消费的数据。
在实现上，抓取候选可以来自三种来源。第一种是模板法：对已知物体预先定义若干 ，运行时用 object pose 将模板变换到机器人坐标系；第二种是几何法：从点云中估计主轴、法线、局部宽度和可接触区域，再构造夹爪坐标系；第三种是学习法：由网络直接预测抓取候选、抓取质量分数和夹爪宽度。无论候选如何产生，最后都需要回到同一套执行参数：抓取点、接近方向、开合宽度、预抓取偏移和可达性校验。
本节小结： 一个完整抓取候选不是单个空间点，也不只是一个 位姿，而是一组相互约束的执行参数。抓取点决定夹爪到哪里，接近方向决定从哪条路径进入，姿态匹配决定手指如何对齐，开合宽度决定能否包住目标，安全偏移决定接近过程是否留有缓冲。任何一个要素定义不清，都会把看似合理的几何候选变成真机上的碰撞、夹偏或闭合失败。只有这些要素共同成立，并且经过坐标转换和可达性校验，抓取候选才会变成可规划、可接近、可闭合的末端目标。
5.3 典型抓取场景的位姿生成逻辑：顶部垂直接近 vs 侧面姿态匹配
桌面操作中最常见的两类构型是顶部抓取（top grasp）与侧向抓取（side grasp）。二者的区别主要体现在接近方向、姿态约束和环境碰撞条件上。
在桌面场景里，系统通常会先尝试最简单的顶部抓取：从上方下来，夹住物体，再向上抬起。如果目标较高、上方被遮挡、物体靠近货架或墙面，顶部路径就可能不可行，此时才需要考虑侧向抓取。这个决策顺序本身也是一种工程取舍：先用约束更少、风险更低的构型；当环境不允许时，再使用对姿态精度要求更高的构型。
5.3.1 桌面顶部抓取：垂直接近范式
顶部抓取的优势是结构简单、碰撞风险较低，通常只需准确估计目标平面位置、顶部高度和开合方向。对于规则块状物体，抓取点可取目标点云在水平面的几何中心，接近方向取桌面法线的反方向，预抓取位姿位于目标上方。它对物体绕竖直轴的旋转误差相对不敏感，因此常被用作观测退化时的保守降级策略。
5.3.2 侧面抓取：水平接近与姿态匹配
侧向抓取更依赖姿态估计和局部几何。夹爪需要从目标侧面进入，接近方向与侧面法线相关，开合方向要与可夹持区域的主方向匹配。它的优势是能处理细高物体、货架物体和上方空间受限场景；风险在于姿态误差会被夹爪长度和接近距离放大。若姿态误差为 ，指尖处的横向偏移近似为：
其中 是夹爪指尖到 TCP 或接触区域的有效距离。以 、 为例，横向偏移约为 ，足以导致侧向抓取从“夹持”变成“顶撞”。因此，侧向抓取通常需要更可靠的点云质量、位姿精化和碰撞检查。
5.3.3 预抓取位姿与安全接近路径设计
执行时，抓取动作通常拆为三段：pre-grasp → grasp → retreat / lift。这种分段并非形式化流程，而是为了将长距离运动、低速接触和撤离验证分开处理。
这三段动作也对应不同的误差容忍度。到达 pre-grasp 时允许较大的路径规划余量，因为夹爪尚未接触物体；从 pre-grasp 到 grasp 的最后一段运动必须低速、短距离、方向明确，因为此时深度误差和姿态误差会直接转化为碰撞或夹空；retreat / lift 阶段则用于验证抓取是否成功，并让物体离开桌面或狭窄空间。MoveIt 等规划框架中的 pre_grasp_approach、post_grasp_retreat、grasp_pose 等字段，本质上就是对这三段动作的结构化描述。
本节小结： 顶部抓取适合开阔桌面和规则低矮物体，它的优点是约束简单、姿态要求较低、失败风险相对可控；侧向抓取适合细高物体、侧面接触和空间受限场景，但对物体姿态估计、夹爪对齐和避碰规划要求更高。两种抓取范式不是谁替代谁，而是对应不同几何条件和任务目标的策略选择。无论采用哪种构型，都应通过 pre-grasp → grasp → retreat 将接近、接触和撤离分开处理。这个三段式结构，是把视觉误差、标定误差和运动误差限制在可控范围内的基本工程手段。
5.4 坐标系转换链路：从相机坐标系到机器人基座坐标系
视觉模块通常输出相机坐标系下的点、点云或位姿；运动控制模块需要机器人基座坐标系下的夹爪目标。因此，抓取位姿生成必须显式经过坐标变换链路，并清楚区分 Eye-to-Hand 与 Eye-in-Hand 两种拓扑。
这是从“算得出来”到“机器人动得对”的关键一步。视觉算法说“目标在相机前方 0.6 米、右侧 0.1 米”，机械臂控制器并不能直接使用这句话；它需要知道这个目标在机器人基座坐标系下的位置和姿态。手眼标定的作用，就是把相机看到的空间和机器人能运动的空间对齐。如果这一步错了，前面的检测、分割、点云和位姿都可能看起来正确，但机械臂一执行就会整体偏到另一边。
5.4.1 完整的坐标转换依赖关系
从像素到执行目标，一条典型链路包含以下变换：
坐标变换错误常见于两个层面：一是矩阵方向写反，例如把 当成 使用；二是忽略 TCP 偏置，导致机械臂法兰到位但夹爪中心未到位。真机部署中，应将每个变换矩阵的“从哪个坐标系到哪个坐标系”写入变量名或文档，避免只用 T、R、t 这类模糊命名。
一个实用的写法是始终使用“左上标表示目标坐标系，右下标表示源坐标系”的约定： 表示把相机系中的点变换到基座系， 表示把相机系中的点变换到末端系。若要把点 变到基座系，应写作 。这种记号看似繁琐，但能显著减少矩阵连乘方向错误。
5.4.2 眼在手上（Eye-in-Hand）与眼在手外（Eye-to-Hand）的物理拓扑差异
Eye-to-Hand 中，相机固定在工作空间外，待求外参通常为 。相机观测到的抓取位姿可直接通过固定外参映射到基座系：
图 6.13 对应的是“眼在手外”。相机像一个固定观察员，从工作空间外持续看着桌面和机械臂。它的优势是视野稳定、容易观察全局，适合桌面分拣、上下料和多个目标的排序选择；缺点是目标可能被机械臂本体遮挡，且相机距离目标较远时，深度噪声和标定误差会更明显。
Eye-in-Hand 中，相机安装在末端并随机械臂运动，待求外参通常为 。运行时必须结合当前末端位姿：
图 6.14 对应的是“眼在手上”。相机随着夹爪运动，机器人可以把相机移动到目标附近做近距离复检，因此适合精细装配、局部对位和遮挡较多的场景。代价是采集图像需要机械臂先移动到合适位置，循环时间更长；相机安装在末端还会带来负载、布线和碰撞规避问题。
5.4.3 手眼标定的数学本质：求解 方程
手眼标定的目标，是基于多组机器人运动与相机观测求解一个固定外参矩阵 ：
该方程依赖的是相对运动关系，而非单次绝对位姿。采样过程是否覆盖足够的平移和旋转激励、标定板观测是否稳定、矩阵方向是否统一，往往比具体调用哪个求解接口更重要。若采样姿态变化过小，或所有位姿近似共面，求得的 可能数值上收敛但物理上不可用。
OpenCV 的 calibrateHandEye() 提供了 Tsai-Lenz、Park-Martin、Horaud、Andreff、Daniilidis 等多种求解方法 [8]。工程上不应把“换求解器”作为第一反应：如果样本激励不足、机器人位姿方向写反、T_cam_board 来自错误 marker 尺寸，那么不同方法只是在同一批错误数据上给出不同数值。更可靠的流程是先检查输入矩阵方向和样本分布，再比较不同求解器的一致性。
采样时应避免只在一个小区域内平移机械臂，也不要让相机始终以几乎相同的角度观察标定板。更可靠的做法是覆盖工作空间内多个位置，并让末端姿态产生足够的 roll、pitch、yaw 变化。标定完成后，不应只检查算法返回的均方误差，还应做点位复现：选择相机视野中的一个已知点，经外参变换到机器人基座系，再让机械臂移动到该点附近观察是否吻合。这个验证步骤往往比单看矩阵数值更能发现方向、单位和 TCP 错误。
5.4.4 眼在手上实操和代码入口
Eye-in-Hand 的物理关系可以这样理解：相机固定在末端执行器上，机械臂每运动到一个新姿态，相机也随之改变观察角度；若标定板在世界中保持相对静止，那么每一组样本都应满足同一条坐标链：
其中 就是要求解的 T_end_camera。对应代码位于 Hand-eye calibration 目录：教学 demo 默认使用合成数据，从 T_base_end 和 T_cam_board 生成一组可复现实验样本；真实机器人实验时，则把这两个 JSON 换成实采数据。完整流程与第 3.4.4 节的点云 demo 类似，都是先准备输入，再一键运行脚本，最后查看输出报告。
pipeline_hand_eye_demo.py | pipeline_summary.json | |
generate_synthetic_hand_eye_data.py | robot_poses.jsoncamera_poses.json、ground_truth.json | |
estimate_board_pose_aruco.py | camera_poses.json | |
inspect_hand_eye_dataset.py | ||
calibrate_eye_in_hand.py | T_end_camera | |
validate_hand_eye.py | T_base_board | |
solve_hand_eye_teaching.py | AX=XB |
最小可运行命令如下：
cd "code/lecture06/Hand-eye calibration"
pip install numpy opencv-python
python pipeline_hand_eye_demo.py --topology eye_in_hand
如果默认镜像下载较慢，可使用清华 PyPI 镜像：
pip install -i https://pypi.tuna.tsinghua.edu.cn/simple numpy opencv-python
运行后会生成 data/hand_eye_demo/eye_in_hand/，其中 eye_in_hand.json 保存求解得到的 T_end_camera，inspect_report.json 记录样本激励检查，validation_report.json 记录 T_base_board 的跨样本一致性，pipeline_summary.json 汇总残差和输出路径。在默认合成数据上，终端会同时打印与 ground_truth.json 的误差，便于确认求解结果是否落在毫米级和小角度范围内。
真实标定时，流程不变：先用 estimate_board_pose_aruco.py 或其他 PnP 程序生成 camera_poses.json，再把机器人控制器记录的 T_base_end 写入 robot_poses.json，最后用 pipeline_hand_eye_demo.py --topology eye_in_hand --demo-dir 你的数据目录 --skip-generate 复用同一套筛查、求解和验证步骤。这三步各自回答一个问题：样本是否足够好，末端到相机的固定外参是多少，以及把外参代回去以后标定板在基座系中是否近似不动。
5.4.5 眼在手外实操和代码入口
Eye-to-Hand 的核心是求解 / T_base_camera，即固定相机到机器人基座的外参。和 Eye-in-Hand 相比，Eye-to-Hand 的相机不再随机械臂运动，运动的是末端上的标定板或被末端夹持的刚性目标。因此，它的闭环关系更适合写成：
这里 在采样过程中保持固定，要求解的是 。两种拓扑可以使用同一批输入格式：机器人侧记录 T_base_end，相机侧记录 T_cam_board；区别在于构造 相对运动时，Eye-to-Hand 需要对机器人侧运动做方向改写。代码入口仍位于 Hand-eye calibration 目录，但一键命令需要把 --topology 切换为 eye_to_hand。
pipeline_hand_eye_demo.py | pipeline_summary.json | |
generate_synthetic_hand_eye_data.py | ground_truth.json | |
estimate_board_pose_aruco.py | T_cam_board | |
inspect_hand_eye_dataset.py | --topology eye_to_hand 检查运动激励 | |
calibrate_eye_to_hand.py | T_base_camera | |
validate_hand_eye.py | T_end_board |
最小可运行命令如下：
cd "code/lecture06/Hand-eye calibration"
pip install numpy opencv-python
python pipeline_hand_eye_demo.py --topology eye_to_hand
运行后会生成 data/hand_eye_demo/eye_to_hand/，其中 eye_to_hand.json 保存求解得到的 T_base_camera，inspect_report.json 记录样本激励检查，validation_report.json 记录 T_end_board 的跨样本一致性，pipeline_summary.json 汇总残差和输出路径。
Eye-to-Hand 的验证重点不是“标定板在基座系中不动”，而是“标定板在末端坐标系中不动”。因此 validate_hand_eye.py 读取 eye_to_hand.json 后，会根据其中的 topology 字段自动选择验证链路，反推出每个样本的 T_end_board，再统计它们相对于平均参考变换的平移和旋转残差。真实标定时，可用 pipeline_hand_eye_demo.py --topology eye_to_hand --demo-dir 你的数据目录 --skip-generate 直接复用实采 robot_poses.json 和 camera_poses.json。若残差呈现随末端位置变化而系统性增大，通常应优先检查相机外参方向、机器人基座坐标定义和相机内参；若只有少数样本异常，则更可能是某几张图的 marker 检测、机器人姿态记录或样本配对出现问题。
5.4.6 真机部署中的标定误差排查
depth_scale | ||
旋转误差的任务影响应按具体坐标链路分析。若某个模型点或接触点相对旋转中心的距离为 ，小角度误差为 ，该点的位移量级约为 ；这里的 是点到相关旋转中心的几何杠杆臂，而不是物体到机器人基座的距离。因此，手眼标定验证不能只看平移残差，还必须检查旋转残差、点位复现误差以及抓取接触点处的实际偏差。
本节小结： 坐标系转换是视觉结果进入机器人控制系统的最后一道几何关口。Eye-in-Hand 与 Eye-to-Hand 的输入数据形式看起来相似，都是机器人位姿配相机观测，但固定对象、运动对象、外参含义和验证链路完全不同。工程实现中最容易出错的不是公式本身，而是矩阵方向、乘法顺序、TCP 偏置和“谁相对谁运动”的拓扑理解。只要这些关系写反，前端感知再准确，也会在执行阶段表现为稳定而危险的系统性偏差。因此，手眼标定结果必须通过点位复现、姿态复现和抓取前的坐标链路检查共同验证。
5.5 可达性校验：由几何可行到机械可行
即使 已经计算完成，也不能直接下发执行。几何上合理的抓取目标不一定满足机械臂的工作空间、关节限位、逆运动学和碰撞约束。抓取候选需要经过从粗到细的可达性筛选。
这一节要强调的是：抓取位姿“算出来”不等于“机械臂能做到”。有些目标点在相机看来很合理，但超出了机械臂工作空间；有些位置能够到达，但要求夹爪以一个机械臂无法实现的姿态进入；还有些位姿本身可达，但从当前位置过去的路径会撞到桌面或旁边物体。因此，抓取候选必须先经过可达性校验，再进入运动规划和执行。
可达性校验应按顺序执行：先剔除明显超出工作空间的候选，再求解 IK，最后做碰撞和路径检查。这样可以减少规划器负担，也便于定位失败原因。对教学系统而言，建议将每个候选的失败原因显式记录为 workspace_fail、ik_fail、collision_fail 或 gripper_fail，避免调试时只看到笼统的 planning failed。
在多候选抓取中，可达性校验还承担排序作用。系统可以先生成多个抓取候选，再按照抓取质量、接近路径长度、IK 解裕量、碰撞距离和夹爪开口余量进行打分。最终被执行的候选，不一定是视觉评分最高的候选，而是综合几何稳定性和机械可行性后最可靠的候选。这样的排序机制能显著降低单一位姿估计误差对最终抓取的影响。
本节小结： 可达性校验把“几何上合理”的抓取候选进一步筛成“机械上可执行”的目标。工作空间判断、IK、关节限位、碰撞检测和夹爪行程检查应按由粗到细的顺序执行，这样既减少规划器负担，也便于定位失败原因。可达性失败并不一定说明位姿估计错误，也可能来自机械臂构型、障碍物布局、TCP 定义或夹爪行程不足。真实系统中，记录 workspace_fail、ik_fail、collision_fail、width_fail 等失败标签，往往比只看到一句 planning failed 更有调试价值，也能指导后续候选排序和策略降级。
5.6 观测退化下的策略降级
真实场景中，6D 位姿估计可能因遮挡、反光、透明材质、低纹理或点云过稀而失效。此时系统不应继续使用低置信度姿态结果，而应根据当前可用观测信息切换到更低信息需求的策略。
降级策略可以理解为机器人系统的保守执行模式。当完整 6D 位姿可靠时，应优先使用姿态匹配抓取；但如果只剩下一个稳定的 3D 中心点，就不应构造缺乏依据的旋转姿态并直接执行。更稳妥的做法，是承认当前信息不足，退回到对姿态要求较低的顶部抓取，或者重新观测。对真实机器人来说，拒绝一次低置信度抓取通常比错误执行一次更安全。
降级策略的核心，是把“精确但不可靠”的高维估计，替换为“粗糙但可验证”的低维控制目标。例如，当物体姿态不可信但目标点云中心稳定时，可以退回到基于 3D 中心的顶部抓取：接近方向固定为桌面法线方向，夹爪姿态取默认水平姿态，开合宽度由点云包围盒估计，随后仍执行可达性和碰撞检查。
触发降级的常见条件包括：位姿置信度低于阈值、配准残差过大、位姿在连续帧间跳变、有效深度像素比例过低、IK 或碰撞检查连续失败。降级不是为了保证高成功率，而是为了避免系统在感知不充分时输出看似精确、实则危险的动作。
降级策略也应当分层，而不是只有“执行 / 不执行”两种状态。若只是旋转不可靠，可以保留 3D 中心并使用顶部抓取；若深度点云也不可靠，可以移动腕部相机或改变视角重新采集；若多次重感知仍失败，则应放弃该目标或请求人工确认。对于面向真实硬件的系统，这种保守决策是安全设计的一部分。
本节小结： 抓取位姿生成是从物体状态表征到机器人动作表征的转换层。它不仅要从物体位姿推导夹爪的抓取点、接近方向与开合宽度，还要通过手眼标定和 TCP 修正完成坐标转换，并经过 IK、碰撞、夹爪行程与策略降级机制筛选。当 6D 位姿可靠时，系统可以使用姿态匹配抓取；当姿态信息不稳定时，则应退回到中心点、顶部抓取或重感知等更保守策略。到这一步，视觉输出才真正被翻译成真实机器人可以尝试执行的抓取目标，也才进入运动规划和执行闭环的责任范围。
6 感知失败分析与鲁棒性优化
在实验室理想环境下，6D 位姿估计算法往往能在标准数据集上达到很高的精度指标，但部署到真实物理场景后，遮挡堆叠、材质反光、标定误差、环境光照扰动等因素都会导致感知失效，直接拉低实际抓取成功率。感知失败并非单一环节的问题，而是贯穿从图像采集、坐标变换到位姿输出的完整链路。
本章系统梳理真实场景中典型感知失败的场景与底层成因，量化不同类型误差对抓取结果的影响程度，并从视觉输入层、位姿计算层、系统机制层三个维度给出可落地的优化方案，最终形成一套标准化的真机调试流程，帮助开发者将算法从 “实验室可用” 推进到 “工业级稳定”。
6.1 典型感知失败场景与成因
感知失败按照产生的链路环节可分为三大类：视觉类失败发生在数据输入前端，源于传感器与环境的不匹配；几何类失败发生在 2D-3D 转换与坐标变换环节，源于标定误差与坐标系定义问题；位姿类失败发生在输出端，源于算法本身的歧义性与运动学约束脱节。
图 6-1 感知失败全链路分类图谱
图 6-1 感知失败全链路分类图谱
6.1.1 视觉类失败：遮挡、反光、低纹理与光照剧变
视觉类失败是最常见的前端失效模式，直接导致后续所有几何计算的输入质量下降，是真实场景中抓取失败的首要诱因。
1. 遮挡与堆叠
现象：目标物体被其他物体遮挡 30% 以上，或处于多层堆叠状态，仅部分表面可见；点云呈现大面积残缺，仅能观测到物体顶面或局部侧面。
成因：料箱拣选、桌面杂乱堆放等场景中，物体相互遮挡，完整几何特征不可见；堆叠状态下下层物体的边缘与轮廓被上层物体覆盖。
影响机制：点云不完整导致 ICP 配准缺少足够的几何约束，算法易收敛到错误的局部最优解，表现为位姿漂移、旋转角错误，甚至完全求解失败。通常遮挡比例超过 50% 时，单视角位姿估计的准确率会下降到 30% 以下。
2. 反光与透明材质
现象：金属零件、玻璃容器、光亮塑料包装等物体表面出现大面积高光，深度图对应区域出现空洞、飞点（距离值远大于真实值）或随机噪声；RGB 图像对应区域过曝，丢失纹理细节。
成因：主动光深度相机的红外光线被镜面反射或穿透，无法按正常路径返回传感器：结构光相机因散斑无法正常成像形成空洞，ToF 相机因多次反射导致飞行时间计算错误产生飞点。
影响：目标点云残缺不全，配准算法失去有效对应点，位姿估计结果严重偏离真值；极端情况下目标点云有效点数不足 10%，位姿求解完全失效。
3. 低纹理 / 无纹理物体
现象：纯色陶瓷、哑光塑料、光滑金属等物体表面缺少明显纹理特征，基于 RGB 特征的检测与位姿算法输出结果不稳定。
成因：基于特征点匹配的算法无法提取足够多的稳定关键点，模板匹配也缺少区分度，几何上多个姿态的观测特征高度相似。
影响：位姿估计出现旋转歧义，姿态结果随机跳动，重复定位精度大幅下降；连续多帧输出的旋转角度波动可达 ±20°，无法用于稳定抓取。
4. 光照剧变
现象：强光直射、逆光、强阴影导致 RGB 图像过曝或过暗，目标与背景对比度急剧下降，目标检测器出现大面积漏检或误检。
成因：环境光照超出传感器动态范围，图像特征分布与训练数据产生显著差异，深度学习模型的特征提取能力退化。
影响：目标检测漏检、误检，实例分割轮廓偏移，后续所有环节连锁失效，属于前端输入级的整体失效。
图 6-2 四类典型视觉失败现象对比图
6.1.2 几何类失败：深度空洞、外参偏差与坐标系错误
几何类失败的本质是 “空间换算出错”—— 即使视觉检测完全正确，二维到三维的映射或坐标系转换出现偏差，最终输出的位姿也会偏离真实位置。这类误差通常具有系统性，隐蔽性强。
1. 深度缺失与点云空洞
现象：目标物体的点云存在大面积空缺，尤其是边缘、大角度斜面和高反光区域；物体边缘出现拖尾、飞点等噪声。
成因：物体表面法线与相机光轴夹角过大（通常超过 60°）时，深度回波信号信噪比急剧下降；黑色吸光材质、透明材质会导致深度无返回值；传感器本身的测量盲区也会产生边缘空洞。
影响：点云无法完整描述物体形状，ICP 配准易陷入局部最优，位姿的平移与旋转均出现系统性偏差；空洞集中在物体一侧时，会导致位姿向点云完整的一侧偏移。
2. 手眼外参偏差
现象：所有抓取结果都呈现固定方向、固定大小的系统性偏移，偏差量不随物体位置改变而改变方向，仅随距离线性放大。
成因：手眼标定样本分布不均、标定板精度不足、相机安装发生松动或碰撞，导致手眼变换矩阵存在固定的平移与旋转误差。
影响：这是最隐蔽的系统性误差 —— 单看位姿估计本身结果稳定、重复性好，但转换到机器人坐标系后全部偏位；若不做标定验证，很容易误判为位姿估计算法精度不足。
3. 坐标系方向定义错误
现象：抓取位姿出现镜像翻转、坐标轴方向颠倒，物体姿态完全相反，或出现量级级的位置偏差。
成因：不同算法库的坐标系约定不一致，例如 OpenCV 相机系（Z 轴朝前、Y 轴朝下）与 OpenGL 相机系（Z 轴朝后、Y 轴朝上）的镜像差异；变换矩阵乘法顺序错误（左乘 / 右乘混淆）；空间单位不统一（毫米 / 米混用）导致 1000 倍的量级偏差。
影响：位姿结果完全不可用，直接导致撞机或抓空，属于必须优先排除的基础错误，也是新手部署时最常踩的坑。
6.1.3 位姿类失败：位姿跳变、方向翻转与目标不可达
位姿类失败发生在输出环节，算法本身给出了数值结果，但结果不稳定、存在歧义或不可被执行系统直接使用。
1. 位姿跳变
现象：连续采集的多帧静态图像中，估计出的位姿没有平滑变化，而是出现无规律的突变跳动，平移跳变量可达厘米级，旋转跳变可达数十度。
成因：单帧点云噪声大、特征不稳定，ICP 算法每次收敛到不同的局部最优解；低纹理物体缺少足够的几何约束，解空间存在多个局部极小值。
影响：机械臂末端跟随位姿剧烈抖动，无法平稳接近物体，甚至在运动过程中碰撞环境；连续抓取的一致性极差，成功率波动剧烈。
2. 方向翻转（对称歧义）
现象：旋转对称物体（如圆柱体、正立方体、螺栓）的位姿出现 180° 或 90° 的随机翻转，平移分量正确但朝向完全错误，多次重复测量结果随机跳变。
成因：物体本身存在旋转对称性，几何上多个姿态对应的点云观测完全等价，算法无法区分，会随机输出一个合法解。
影响：夹爪开合方向与物体长轴错位，无法形成有效夹持，抓取成功率骤降；对于需要精确朝向的装配任务，方向翻转会直接导致任务失败。
3. 目标不可达
现象：位姿估计本身数值正确，但生成的抓取位姿超出机械臂工作范围，或接近路径存在碰撞，运动规划器直接报错无解。
成因：位姿估计模块未考虑机械臂运动学约束，抓取点选在工作空间边缘；接近方向被障碍物或工作台阻挡；夹爪姿态超出腕部关节限位。
影响：感知输出的位姿无法被执行，感知与规划环节脱节，看似正确的感知结果实际没有工程价值。
本节小结
三类失败分别对应感知链路的不同阶段：视觉类失败是 “输入坏了”，发生在最前端；几何类失败是 “换算错了”，发生在坐标变换中间环节；位姿类失败是 “输出不稳 / 不可用”，发生在最终结果端。真机排查时可按 “前端输入→几何转换→输出结果” 的顺序逐级定位，能快速锁定问题根源，避免盲目调参。
6.2 感知误差对抓取成功率的影响量化
不同类型的感知误差对抓取结果的破坏程度并不相同。量化误差与成功率的对应关系，有助于设定合理的精度指标、判断误差来源，以及评估优化效果。
6.2.1 平移误差：从毫米级偏差到厘米级失效
平移误差指估计出的物体质心位置与真实位置的欧氏距离偏差，是最直观的误差形式，主要影响夹爪与物体的横向对准。
对于两指平行夹爪，平移容错量与夹爪开口宽度、物体宽度直接相关，近似满足：
其中为夹爪开口宽度，为物体夹持处的宽度。当物体宽度远小于夹爪开口时，容错空间更大，反之则对平移精度要求极高。
按照误差量级对抓取成功率的影响，可分为四个等级：
1-3mm 轻微偏差：对平行夹爪抓取几乎无影响，夹持位置略有偏移但仍能形成稳定夹持，成功率接近 100%；仅对高精度装配任务有影响。
3-5mm 中度偏差：夹持位置偏离最佳受力点，物体受力不均，抓取成功率下降至 70%-90%；表面光滑的物体易在提升过程中滑落。
5-10mm 较大偏差：夹爪仅能夹住物体边缘，或部分手指落空，抓取成功率降至 30%-50%；夹爪接近时易碰倒物体。
10mm 以上（厘米级）严重偏差：夹爪大概率完全错过物体，直接抓空，或撞击物体导致其移位，抓取成功率低于 20%，基本失效。
平移误差的主要来源是点云提取偏差、深度噪声与标定误差，它是抓取系统最基础的精度指标。
6.2.2 旋转误差：角度偏差的空间放大效应
旋转误差指估计姿态与真实姿态的角度差，其最核心的特点是误差会随操作距离被空间放大 —— 离旋转中心越远，同样的角度偏差造成的末端位置偏移越大。这一特性使得旋转误差对长臂展机器人的影响远大于桌面机械臂。
图 6-3 旋转误差空间放大效应示意图
旋转误差的放大效应满足几何关系：
其中：
为末端位置偏移量；
L为末端到旋转中心的距离（臂展长度）；
为旋转误差角度（弧度制）。
小角度下（），，偏移量与角度近似成正比。例如在 1 米臂展处，1° 的旋转误差会带来约 17.5mm 的位置偏移；5° 的旋转误差则会带来约 87mm 的偏移，足以让夹爪完全错过物体。
按照误差量级对抓取成功率的影响，可分为四个等级：
2° 以内：平行夹爪可完全容忍，夹持面仍能有效接触物体，抓取稳定性不受明显影响。
2°-5°：夹爪与物体的接触面积减小，摩擦力下降，抓取成功率降至 60%-80%；提升物体时易倾斜滑落。
5°-15°：夹爪以侧面接触物体，无法形成有效夹持面，抓取力大幅衰减，成功率通常低于 30%。
15° 以上：夹爪端面直接撞向物体侧面，无法形成夹持动作，抓取基本失败，甚至可能损坏物体或夹爪。
正因旋转误差存在空间放大效应，位姿估计的旋转精度往往比平移精度更重要，也是真机调试的重点优化对象。
6.2.3 深度误差：接近方向的致命影响
深度误差是沿夹爪接近方向的平移误差，它比平面内的平移误差更致命 —— 夹爪在接近方向上几乎没有容错空间，偏差直接决定 “抓不到” 还是 “撞上去”。
深度误差的特殊性在于：夹爪沿接近方向运动时，在接触物体前通常没有力觉反馈，属于开环运动，深度偏差会直接导致夹爪停止位置错误。对于吸盘式末端，深度误差的容忍度更低，通常要求 ±2mm 以内，否则无法形成有效密封负压。
按照误差量级对抓取成功率的影响，可分为四个等级：
1-2mm 轻微偏差：无明显影响，夹爪闭合后仍能稳定夹持。
3-5mm 中度偏差：夹持深度不足，物体仅被夹住前端一小部分，搬运过程中易脱落；或夹爪深入过多，顶推物体导致移位。
5-10mm 较大偏差：要么夹爪没够到物体、完全抓空，要么夹爪撞击物体表面，将物体推走。
10mm 以上 严重偏差：几乎 100% 出现抓空或撞物，抓取完全失败，甚至存在撞坏传感器或工作台的风险。
图 6-4 三类感知误差与抓取成功率关系曲线
本节小结
三类误差对抓取的影响各有特点：平移误差影响横向对准，误差与成功率近似线性下降；旋转误差存在空间放大效应，长臂展场景下危害远大于平移误差；深度误差沿接近方向作用，容错空间最小，最为致命。其中标定误差带来的是系统性偏差，决定了整个系统的精度上限；单帧位姿的随机误差决定了单次抓取的波动范围。
6.3 三类优化方向
针对上述失败场景与误差来源，可从视觉输入层、位姿计算层、系统机制层三个维度分层优化，实现从 “单次准确” 到 “长期稳定” 的鲁棒性提升。三者并非替代关系，而是层层递进、互为补充。
图 6-5 三层鲁棒性优化体系架构图
6.3.1 视觉层面：光照、反光、透明物体的处理
视觉层面的优化目标是提升输入数据质量，从源头减少噪声与缺失，是所有优化的基础。前端输入质量提升带来的收益，远大于后端算法的调优。
1. 光照环境标准化
采用漫射环形补光方案，光源与相机光轴呈 30°-45° 斜射，避免点光源垂直正射，减少物体表面高光与硬阴影；
针对高反光金属零件，搭配同轴光源与偏振镜头，通过偏振片过滤镜面反射光，保留漫反射光，大幅提升深度成像质量；
封闭工作空间，隔绝环境光干扰，保证光照条件长期稳定，避免检测分割精度随昼夜、天气波动。
2. 多模态数据互补
融合 RGB、深度、红外三个通道的信息，单一模态失效时其他模态兜底；例如反光物体 RGB 过曝，但红外灰度通道仍能提供有效轮廓，可用于辅助目标定位；
针对透明玻璃、塑料等普通深度相机无法测量的物体，补充线激光模组或红外结构光传感器，替代普通 RGB-D 相机，获取稳定的深度数据；
多通道数据做空间对齐与时间同步，保证不同模态的像素级对应。
3. 图像与深度后处理
对实例分割掩膜做形态学开运算（先腐蚀后膨胀）与边缘平滑处理，减少边缘锯齿，降低点云边缘噪声，避免背景像素混入目标点云；
对深度图做空洞补全：小范围空洞采用双边滤波保留边缘的同时填充，大面积空洞可使用轻量级深度补全网络修复，提升点云完整性；
增加边缘深度滤波，剔除物体边缘的飞点与拖尾噪声，通过统计滤波去除距离均值过远的离群点。
6.3.2 位姿层面：多帧融合与时序平滑
位姿层面的优化目标是抑制单帧波动、提升绝对精度，让输出的位姿既准又稳，是提升抓取一致性的核心手段。
图 6-6 两阶段粗到精位姿优化流程图
1. 时序滤波平滑
引入卡尔曼滤波或滑动窗口平均，对连续多帧的位姿结果做时序滤波：平移分量采用线性卡尔曼滤波，旋转分量采用四元数球面线性插值（SLERP），抑制单帧跳变，输出平稳的位姿序列；
设置位姿变化阈值，单帧平移变化超过 10mm、旋转变化超过 5° 时判定为异常帧，丢弃该帧并沿用历史平滑结果，避免机械臂跟随异常位姿抖动；
对于静态场景，可采用多帧点云累加融合的方式，提升点云密度与完整性，再进行单次位姿求解。
2. 粗到精的两阶段位姿精修
第一阶段粗估计：用深度学习网络输出粗位姿，提供较好的初始值，兼顾泛化性与速度；要求初始平移误差小于物体直径的 20%，旋转误差小于 30°，保证后续 ICP 不陷入局部最优。
第二阶段精配准：用点到平面 ICP 做几何精配准，在粗位姿基础上迭代优化，提升位姿绝对精度；点到平面 ICP 的收敛速度比传统点到点 ICP 快 3-5 倍，且对噪声更鲁棒。
这是当前工业界的主流方案，兼顾了深度学习的泛化性与几何方法的高精度，性价比最高。
3. 多视角融合
针对遮挡严重的场景，控制机械臂带动腕部相机移动到 2-3 个不同角度观测物体，或部署多台固定相机从不同视角采集；
融合多视角的位姿估计结果，采用加权平均策略，权重由各视角的点云有效率、配准残差共同决定，配准残差越小、点云越完整的视角权重越高；
多视角融合可大幅降低遮挡带来的误差，使严重遮挡场景下的位姿准确率提升 40% 以上。
6.3.3 系统层面：失败检测与重感知机制
系统层面的优化目标是建立容错兜底机制，即使感知出错，也能避免无效抓取，通过重试与闭环迭代提升整体成功率。这是成本最低、见效最快的优化手段。
1. 前置失败检测
抓取执行前设置三重校验门槛，任一不达标则判定为感知失败，不执行抓取，避免无效动作与碰撞风险：
点云有效率校验：目标点云的有效点数占完整模型点数的比例低于阈值（通常 < 40%），判定为点云质量不足；
位姿置信度校验：ICP 配准的均方根误差（RMSE）大于 2mm，或重投影误差大于 3 像素，判定为位姿不可信；
可达性校验：通过运动学逆解验证抓取位姿与预抓取位姿是否存在有效解，且接近路径无碰撞，判定为不可执行。
2. 重感知重试机制
单次感知失败后，系统自动调整策略重新采集，采用分级升级策略：第一次失败仅重新采集单帧计算；第二次失败调整相机曝光、增益与滤波参数；第三次失败控制机械臂移动到侧方视角进行多视角重试；
通常设置 2-3 次重试机会，可在不增加硬件成本的前提下，将整体抓取成功率提升 10%-20%；
设置最大重试次数上限，避免无限循环导致系统卡死。
3. 失败样本闭环回流
自动记录所有失败场景的原始数据（RGB、深度图、位姿结果、失败类型、机器人状态），形成结构化失败样本库；
定期将样本标注后用于模型微调或参数优化，形成 “部署 - 失败 - 优化” 的闭环迭代机制；典型情况下每月迭代一次，可使场景内成功率持续提升 5%-10%；
这是系统长期稳定性提升的核心机制，也是工业部署与实验室演示的核心区别。
本节小结
三个层面的优化各有侧重：视觉层面是 “治本”，提升输入质量，从源头减少错误；位姿层面是 “求精”，提升输出精度与平滑度，让结果更可靠；系统层面是 “兜底”，建立容错与重试机制，保证极端情况下仍能正常工作。三者结合，才能实现从实验室精度到真机鲁棒性的跨越。
6.4 真机部署中的感知调试流程
真机调试切忌一上来就跑全链路抓取，否则出现失败时无法定位是哪个环节出了问题。正确的调试路径遵循先基础后上层、先单步后全链路的原则，逐级验证、逐级排查，可大幅提升调试效率。
图 6-7 真机感知调试逐级验证流程图
6.4.1 基础校验：标定与坐标系正确性验证
标定与坐标系是所有感知的基础，基础错了，上层算法再准也没用。调试第一步必须先排除系统性误差，这一步跳过后患无穷。
1. 校验内容
相机内参：确认相机内参标定的重投影误差小于 0.5 像素，畸变校正正确，深度与 RGB 图像的像素对齐误差小于 1 像素；
手眼外参：确认手眼标定矩阵的平移与旋转精度，排除固定方向的系统性偏移；
TCP 工具标定：确认夹爪工具中心点标定准确，无偏心偏差，TCP 误差应小于 0.5mm；
坐标系约定：确认像素坐标系、相机坐标系、机器人基座坐标系的轴方向与单位统一，排除镜像、翻转与量级错误。
2. 验证方法
固定高精度棋盘格标定板，控制机械臂移动到 5-8 个覆盖工作空间的不同位姿，分别对比 “视觉计算的标定板位姿” 与 “机器人正运动学计算的位姿”。
合格标准：全工作空间内平移误差 RMSE≤2mm，旋转误差 RMSE≤1°。
3. 常见问题排查
固定方向的系统性偏移：优先检查手眼标定与 TCP 标定；
镜像 / 翻转错误：优先检查坐标系方向约定与变换矩阵乘法顺序；
误差随深度线性变化：优先检查相机内参标定精度；
误差随位置变化大：优先检查标定样本是否覆盖了完整工作空间。
6.4.2 单步验证：检测分割效果与静态位姿精度测试
基础校验通过后，逐个验证每个感知模块的输出质量，确保单模块性能达标，再进行全链路联调。
1. 检测分割验证
固定目标物体，连续采集 20 帧 RGB 图像，统计目标检测的召回率与准确率，工业场景要求召回率≥99.5%，无漏检、无误检；
对比实例分割掩膜与物体真实轮廓，要求边缘平均误差小于 2 像素，无大面积空洞或溢出；
改变光照角度、背景复杂度、遮挡程度，验证检测分割的鲁棒性，找出最易失效的边界场景。
2. 静态位姿精度验证
使用标准立方体、标定球等已知尺寸的高精度物体（尺寸精度优于 ±0.01mm）作为真值基准；
连续采集 30 帧数据，对比感知输出的位姿与真值，分别统计平移误差与旋转误差的均值、标准差、最大值；
合格参考标准：桌面抓取场景平移误差 RMSE≤3mm，旋转误差 RMSE≤2°；精密装配场景需相应提高要求。
3. 点云质量验证
可视化目标点云，观察完整性、边缘平滑度与噪声水平；
调整滤波参数（体素降采样尺寸、统计滤波阈值），在保留几何细节与去除噪声之间找到平衡点；
验证不同材质、不同角度下的点云有效率，评估点云质量的边界。
6.4.3 全链路验证：动态抓取评测与迭代优化
单模块全部达标后，再打通完整抓取链路，进行成功率评测与问题迭代。
1. 固定场景基准测试
同一物体、同一摆放位置与姿态，重复执行 10-20 次完整抓取，统计基准成功率；
对每一次失败进行归因分类：检测失败、分割失败、位姿失败、标定误差、运动规划失败、执行失败；
绘制失败原因帕累托图，找到占比最高的失败原因，针对性优先优化，遵循 80/20 法则。
2. 泛化场景压力测试
改变物体的摆放角度、位置、堆叠状态，调整光照条件，测试不同工况下的抓取成功率；
评估系统的鲁棒性边界，找出最容易失效的场景，作为后续优化重点；
记录每个工况的成功率，形成系统的性能指标手册。
3. 闭环迭代优化
针对占比最高的失败类型，对应优化对应模块：反光失败多就优化补光方案，位姿跳变就加时序滤波，标定不准就重新标定；
每轮优化后重新测试成功率，验证优化效果，避免无效调参；
持续积累失败样本，定期迭代模型与参数，逐步逼近目标成功率。
本节小结
真机感知调试的核心逻辑是 “逐级验证、逐级排查”：先搞定标定与坐标系这个基础，排除系统性误差；再逐个验证检测、分割、位姿等单模块性能；最后做全链路的成功率评测与迭代优化。遵循这个流程，可以避免盲目调参，高效定位并解决问题，快速将系统从 “能跑” 优化到 “稳定好用”。
7 位姿估计抓取实战实验与作业解析
前面几节分别讲了检测、分割、深度反投影、点云清洗、6D 位姿、抓取候选和手眼标定。本节把这些内容组织成一次完整实验。实验的目标不是让所有同学都在同一套硬件上完成抓取，而是让每个同学都能交付一条可复现、可检查、可归因的链路：
RGB-D 输入
-> 目标 mask
-> 目标点云
-> 几何位姿或抓取候选
-> 手眼转换后的机器人基座系目标
-> 可视化、日志与误差分析
因此，本节的作业评价重点不是“是否刚好抓起来一次”，而是学生能否讲清楚每个中间量从哪里来、单位是什么、坐标系是什么、失败时应该检查哪一层。有 SO101 真机的同学可以继续做低速执行验证；没有硬件的同学也可以用 Open3D、合成手眼数据和可视化报告完成同等核心能力训练。
当前仓库第 6 讲已经落地了两个稳定代码入口：
code/lecture06/Visual_Perception/ | pipeline_from_syllabus.py | |
code/lecture06/Hand-eye calibration/ | pipeline_hand_eye_demo.py |
YOLO 检测、真实 SAM2 后端、ManiSkill 抓取执行在本讲中作为扩展接口处理。也就是说，文档会说明它们应该接在哪个位置、输入输出是什么、报告中应如何解释，但不会把尚未在仓库中提供的外部系统写成基础必做命令。这样可以保证基础实验可复现，同时给进阶同学保留扩展空间。
7.1 有硬件版与无硬件仿真版实验流程
建议把本讲实验分成四个层级。课堂最低要求是 A；想训练交互式感知的同学可以做 B；有 SO101 或等价机械臂的同学可以做 C；希望衔接后续仿真和强化学习内容的同学可以做 D。
.pcd、阶段截图、summary JSON | ||||
这四个版本共享同一条主线：前端感知只负责产生目标区域，点云模块负责把目标区域变成三维几何，位姿或抓取模块负责定义 TCP 应到达的位置和方向，手眼标定负责把相机系结果转换到机器人可执行的坐标系。
7.1.1 有硬件版（SO101）：真机全流程操作步骤
有硬件版不要一开始就追求全自动抓取成功率。更稳妥的做法是先证明数据可信、坐标可信、目标可信，再让机械臂低速执行。推荐按下面顺序完成。
第一步：采集前检查。 固定相机安装方式，明确是 Eye-in-Hand 还是 Eye-to-Hand；确认 RGB 与深度图已经对齐；确认相机内参中的 fx、fy、cx、cy 与图像分辨率匹配；确认深度单位。如果深度图以毫米保存，depth_scale 通常取 1000.0；如果相机 SDK 已经输出米，则不能再按毫米缩放。
第二步：准备目标前景。 初期可以用人工 mask 或数据集 mask 跑通流程；真机扩展时，可以由 YOLO 给出 bbox，再由 SAM / SAM2 或交互式分割工具输出 mask。无论前端模型如何变化，传给点云流水线的文件都应整理成同一套约定：
rgb.png
depth.png
mask.png
intrinsics.json
这个约定很重要。它把“用什么模型分割”与“后端如何做三维几何”解耦，使后续代码不必关心 mask 是人工画的、数据集给的，还是模型预测的。
第三步：运行点云流水线。 在 code/lecture06/Visual_Perception/ 目录下运行：
python pipeline_from_syllabus.py --demo-dir data/rgbd_object_demo --no-vis
脚本会一次性完成掩膜筛选、反投影、直通滤波、统计滤波、体素降采样和点云导出。真机实验中，先检查 syllabus_masked_depth.png 是否只保留目标，再检查 syllabus_target_clean.pcd 的空间范围是否符合桌面物体的真实尺寸。若一个杯子的点云宽度显示成几米，通常是深度尺度或相机内参错了；若点云只剩零散碎片，通常是 mask 或滤波参数过激。
第四步：完成手眼标定。 若相机固定在末端，运行 Eye-in-Hand 链路；若相机固定在工作空间外，运行 Eye-to-Hand 链路：
cd "code/lecture06/Hand-eye calibration"
python pipeline_hand_eye_demo.py --topology eye_in_hand
python pipeline_hand_eye_demo.py --topology eye_to_hand
真实数据接入时，把实采 robot_poses.json 和 camera_poses.json 放入自己的数据目录，再使用：
python pipeline_hand_eye_demo.py --topology eye_in_hand --demo-dir 你的数据目录 --skip-generate
或：
python pipeline_hand_eye_demo.py --topology eye_to_hand --demo-dir 你的数据目录 --skip-generate
真实数据接入时，重点检查三件事：机器人位姿是否与相机观测一一对应，旋转矩阵或四元数的方向是否使用一致，标定板或目标的坐标系定义是否前后一致。手眼标定结果不是一个孤立矩阵，而是后续所有抓取目标转换的基础。
第五步：构造抓取候选。 基础版可以从目标点云中计算包围盒、主轴、顶部中心点或局部平面中心，构造一个保守的顶部抓取候选。进阶版可以接入 GraspNet / 6-DoF GraspNet 类方法生成多个 grasp candidates，再由 IK、碰撞检测和夹爪开口范围筛选 [11]。
第六步：低速执行前验证。 不建议第一次就把视觉输出直接交给机械臂执行。应先把候选抓取点转换到机器人基座系，在 RViz / MoveIt 或厂商示教界面中显示目标位姿，确认接近方向、单位、TCP 偏置和安全高度无误后，再低速执行：
pre-grasp -> approach -> close gripper -> lift -> retreat
以下情况应禁止直接执行：目标点云空间范围明显异常；手眼标定残差没有记录；TCP 坐标系方向无法解释；抓取候选没有 pre-grasp 安全位姿；规划路径穿过桌面、相机支架或机器人本体。
本节小结： 有硬件实验要按“相机数据可信 → 点云可信 → 手眼可信 → 候选抓取可信 → 低速执行”的顺序推进。最危险的做法是直接把视觉输出交给机械臂执行，因为一旦坐标方向、深度尺度或 TCP 偏置写错，前端结果越稳定，后端偏差反而越具有迷惑性。
7.1.2 无硬件仿真版：Open3D 与 ManiSkill 离线实验
无硬件版的目标是完成同样的数据闭环，只是不驱动真实机械臂。基础要求使用 Open3D 完成点云生成、滤波、截图和 .pcd 导出；进阶要求可以把干净目标点云和抓取候选导入 ManiSkill 或其他仿真环境进行可视化验证。
基础流程如下：
从 University of Washington RGB-D Object Dataset 下载一组 RGB-D 数据，例如 https://rgbd-dataset.cs.washington.edu/dataset/rgbd-dataset_full/coffee_mug_1.tar，按代码目录 README 的说明整理为data/rgbd_object_demo/。作业中应记录下载源和整理命令，不应依赖某台电脑上已有的本地数据。执行 pipeline_from_syllabus.py，得到syllabus_target_clean.pcd、阶段截图和syllabus_pipeline_summary.json。在 Open3D 中查看目标点云，检查点数、XYZ 范围、体素降采样前后变化是否合理。 使用 pipeline_hand_eye_demo.py跑通 Eye-in-Hand 或 Eye-to-Hand 合成数据，理解T_end_camera与T_base_camera的区别。编写一段简化抓取候选生成说明：例如“顶部抓取候选取点云包围盒中心上方，接近方向沿负 Z，夹爪开口取包围盒短边加安全余量”。
离线版的报告要重点检查以下输出：
syllabus_masked_depth.png | ||
syllabus_target_raw.pcd | ||
syllabus_target_passthrough.pcd | ||
syllabus_target_statistical.pcd | ||
syllabus_target_clean.pcd | ||
syllabus_pipeline_summary.json |
进阶仿真版可以把第 5 步生成的候选转成仿真环境中的末端目标位姿。此时 ManiSkill 的角色不是替代感知算法，而是验证候选动作在一个物理环境中是否可达、是否碰撞、是否能完成 grasp / lift。作业中只要说明坐标系映射、动作接口和成功判据即可，不要求所有同学都完成完整仿真控制器。
本节小结： 无硬件版不是“弱化版”，而是把真机风险替换成可观察的离线证据。只要能保存输入数据、点云中间结果、手眼标定报告、抓取候选定义和失败分析，就已经覆盖了位姿驱动抓取最核心的工程链路。
7.2 核心代码逐层解析：从检测分割到抓取生成
参考第 10 讲 3.3 代码解析：LeRobot 源码逐层拆解 的写法，本节不只给命令，而是沿数据流拆解代码。入口脚本负责串流程，数据加载层负责统一格式，算法函数负责单步处理，输出层负责保存可复现证据。读代码时要始终追问三个问题：这一层输入是什么，输出是什么，失败时看哪个中间文件。
7.2.1 检测分割模块：YOLO 检测与 SAM 分割的调用逻辑
当前仓库没有内置 YOLO 权重，也没有绑定具体 SAM2 predictor；它提供的是可替换的分割接口。真实系统中，YOLO 的作用是从 RGB 图中找出目标候选，SAM / SAM2 的作用是把 bbox、点击点或文本提示转成像素级 mask。接口关系可以抽象为：
# 伪代码：真实 YOLO 后端输出 bbox
bbox_xyxy = detector.detect(rgb, text_or_class="coffee_mug")
# SAM / SAM2 或当前 fallback 接口把 bbox 转成 mask
segmentation = segmenter.segment(rgb, box_xyxy=bbox_xyxy)
mask = segmentation.mask
对应到仓库代码，交互式入口是 interactive_depth_pipeline.py。它做了三层封装：
create_depth_source()：统一 file、usb、network 三种输入源，使后续代码只面对 RGB-D 帧。InteractiveMaskSelector：用点击点或框选区域生成 prompt，降低课堂实验对检测模型权重的依赖。Sam2Segmenter.segment()：把 prompt 转成二值 mask。当前实现包含 fallback 逻辑，点提示生成圆形 mask，框提示生成矩形 mask；安装真实 SAM2 后，可以在同一类中补齐 predictor 调用。
这一层的关键不是模型名字，而是输出契约：后续点云模块只需要一个与深度图同尺寸的二值 mask。因此，只要 YOLO、Grounding DINO、SAM、人工点击或数据集标注最终能产出同样格式的 mask，就可以接入同一条点云流水线。
调试这一层时，不要只看模型置信度。更可靠的检查是：mask 尺寸是否与深度图一致，前景像素比例是否合理，mask overlay 是否漏掉目标关键部位，是否把桌面、遮挡物或邻近物体一起选进来。前端 mask 的错误会直接变成点云污染，后端滤波通常无法完全补救。
7.2.2 点云处理模块：滤波预处理与目标点云提取
点云主入口是 pipeline_from_syllabus.py。它对应课堂 5 步法：
load_depth()/load_mask()/load_intrinsics()：加载深度图、mask 和相机内参，并检查尺寸、字段和深度尺度。step1_mask_filtering()：执行depth × mask，把非目标像素置零，保存syllabus_masked_depth.png。step2_backprojection()：用 Open3Dcreate_from_depth_image()根据内参反投影为相机坐标系下的目标点云。step3_passthrough()：用 numpy 布尔切片按 Z 范围做直通滤波，去掉工作空间外点。step4_denoise_and_downsample()：先做统计离群点滤波，再做体素降采样。step5_export()：导出syllabus_target_clean.pcd，并记录点数和 XYZ 范围。
可以把这个脚本理解为第 6 讲的“最小可复现几何核心”。它的各层输入输出可以整理如下：
depthmask | |||
.pcd |
如果某次结果不对，排查顺序应是：先看 syllabus_masked_depth.png 是否只保留目标，再看原始点云尺度是否合理，再看直通滤波有没有误删目标，最后检查统计滤波和体素降采样是否过强。不要一开始就调最后的抓取候选参数，因为前面任何一步出错，后面的候选都会建立在错误几何上。
7.2.3 位姿生成模块：6D 位姿解算与抓取候选生成
本仓库当前没有强行绑定某个 6D 位姿网络，因为不同方法输入条件差异很大：ICP 需要 CAD 和初值，SAM-6D 需要 RGB-D、内参和 CAD，FoundationPose 需要 CAD 或少量 RGB-D 参考图像，直接抓取检测则可能跳过 object pose。课程基础作业可以使用一个几何近似版本理解数据流：
pcd = o3d.io.read_point_cloud("data/rgbd_object_demo/output/syllabus_target_clean.pcd")
obb = pcd.get_oriented_bounding_box()
center = obb.center
R_obj = obb.R
# 简化 object pose：用 OBB 中心和主轴构造候选物体坐标系
T_cam_object = np.eye(4)
T_cam_object[:3, :3] = R_obj
T_cam_object[:3, 3] = center
这个 OBB 结果不能等价于严肃的 6D object pose benchmark。它没有 CAD 语义约束，主轴方向可能存在正负号翻转，对圆柱、盒体、对称物体也可能给出多个等价姿态。但它足以说明“点云几何如何变成一个局部坐标系”。随后可以定义保守顶部抓取候选：
T_object_grasp = np.eye(4)
T_object_grasp[:3, 3] = [0.0, 0.0, z_offset]
T_cam_grasp = T_cam_object @ T_object_grasp
抓取候选建议不要只保存一个 4×4 矩阵，而应至少包含以下字段：
{
"grasp_pose": "4x4 homogeneous matrix",
"frame_id": "camera or base",
"approach": "unit vector",
"width": "gripper opening in meters",
"pre_grasp_distance": "offset before closing",
"score": "heuristic or model confidence",
"checks": ["workspace", "ik", "collision", "width_limit"]
}
如果是 Eye-to-Hand，相机固定在外部，抓取目标转换为：
如果是 Eye-in-Hand，相机固定在末端，转换链路通常写成：
其中 是感知模块输出的相机系物体位姿， 是在物体坐标系中定义的抓取模板， 或 来自手眼标定， 才是提交给运动规划器的抓取目标。由此可见，6D 位姿不是链路终点，而是连接视觉几何与抓取动作的中间状态。
本节小结： 代码解析要沿数据流读，而不是按文件名背功能。检测分割层输出 mask，点云层输出 clean point cloud，位姿层输出 object frame 或 grasp candidates，手眼层负责把相机系结果变到机器人基座系。每一层都必须留下可检查的中间文件，否则全链路失败时很难定位责任环节。
7.3 可视化输出规范：多维度结果的 Open3D 展示
位姿估计和抓取实验不能只交终端日志。机器人系统的错误常常来自坐标方向、尺度和遮挡，这些问题必须靠图像、点云和坐标轴可视化发现。
7.3.1 前端感知结果可视化：边界框与实例掩膜
前端结果至少应包含三张图：原始 RGB、bbox 或交互提示位置、mask overlay。mask overlay 应使用半透明颜色叠加在 RGB 上，让读者能看出是否混入桌面、邻物或背景。若使用 YOLO / Grounding DINO，还应保存类别、置信度、bbox 坐标和阈值；若使用人工 mask 或数据集 mask，也要说明来源。
7.3.2 几何结果可视化：点云与物体 6D 位姿
几何结果至少应包含 raw point cloud、passthrough point cloud、clean point cloud 三个阶段。Open3D 可视化时建议统一坐标轴颜色：X 红、Y 绿、Z 蓝；若绘制物体坐标系或 OBB，应在图注中说明它是 CAD / ICP / OBB / 学习模型中的哪一种来源。仅凭一张最终点云截图不足以说明流程正确，必须同时报告点数变化、XYZ 范围和滤波参数。
7.3.3 抓取结果可视化：夹爪模型与接近路径
抓取结果可视化应至少画出三个元素：grasp_pose 的 TCP 坐标系、pre-grasp 到 grasp 的接近线段、夹爪开口方向。若没有真实夹爪网格，可以用两条平行线段表示手指，用一条箭头表示接近方向。图中应标注该抓取候选是否已经过 IK、碰撞和夹爪行程检查；如果只是几何候选，应明确写为“未执行候选”，避免读者误以为已经能直接发给机器人。
建议报告中的可视化材料按下面方式组织：
本节小结： 可视化不是装饰，而是机器人感知实验的证据链。前端看 mask 是否选对目标，几何看点云是否干净和尺度是否合理，抓取看 TCP、接近方向和开口是否符合物理直觉。没有这些可视化，单独报告“成功 / 失败”很难形成可复现的技术结论。
7.4 作业交付标准与评分细则
7.4.1 8 项交付物清单与要求
本讲作业建议采用分层交付。所有同学都应完成基础项；有能力的同学可以继续提交进阶项或真机项。
具体交付物按 8 项组织：
.pcd、阶段截图、点数变化表、XYZ 范围 | ||
grasp_poseframe_id、width、approach、pre_grasp_distance、筛选规则 | ||
7.4.2 评分维度与量化细则
评分不应只看最终是否“抓起来”。建议按以下维度评估：
为了避免评分只看演示效果，可以进一步使用下面的检查标准：
对于无硬件同学，不扣“真机抓取”分，但必须提供更完整的中间结果和解释；对于有硬件同学，不应只交成功视频，也要提交失败样本和坐标链路检查。真正重要的是学生能否说明：输入是什么，经过哪些变换，输出为什么可信，失败时该查哪里。
本节小结： 本讲作业的核心不是堆模型，而是建立一条可复现、可检查、可归因的感知到抓取链路。交付物应覆盖输入、代码、点云、位姿、手眼、抓取候选、可视化和失败分析。评分也应围绕工程证据展开，而不是只看单次演示是否成功。
8 本讲小结
8.1 两条技术主线回顾
8.1.1 模块化感知路线：工程落地的主流范式
模块化路线把抓取问题拆成检测、分割、深度反投影、点云滤波、位姿估计、抓取候选生成、手眼转换和运动规划。它的优势是每一步都有明确输入输出，便于保存中间结果、调试误差和做安全检查。它尤其适合教学实验、工业部署、已知物体和需要严格复现的任务。
但模块化路线也有成本：链路长、接口多、标定要求高，任何一步的坐标方向或单位错误都会传导到后端。因此，模块化不是“简单路线”，而是一种把复杂系统拆成可验证环节的工程方法。
8.1.2 端到端感知路线：前沿发展的重要方向
端到端路线试图从图像、点云或机器人状态直接预测 grasp pose、grasp candidates 或动作分布。它减少了显式中间接口，适合开放场景、复杂形状和大规模数据驱动策略学习。直接 6-DoF 抓取检测、点云抓取候选网络、VLA / 模仿学习策略都属于这条思想的不同形态。
它的挑战是数据成本高、失败归因困难、安全边界不易证明。真实机器人系统常采用混合架构：前端利用大模型或学习方法提高泛化能力，后端仍保留点云几何、手眼标定、IK、碰撞检测和失败检测等硬约束。
本节小结： 模块化路线和端到端路线不是非此即彼。前者提供可解释、可调试、可验证的工程骨架；后者提供更强的泛化和动作生成能力。理解二者的边界，才能在真实系统中做出稳妥组合。
8.2 感知 - 位姿 - 抓取的完整链路梳理
8.2.1 从像素输入到抓取指令的全链路映射
本讲从 RGB-D 输入出发，依次完成了以下映射：
RGB-D + intrinsics
-> bbox / mask
-> target depth
-> target point cloud
-> object pose 或几何候选坐标系
-> grasp candidate
-> base-frame grasp target
-> IK / collision / execution check
每个箭头都对应一个可能出错的工程环节：mask 会影响点云纯净度，深度尺度会影响三维尺寸，点云滤波会影响几何结构，位姿估计会影响夹爪姿态，手眼标定会影响基座系目标，运动规划会决定候选是否真的可执行。
8.2.2 机器人感知的核心目标：生成可执行操作指令
机器人感知的最终目标不是在图像上画框，也不是单独输出一个漂亮的 6D 位姿矩阵，而是为机器人生成可执行、可验证、可回退的操作目标。这个目标可以是显式抓取候选，也可以是策略动作，但必须接受坐标、碰撞、可达性、安全和任务成功标准的检验。
本节小结： 本讲的主线可以概括为“像素证据进入三维，三维几何进入位姿，位姿和任务约束进入抓取候选，抓取候选再进入机器人控制系统”。掌握这条链路后，读者就能把一次抓取失败拆解为可定位的问题，而不是笼统归因于“模型不准”。
8.3 通向下一讲：从感知结果到抓取动作执行
8.3.1 本讲的能力边界与遗留问题
本讲解决的是“目标在哪里、如何摆放、夹爪候选应如何表达”。它还没有完整展开运动规划、抓取执行状态机、接触检测、力控闭环和失败恢复策略。也就是说，本讲把视觉结果推进到了机器人可以消费的候选目标，但候选目标能否安全执行，还需要下一讲继续处理。
遗留问题包括：如何从多个抓取候选中排序，如何做 IK 和碰撞检测，如何设计 pre-grasp / grasp / lift 状态机，如何在接触失败时重试，如何把一次失败记录成可训练数据。
8.3.2 第 7 讲核心内容预告
下一讲将从“候选抓取”进入“动作执行”：重点讨论末端执行器、抓取技能、运动规划、接近路径、夹爪控制、状态机和执行反馈。第 6 讲输出的 grasp candidate 会成为第 7 讲的输入，后者负责把它变成真实机器人上的动作序列。
本节小结： 第 6 讲把视觉世界和机器人控制世界接起来，但只走到了“可执行目标”的门口。第 7 讲继续回答：目标给定以后，机械臂怎样安全接近、闭合、抬升、检测失败并恢复。两讲合起来，才构成从“看见目标”到“完成抓取”的完整操作闭环。
参考资料
[1] HE K, GKIOXARI G, DOLLAR P, et al. Mask R-CNN[EB/OL]. (2017-03-20)[2026-07-26]. arxiv.org/abs/1703.06870.
[2] BOLYA D, ZHOU C, XIAO F, et al. YOLACT: real-time instance segmentation[EB/OL]. (2019-04-04)[2026-07-26]. arxiv.org/abs/1904.02689.
[3] RAVI N, GABEUR V, HU Y T, et al. SAM 2: segment anything in images and videos[EB/OL]. (2024-08-01)[2026-07-26]. arxiv.org/abs/2408.00714.
[4] Open3D. Point cloud outlier removal[EB/OL]. [2026-07-26]. open3d.org/docs/release/tutorial/geometry/pointcloud_outlier_removal.html.
[5] Stanford Computer Graphics Laboratory. The Stanford 3D Scanning Repository: Stanford Bunny[EB/OL]. [2026-07-26]. graphics.stanford.edu/data/3Dscanrep/.
[6] MoveIt. Pick and Place[EB/OL]. [2026-07-26]. moveit.picknik.ai/main/doc/examples/pick_place/pick_place_tutorial.html.
[7] ROS. moveit_msgs/Grasp Message[EB/OL]. [2026-07-26]. docs.ros.org/lunar/api/moveit_msgs/html/msg/Grasp.html.
[8] OpenCV. Camera Calibration and 3D Reconstruction: calibrateHandEye[EB/OL]. [2026-07-26]. docs.opencv.org/master/d9/d0c/group__calib3d.html.
[9] WEN B, YANG W, KAUTZ J, et al. FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects[EB/OL]. (2024)[2026-07-26]. nvlabs.github.io/FoundationPose/.
[10] LIN J, LIU L, LU D, et al. SAM-6D: Segment Anything Model Meets Zero-Shot 6D Object Pose Estimation[EB/OL]. (2023-11-27)[2026-07-26]. arxiv.org/abs/2311.15707.
[11] MOUSAVIAN A, EPPNER C, FOX D. 6-DOF GraspNet: Variational Grasp Generation for Object Manipulation[EB/OL]. (2019)[2026-07-26]. research.nvidia.com/labs/srl/publication/mousavian-20196-dof/.
往期回顾
-END-
点击下方卡片，关注【Xbotics具身智能实验室】公众号
你想要的这里都有~~
Ask Me Anything｜提问箱
对文章有疑惑，或想聊更深？欢迎把你的问题丢给我们：技术方案、实操踩坑、课程与资料、项目合作、职业发展，都可以问。
怎么问：在评论区留言，或私信公众号
我们会做什么：每周集中整理高质量问题并公开回复，重点问题邀请作者或嘉宾深度解答；典型问题会加入知识库并持续更新。
提问小提示：尽量说明「你的目标—当前做法—期望产出」，附上必要信息（硬件/软件版本、数据规模等），能更快获得有用答案。
一起把问题变成知识，推动社区进步 🚀
