Particle swarm optimization algorithm for path planning of icebreaking underwater vehicle

Through the construction of particle swarm optimization algorithm and cost function, the safety and efficiency of path planning of ice-breaking submarines in complex ice environments are solved, and more efficient path planning is achieved, which reduces energy consumption and extends the life of the submarine. time.

CN118938968AInactive Publication Date: 2024-11-12NAVAL UNIV OF ENG PLA
View PDF 5 Cites 0 Cited by

Patent Information

Application Number
CN202410977595.X
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-22
Publication Date
2024-11-12
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The prior art is difficult to efficiently complete the path planning of ice-breaking tasks when considering the operating costs of ice-breaking submarines, especially under complex ice and environmental factors, making it difficult to ensure the safety and efficiency of the submarines.

Method used

Using particle swarm optimization algorithm, the cost function is constructed, including depth cost, smooth navigation cost, path cost, threat cost and continuous ice breaking cost. The path is optimized through particle swarm algorithm to find a list of waypoints that minimize the overall cost, combining spherical vectors and velocity Vector updates equations to implement path planning.

Benefits of technology

With the cost and risks of the submarine operation, providing the most reasonable navigation path planning improves the mission execution efficiency of the ice-breaking submarine, reduces energy consumption, and extends the battery life and range of the submarine.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938968A_ABST
    Figure CN118938968A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of unmanned underwater vehicle control methods, and particularly relates to a particle swarm optimization algorithm for path planning of an icebreaking underwater vehicle. Comprising the following steps: constructing a cost function, determining smooth navigation cost, determining path cost, determining threat cost, determining continuous icebreaking cost, determining total cost, performing path optimization based on a particle swarm algorithm, performing continuous iterative calculation on optimization convergence based on an evaluation basis given by a total cost function, and finally giving an optimal path. And the total cost function is minimized. According to the method, the most reasonable navigation path planning data can be provided on the basis of fully considering various operation costs and risk costs in the use process of the underwater vehicle, the task execution efficiency of the icebreaking underwater vehicle can be effectively improved, the energy consumption during cruising is reduced, the endurance time and range of the underwater vehicle are further improved, and the service capability of the underwater vehicle is expanded.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned underwater vehicle control methods, and in particular relates to a particle swarm optimization algorithm for path planning of an icebreaking underwater vehicle. Background Art

[0002] Path planning is crucial for icebreakers to perform icebreaking tasks. Reasonable path planning can not only reduce the travel distance of icebreakers, but also prevent them from encountering thick ice or even icebergs. The core of this is to use path planning algorithms to determine the optimal route from the starting point to the end point of the icebreaker. This process should take into account the thickness of the ice, physical limitations, and other potential environmental factors to ensure that the icebreaker can complete the icebreaking mission safely and effectively. Summary of the invention

[0003] The purpose of the present invention is to provide a path planning particle swarm optimization algorithm based on the mission requirements of an icebreaking submersible, which can efficiently complete the icebreaking mission under the premise of fully considering the operating cost of the submersible.

[0004] To achieve the above purpose, the present invention adopts the following technical solution.

[0005] A particle swarm optimization algorithm for icebreaker submersible path planning includes the following steps:

[0006] Step 1: Construct a cost function, specifically:

[0007] A1. Determine the depth cost: the navigation path P planned by particle i i It includes N waypoints, each of which corresponds to a path node P in the search map. ij =(x ij ,y ij , z ij ), where j represents the waypoint number; the diving depth of the underwater icebreaker submersible is limited to the extreme minimum depth h min and the maximum depth h max The depth cost F calculated for a certain waypoint 1 (P i )for:

[0008]

[0009]

[0010] Among them, h ij Indicates that the underwater icebreaker submersible passes P ij The absolute depth at which ij It represents the cost when the underwater icebreaker vehicle does not sail at the average depth;

