A Stochastic Framework for Continuous-Time State Estimation of Continuum Robots

TL;DR

This work introduces a fully stochastic, continuous-time state estimator for continuum robots by unifying space-time Gaussian-process priors with a sparse factor-graph optimization. The state is represented as $\boldsymbol{x}(s,t)=\{\boldsymbol{T}(s,t),\boldsymbol{\epsilon}(s,t),\boldsymbol{\varpi}(s,t)\}$ and a white-noise-on-acceleration assumption is applied to form a tractable prior, enabling continuous interpolation in time and arc length. The framework fuses asynchronous measurements from pose and gyro sensors, achieving real-time-like mean estimates with robust handling of data dropout, and provides uncertainty through interpolated covariances. Validation on simulations and a tendon-driven prototype demonstrates accurate tip/body pose tracking, effective interpolation, and favorable scalability, with open-source code made available for the community.

Abstract

State estimation techniques for continuum robots (CRs) typically involve using computationally complex dynamic models, simplistic shape approximations, or are limited to quasi-static methods. These limitations can be sensitive to unmodelled disturbances acting on the robot. Inspired by a factor-graph optimization paradigm, this work introduces a continuous-time stochastic state estimation framework for continuum robots. We introduce factors based on continuous-time kinematics that are corrupted by a white noise Gaussian process (GP). By using a simple robot model paired with high-rate sensing, we show adaptability to unmodelled external forces and data dropout. The result contains an estimate of the mean and covariance for the robot's pose, velocity, and strain, each of which can be interpolated continuously in time or space. This same interpolation scheme can be used during estimation, allowing for inclusion of measurements on states that are not explicitly estimated. Our method's inherent sparsity leads to a linear solve complexity with respect to time and interpolation queries in constant time. We demonstrate our method on a CR with gyroscope and pose sensors, highlighting its versatility in real-world systems.

Figures (11)

• Figure 1: A CR moving through time, taking asynchronous and noisy measurements of its state. This situation arises in practical senarios where multiple high-rate sensors are used on the robot. In such senarios, data from multiple high-rate sensors produce update rates much faster than a CR shape can reasonably be solved for. In this work, we propose addressing this issue by using a physically motivated Gaussian process prior defined over both space and time that is capable of continuous-time interpolation in constant time. This efficient interpolation can be used to fuse high-rate measurements from multiple sensors at slower time steps while maintaining accurate estimates in between time steps.
• Figure 2: Factor-graph representation of the prior for the continuous-time estimation framework. Unary factors are depicted using green dots, binary space factors are represented with blue dots, and binary time factors are depicted using yellow dots. Each triangle represents a node at a specific point in arclength and time that is estimated directly in our solver. Each factor is connected to one or two nodes, and never between two nodes that vary in both arclength and time.
• Figure 3: Factor graph representation of the full continuous-time estimation framework. Unary factors are depicted using green dots, binary space factors with blue dots, and binary time factors with yellow dots. Each triangle represents a node at a specific point in arclength and time that is estimated directly in our solver. Measurements are added sparsely either directly to an estimation node or as an interpolated factor between two estimation nodes and are depicted as red dots. In practice, measurements will solely be added as interpolated factors in time, as the spatial estimation points included in the graph will be selected to align with the physical placement of each sensor on the robot.
• Figure 4: Sparsity structure of the problem's inverse Kernel matrix $\check{\boldsymbol{P}}^{-1}$ (left) and its associated Cholesky factor $\check{\boldsymbol{L}}$ (right) for a estimation problem with $N = 5, K = 4$. The sparsity structure follows from the factor graph in Fig. \ref{['fig:factor_graph']}. Each row and column of the matrix corresponds to a node in the factor graph, with each non-zero entry corresponding to a correlation between two nodes. The Cholesky factor is lower triangular, with non-zero entries bounded about the diagonal. The band scales with $N$, the number of spatial nodes in our state, leading to a solve complexity of $O(N^3 K)$, where $K$ is the number of time steps in our estimation window. Given $M$ measurements, the total system time complexity, including matrix construction, becomes $O(N^3 K + M)$.
• Figure 5: Picture of the robot prototype described in Section \ref{['sec:prototype']} during data collection. Sensor locations are labelled in red with other relevant parts labelled in black.