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

Lorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras · 2026

Read Factor Graph-Based Shape Estimation for Continuum Robots via Magnus Expansion by Lorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras (2026) in the Tower of Babel, a free online library of public-domain works preserved in clean, searchable text. No sign-up. The full text is served as a plain-text file at a predictable URL, where text is present: https://antarctic-labs.com/tower-of-babel/arxiv-2604-15619.txt.

Introduction

Ticozzi, Vela, and Tsiotras estimate continuum robot shapes from sparse noisy sensor data by fitting low-dimensional Geometric Variable Strain coefficients inside a factor graph. A new kinematic factor built from the strain field's Magnus expansion ties strain coefficients to backbone poses as a prior constraint. On a 0.4-meter tendon-driven robot, mean position errors stay below 2 millimeters across three sensor setups, with sixfold lower orientation error than Gaussian process regression on position-only data.

Catalog record

CREATORLorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras
YEAR2026
COLLECTIONPAPERS
FORMATTXT
FILE SIZE36341
LICENSEhttp://creativecommons.org/licenses/by/4.0/
RIGHTSCC BY
SOURCEarXiv

Source: https://arxiv.org/abs/2604.15619

Browse the full library · About the Tower of Babel

Full text

The opening of the work, from the library's hosted text:

Factor Graph-Based Shape Estimation for Continuum Robots via Magnus Expansion Thanks: This work has been supported by AFOSR Award FA9550-23-1-0723 and NSF Award FRR-2530577. Lorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras Abstract Reconstructing the shape of continuum manipulators from sparse, noisy sensor data is a challenging task, owing to the infinite-dimensional nature of such systems. Existing approaches broadly trade off between parametric methods that yield compact state representations but lack probabilistic structure, and Cosserat rod inference on factor graphs, which provides principled uncertainty quantification at the cost of a state dimension that grows with the spatial discretization. This letter combines the strength of both paradigms by estimating the coefficients of a low-dimensional Geometric Variable Strain (GVS) parameterization within a factor graph framework. A novel kinematic factor, derived from the Magnus expansion of the strain field, encodes the closed-form rod geometry as a prior constraint linking the GVS strain coefficients to the backbone pose variables. The resulting formulation yields a compact state vector directly amenable to model-based control, while retaining the modularity, probabilistic treatment and computational efficiency of factor graph inference. The proposed method is evaluated in simulation on a 0.4 m long tendon-driven continuum robot under three measurement configurations, achieving mean position errors below 2 mm for all three scenarios and demonstrating a sixfold reduction in orientation error compared to a Gaussian process regression baseline when only position measurements are available. I Introduction Soft robots are increasingly prevalent across application domains, spanning minimally invasive surgery and drug delivery [ 3 ] , robotic-assisted berry picking [ 23 ] , and emerging concepts for on-orbit servicing [ 22 ] . This versatility stems from compliant materials and structures that enable safe interaction with humans and the environment, often with reduced reliance on sensing and active control compared to rigid-bodied counterparts [ 2 ] . From a mathematical standpoint, soft robots exhibit distributed-parameter dynamics with strong geometric nonlinearities, making accurate modeling, state estimation, and model-based control challenging. Consequently, many works have favored data-driven methods over first-principles models [ 21 , 18 ] , sometimes at the expense of systematic frameworks that generalize across platforms and admit formal analysis. Motivated by this gap, a strain-based formulation grounded in Cosserat rod mechanics, known as Geometric Variable Strain (GVS) [ 13 ] , has led to a reduced-order modeling (ROM) framework which enables efficient derivation of the Lagrangian equations of motion for a broad class of hybrid soft-rigid systems. Notably, earlier strain-parameterization models, including Piecewise Constant Strain (PCS) [ 15 ] , can be recovered as special cases within the more general GVS formulation. Beyond modeling, in many applications, planning and feedback control require an estimate of the soft robot configuration from available measurements. This task, commonly referred to as shape estimation , is often formulated in a model-dependent manner. In the context of GVS, for instance, it amounts to reconstructing the discretized strain coordinates and, when needed, their time derivatives, which parameterize the robot configuration. I-A Related Work Several shape estimation approaches rely on a parameterized representation of the continuum manipulator shape and then infer the shape parameters from available observations. For example, in [ 17 ] , Simultaneous Localization and Mapping (SLAM)-based camera pose estimation is combined with a fitted Piecewise Constant Curvature (PCC) model [ 25 ] . A similar strategy is employed in [ 20 ] , which uses IMUs and a PCC model to mitigate drift. In [ 11 ] , an \mathcal{H}_{\infty} -based extended Kalman filter (EKF) is used to estimate PCC generalized coordinates and velocities from a Lagrangian dynamic model. These approaches have only been tested on kinematic PCC representations, which enable reconstruction of the backbone pose along the arclength but do not directly recover the underlying strain field. A GVS-based dynamic observer built upon a state-dependent Riccati equation (SDRE) is presented in [ 6 ] , where strain coefficients and their time derivatives define the state, while actuator readings provide measurements. However, the per-step cost of solving the SDRE scales poorly with the GVS discretization order. Moreover, increasing the state dimension can undermine the conditions under which the SDRE-based observer remains well-posed, thereby requiring additional measurements. An alternative to parameter estimation is to model the pose and strain fields along the robot backbone as continuous functions of arclength. This viewpoint is adopted in [ 10 ] , where Gaussian process (GP) regression over \mathrm{SE}(3) is used to estimate both pose and strain variables along a continuum robot modeled as a Cosserat rod, and extended in [ 8 ] to additionally infer external loads. While this functional formulation naturally provides uncertainty quantification and induces a sparse computational structure, the robot configuration is represented by a GP posterior rather than a compact state vector, which may hinder its downstream applicability to model-based control design. More recently, [ 7 ] discretizes the full Cosserat rod equations within a factor graph, jointly estimating pose, internal stress, and actuation inputs at each backbone node. This yields a principled framework for force and actuation estimation, but the state dimension grows with the number of discretization nodes, as each node carries pose, stress, and wrench variables. In summary, a persistent trade-off is revealed in the existing literature. On the one hand, parametric methods [ 17 , 20 , 11 , 6 ] yield compact state representations amenable to model-based control design, yet lack the computational efficiency and measurement flexibility of graphical model formulations. On the other hand, Cosserat rod inference on factor graphs [ 10 , 8 , 7 ] provides principled uncertainty quantification and modularity with respect to sensing modalities, but at the cost of a state dimension that scales with the spatial discretization rather than with the intrinsic complexity of the deformation. I-B Contributions This letter bridges the two paradigms identified above: low-dimensional shape reconstruction versus probabilistic Cosserat rod inference. Adopting the factor graph formulation of [ 10 , 8 , 7 ] , we retain the graphical structure and probabilistic treatment inherent to that framework. However, rather than inferring the strain field as a continuous function along the backbone or estimating the full rod state at a discrete set of nodes, we estimate the coefficients of a low-dimensional GVS parameterization. To this end, we introduce a novel Magnus factor, which encodes the closed-form solution of the soft robot kinematics via the Magnus expansion [ 12 , 13 ] as prior knowledge within the graph. The proposed method offers three advantages over existing approaches: 1) improved accuracy, 2) robustness to partial measurement scenarios, and 3) a compact strain representation directly amenable to model-based control design. II Mathematical Preliminaries II-A Soft Robot Kinematics The configuration of a soft (equivalently, continuum) manipulator is described by a curve \bm{g}_{\scriptscriptstyle\mathcal{I}/\mathcal{X}}(\cdot):[0,L]\to\mathrm{SE}(3) , parameterized by the arclength coordinate s\in[0,L] , where L is the backbone length, \mathcal{I} is a fixed reference frame, and \mathcal{X}=\mathcal{X}(s) denotes a cross-sectional frame attached to the backbone, as illustrated in Fig. 1 . Moreover, we assume \bm{g}_{\scriptscriptstyle\mathcal{I}/\mathcal{X}}(0)=\bm{I}_{4} , where \bm{I}_{4} is the 4 \times 4 identity matrix, and define the end-effector frame as \mathcal{E}\triangleq\mathcal{X}(L) . Fig. 1 : Schematic of a soft manipulator. The evolution of the cross-sectional pose along the arclength is governed by the following kinematic equation [ 16 ] ,

Read the complete text