Continuum Robot State Estimation Using Gaussian Process Regression on $SE(3)$

TL;DR

This work presents a probabilistic state estimation framework for continuum robots modeled by Cosserat rod theory, using sparse Gaussian process regression on SE(3) to infer both pose and internal strain along the arclength with uncertainty. By fusing a physically informed prior with discrete pose and strain measurements, the method yields a continuous posterior over the robot shape and provides uncertainty envelopes and interpolations between estimation nodes. The approach is demonstrated on tendon-driven continuum robots through simulations and real experiments, achieving end-effector accuracies on the order of a few millimeters and fractions of a degree, with reasonable computational efficiency due to the sparse GP structure. The framework is versatile, capable of handling different robot designs without robot-specific kinematics, and offers potential extensions to include higher-order dynamics and loops in topology for broader applications in continuum robotics.

Abstract

Continuum robots have the potential to enable new applications in medicine, inspection, and countless other areas due to their unique shape, compliance, and size. Excellent progess has been made in the mechanical design and dynamic modelling of continuum robots, to the point that there are some canonical designs, although new concepts continue to be explored. In this paper, we turn to the problem of state estimation for continuum robots that can been modelled with the common Cosserat rod model. Sensing for continuum robots might comprise external camera observations, embedded tracking coils or strain gauges. We repurpose a Gaussian process (GP) regression approach to state estimation, initially developed for continuous-time trajectory estimation in $SE(3)$. In our case, the continuous variable is not time but arclength and we show how to estimate the continuous shape (and strain) of the robot (along with associated uncertainties) given discrete, noisy measurements of both pose and strain along the length. We demonstrate our approach quantitatively through simulations as well as through experiments. Our evaluations show that accurate and continuous estimates of a continuum robot's shape can be achieved, resulting in average end-effector errors between the estimated and ground truth shape as low as 3.5mm and 0.016$^\circ$ in simulation or 3.3mm and 0.035$^\circ$ for unloaded configurations and 6.2mm and 0.041$^\circ$ for loaded ones during experiments, when using discrete pose measurements.

Figures (18)

• Figure 1: Comparison of the equations of motion (parameterized by time) of a rigid body deleuterio85 to those of the quasi-static Cosserat rod model (parameterized by arclength) pai02. Quantities are expressed in the body frame and $\mathbf{T} = \mathbf{T}_{bi}$, i.e., it transforms quantities from a static frame, $i$, to the body frame, $b$.
• Figure 2: Local pose variables, ${\boldsymbol{\xi}}_k(s)$, are used to simplify the process of defining a GP prior over the robot state.
• Figure 3: The prior for the entire robot state can be broken down into a sequence of binary factors (black dots), each of which represents a squared error term for how that pair of states is related. High cost is associated with shapes and strains that deviate from the prior mean (e.g., straight rod with no strain).
• Figure 4: Graphical depiction of the prior distribution over robot states. Here we show the mean shape for the robot, a straight line along the $x$ axis, as well as $300$ random samples drawn from our GP prior. Here we took $S = 10$, $\bar{{\boldsymbol{\varepsilon}}} = (1,0,0,0,0,0)$, and $\mathbf{Q}_c = \hbox{diag}\left(0.01, 0.01, 0.01, 0.001, 0.001, 0.001 \right)$. In rough terms, our state estimator will downselect these possibilities based on the measurements.
• Figure 5: The measurements (both pose and strain) make up unary factors (black dots), each of which represents a squared error term for how that state should be. High cost is associated with shapes and strains that deviate from the measurements. Here we have a typical setup with a pose measurement of the end effector and two strain measurements at intermediate arclengths.