Stochastic Adaptive Estimation in Polynomial Curvature Shape State Space for Continuum Robots

TL;DR

This work tackles real-time shape estimation for continuum robots under sparse sensing by formulating the shape as a polynomial curvature (PCK) state and estimating the modal coefficients with a stochastic observer. It advances robustness and adaptability by employing an Interacting Multiple Model Extended Kalman Filter (IMM-EKF) over a family of curvature models (CC, PCK-1, PCK-2) and by introducing a noise-weighted curvature-space observability framework to guide sensor deployment. Key contributions include (i) a curvature-space stochastic observer, (ii) adaptive multi-model IMM-EKF with unbiased mixing and online TPM adjustments, and (iii) a practical observability analysis that informs sensor configuration, validated through both simulations and hardware experiments in planar bending. The approach enables accurate, low-latency shape estimation with sparse measurements, enhancing real-time planning and control for continuum robots and offering a pathway to extending to full 3D estimation with additional kinematic modules.

Abstract

In continuum robotics, real-time robust shape estimation is crucial for planning and control tasks that involve physical manipulation in complex environments. In this paper, we present a novel stochastic observer-based shape estimation framework designed specifically for continuum robots. The shape state space is uniquely represented by the modal coefficients of a polynomial, enabled by leveraging polynomial curvature kinematics (PCK) to describe the curvature distribution along the arclength. Our framework processes noisy measurements from limited discrete position, orientation, or pose sensors to estimate the shape state robustly. We derive a novel noise-weighted observability matrix, providing a detailed assessment of observability variations under diverse sensor configurations. To overcome the limitations of a single model, our observer employs the Interacting Multiple Model (IMM) method, coupled with Extended Kalman Filters (EKFs), to mix polynomial curvature models of different orders. The IMM approach, rooted in Markov processes, effectively manages multiple model scenarios by dynamically adapting to different polynomial orders based on real-time model probabilities. This adaptability is key to ensuring robust shape estimation of the robot's behaviors under various conditions. Our comprehensive analysis, supported by both simulation studies and experimental validations, confirms the robustness and accuracy of our methods.

Figures (16)

• Figure 1: Diagram of proposed shape estimation method
• Figure 2: Schematic of kinematics
• Figure 3: Bending shapes of a continuum robot's central backbone (length $L = 100$$mm$) generated using $0^{th}$, $1^{st}$ and $2^{nd}$ order polynomial shape state $\mathbf{m}$. (Left to Right: Higher-order polynomials (e.g., $2^{nd}$ order) enable non-uniform curvature profiles with varying curvature rates. Top to Bottom: Increasing $k=1,2,3$ amplifies curvature magnitude, leading to sharper bends for each polynomial order.)
• Figure 4: Schematic of IMM-EKF algorithm
• Figure 5: Algorithmic latency in a matched Python setup (single thread): (a) Forward kinematics (joint inputs $\rightarrow$ disk poses), (b) Single-update estimation (tip pose $\rightarrow$ disk poses). Values are mean ± s.d. over 10 repeated runs and are implementation-dependent.