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

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. Continuum robots (CRs) are jointless, flexible, and easily miniaturizable manipulators capable of bending into contorted spatial shapes. They are often said to be inspired by the animal kingdom, resembling the motion of snakes, elephant trunks, or worms [1]. Their unique properties allow them to navigate in confined and cluttered environments. This makes them particularly suitable for applications such as minimally invasive surgery [2], [3], industrial inspection and repair in hard-to-reach places [4], [5], as well as search and rescue operations in disaster areas [6]. To date, great progress has been made on the modeling of continuum robots [7], using a variety of kinematic, static, and dynamic assumptions and approaches. Such methods aim to accurately predict the robot's shape given its material properties, actuation, and external loads, which is crucial for the aforementioned applications.