Unified Shape and External Load State Estimation for Continuum Robots

Continuum robots navigate narrow, winding passageways while safely and compliantly interacting with their environments. Sensing the robot's shape under these conditions is often done indirectly, using a few coarsely distributed (e.g. strain or position) sensors combined with the robot's mechanics-based model. More recently, given high-fidelity shape data, external interaction loads along the robot have been estimated by solving an inverse problem on the mechanics model of the robot. In this paper, we argue that since shape and force are fundamentally coupled, they should be estimated simultaneously in a statistically principled approach. We accomplish this by applying continuous-time batch estimation directly to the arclength domain. A general continuum robot model serves as a statistical prior which is fused with discrete, noisy measurements taken along the robot's backbone. The result is a continuous posterior containing both shape and load functions of arclength, as well as their uncertainties. We first test the approach with a Cosserat rod, i.e. the underlying modeling framework that is the basis for a variety of continuum robots. We verify our approach numerically using distributed loads with various sensor combinations. Next, we experimentally validate shape and external load errors for highly concentrated force distributions (point loads). Finally, we apply the approach to a tendon-actuated continuum robot demonstrating applicability to more complex actuated robots.