Factor Graph-Based Shape Estimation for Continuum Robots via Magnus Expansion

A team from Georgia Tech led by Lorenzo Ticozzi, Patricio A. Vela, and Panagiotis Tsiotras has published a breakthrough method for estimating the shape of continuum robots—flexible, snake-like manipulators used in surgery and inspection. The research addresses a fundamental challenge: reconstructing the shape of these infinite-dimensional systems from sparse, noisy sensor data. Existing approaches either used parametric methods that lacked uncertainty quantification or employed Cosserat rod inference on factor graphs that became computationally expensive as state dimensions grew with spatial discretization.

Their novel solution combines the strengths of both paradigms by estimating coefficients of a low-dimensional Geometric Variable Strain (GVS) parameterization within a factor graph framework. The key innovation is a kinematic factor derived from the Magnus expansion of the strain field, which encodes closed-form rod geometry as a prior constraint linking GVS strain coefficients to backbone pose variables. This yields a compact state vector directly usable for model-based control while retaining the modularity, probabilistic treatment, and computational efficiency of factor graph inference.

In simulation tests on a 0.4-meter-long tendon-driven continuum robot, the method demonstrated impressive performance across three measurement configurations. It achieved mean position errors below 2 millimeters in all scenarios and showed a sixfold reduction in orientation error compared to Gaussian process regression baselines when only position measurements were available. This represents significant progress toward making continuum robots more reliable and controllable in real-world applications where precise shape awareness is critical for safety and effectiveness.