[0011] A2. Determine the smooth navigation cost: The variable of the smooth navigation cost is the horizontal angle of the path of the underwater icebreaker submersible when performing the mission. and the pitch angle ψ ij , Defined as the angle between two consecutive paths in the horizontal direction, ψ ij Defined as the angle between the path and its projection in the horizontal direction, define the waypoint P ij =(x ij ,y ij , z ij ) on the xy plane is P′ ij =(x′ ij , y′ ij , 0), then the horizontal angle and the pitch angle ψ ij The expressions are:

[0012]

[0013] Smooth sailing cost F 2 (P i )for:

[0014] where a 1 and a 2 are the cost coefficients of the turning angle and the climbing angle respectively; since the navigation path of the ship is the projection of the planned path of the underwater icebreaker submersible on the xy plane, and the underwater icebreaker submersible is set to the fixed depth mode, the parameter a2 is 0.

[0015] A3. Determine the path cost: Considering the limitation of the navigation distance of the underwater icebreaker submersible, choose to minimize the path length to provide a longer distance path planning; waypoint P ij =( xij, y ij , z ij ) on the xy plane is P′ ij =(x′ ij, y′ ij, 0); Since the displacement of the icebreaker submersible in the z-axis direction is too small compared to the entire navigation path, it is negligible, so the Euclidean distance between the two waypoints is Path cost F 3 (P i ) can be expressed as:

[0016] A4. Determine the threat cost: The path planning needs to guide the ship to avoid the threat posed by underwater obstacles; define Z as the set of all threats; model the threat as a cylinder, whose center coordinates projected on the xy plane are Cz and radius is Rz; for a given path segment Threat cost and Cz to The distance dz is proportional to the diameter D of the underwater icebreaker submersible and the dangerous distance S. ij The threat cost F 4 (P i )for:

[0017]

[0018] refers to the path through a given segment The threat cost

[0019] dz refers to the center coordinate of the underwater obstacle projected on the xy plane as CZ. For a given path segment dz is Cz to The distance; z∈Z refers to the zth obstacle;

[0020] A5. Determine the continuous icebreaking cost: The continuous icebreaking cost J depends mainly on the sea ice thickness t, and its expression can be described by a piecewise function:

[0021]

[0022] The path of the underwater icebreaker is sampled and projected to the bottom of the sea ice to obtain npts elevation data of the bottom of the sea ice z i ″, its absolute value is the thickness of sea ice t i , the corresponding ice-breaking cost is J i ; For the planned navigation path P i , its continuous ice-breaking cost can be expressed as:

[0023] A6. Determine the overall cost: The planned path must be optimal under the conditions of meeting the actual application requirements; by considering the waypoint P i The associated optimality, safety and feasibility constraints lead to the overall cost function which can be expressed as:

[0024]

[0025] Step 2: Path optimization based on particle swarm algorithm, specifically refers to:

[0026] The path planning of the underwater icebreaker vehicle is transformed into an optimization problem, whose goal is to find a suitable waypoint list P iSo that F(P i ) is the smallest; each particle i in the group is given position information P i =(p i1 , p i2 ,…,p iN ) and speed information V i =(v i1 , v i2 , ..., v iN ) is used to represent the local optimal position that the i-th particle has searched so far is L i =(l i1, l i2, …, l iN ), the global optimal position searched by the entire particle swarm is L g =(l g1 , l g2 , ..., l gN );

[0027] For a particle swarm with a total number of M, after finding two optimal positions, first update their own speed:

[0028]

[0029] The particle updates its position according to the velocity:

[0030]

[0031] Where r is the number of iterations; c 1 and c 2 is the learning factor; d 1j and d 2j is a uniform random number between [0, 1];

[0032] Since the starting and ending points of all paths are fixed, the position encoding of each particle does not include the starting and ending points; then the M path matrix P corresponding to the particle swarm is [P 1 , P 2 , ..., P M ] T Each consists of a list of N waypoints of the following form:

[0033] P i =(x i1 ,y i1 , z i1 , x i2 ,y i2 , z i2 , ..., x i,N , yi,N , Z i,N );

