Continuum Robot State Estimation with Actuation Uncertainty

摘要

Continuum robots are flexible, thin manipulators capable of navigating confined or delicate environments making them well suited for surgical applications. Previous approaches to continuum robot state estimation typically rely on simplified, deterministic actuation models. In contrast, our method jointly estimates robot shape, external loads, internal stresses, and actuation inputs. We adopt a discrete Cosserat rod formulation and show that, when paired with a midpoint integration rule, it achieves high numerical accuracy with relatively few state nodes. This discretization naturally induces a factor-graph structure for sparse nonlinear optimization on SE(3). We extend the formulation with actuation factors for tendon-driven robots and combine multiple rod graphs for parallel continuum robots with closed-loop topologies. By explicitly including actuation variables in the state, the linearized system can be reused to extract manipulator Jacobians, which we leverage in performing trajectory tracking. Finally, we validate the approach experimentally on a surgical concentric tube robot. Overall, our approach enables principled, real-time estimation across multiple continuum robot architectures, accounting for actuation uncertainty and providing direct access to manipulator Jacobians.