Robotic Grasp Detection (10): 多模态感知与闭环抓取优化 • Hana's Blog

为什么单次视觉预测不够（Why One-Shot Vision Is Insufficient）#

开环抓取（Open-Loop Grasping）在运动前预测一次姿态，随后假设物体、相机和机器人状态不再变化。真实执行中，深度噪声、手眼标定误差、物体移动、接触偏差和滑移会在预测后继续累积。

闭环抓取（Closed-Loop Grasping）根据连续观测更新状态、抓取和控制动作。设平行夹爪抓取为 $g_t=(R_t,\vec t_t,w_t)$ ，其中 $R_t\in SO(3)$ 、 $\vec t_t\in\mathbb R^3$ 、 $w_t\in\mathbb R_{\geq0}$ ，闭环策略可以写成：

(g_{t+1},a_t)=\pi(o_{0:t},a_{0:t-1},f_{0:t}),

其中 $o_{0:t}$ 是视觉与触觉观测历史， $a_{0:t-1}$ 是已执行动作， $f_{0:t}$ 是力与接触反馈， $a_t$ 是下一段短时控制动作。不同模态在不同时间点有效：视觉用于接触前定位，触觉和力觉在接触后观测接触状态，滑移信号在抬升和搬运阶段更新稳定性。

闭环系统可以维护隐藏状态 $x_t\in\mathbb R^{d_x}$ ，例如物体位姿、接触位置、滑移状态和夹紧力。贝叶斯滤波（Bayesian Filtering）写成：

\underbrace{p(x_t\mid o_{0:t},a_{0:t-1})}_{b_t(x_t)} \propto p(o_t\mid x_t) \int p(x_t\mid x_{t-1},a_{t-1})b_{t-1}(x_{t-1})\,dx_{t-1}.

其中 $b_t(x_t)$ 是时刻 $t$ 的状态信念， $p(x_t\mid x_{t-1},a_{t-1})$ 是动作后的状态转移， $p(o_t\mid x_t)$ 是多模态观测模型。控制器根据 $b_t$ 选择短时动作，并在下一观测到来后更新；工程实现可使用卡尔曼滤波（Kalman Filter）、粒子滤波（Particle Filter）或神经状态编码器（Neural State Encoder）。

多模态闭环的状态接口（State Interfaces in the Closed Loop）#

执行阶段	主要传感器	状态估计	控制输出
接触前（Pre-Contact）	RGB-D、点云、关节编码器	物体与夹爪相对位姿	候选更新、接近速度
接近中（Approach）	连续视觉、机器人位姿	目标运动与标定残差	末端位姿修正、停止
闭合时（Closure）	指尖触觉、夹爪电流、力	左右接触位置与接触强度	继续闭合、平移或旋转调整
抬升中（Lift）	触觉时间序列、腕部力/力矩、视觉	滑移、质心偏移与稳定性	增力、减速、放回或重抓
任务执行（Task Execution）	多模态状态与任务反馈	物体姿态和任务进度	搬运、放置与恢复

视觉闭环与时间一致性（Visual Closed Loop and Temporal Consistency）#

闭环不一定需要触觉。低延迟视觉检测器可以在机械臂接近时反复修正目标。Grasping in the Wild 从低成本示范学习六自由度（Six Degrees of Freedom, 6-DoF）闭环抓取 [1]；Real-Time Generative Grasping 使用时空稀疏卷积（Spatio-Temporal Sparse Convolution）融合连续深度帧 [2]；AnyGrasp 则关注空间和时间域中的鲁棒抓取感知 [3]。

时间融合（Temporal Fusion）可以：

平滑单帧姿态抖动；
累积不同视角看到的表面；
跟踪移动物体和候选；
删除与当前场景不一致的旧抓取。

融合前先把不同帧注册到同一坐标系。若第 $t$ 帧中的点 $\vec p_C^{\,t}\in\mathbb R^3$ 位于相机坐标系，机器人状态提供相机到机器人基座的齐次变换 ${}^{B}T_{C_t}\in SE(3)$ ，则：

\begin{bmatrix}\vec p_B^{\,t}\\1\end{bmatrix} ={}^{B}T_{C_t} \begin{bmatrix}\vec p_C^{\,t}\\1\end{bmatrix}.

