Fast Continuum Robot Shape and External Load State Estimation on SE(3)

TL;DR

The paper tackles estimating backbone shape and external loads for continuum robots under actuation by tendons, framed on SE(3) with uncertainty. It introduces a probabilistic Cosserat rod model discretized along arclength and cast as a sparse factor-graph, enabling spacetime state estimation via MAP with temporal priors. Demonstrations include real-time tendon-robot simulations for forward kinematics with uncertainty, tip-force sensing, and distributed-load estimation, plus experimental validation on a surgical concentric-tube robot showing accurate kinematics and tip-force estimation with potential for palpation-based tasks. The approach yields actionable Jacobians for resolved-rate control and provides principled uncertainty quantification, broadening the applicability of continuum robots in medical and manipulation tasks.

Abstract

Previous on-manifold approaches to continuum robot state estimation have typically adopted simplified Cosserat rod models, which cannot directly account for actuation inputs or external loads. We introduce a general framework that incorporates uncertainty models for actuation (e.g., tendon tensions), applied forces and moments, process noise, boundary conditions, and arbitrary backbone measurements. By adding temporal priors across time steps, our method additionally performs joint estimation in both the spatial (arclength) and temporal domains, enabling full \textit{spacetime} state estimation. Discretizing the arclength domain yields a factor graph representation of the continuum robot model, which can be exploited for fast batch sparse nonlinear optimization in the style of SLAM. The framework is general and applies to a broad class of continuum robots; as illustrative cases, we show (i) tendon-driven robots in simulation, where we demonstrate real-time kinematics with uncertainty, tip force sensing from position feedback, and distributed load estimation from backbone strain, and (ii) a surgical concentric tube robot in experiment, where we validate accurate kinematics and tip force estimation, highlighting potential for surgical palpation.

Figures (10)

• Figure 1: Conceptual illustration of our approach applied to a surgical robot. Our method fuses uncertain actuation inputs and external backbone measurements with a prior mechanics model to estimate shape and external loads. Our factor-graph formulation enables principled, real-time estimation of all states and uncertainties.
• Figure 2: Factor graph representation of the discrete Cosserat rod model with tendon actuation, assuming only one node between each disc for clarity. Each of the $K$ nodes along the rod includes pose $\mathbf{T}_k$, internal stress $\mathbf{S}_k$, and external wrench $\mathbf{F}_k$. Given actuation tensions $\mathbf{Q}$, the graph models forward kinematics with uncertainty. The inverse problem—estimating $\mathbf{F}_k$—is solved by attaching measurement factors to backbone states (e.g., $\mathbf{T}_k$, $\mathbf{S}_k$). External load $\mathbf{F}_k$ priors are chosen based on sensing application (e.g., tip forces only), and we also include temporal priors to smooth over time, as well as spatial priors along the rod for distributed loads.
• Figure 3: Example snapshots from our tendon robot simulations. Left: Forward kinematics with uncertainty. The red ellipses illustrate the position components of pose uncertainty ($2-\sigma$) along the backbone, subsampled at the disc locations. Middle: Prior distribution, given an unknown (zero mean with high prior uncertainty) tip force. Right: Posterior tip force distribution, given a noisy tip position measurement (red point). The violet and green arrows indicate the MAP estimate and ground truth forces with force uncertainty (gold). Additionally, the robot tracks the red point throughout our simulation, in spite of the uncertain tip force.
• Figure 4: Simulation results showing good trajectory tracking while also estimating unknown tip forces in real time (26 ms mean solve time for 900 simulation steps), given noisy tip position feedback. The shaded regions show 2-$\sigma$ uncertainty envelopes, and the dotted lines indicate ground truth. For comparison, we additionally show an open-loop (OL) trajectory, with no position feedback.
• Figure 5: Numerical tip position accuracy of forward kinematics with our approach, compared to a numerical integration benchmark, evaluated over a simulated trajectory. As the number of discretization nodes increases, the MAP tip position approaches the benchmark, and using a midpoint method substantially improves performance.