Constrained filtering with contact detection data for the localization and registration of continuum robots in flexible environments

This paper presents a novel filtering technique that uses contact detection data and environmental stiffness estimates to register and localize a robot with respect to an a priori 3D surface model. The algorithm leverages geometric constraints within a Kalman filter framework and relies on two distinct update procedures: 1) an equality constrained step for when the robot is forcefully contacting the environment, and 2) an inequality constrained step for when the robot lies in the free-space of the environment. This filtering procedure registers the robot by incrementally eliminating probabilistically infeasible state space regions until a high likelihood solution emerges. In addition to registration and localization, the algorithm can estimate the deformation of the surface model and can detect false positives with respect to contact estimation. This method is experimentally evaluated with an experiment involving a continuum robot interacting with a bench-top flexible structure. The presented algorithm produces an experimental error in registration (with respect to the end-effector position) of 1.1 mm, which is less than 0.8 percent of the robot length.