随后使用体素哈希（Voxel Hashing）、迭代最近点（Iterative Closest Point, ICP）或对象跟踪关联新旧表面。候选也随被跟踪物体更新：若物体位姿从 ${}^{B}T_O^{t-1}$ 变化到 ${}^{B}T_O^t$ ，则旧抓取齐次变换 ${}^{B}T_G^{t-1}$ 更新为：

{}^{B}T_G^t ={}^{B}T_O^t \left({}^{B}T_O^{t-1}\right)^{-1} {}^{B}T_G^{t-1}.

平移可在 $\mathbb R^3$ 中加权，旋转则用四元数平均或李代数增量进行时间平滑。

时间窗口长度决定平滑程度与时延（Latency）。实现中可按时间戳衰减旧帧权重，并在估计残差超过阈值时丢弃过期状态（Stale State）。

接触前的不确定性处理（Pre-Contact Uncertainty）#

夹爪位姿误差会让解析高分抓取在执行时错过接触面。Johns 等人学习稠密抓取函数，再与夹爪位姿不确定性卷积 [4]：

\tilde Q(g)=\int Q(g+\delta)\,p(\delta)\,d\delta.

其中位姿扰动 $\delta\in\mathbb R^6$ 表示局部平移和旋转误差， $p(\delta)$ 是标定与控制误差模型。卷积后的 $\tilde Q(g)$ 偏向邻域内都保持较高质量的候选，用于接触前排序。

vMF-Contact 使用 von Mises–Fisher 分布（von Mises–Fisher Distribution, vMF）和证据学习（Evidential Learning）表示噪声杂乱场景中的接触方向不确定性 [9]。概率表示的价值在于让候选分数随观测质量变化，而不是对所有姿态输出同样尖锐的置信度。

对单位方向 $n\in\mathbb S^2$ ，vMF 密度为：

p(n\mid\mu,\kappa)=C_3(\kappa) \exp(\kappa\mu^{\mathsf T}n),

其中 $n,\mu\in\mathbb R^3$ 是单位向量， $\kappa\in\mathbb R_{\geq0}$ 是集中参数， $C_3(\kappa)$ 是三维归一化常数； $\kappa\to0$ 时接近球面均匀分布， $\kappa$ 增大时方向集中在 $\mu$ 附近。解码时可以从分布采样多个接触方向，或用 $\kappa$ 调整候选排序。

视觉—触觉重新抓取（Visuo-Tactile Regrasping）#

触觉感知（Tactile Sensing）提供指尖压力分布、接触位置和局部纹理。More Than a Feeling 将视觉与触觉共同用于初始抓取和重新抓取（Regrasp）[5]。

视觉负责生成接触前候选和检查远距离碰撞，触觉负责在闭合后测量局部接触。二者通过当前夹爪姿态和指尖侧别关联到同一抓取状态。

视觉—触觉重新抓取通常包含两个输出头：稳定性分类器估计当前接触是否足以抬升，调整策略预测平移、旋转或重新张开夹爪的动作。原始触觉图先做空载基线校正，再计算压力或形变差分；左右指尖图像保留夹爪侧别。一个简化决策为：

a_t= \begin{cases} \text{lift}, & p_{\text{stable}}>\eta,\\ \Delta g_t, & p_{\text{stable}}\leq\eta \text{ 且调整在安全范围内},\\ \text{release and replan}, & \text{otherwise}. \end{cases}

稳定性头使用带成功/失败标签的静态触觉样本；调整头使用“调整前触觉、所执行调整、调整后结果”三元组学习动作对接触状态的影响。

Algorithm 1 视觉—触觉闭环重新抓取

输入:

连续 RGB-D、左右指尖触觉、腕部力/力矩、机器人位姿和平行夹爪状态

输出:

稳定抓取，或安全终止状态

require 由当前视觉观测生成初始抓取，并规划接近轨迹
for 在给定重新抓取预算内循环
执行短时接近，根据新视觉帧更新目标和抓取姿态
闭合夹爪，校正左右触觉基线并提取接触特征
预测稳定概率、滑移风险和局部姿态调整 $\Delta g_t$
若稳定概率超过阈值，则抬升并进入滑移监测
若调整在安全范围内，则释放部分夹紧力并执行 $\Delta g_t$
否则放回物体，并由更新后的场景重新检测抓取
end for 直到稳定持有或耗尽尝试预算
return 稳定抓取或安全终止

力、力矩与质心（Force, Torque, and Center of Mass）#

