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

1. Problem Statement

Continuum robots (soft robots) possess infinite-dimensional dynamics due to their compliant nature, making accurate shape estimation from sparse and noisy sensor data a significant challenge.

• The Trade-off: Existing methods face a dichotomy:
  • Parametric Methods (e.g., Piecewise Constant Curvature): Offer compact state representations suitable for model-based control but often lack rigorous probabilistic uncertainty quantification and flexibility in sensor integration.
  • Cosserat Rod Inference on Factor Graphs: Provides principled uncertainty quantification and modularity but suffers from a state dimension that scales linearly with spatial discretization (number of nodes), hindering computational efficiency and direct application to control.
• The Goal: Develop a shape estimation framework that combines the compactness of parametric models with the probabilistic robustness and modularity of factor graphs.

2. Methodology

The authors propose a hybrid approach that estimates the coefficients of a low-dimensional Geometric Variable Strain (GVS) parameterization within a factor graph framework.

A. Kinematic Modeling (GVS & Magnus Expansion)

• GVS Parameterization: Instead of estimating the strain field at every discrete node, the strain field $\xi(s)$ is approximated as a projection onto a finite set of basis functions (e.g., B-splines):
  $$\xi(s; q) = \Phi_\xi(s)q + \xi^*$$
  where $q$ is a low-dimensional vector of generalized strain coefficients.
• Magnus Expansion: The relationship between the robot's backbone pose $g(s)$ and the strain field is governed by the kinematic equation $g'(s) = g(s)\hat{\xi}(s)$. The solution is expressed via the Magnus expansion, a convergent infinite series that provides a closed-form expression for the pose:
  $$g(s) = \exp(\hat{\Omega}(s))$$
  where $\hat{\Omega}(s)$ is derived from nested commutators of the strain integral.

B. Factor Graph Formulation

The estimation problem is formulated as a Maximum A Posteriori (MAP) estimate on a factor graph:

• Variable Nodes:
  • $g_i$: Discrete backbone poses at specific arclength nodes.
  • $q$: The global vector of GVS strain coefficients.
• Factor Nodes (Constraints):
  1. Magnus Factors (Novel Contribution): These encode the kinematic constraint between adjacent poses $g_i, g_{i+1}$ and the global strain coefficients $q$. They utilize the closed-form Magnus solution to link the strain parameters directly to the geometry, acting as a strong prior.
  2. Measurement Factors: Incorporate noisy sensor data (strain, position, or full pose) at specific locations.
  3. Prior Factors: Anchor the root pose to the identity and regularize the strain coefficients (Tikhonov regularization).

C. Optimization

The MAP estimation is converted into a Nonlinear Least Squares (NLLS) problem, solved efficiently using the Levenberg-Marquardt (LM) algorithm. The use of analytic Jacobians ensures computational efficiency.

3. Key Contributions

1. Novel Magnus Factor: Introduces a kinematic factor derived from the Magnus expansion that encodes the exact closed-form rod geometry as a prior constraint. This links low-dimensional strain coefficients to backbone poses without requiring full discretization of the strain field.
2. Hybrid Paradigm: Successfully bridges the gap between parametric modeling (compact state for control) and probabilistic inference (uncertainty quantification). The resulting state vector is low-dimensional ($q$) yet retains the modularity of factor graphs.
3. Robustness to Sparse Data: The method leverages the nonlinear coupling between bending and torsional strains (encoded in the Magnus commutator terms) to infer orientation even when only position measurements are available.

4. Experimental Results

The method was evaluated in simulation on a 0.4 m tendon-driven continuum robot (TDCR) under three scenarios:

• S1 (Pose Measurements): Measurements at the midpoint and tip.
  • Result: Mean position error 1.45 mm, mean orientation error 1.46°.
• S2 (Mixed Strain & Pose): Strain measurements at three points and pose at the tip.
  • Result: Mean position error 1.92 mm. Errors were minimized at sensor locations.
• S3 (Position Only): Position measurements at five nodes (no direct orientation or strain data).
  • Result: Mean position error 1.23 mm. Despite lacking orientation data, the method achieved a mean orientation error of 10.60°.

Comparison with Baseline (Gaussian Process Regression):

• The proposed method significantly outperformed a GP regression baseline (re-implemented from [14]).
• Orientation Accuracy: In the position-only scenario (S3), the proposed method reduced orientation error by a sixfold margin compared to the baseline (10.60° vs. 63.06°).
• Grid Independence: Unlike the baseline, which suffers from "snapping errors" when measurement locations do not align with estimation nodes, the proposed method evaluates constraints at exact continuous arclengths via the B-spline model, ensuring consistent accuracy regardless of grid density.

5. Significance and Impact

• Control-Ready: By maintaining a compact state vector ($q$) rather than a high-dimensional node-based state, the output is directly amenable to model-based control design, a limitation of previous factor-graph-based Cosserat rod methods.
• Sensor Flexibility: The framework can seamlessly integrate heterogeneous sensors (strain, position, pose) and handle sparse data scenarios where traditional kinematic observers fail.
• Computational Efficiency: While the global connectivity of the strain variable $q$ reduces graph sparsity, the use of locally supported B-splines and low-dimensional parameterization keeps computation times competitive (approx. 1–2 ms per batch).
• Future Work: The authors plan to validate the method on physical hardware and integrate it into real-time model-based control loops.

In summary, this work presents a mathematically rigorous and computationally efficient solution for continuum robot shape estimation, overcoming the trade-off between model compactness and probabilistic robustness through the innovative application of the Magnus expansion within a factor graph.