[0034] Based on the evaluation basis given by the overall cost function, the optimization convergence is calculated iteratively and the optimal path is finally given to minimize the total cost function.

[0035] A further improvement or preferred implementation scheme of the particle swarm optimization algorithm for icebreaker submersible path planning is as follows: In step 2, the optimal search is achieved based on the following steps:

[0036] Build an underwater path U with N waypoints i Position vector on the 3N-dimensional hypersphere:

[0037] U i =(ρ i1 , ψ i1 ,φ i1 , ρ i2 , ψ i2 ,φ i2 ,…,ρ iN , ψ iN ,φ iN ), N = n-2;

[0038] where ρ ij , φ ij Represents the attitude information of the jth waypoint on the underwater path of the underwater icebreaker submersible

[0039] , j∈(1,N); where ρin ∈(0, path_length) represents the underwater path U of the underwater icebreaker submersible i The path length of the nth waypoint on the path. path_length refers to the total length of the path. Indicates the underwater path U of the underwater icebreaker submersible i The pitch angle of the jth waypoint; φ ij ∈(-π,π) represents the underwater path U of the underwater icebreaker submersible i The azimuth of the j-th waypoint on the surface; N refers to the underwater path U i The total number of waypoints;

[0040] Establish the velocity vector ΔU of the drone under the tree at the aforementioned position i :

[0041] ΔU i =(Δρ i1 , Δψ i1 , Δφ i1 …Δρ iN , Δψ iN , Δφ iN );

[0042] where Δρ ij , Δφ ij Indicates the underwater path U of the underwater icebreaker submersible i The speed information of the j-th waypoint on the previous path; Δρ ij Indicates the underwater path U of the underwater icebreaker submersible i The speed of the j-th waypoint;

[0043] Indicates the underwater path U of the underwater icebreaker submersible i The pitch angle change rate of the j-th waypoint on the previous path; Δφ ij Indicates the underwater path U of the underwater icebreaker submersible i The azimuth change rate of the j-th waypoint on the path;

[0044] Defining spherical vectors Velocity Vector The path update equation of the particle swarm algorithm based on the set is obtained as:

[0045]

[0046] Where i refers to the i-th particle, j refers to the j-th waypoint in the path; r refers to the number of iterations; w refers to the inertia weight, which affects the particle search speed; M refers to the total number of particles;

[0047] Where Q i =(q i1 ,q i2 , ..., q iN ) and Q g =(q g1 ,q g2 , ..., q gN ) represent the vector sets representing the local and global optimal positions of particle i respectively; to determine Q i and Q g , through the vector U i Find the corresponding P i , in order to assess the associated costs;

[0048] where the vector To waypoint P ij =(x ij ,y ij , z ij )∈P i The mapping can be represented as:

[0049]

[0050] Define the mapping ξ: U→P, then the calculation method of the local optimal position and the global optimal position is as follows:

[0051]

[0052]

[0053] Its beneficial effects are:

[0054] The particle swarm optimization algorithm for icebreaker submersible path planning of the present invention can provide the most reasonable navigation path planning data on the basis of fully considering various operating costs and risk costs during the use of the submersible, and can effectively improve the efficiency of icebreaker submersible mission execution, reduce energy consumption during cruising, and thereby increase the endurance time and range of the submersible, and expand its business capabilities. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 It is a schematic diagram of icebreaker vehicle perception and path planning; Figure 2 This is a schematic diagram of the cruising angle of the icebreaker vehicle;

[0056] Figure 3 It is a schematic diagram of the threat model encountered by icebreaker vehicles during their cruise;

[0057] Figure 4 It is a schematic diagram of continuous ice breaking;

[0058] Figure 5 is a schematic diagram of simulated sea ice distribution (top view);

[0059] Figure 6 This is a schematic diagram of simulated sea ice thickness (side view)

[0060] Figure 7 This is the result of path planning. Figure 1 ;

[0061] Figure 8 This is the result of path planning. Figure 2 ;

