PinPoint：基于 Stein Variational Newton 与几何残差的单目机器人缝合针姿态估计

1. 研究背景与问题定义

背景：
机器人辅助微创手术（RMIS）中的缝合是一项认知负荷高且要求极高的任务。现有的自主缝合研究主要集中在具有立体视觉（Stereo-vision）的腹腔镜环境中，立体深度信息使得针的姿态估计问题易于求解。然而，许多重要的临床程序（如经内镜和管腔内手术）受限于狭窄的解剖通道，仅能提供单目视觉（Monocular vision）。

核心挑战：
在单目设置下，针的姿态估计是一个**病态（Ill-posed）**问题，主要面临以下困难：

1. 深度模糊性：缺乏深度线索，导致多个 3D 构型可能产生相同的图像投影。
2. 多模态分布（Multimodal Distribution）：由于针的旋转对称性和部分遮挡，可行的姿态解往往不是单一的，而是呈现多峰分布。
3. 现有方法的局限：传统的粒子滤波（Particle Filter）在强几何约束下容易发生权重坍缩（Weight Collapse），导致过早收敛到单一模式，丢失了真实的模糊性信息；而基于学习的方法通常缺乏对机器人运动学和抓取约束的利用。

2. 方法论：PinPoint 框架

本文提出了 PinPoint，一种用于实时单目缝合针姿态估计的概率变分推断框架。其核心思想是直接处理模糊性，通过维护姿态假设的分布（而非单一估计）来保留后验概率的多模态结构。

2.1 观测模型与几何残差

PinPoint 结合了单目图像观测与机器人抓取约束，构建了负对数似然（NLL）目标函数 $L(T)$，包含三个部分：

1. 稀疏关键点似然 ($L_{sparse}$)：利用学习到的针尖和针尾 2D 关键点检测，通过重投影误差约束针的姿态。
2. 密集骨架分割似然 ($L_{dense}$)：利用针背部的密集分割掩码，通过计算图像圆锥曲线（Conic）与分割区域的 Sampson 距离，提供全局形状约束，即使在部分遮挡下也有效。
3. 机器人运动学似然 ($L_{grasp}$)：利用 C-Lock 夹持器（专为同心管机器人设计）的刚性约束。
   • 位置约束：夹持器将针固定在已知点，提供 3D 位置锚点，解决深度模糊。
   • 方向约束：夹持器强制夹持轴与针切线垂直，减少旋转自由度。
   • 优势：即使在视觉完全遮挡时，这些约束仍能提供 3D 信息。

2.2 推理算法：Stein Variational Newton (SVN)

为了高效地推断多模态后验分布，PinPoint 采用了 Stein Variational Newton (SVN) 方法，而非传统的粒子滤波：

• 粒子传输：使用一组相互作用的粒子来近似后验分布。
• 确定性传输：利用梯度信息将粒子推向高概率区域。
• 核排斥（Kernel Repulsion）：通过核函数保持粒子间的多样性，防止粒子坍缩到单一模式，从而保留多模态结构。
• 二阶预条件（Gauss-Newton Preconditioning）：
  • 这是本文的关键创新。利用解析几何残差的雅可比矩阵构建 Gauss-Newton Hessian 近似。
  • 该 Hessian 矩阵用于预条件粒子传输，使算法能根据局部曲率自适应调整步长。
  • 解决病态问题：针对单目观测中深度和出平面旋转方向不可观测（Hessian 秩亏）的问题，采用特征值截断（Eigenvalue flooring）正则化，既抑制了不可观测方向的过大步长，又保留了不确定性估计。

2.3 不确定性估计

算法不仅输出最大后验概率（MAP）姿态，还通过粒子在切空间中的分布直接量化不确定性（平移和旋转的 95% 分位数及协方差）。

3. 主要贡献

1. 首个针对单目缝合针的多模态推断框架：明确提出并解决了单目视觉下针姿态估计的多模态模糊性问题，避免了传统方法过早收敛到错误模式。
2. 融合几何约束的变分推断：创新性地将机器人抓取约束（C-Lock）作为几何残差引入变分推断，在缺乏立体深度时提供了关键的 3D 锚定。
3. 基于二阶信息的 SVN 实现：推导了闭合形式的 Gauss-Newton Hessian 近似，实现了高效的二阶粒子传输，显著提升了在病态条件下的收敛速度和精度。
4. 鲁棒的不确定性校准：能够准确反映深度模糊带来的不确定性，为下游的自主决策（如是否重新抓取、何时插入）提供风险感知依据。

4. 实验结果

实验在 Virtuoso 内镜系统上进行，包括慢速/正常速度跟踪、诱导旋转序列以及离体（ex vivo）组织缝合任务。

4.1 精度提升

与粒子滤波（PF）基线相比，PinPoint 在正常速度运动下表现优异：

• 平移误差：降低 80%（从 5.04 mm 降至 1.00 mm）。
• 旋转误差：降低 78%（从 63.27° 降至 13.80°）。
• 消融实验：证明了图像观测（关键点 + 分割）与机器人抓取约束的互补性。移除任何一方都会导致性能显著下降，特别是移除机器人约束会导致深度方向误差剧增。

4.2 多模态保持能力

在诱导的大角度出平面旋转序列中（模糊性最严重）：

• 双模态保持率：PinPoint 在 84% 的帧中成功保持了双模态后验分布，而粒子滤波仅为 28.9%。
• 正确性：在双模态情况下，PinPoint 总是将真实姿态（Ground Truth）分配给权重较高的主导模式（100%），而粒子滤波仅能做到 60%。这表明 PinPoint 不仅保留了假设，还能正确评估哪个假设更可信。

4.3 遮挡鲁棒性

在离体组织缝合实验中，针在穿刺过程中会经历部分或完全遮挡：

• 即使在针完全嵌入组织（仅可见针尾）的重度遮挡下，PinPoint 仍能保持稳定跟踪。
• 遮挡期间的平均误差为：平移 1.34 mm，旋转 19.18°。
• 一旦针重新露出，粒子分布能迅速收缩回正确姿态。

5. 意义与展望

科学意义：
PinPoint 证明了在缺乏立体视觉的复杂手术场景中，通过结合概率变分推断与机器人运动学约束，可以有效解决病态几何估计问题。它改变了传统方法试图“消除”模糊性的思路，转而“管理”模糊性，为自主机器人提供了更可靠的风险感知能力。

应用价值：

• 扩大适用范围：使自主缝合技术能够应用于仅支持单目视觉的狭窄管腔手术（如经自然腔道内镜手术 NOTES）。
• 主动感知与决策：利用后验分布的不确定性（如 Hessian 的特征向量），机器人可以主动识别最不确定的方向，并调整针的位置以减少不确定性，从而在关键操作前降低风险。

未来工作：
计划将 PinPoint 集成到不确定性感知的自主缝合规划器中，利用后验分布来指导插入时机、重新抓取请求以及组织接触策略，实现全流程的风险控制。