Underwater vehicle three-dimensional path planning method based on Lazy Theta satellite and particle swarm hybrid algorithm

Abstract

The invention provides an underwater vehicle three-dimensional path planning method based on a Lazy Theta satellite and particle swarm hybrid algorithm. The method comprises the following steps of 1, performing navigation space modeling; 2, building a Lazy Theta satellite algorithm cost function; 3, performing two-dimensional path planning in a horizontal plane by using a z=zsafemin horizontal plane as a Lazy Theta satellite algorithm two-dimensional path plan plane, and using X-axis and Y-axis coordinates (xs, ys) of a path starting point S and X and Y-axis coordinates (xd, yd) of a path terminal point D as a starting point and a terminal point of a two-dimensional path; 4, finding a collision-free length-shortest path optimization target according to three-dimensional path planning; designing a depth plan evaluation function; 5, using a particle swarm algorithm to perform depth planning; 6, outputting an optimum three-dimensional path. Through the simplification on a three-dimensional problem, the advantages of two kinds of different algorithms are combined; the calculation complexity of the algorithm is reduced; the high speed performance and the reliability of the three-dimensional path planning are improved.

Description

technical field

[0001] The invention relates to a path planning method for an underwater submersible. Specifically, it is a three-dimensional path planning method for underwater vehicles. Background technique

[0002] Three-dimensional route planning is one of the key technologies for intelligent machines with space mobility capabilities such as underwater vehicles to perform underwater tasks, and it has attracted more and more attention. Most of the existing 3D path planning algorithms are developed from 2D path planning algorithms. In 2D path planning, the A star algorithm is widely used. For example, in the patent document whose application number is 201110172301.9, a simplified method of game path search is proposed; in the patent document whose application number is 201410531309.3, he proposes a system of A-star pathfinding methods based on binary heap node sorting; application In the patent document No. 201410010003.3, a method for determining the optimal path of an ...

Images