[0062] Fig. 9 It is a schematic diagram of path planning cost. DETAILED DESCRIPTION

[0063] Path planning is crucial for icebreakers to perform icebreaking tasks. Reasonable path planning can not only reduce the travel distance of icebreakers, but also prevent them from encountering potential threats such as thick ice or even icebergs.

[0064] The present invention is described in detail below in conjunction with specific embodiments.

[0065] The implementation plan is as follows: The current position of the polar underwater icebreaker is used as the starting point, and the scheduled arrival position is used as the end point. Combined with the radar map of sea ice thickness, the particle swarm optimization algorithm is used to complete the path planning of the polar underwater icebreaker. The projection of the planned path on the ice surface is the navigation path of the polar multi-functional transport ship. Path planning is formulated based on the cost function. The essence of path planning is how to reduce the optimization problem of the cost function. In the present invention, in order to ensure the effectiveness of the cost function, we mainly consider the safety and feasibility of underwater vehicles and ships, and construct the corresponding cost function. The specific contents are as follows:

[0066] Step 1: Construct a cost function, specifically:

[0067] A1. Determine the depth cost:

[0068] like Figure 1 As shown in Figure 1, the underwater conditions of polar sea ice are complex. In order to ensure efficient solution of the optimal path, it is necessary to set a certain number of waypoints in the planned path. The navigation path P planned by particle i i There are N waypoints. Each waypoint corresponds to a path node P in the search map. ij =(x ij ,y ij , z ij ), where j represents the waypoint number. When performing a mission, the underwater vehicle needs to fill or drain water or perform additional maneuvers to complete the diving and surfacing actions, which consumes more energy. Therefore, the diving depth of the underwater vehicle is usually limited to two given extreme minimum depths h min and the maximum depth h max The depth cost calculated for a certain waypoint is as follows:

[0069]

[0070]

[0071] Among them, h ij Indicates that the underwater icebreaker submersible passes P ij The absolute depth at which ij It represents the cost required when the underwater icebreaker submersible does not navigate at the average depth; H of all waypoints ij The depth cost of the planned path of particle i can be obtained by adding them together.

[0072] The final navigation path of the icebreaker is the projection of the planned path of the underwater vehicle on the ice surface, so the change of the underwater vehicle's navigation depth has little effect on solving the optimal path of the ship. min and h max, the diving depth of the underwater vehicle can remain approximately unchanged. Depth-fixed navigation reduces the depth cost while narrowing the search space to search for the optimal path faster.

[0073] A2. Determine the smoothing cost:

[0074] To ensure the feasibility of the planned path, we introduce a smooth navigation cost. The key variable of the smooth navigation cost is the horizontal angle of the path when the underwater vehicle performs the task. and the pitch angle ψ ij .like Figure 2 As shown, Defined as the angle between two consecutive paths in the horizontal direction, ψ ij Defined as the angle between the path and its projection in the horizontal direction, define the waypoint P ij =(x ij ,y ij , z ij ) on the xy plane is P′ ij =(x′ ij , y′ ij , 0), then the horizontal angle and the pitch angle ψ ij The expressions are:

[0075]

[0076] Smooth sailing cost F 2 (P i )for:

[0077] where a 1 and a 2 are the cost coefficients of the turning angle and the climbing angle respectively; since the navigation path of the ship is the projection of the planned path of the underwater icebreaker submersible on the xy plane, and the underwater icebreaker submersible is set to the fixed depth mode, the parameter a2 can be set to 0.

[0078] A3. Determine the path cost:

[0079] Considering the limitation of the navigation distance of the underwater icebreaker submersible, the path length is minimized to provide longer distance path planning; waypoint P ij =(x ij ,y ij , z ij ) on the xy plane is P′ ij =(x′ ij , y′ ij , 0); Since the displacement of the icebreaker submersible in the z-axis direction is too small compared to the entire navigation path, it is negligible, so the Euclidean distance between the two waypoints is Path cost F 3 (P i ) can be expressed as:

