State Estimation for Continuum Multi-Robot Systems on SE(3)

TL;DR

This work addresses the challenge of state estimation in systems composed of multiple coupled continuum robots by introducing a sparse Gaussian-process prior on SE(3) for each robot, integrated through a factor-graph-based Maximum A Posteriori objective that includes prior, pose/strain measurements, and inter-robot coupling terms. The method uses Gauss-Newton linearization with carefully derived Jacobians, yielding a sparse linear system that can be solved efficiently, enabling real-time updates at 100–200 Hz in quasi-static scenarios. The approach achieves accurate estimates with average end-effector errors around $3.3$ mm and $5.02^$ in experiments, and demonstrates robust performance across a range of topologies (collaborative and parallel), sensor configurations (FBG and EM tracking), and extended topologies, supported by simulations and an open-source C++ implementation. This framework provides a general, topology-agnostic tool for continuum multi-robot state estimation that can inform real-time control and planning in cluttered environments and complex interactions.

Abstract

In contrast to conventional robots, accurately modeling the kinematics and statics of continuum robots is challenging due to partially unknown material properties, parasitic effects, or unknown forces acting on the continuous body. Consequentially, state estimation approaches that utilize additional sensor information to predict the shape of continuum robots have garnered significant interest. This paper presents a novel approach to state estimation for systems with multiple coupled continuum robots, which allows estimating the shape and strain variables of multiple continuum robots in an arbitrary coupled topology. Simulations and experiments demonstrate the capabilities and versatility of the proposed method, while achieving accurate and continuous estimates for the state of such systems, resulting in average end-effector errors of 3.3 mm and 5.02° depending on the sensor setup. It is further shown, that the approach offers fast computation times of below 10 ms, enabling its utilization in quasi-static real-time scenarios with average update rates of 100-200 Hz. An open-source C++ implementation of the proposed state estimation method is made publicly available to the community.

Figures (18)

• Figure 1: Example state estimate of a continuum multi-robot system consisting of two individual manipulators coupled to a common end-effector.
• Figure 2: Example continuum multi-robot systems considered for the proposed state estimation approach; Left: Collaborative continuum robots subject to coupling; Right: Parallel continuum robot consisting of multiple individual manipulators coupled to a common end-effector platform.
• Figure 3: Factor graph representation of an example multi-robot system, consisting of two continuum robots coupled to a common end-effector. Left: The prior cost terms for each continuum robot are represented by binary factors, each involving two consecutive discrete states (red dots). Middle: The measurement cost terms are represented by unary factors, each involving only one discrete state that is associated with the measurement (blue dots). Right: The coupling cost terms are represented by binary factors, each involving two discrete states that are subject to a coupling constraint with respect to each other (green dots).
• Figure 4: FBG sensor cross-section of continuum robot $n$ showing the parallel arrangement of four individual optical fibers. The outer fibers are arranged in a circular pattern with distance $r_n$ to the center and angles $\theta_{n,i}$ with respect to the body frame. The overall bending occurs about the the vector $\boldsymbol{\omega}_{n,b}$, with curvature $\kappa_n$ and bending angle $\theta_{n,b}$. $\boldsymbol{\omega}_{n,b}$ is orthogonal to the bending plane and directly related to the bending curvature strains $\omega^{(2)}_{n,k}$ and $\omega^{(3)}_{n,k}$.
• Figure 5: Example of two continuum robots subject to a single coupling constraint. The frames of the coupled poses are denoted $\left\{c_1\right\}$ and $\left\{c_2\right\}$, while the coupling joint frame is denoted with $\{g\}$. Transformation matrices relate each coupled pose to the coupling joint and to the inertial, static frame $\{i\}$.