Gravity Aided Inertial Navigation Path Planning Algorithm based on Underwater Vehicle Constraint and Bezier Curve

Abstract

In gravity aided inertial navigation, a good path planning, which makes the path pass the area where the gravity changes drastically, in matching area will improve the matching accuracy effectively. The underwater vehicle is restricted by kinematics and other constraints, and cannot move in any direction. However, The path planned by the traditional path planning algorithm is a broken line and is not suitable for actual underwater vehicle navigation. Aiming at underwater vehicle path planning problem, this paper proposes a gravity aided inertial navigation path planning algorithm which is based on vehicle constraints and Bezier curves. The $A^{\ast}$ algorithm is optimized by vehicle constraints and gravity characteristics. The Bezier curve is used to smooth the planned path, making the planned path more suitable for the actual situation. Simulation experiments show that the matching accuracy is effectively improved by the planned path based on vehicle constraints and Bezier curve.