[0080] A4. Determine the threat cost:

[0081] In addition to the above optimal conditions, the planning path needs to guide the ship to avoid the threat posed by underwater icebergs. Define Z as the set of all threats; model the threat as a cylinder, whose center coordinates projected on the xy plane are Cz and radius is Rz; for a given path segment Threat cost and Cz to The distance dz is proportional to the diameter D of the underwater icebreaker submersible and the dangerous distance S. ij The threat cost F 4 (P i )for:

[0082]

[0083] refers to the path through a given segment The threat cost;

[0084] dz refers to the center coordinate of the underwater obstacle projected on the xy plane, which is Cz. For a given path segment dz is Cz to The distance; z∈Z refers to the zth obstacle;

[0085] A5. Determine the cost of continuous ice breaking:

[0086] When an icebreaker is sailing with an icebreaker submersible, the bow will hit the ice, using the weight and impact of the hull to break the ice, and then the hull will continue to sail forward and push the broken ice away. Therefore, the planning route must take into account the continuous icebreaking cost of polar ships and try to avoid deep sea ice.

[0087] like Figure 4 As shown in Figure 2, the continuous icebreaking cost J mainly depends on the sea ice thickness t, and its expression can be described by a piecewise function:

[0088]

[0089] The path of the underwater icebreaker is sampled and projected to the bottom of the sea ice to obtain npts elevation data of the bottom of the sea ice z i ”, whose absolute value is the thickness of sea ice t i , the corresponding ice-breaking cost is J i ; For the planned navigation path P i , its continuous ice-breaking cost can be expressed as:

[0090] A6. Determine the overall cost:

[0091] The planned path must be optimal under the conditions of meeting the actual application requirements; by considering the waypoint P i The associated optimality, safety and feasibility constraints lead to the overall cost function which can be expressed as:

[0092]

[0093] where b k is the weight coefficient.

[0094] Step 2: Path optimization based on particle swarm algorithm.

[0095] Path planning is crucial for a spacecraft to perform icebreaking tasks. Reasonable path planning can not only reduce the travel distance of the spacecraft, but also prevent the spacecraft from encountering thick ice or even icebergs and other potential threats. This application proposes a path optimization method based on particle swarm optimization (PSO), specifically:

[0096] The path planning of the underwater icebreaker vehicle is transformed into an optimization problem, whose goal is to find a suitable waypoint list P i So that F(P i ) is the smallest; when the number of waypoints is determined to be N,

[0097] Each particle i in the group is given by position information P i =(p i1 , P i2 , ..., P iN ) and speed information V i =(v i1 , v i2 , ..., v iN ) is used to represent the local optimal position that the i-th particle has searched so far is L i =(l i1 , l i2 ,…,l iN ), the global optimal position searched by the entire particle swarm is L g =(l g1 , l g2 , ..., l gN );

[0098] For a particle swarm with a total number of M, after finding two optimal positions, first update their own speed:

[0099]

[0100] The right side of the formula consists of three parts. The first part is the "inertia" part, which reflects the particle's movement "habit"; the second part is the "cognition" part, which reflects the particle's memory or recollection from its own experience, which means that the particle has a tendency to approach its own historical optimal position; the third part is the "social" part, which reflects the collaboration between particles and the sharing of group experience, which means that the particle has a tendency to approach the group's historical optimal position. The particle updates its position according to the speed:

[0101]

[0102] Where r is the number of iterations; c 1 and c 2 is the learning factor; d 1j and d 2j is a uniform random number between [0, 1];

[0103] Since the starting and ending points of all paths are fixed, the position encoding of each particle does not include the starting and ending points; then the M path matrix P corresponding to the particle swarm is [P 1 , P 2 , ..., P M ] T Each consists of a list of N waypoints of the following form:

[0104] P i =(x i1 ,y i1 , z i1 , x i2 ,y i2 , z i2 , ..., x i,N ,y i,N , z i,N );

