Optimal Path Following Control for Motion Recovery From Joint Failure of Snake Robots Using Improved Lateral Undulatory Gait