腕部力/力矩传感器（Wrist Force/Torque Sensor）和触觉可以估计物体受力及质心偏移。Center-of-Mass-Based Robust Grasp Planning 使用触觉—视觉传感器调整未知物体抓取 [6]。

设质心和抓取参考点分别为 $\vec p_{\text{CoM}},\vec p_{\text{grasp}}\in\mathbb R^3$ ，物体质量 $m\in\mathbb R_{>0}$ ，重力加速度 $\vec g\in\mathbb R^3$ 。夹持点远离质心（Center of Mass, CoM）时会产生力矩：

\vec\tau =(\vec p_{\text{CoM}}-\vec p_{\text{grasp}}) \times m\vec g, \qquad \vec\tau\in\mathbb R^3.

系统以抬升后的力矩作为重新抓取反馈，使夹持点逐步接近质心；执行协议记录每次调整后的状态变化。

在准静态悬持、已知重力方向的条件下，腕部测得的力 $\vec F$ 与力矩 $\vec\tau$ 满足：

\vec F\approx m\vec g,\qquad \vec\tau\approx \vec r_{\text{CoM}}\times\vec F+\vec\tau_0,

其中 $\vec F,\vec\tau\in\mathbb R^3$ 是腕部测得的力和力矩， $\vec r_{\text{CoM}}\in\mathbb R^3$ 是质心相对传感器原点的位置， $\vec\tau_0\in\mathbb R^3$ 包含传感器偏置、夹爪自重和安装误差。先用空夹爪标定 $\vec\tau_0$ ，再在一个或多个手腕方向上测量，可以解出质心在垂直于 $\vec F$ 平面内的分量；运动阶段则加入加速度补偿。

滑移检测与稳定控制（Slip Detection and Stable Control）#

滑移（Slip）是接触面相对运动，可能由夹紧力不足、质心偏移、材料摩擦变化或机器人加速度引起。滑移检测可以基于：

触觉图像变化；
高频振动与剪切力；
腕部力/力矩；
视觉中的物体相对运动。

检测到滑移后，控制器可以增加夹紧力、改变手腕运动、放慢加速度或重新抓取。稳定性评价（Stability Evaluation）分别记录首次抬升和搬运过程中的持续持有。

滑移检测一般使用时间窗而不是单帧阈值。设触觉或剪切特征为 $z_t$ ，可以比较短窗变化：

s_t=\frac{1}{L}\sum_{k=0}^{L-1} \lVert z_{t-k}-z_{t-k-1}\rVert,

其中 $z_t\in\mathbb R^{d_z}$ 是触觉或剪切特征， $L$ 是时间窗长度， $s_t\in\mathbb R_{\geq0}$ 是滑移分数。也可以用一维卷积或循环网络区分接触建立、正常形变和持续滑移。夹紧力控制采用带上限的增量律：

F_{t+1} =\operatorname{clip}(F_t+K_s s_t,F_{\min},F_{\max}),

其中 $F_t$ 是夹紧力， $K_s\geq0$ 是控制增益， $F_{\min}$ 和 $F_{\max}$ 由夹爪与物体材料共同确定。

刚度与形变感知（Stiffness and Deformation Awareness）#

柔性物体的几何会随夹持力改变。Deformation-Aware Grasp Synthesis 将刚度和形变加入抓取选择 [7]。对软物体，质量函数可能同时包含稳定性和形变惩罚：

Q(g)=Q_{\text{hold}}(g)-\lambda D_{\text{deform}}(g).

$Q_{\text{hold}}$ 评价持有稳定性， $D_{\text{deform}}$ 评价预测形变或损伤， $\lambda\geq0$ 控制二者权衡。视觉形状和材料先验用于初始候选，触觉与力反馈在闭合后更新形变估计。

少样本物理学习与示范（Sample-Efficient Physical Learning and Demonstration）#

真实抓取试验昂贵。Sample Efficient Grasp Learning 使用等变模型（Equivariant Model）复用旋转和平移对称性，提高有限物理数据的利用率 [8]。自监督增强现实遥操作（Self-Supervised Augmented-Reality Teleoperation）则使用人类示范生成六自由度抓取训练信号 [10]。

示范数据同时记录操作者选择、机器人平台和相机设置；自监督执行数据还记录成功检测、规划过滤和硬件保护触发状态，便于分析训练样本是如何产生的。

多模态融合发生在哪里（Where Multimodal Fusion Happens）#