[0105] Based on the evaluation basis given by the overall cost function, the optimization convergence is calculated iteratively and finally the optimal path is given. The optimized path can be obtained by minimizing the total cost function.

[0106] In the aforementioned steps, in order to achieve the efficiency of searching for solutions in the configuration space (non-Cartesian space), improve the probability of particles finding high-quality solutions, and directly implement the constraints related to the turning and climbing angles of the icebreaker submersible through the elevation and azimuth angles of the spherical vector, thereby greatly reducing the search space, the present application proposes an optimal solution search scheme that can further effectively narrow the search space and obtain the optimal solution faster in scenarios such as when the underwater vehicle is sailing at a constant depth.

[0107] Specifically, in step 2, the optimal search is achieved based on the following steps:

[0108] Build an underwater path U with N waypointsi Position vector on the 3N-dimensional hypersphere:

[0109] U i =(ρ i1 , ψ i1 ,φ i1 , ρ i2 , ψ i2 ,φ i2 ,…,ρ iN , ψ iN ,φ iN ), N = n-2;

[0110] in Represents the attitude information of the jth waypoint on the underwater path of the underwater icebreaker submersible

[0111] , j∈(1,N); where ρin ∈(0, path_length) represents the underwater path U of the underwater icebreaker submersible i The path length of the nth waypoint on the path. path_length refers to the total length of the path. Indicates the underwater path U of the underwater icebreaker submersible i The pitch angle of the jth waypoint; φ ij ∈-π,π) represents the underwater path U of the underwater icebreaker submersible i The azimuth of the j-th waypoint on the surface; N refers to the underwater path U i The total number of waypoints;

[0112] Establish the velocity vector ΔU of the drone under the tree at the aforementioned position i :

[0113] ΔU i =(Δρ i1 , Δψ i1 , Δφ i1 …Δρ iN , Δψ iN , Δφ iN );

[0114] where Δρ ij , Δφ ij Indicates the underwater path U of the underwater icebreaker submersible i The speed information of the j-th waypoint on the previous path; Δρ ij Indicates the underwater path U of the underwater icebreaker submersible i The speed of the j-th waypoint;

[0115] Indicates the underwater path U of the underwater icebreaker submersible i The pitch angle change rate of the j-th waypoint on the previous path; Δφij Indicates the underwater path U of the underwater icebreaker submersible i The azimuth change rate of the j-th waypoint on the path;

[0116] Defining spherical vectors Velocity Vector The path update equation of the particle swarm algorithm based on the set is obtained as:

[0117]

[0118] Where i refers to the i-th particle, j refers to the j-th waypoint in the path; r refers to the number of iterations; w refers to the inertia weight, which affects the particle search speed; M refers to the total number of particles;

[0119] Where Q i =(q i1 ,q i2 , ..., q iN ) and Q g =(q g1 ,q g2 , ..., q gN ) represent the vector sets representing the local and global optimal positions of particle i respectively; to determine Q i and Q g , through the vector U i Find the corresponding P i , in order to assess the associated costs;

[0120] where the vector To waypoint P ij =(x ij ,y ij , z ij )∈P i The mapping can be represented as:

[0121]

[0122] Define the mapping ξ: U→P, then the calculation method of the local optimal position and the global optimal position is as follows:

[0123]

[0124]

[0125] The above contents are verified by combining specific examples as follows.

[0126] First, simulated sea ice is generated, such as Figure 5 , Figure 6As shown in the figure, the thickness data, the drawn surface represents the bottom of the sea ice. From this figure, we can see that the simulated sea ice thickness is generally about 1.8 meters, and the polar multi-purpose transport ship can sail normally. However, the thickness of the sea ice near the underwater iceberg will increase significantly, and the ship needs to avoid passing through its corresponding area.

