Manifold-Constrained Visual Odometry for Soft Worm-Inspired Robots in Pipe-Inspection Tasks

Abstract

Feature tracking for soft robot state estimation is challenging due to inherent structural unpredictability. This paper presents a cylindrically constrained Visual Odometry (VO) framework for soft, worm-like robots navigating pipes. Leveraging periodic inchworm gaits, the algorithm predicts robot motion between gait cycles and applies a minimum-error optimization based on VO feature reprojection errors to achieve accurate state estimation. A Levenberg-Marquardt optimization minimizes this reprojection error, enforcing hard constraints to fix feature positions on the pipe surface and soft constraints to bound the robot’s pose within the pipe geometry. Validated via simulation and physical experiments, the system effectively corrects for locomotion variations caused by compliance. Simulation results demonstrate an error reduction of up to 50% when simulated displacement falls 25% short of the motion model. Experimental data from a robot traversing a 308 mm pipe show vertical displacement error reduced from 11.35 mm to 2.5 mm. Finally, the algorithm is evaluated on a manually inserted camera probe to assess its versatility across platforms. Its suitability as an odometry source for wheeled robots and handheld probes is discussed, alongside its utility as a centralization algorithm for surface inspection and control feedback.