SURESTEP: An Uncertainty-Aware Trajectory Optimization Framework to Enhance Visual Tool Tracking for Robust Surgical Automation

TL;DR

SURESTEP addresses the challenge of robust surgical automation under perception and kinematic uncertainty by propagating motion and observation noise in a Gaussian EKF belief space and explicitly minimizing the uncertainty at the task horizon. By modeling depth-, FOV-, and orientation-based observation noise and optimizing trajectories to minimize the final covariance trace $\text{Tr}(\boldsymbol{\Sigma}_{T|T})$ (while enforcing $\mathbb{E}[\boldsymbol{x}_T]=\boldsymbol{x}^G$ and adding a pose-loss), it yields trajectories that enhance tool visibility and pose estimation reliability. Empirical results on real dVRK tasks show substantial performance gains: an $82\%$ success rate for needle regrasps versus $18\%$ for baselines, and favorable simulation ablations across multiple uncertainty components. The framework demonstrates the value of uncertainty-aware planning in surgical automation and offers avenues to extend to camera-motion optimization and differentiable collision checking, potentially broadening applicability and robustness in real-world settings.

Abstract

Inaccurate tool localization is one of the main reasons for failures in automating surgical tasks. Imprecise robot kinematics and noisy observations caused by the poor visual acuity of an endoscopic camera make tool tracking challenging. Previous works in surgical automation adopt environment-specific setups or hard-coded strategies instead of explicitly considering motion and observation uncertainty of tool tracking in their policies. In this work, we present SURESTEP, an uncertainty-aware trajectory optimization framework for robust surgical automation. We model the uncertainty of tool tracking with the components motivated by the sources of noise in typical surgical scenes. Using a Gaussian assumption to propagate our uncertainty models through a given tool trajectory, SURESTEP provides a general framework that minimizes the upper bound on the entropy of the final estimated tool distribution. We compare SURESTEP with a baseline method on a real-world suture needle regrasping task under challenging environmental conditions, such as poor lighting and a moving endoscopic camera. The results over 60 regrasps on the da Vinci Research Kit (dVRK) demonstrate that our optimized trajectories significantly outperform the un-optimized baseline.

Figures (5)

• Figure 1: Visualization of the baseline's (top) and SURESTEP's (bottom) trajectories on the dVRK. The first patient side manipulator (PSM 1) moves along a trajectory (red-dot arrows) to regrasp the needle held in PSM 2. The PSMs and the needle are tracked using an endoscopic camera manipulator (ECM). In our experiments, the ECM also moves along a given trajectory (green-dot arrows), adding extra noise to tool tracking. The baseline trajectory fails to regrasp the needle due to significant tracking noise. SURESTEP considers motion and observation uncertainty during trajectory optimization, improving tool tracking and achieving successful regrasps.
• Figure 2: Tracking results on the surgical manipulators and suture needles from an endoscopic camera using richter2021roboticchiu2022markerlesschiu2023real. The green curves show the tracked tool poses. Each column highlights a source of observational uncertainty that impacts tool tracking.
• Figure 3: Trajectories before and after optimization through SURESTEP. The optimized trajectory demonstrates the expected behavior of a tool moving closer to the camera and the center of FOV to reduce tracking uncertainty before returning to the desired goal pose.
• Figure 4: First-person view of a Type 2 trajectory, in which the needle arm moves to the regrasping arm. Here, real-time needle tracking is required to perform a regrasp. Note that dim lighting makes the visibility conditions challenging, so needle segmentation often fails. The baseline trajectory cannot recover from inaccurate needle pose estimation, leading to a failed regrasp. SURESTEP's trajectory considers observational uncertainty and re-orients the needle for better pose estimation, hence succeeding in regrasping.
• Figure 5: First-person view of a Type 4 trajectory, in which the ECM moves in the middle of the regrasping arm's trajectory. The top row depicts each camera movement, from the frame with the green icon to the one with the red icon. The baseline trajectory fails to regrasp since compounding noise from the ECM's motion and other sources leads to inaccurate tool tracking. SURESTEP's trajectory considers this uncertainty and is re-optimized after each ECM's movement, leading to better tracking and successful needle regrasps.