Abstract
Underwater navigation is an area of increasing research interest due to its fundamental complexity and industrial applications. However, due to convenience and current theoretical understanding, the vast majority of underwater platforms utilize thrusters, while other forms of propulsion, such as undulatory locomotion, have been given limited exposure. This paper provides the first real-time motion planning framework that produces energy and time efficient paths with empirical local optimality for articulated swimming robots in 3D, called SIMP. SIMP utilizes learned associations between parameterized dynamically feasible undulatory gaits with their expected energy cost, velocity, and swept-out volume of the robot during execution, to formulate a simplified optimization problem that decides the path to be followed with the corresponding consecutive gaits, and navigates the robot safely in complex 3D environments. The proposed pipeline is tested in numerical experiments with realistic dynamics for a 10-link underwater snake robot (USR) with anguilliform gaits, in simulated cluttered environments of significant challenge, displaying real-time replanning performance of more than 1 Hz.