[0127] The schematic diagram of the path planning scheme based on this application is as follows Figure 7 , Figure 8 As shown in the figure, the black solid line is the underwater cruising path of the icebreaker submersible, and the gray solid line is the cooperative cruising path of the icebreaker. The navigation path of the icebreaker can be obtained by projecting the path of the vehicle onto the xy plane. It can be seen from the figure that the navigation path of the polar multi-purpose transport ship is short and smooth, which avoids all threat points well. During the navigation of the polar multi-purpose transport ship, the sea ice thickness is relatively good. Fig. 9 is the total cost corresponding to the planned path. As can be seen from the figure, as the number of iterations increases, the total cost gradually decreases. When the number of iterations reaches about 77, the total cost tends to be stable and changes little. When the number of iterations reaches the set 100 times, the optimal path with the minimum total cost is obtained.

[0128] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention, rather than to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the essence and scope of the technical solution of the present invention.

Claims

1. A particle swarm optimization algorithm for icebreaker submersible path planning, characterized in that: The steps include: Step 1: Construct a cost function, specifically: A1. Determine the depth cost: the navigation path P planned by particle i i It includes N waypoints, each of which corresponds to a path node P in the search map. ij =(x ij ,y ij ,z ij ), where j represents the waypoint number; the underwater vehicle diving depth is limited to the extreme minimum depth h min and the maximum depth h max The depth cost F1(P i )for: Among them, h ij Indicates that the underwater vehicle passes through P ij The absolute depth at which ij It represents the cost when the underwater vehicle does not navigate at the average depth; A2. Determine the smooth navigation cost: The variable of the smooth navigation cost is the horizontal angle of the path of the underwater vehicle when performing the mission. and the pitch angle ψ ij , Defined as the angle between two consecutive paths in the horizontal direction, ψ ij Defined as the angle between the path and its projection in the horizontal direction, define the waypoint P ij =(x ij ,y ij ,z ij ) on the xy plane is P' ij =(z' ij ,y' ij ,0), then the horizontal angle and the pitch angle ψ ij The expressions are: Smooth sailing cost F2(P i )for: Where a1 and a2 are the cost coefficients for turning angle and climbing angle respectively; A3. Determine the path cost: Considering the limitation of the navigation distance of the underwater icebreaker, choose to minimize the path length to provide a longer distance path planning; waypoint P ij =(x ij ,y ij ,z ij ) on the xy plane is P' ij =(x' ij ,y' ij ,0); Since the displacement of the aircraft in the z-axis direction is too small compared to the entire navigation path, it is negligible, so the Euclidean distance between two waypoints Path cost F3(P i ) can be expressed as: A4. Determine the threat cost: The path planning needs to guide the ship to avoid the threat posed by underwater obstacles; define Z as the set of all threats; model the threat as a cylinder, whose center coordinates projected on the xy plane are Cz and radius is Rz; for a given path segment Threat cost and Cz to The distance dz is proportional to the diameter D of the underwater vehicle and the danger distance S. ij The threat cost F4(P i )for: refers to the path through a given segment The threat cost; dz refers to the center coordinate of the underwater obstacle projected on the xy plane, which is Cz. For a given path segment dz is Cz to The distance; z∈Z refers to the zth obstacle; A5. Determine the continuous icebreaking cost: The continuous icebreaking cost J depends mainly on the sea ice thickness t, and its expression can be described by a piecewise function: The underwater vehicle path is sampled and projected to the bottom of the sea ice to obtain npts elevation data of the bottom of the sea ice z i ”, whose absolute value is the thickness of sea ice t i , the corresponding ice-breaking cost is J i ; For the planned navigation path P i , its continuous ice-breaking cost can be expressed as: A6. Determine the overall cost: The planned path must be optimal under the conditions of meeting the actual application requirements; by considering the waypoint P i The associated optimality, safety and feasibility constraints lead to the overall cost function which can be expressed as: where b k is the weight coefficient. Step 2: Path optimization based on particle swarm algorithm, specifically refers to: The path planning of underwater icebreaker is transformed into an optimization problem, whose goal is to find a suitable waypoint list P i So that F(P i ) is the smallest; each particle i in the group is given position information P i =(p i1 ,p i2 ,...,p iN ) and speed information V i =(v i1 ,v i2 ,...,v iN ) is used to represent the local optimal position that the i-th particle has searched so far is L i =(l i1 ,l i2 ,...,l iN ), the global optimal position searched by the entire particle swarm is L g =(l g1 ,l g2 ,...,l gN ); For a particle swarm with a total number of M, after finding two optimal positions, first update their own speed: The particle updates its position according to the velocity: Among them, r is the number of iterations; c1 and c2 are learning factors; d 1j and d 2j is a uniform random number between [0,1]; Since the starting and ending points of all paths are fixed, the position encoding of each particle does not include the starting and ending points; then the M path matrix P corresponding to the particle swarm is [P1, P2, ..., P M ] T Each consists of a list of N waypoints of the following form: P i =(x i1 ,y i1 ,z i1 ,x i2 ,y i2 ,z i2 ,…,x i,N ,y i,N ,z i,N ); Based on the evaluation basis given by the overall cost function, the optimization convergence is calculated iteratively and finally the optimal path is given. The optimized path can be obtained by minimizing the total cost function.