阶段	可用信息	典型决策
接触前（Pre-Contact）	红绿蓝—深度（Red–Green–Blue and Depth, RGB-D）、点云、语言、机器人状态	目标、姿态、接近轨迹
接近中（During Approach）	连续视觉、位姿反馈	修正姿态、停止或重新规划
闭合时（At Closure）	触觉、夹爪电流、力	判断是否接触、调整夹紧力
抬升中（During Lift）	滑移、力矩、视觉跟踪	增力、减速、放回或重抓
任务中（During Task）	物体状态、语言、放置约束	保持、重新定向、恢复失败

早期融合（Early Fusion）把多模态输入送入同一网络，便于学习联合特征，但要求同步和大量配对数据；后期融合（Late Fusion）让各模态独立估计，再在质量或控制层合并，模块更清晰但可能丢失细粒度相关性。

早期融合可以写成 $h=F([e_v,e_t,e_f])$ ，其中视觉、触觉和力特征在编码后拼接；后期融合则先产生各自证据 $q_m$ ，再按可靠度组合：

q=\frac{\sum_m w_m q_m}{\sum_m w_m}, \qquad w_m\propto\frac{1}{\sigma_m^2+\epsilon}.

其中 $q_m\in\mathbb R$ 是第 $m$ 个模态给出的质量证据， $\sigma_m^2\in\mathbb R_{>0}$ 是对应不确定性， $\epsilon>0$ 防止除零。深度大面积缺失时降低视觉权重；触觉尚未接触时用可用性掩码关闭触觉分支。异步传感器为每个特征保留采样时间，并将机器人状态插值到同一时刻。

闭环系统应如何评价（Evaluating Closed-Loop Systems）#

闭环评测同时记录首次尝试、反馈调整和最终结果：

首次成功率（First-Attempt Success）；
给定尝试预算下的累计成功率（Cumulative Success）；
平均重新抓取次数（Average Regrasp Count）；
达成稳定所需时间（Time to Stable Grasp）；
滑移率和恢复率（Slip and Recovery Rate）；
接触力、物体损伤和安全终止；
传感器与控制循环频率。

这些指标把检测精度、反馈收益、循环速度和安全代价连接到同一个执行协议中。

参考文献（References）#

S. Song, A. Zeng, J. Lee, and T. Funkhouser, “Grasping in the Wild: Learning 6DoF Closed-Loop Grasping from Low-Cost Demonstrations,” IEEE Robotics and Automation Letters, 2020. DOI ↗
T. R. Player, D. Chang, F. Li, and G. A. Hollinger, “Real-Time Generative Grasping with Spatio-Temporal Sparse Convolution,” 2023 IEEE International Conference on Robotics and Automation, 2023. DOI ↗
H.-S. Fang et al., “AnyGrasp: Robust and Efficient Grasp Perception in Spatial and Temporal Domains,” IEEE Transactions on Robotics, 2023. DOI ↗
E. Johns, S. Leutenegger, and A. J. Davison, “Deep Learning a Grasp Function for Grasping under Gripper Pose Uncertainty,” 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2016. DOI ↗
R. Calandra et al., “More Than a Feeling: Learning to Grasp and Regrasp Using Vision and Touch,” IEEE Robotics and Automation Letters, 2018. DOI ↗
Q. Feng et al., “Center-of-Mass-Based Robust Grasp Planning for Unknown Objects Using Tactile-Visual Sensors,” 2020 IEEE International Conference on Robotics and Automation, 2020. DOI ↗
T. N. Le, J. Lundell, F. J. Abu-Dakka, and V. Kyrki, “Deformation-Aware Data-Driven Grasp Synthesis,” IEEE Robotics and Automation Letters, 2022. DOI ↗
X. Zhu et al., “Sample Efficient Grasp Learning Using Equivariant Models,” Robotics: Science and Systems XVIII, 2022. DOI ↗
Y. Shi, E. Welte, M. Gilles, and R. Rayyes, “vMF-Contact: Uncertainty-Aware Evidential Learning for Probabilistic Contact-Grasp in Noisy Clutter,” 2025 IEEE International Conference on Robotics and Automation, 2025. DOI ↗
X. Dengxiong, X. Wang, S. Bai, and Y. Zhang, “Self-Supervised 6-DoF Robot Grasping by Demonstration via Augmented Reality Teleoperation System,” 2024 IEEE International Conference on Robotics and Automation, 2024. DOI ↗