2. The particle swarm optimization algorithm for path planning of an icebreaker submersible according to claim 1, characterized in that: In step 2, the optimal search is achieved based on the following steps: Build an underwater path U with N waypoints i Position vector on the 3N-dimensional hypersphere: U i =(ρ i1 ,ψ i1 ,f i1 ,r i2 ,ψ i2 ,f i2 ,…,r iN ,ψ iN ,f iN ),N=n-2; where ρ ij , φ ij Represents the attitude information of the underwater vehicle at the jth waypoint on the underwater path , j∈(1,N); where ρ in ∈(0,path_length) represents the underwater path U of the underwater vehicle. i The path length of the nth waypoint on the path. path_length refers to the total length of the path. The underwater vehicle is on the underwater path U i The pitch angle of the jth waypoint; φ ij ∈(-π,π) represents the underwater vehicle on the underwater path U i The azimuth of the j-th waypoint on the surface; N refers to the underwater path U i The total number of waypoints; Establish the velocity vector ΔU of the drone under the tree at the aforementioned position i : D.U. i =(Dr. i1 ,Dp i1 ,Df i1 …Dr. iN ,Dp iN ,Df iN ); where Δρ ij , Δφ ij The underwater vehicle is on the underwater path U i The speed information of the j-th waypoint on the previous path; Δρ ij The underwater vehicle is on the underwater path U i The speed of the j-th waypoint; The underwater vehicle is on the underwater path U i The pitch angle change rate of the j-th waypoint on the previous path; Δφ ij The underwater vehicle is on the underwater path U i The azimuth change rate of the j-th waypoint on the path; Defining spherical vectors Velocity Vector The path update equation of the particle swarm algorithm based on the set is obtained as: Where i refers to the i-th particle, j refers to the j-th waypoint in the path; r refers to the number of iterations; w refers to the inertia weight, which affects the particle search speed; M refers to the total number of particles; Where Q i =(q i1 ,q i2 ,...,q iN ) and Q g =(q g1 ,q g2 ,...,q gN ) represent the vector sets representing the local and global optimal positions of particle i respectively; to determine Q i and Q g , through the vector U i Find the corresponding P i , in order to assess the associated costs; where the vector To waypoint P ij =(x ij ,y ij ,z ij )∈P i The mapping can be represented as: Define the mapping ξ:U→P, then the calculation method of the local optimal position and the global optimal position is as follows:

Citation Information

Patent Citations

  • Three-dimensional unmanned aerial vehicle path planning method and system based on particle swarm algorithm

    CN112230678A

  • Three-dimensional obstacle avoidance route planning method based on spherical vector pigeon flock algorithm

    CN114777789A

  • Ice region ship path planning method and system based on optimal control and storage medium

    CN115877843A

  • Helicopter route planning method based on weather forecast information

    CN116659512A

  • Method and apparatus for generating optimal path for an unmanned aerial vehicle

    US20210287556A1