A multi-route planning method

Abstract

The present invention discloses a multi-route planning method, comprising the following steps: S1, initializing the planning space; S2, constructing a two-dimensional simulated ocean current field model; S3, setting a virtual obstacle wall in the planning space, dividing the planning space into p+q sub-planning spaces; S4, using the level set method to plan the route for the entire planning space to obtain the optimal route; planning the route for all sub-spaces to obtain p+q sub-optimal routes; S5, merging the same routes; S6, sorting the p+q+1 optimized routes obtained by planning from small to large according to the value of the navigation time, and completing the route planning. The present invention realizes the efficient use of ocean currents to plan reliable and reference-worthy optimized routes with large discrimination for commanders to choose from, and solves the problem of no effective alternative routes for commanders to choose in actual underwater route planning. The spatial subdivision level set method based on the commander's intention can well implement the commander's intention.

Description

Technical Field

[0001] The invention relates to the field of route planning, and in particular to a multi-route planning method. Background Art

[0002] At present, the level set method, ant colony optimization algorithm and quantum particle swarm optimization algorithm are generally used to complete the route planning of underwater ships in ocean current environments, but these algorithms can only plan one optimized route, that is, one optimal route; and when there is only one optimized route alternative, due to the time-varying nature of the underwater environment, the planned route is likely to cause the underwater ship to be in distress or even destroyed. Once this optimal route is impassable, the ship will lose its direction and is likely to drift underwater and unable to reach its destination. Summary of the invention

[0003] The present invention provides a multi-route planning method to overcome the above problems.

[0004] In order to achieve the above object, the technical solution of the present invention is:

[0005] S1. Initialize the planning space;

[0006] S2, construct a two-dimensional simulated ocean current field model;

[0007] S3, setting a virtual obstacle wall in the planning space to divide the planning space into p+q sub-planning spaces;

[0008] S4. Perform route planning on the entire planning space to obtain the optimal route; perform route planning on p+q sub-planning spaces respectively to obtain p+q sub-optimal routes; store the coordinate values ​​of the optimal route and p+q sub-optimal routes in cell array Cell1 in sequence; store the flight time and flight length of p+q+1 routes in cell array Cell2; the length cell_num of Cell1 and Cell2 are both p+q+1;

[0009] S5, merge the suboptimal routes with the same sailing time and sailing length;

[0010] S6, sorting the suboptimal routes in the cell arrays Cell1 and Cell2 from small to large according to the values ​​of the flight duration of the suboptimal routes, to obtain cell arrays Cell3 and Cell4; Cell3 is the sorted Cell1, and Cell4 is the sorted Cell2;

[0011] The optimal route is stored in the first cell of Cell3, and the flight time and length of the optimal route are stored in the first cell of Cell4 to complete the route planning.

[0012] Furthermore, the S1 planning space initialization includes:

[0013] S11, rasterizing the planned space into a 100×100 grid, setting the horizontal grid spacing to Δx and the vertical grid spacing to Δy;

[0014] S12, setting obstacles and threat areas, and setting the areas where obstacles are located and threat areas as no-fly zones; setting the source point position and the end point position, and setting the initial zero level set to a circular area with the source point position as the center and Δl / 2 as the radius;

[0015] S13, in the planning space after rasterization, select reference points based on experience and establish a two-dimensional coordinate system, where Δx is the unit length of the X axis and Δy is the unit length of the Y axis;

[0016] S14. Let the number of polygon vertices corresponding to the i-th obstacle be n i , then the vertex coordinate set of the i-th obstacle is represents the horizontal component of vertex j of the ith obstacle, Represents the longitudinal direction component of vertex j of the i-th obstacle.

[0017] S15. Let the coordinates of the center of the kth threat zone be The radius of the kth threat zone is r k ; The coordinates of the highest point in the kth threat zone are The coordinates of the lowest point are:

[0018] Furthermore, S2 constructs a two-dimensional simulated ocean current field model including:

[0019]

[0020] Where r is the position vector of the ship in the ocean current field, V x (r) is the horizontal component of the ocean current velocity V(X,t), V y (r) is the longitudinal component of the ocean current velocity V(X,t), V z (r) is the vertical component of the ocean current velocity V(X,t); κ is the eddy strength, ξ is the eddy radius coordinate, r i represents the position of the i-th vortex center, which is randomly generated, and n is the number of superimposed vortices; V cx V is a steady flow c The velocity component in the horizontal direction, V cy V is a steady flow c The velocity component in the longitudinal direction, V cz V is a steady flow c The velocity component in the vertical direction.

[0021] Furthermore, S3 includes:

[0022] S31, setting a blank matrix A of a size of 2×(p+q), sorting the y components of the coordinates of the highest points of each obstacle and threat area, and placing the sorted coordinates into the matrix A in order;

[0023] S32, dividing the planning space into subspaces based on the matrix A and the upper and lower boundaries of the planning space;

[0024] S33. Update subspace according to commander's intention.

[0025] Further, S31 includes:

[0026] S311. The y component of the coordinates of the highest point among all vertices of each obstacle is:

[0027]

[0028] in, is the set of y-component coordinates of the highest point among all vertices of each obstacle, is the jth vertex of the ith obstacle; n i is the number of vertices in the i-th obstacle;

[0029] The y component of the highest point of the k-th circular threat area is:

[0030]

[0031] in, is the y component of the highest point of the kth threat zone, is the y component of the center coordinate of the threat area; r k is the radius of the threat zone;

[0032] S312, let the number of obstacles between the source point and the end point be p, the number of threat areas be q, and q Sort them from large to small, where 1≤i≤p, 1≤k≤q;

[0033] S313, sort the p and q The y-component and x-component coordinate values ​​of the corresponding coordinate points are sequentially placed into the 2×(p+q) matrix A;

[0034] The x component is stored in the first row of matrix A, the y component is stored in the second row of matrix A, and the coordinate point of the i-th obstacle or threat area is (A(1, i), A(2, i)).

[0035] Further, S32 includes:

[0036] S321, dividing the first sub-planning space:

[0037] The grid point between point (A(1,1), A(2,1)) and point (A(1,1),0) is set as the first virtual obstacle wall; the first sub-planning space is composed of the first virtual obstacle wall, obstacles, threat areas, simulated ocean currents and planning space boundaries;

[0038] S322. Update all sub-planning spaces according to the commander's preferred area:

[0039] like Then the i-th preference subplanning space is updated from point P i and point (P i x , 100) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between the obstacles, threat areas, simulated ocean currents in the two-dimensional simulated ocean current field model and the planning space boundary;

[0040] like Then the i-th subplanning space is updated to point P i and point (P i x , 0) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between the obstacles, threat areas, simulated ocean currents in the two-dimensional simulated ocean current field model and the planning space boundary;

[0041] Among them, P i x Represents P i The x-coordinate of the point, P i y Represents P i The y coordinate of the point, i∈N + .

[0042] Further, S33 includes:

[0043] S33. Update all sub-planning spaces based on the commander's intention:

[0044] S331. Set the commander's preferred area, which refers to the area that the commander intends the ship to pass through. The commander's preferred area is set to

[0045] Among them, P i , P i+1 To set the preference point, preference area To set the preference point P i and P i+1 The straight line area between.

[0046] S332. Update all sub-planning spaces according to the commander's preferred area:

[0047] like Then the i-th preference subplanning space is updated from point P i and point (P i x , 100) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between the obstacles, threat areas, simulated ocean currents and planned space boundaries;

[0048] like Then the i-th subplanning space is updated to point P i and point (P i x , 0) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between the obstacles, threat areas, simulated ocean currents and planned space boundaries;

[0049] Among them, P i x Represents P i The x-coordinate of the point, P i y Represents P i The y coordinate of the point, i∈N + .

[0050] Furthermore, in S4, the following operation strategies are adopted when performing route planning for the entire planning space and for all subspaces:

[0051] S41. According to the initial zero level set, the route planning evolution equation based on the level set method is established:

[0052]

[0053] Among them, V(X,t) is the ocean current velocity, and the ocean current velocity V(X,t) is V in S2 x (r), V y (r), V z (r) A vector with three components; X represents the instantaneous position of the ship, Y f represents the terminal position, φ is the level set function φ(X, t) constructed based on the ship motion and current velocity; is the partial derivative of the level set function, F AUV is the ship speed, F AUV To be set based on experience;

[0054] S42, according to the iterative time step, numerically iterate and solve the evolution equation. When the iteration reaches any position point Y t When 0≤|Y t -Y f |<Δl, it is judged that the ship has reached the terminal position Y f , execute S43; the iteration time step is set based on experience;

[0055] S43, when the ship reaches the terminal position Y f After that, the ship position is iterated back from the end point to the source point Y through the backtracking equation s , the optimal route within the current range can be obtained. The backtracking equation is:

[0056]

[0057] Where t represents the evolution time.

[0058] Furthermore, S5 includes:

[0059] S51, set loop parameters i=1, j=1;

[0060] S52, compare whether the cell Cell1(i) and the cell Cell1(i+j) are equal,

[0061] If not, execute S53;

[0062] If yes, execute S55;

[0063] S53, add 1 to the value of j to determine whether the value of j is less than or equal to the value of cell_num.

[0064] If yes, execute S52;

[0065] If not, execute S54;

[0066] S54, add 1 to the value of i, j = i, and determine whether the value of i is less than or equal to the length cell_num of the cell array Cell1.

[0067] If yes, execute S52;

[0068] If not, execute S6;

[0069] S55, delete the Cell1(i+j) element in the cell array Cell1 and the Cell2(i+j) element in the cell array Cell2, reduce the value of cell_num by 1, increase the value of j by 1, and execute S56;

[0070] S56. Determine whether j is less than or equal to cell_num. If so, execute S52; if not, execute S54.

[0071] The present invention realizes the efficient use of ocean currents to plan reliable and reference-worthy optimized routes with high differentiation for commanders to choose from, and the route quality is higher, the smoothness is better, and the planning speed is faster. It solves the problem of no effective alternative routes for commanders to choose in actual underwater route planning, and improves the auxiliary command decision-making ability of the mission planning system. In addition, the spatial subdivision level set method based on the commander's intention can well implement the commander's intention. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0073] Figure 1 It is a flow chart of the steps of the present invention;

[0074] Figure 2 It is a flow chart of the present invention;

[0075] Figure 3 It is a schematic diagram of two-dimensional ocean current simulation of the present invention;

[0076] Figure 4 This is a schematic diagram of the first sub-planning space of the present invention;

[0077] Figure 5 This is a schematic diagram of the third sub-planning space of the present invention;

[0078] Figure 6 This is a schematic diagram of the last sub-planning space of the present invention;

[0079] Figure 7 This is a schematic diagram of the preferred sub-planning space of the present invention;

[0080] Figure 8 It is a simulation diagram of the hybrid level set method based on the spatial partitioning of obstacles and threat areas of the present invention;

[0081] Fig. 9 This is a simulation diagram of the ant colony algorithm based on the spatial partitioning of obstacles and threat areas of the present invention;

[0082] Fig.10 This is a simulation diagram of the quantum particle swarm algorithm based on the spatial segmentation of obstacles and threat areas of the present invention;

[0083] Fig.11 This is a simulation diagram comparing three algorithms based on the spatial segmentation of obstacles and threat areas of the present invention;

[0084] Fig.12 This is a simulation diagram of the spatial subdivision level set algorithm based on the commander's intention in the present invention. DETAILED DESCRIPTION

[0085] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0086] Example 1

[0087] like Figure 1 , Figure 2 As shown, this embodiment provides a multi-route planning method, comprising the following steps:

[0088] S1. Initialize the planning space;

[0089] S2, construct a two-dimensional simulated ocean current field model;

[0090] S3, setting a virtual obstacle wall in the planning space to divide the planning space into p+q sub-planning spaces;

[0091] S4. Perform route planning on the entire planning space to obtain the optimal route; perform route planning on all subspaces to obtain p+q suboptimal routes; store the waypoints of the optimal route and p+q suboptimal routes in cell array Cell1 in sequence; store the flight time and flight length of p+q+1 routes in cell array Cell2;

[0092] S5, merge the suboptimal routes with the same sailing time and sailing length;

[0093] S6. Sort the suboptimal routes in the cell arrays Cell1 and Cell2 from small to large according to the values ​​of the flight duration of the suboptimal routes to obtain cell arrays Cell3 and Cell4; Cell3 is the sorted Cell1, and Cell4 is the sorted Cell2; and store the optimal route in the first cell of Cell3, and store the flight duration and the flight length of the optimal route in the first cell of Cell4, to complete the route planning.

[0094] Preferably, S1 planning space initialization includes:

[0095] S11, rasterizing the planned space into a 100×100 grid, setting the horizontal grid spacing to Δx and the vertical grid spacing to Δy;

[0096] S12, setting obstacles and threat areas, and setting the areas where obstacles are located and threat areas as no-fly zones; setting the source point position and the end point position, and setting the initial zero level set to a circular area with the source point position as the center and Δl / 2 as the radius;

[0097] S13, in the planning space after rasterization, select reference points based on experience and establish a two-dimensional coordinate system, where Δx is the unit length of the X axis and Δy is the unit length of the Y axis;

[0098] S14. Let the number of polygon vertices corresponding to the i-th obstacle be n i , then the vertex coordinate set of the ith obstacle is {O i，j :i,j∈N + and j≤n i}, represents the horizontal component of vertex j of the ith obstacle, Represents the longitudinal direction component of vertex j of the i-th obstacle.

[0099] S15. Let the coordinates of the center of the kth threat zone be The radius of the kth threat zone is r k ; The coordinates of the highest point in the kth threat zone are The coordinates of the lowest point are

[0100] Preferably, S2 constructs a two-dimensional simulated ocean current field model including:

[0101]

[0102] Among them, V x (r) is the horizontal component of the ocean current velocity V(X,t), V y (r) is the longitudinal component of the ocean current velocity V(X,t), V z (r) is the vertical component of the ocean current velocity V(X,t); κ is the eddy strength, ξ is the eddy radius coordinate, r i represents the position of the i-th vortex center, which is randomly generated, and n is the number of superimposed vortices; V cx V is a steady flow c The velocity component in the horizontal direction, V cy V is a steady flow c The velocity component in the longitudinal direction, V cz V is a steady flow c The velocity component in the vertical direction.

[0103] Specifically, considering the influence of ocean currents, it is necessary to model the ocean currents. Generally speaking, the smaller ocean currents that can cause ships to deviate can be roughly divided into wind currents, gradient currents, and tidal currents. In a short period of time, the ocean currents in a certain sea area can usually be understood as consisting of different eddies and steady currents. Therefore, this paper uses the superposition of multiple Lamb eddies and steady currents to model the smaller ocean currents that cause ships to deviate, that is, Formula 1. Figure 3 As shown, this is a schematic diagram of the two-dimensional ocean current simulation generated by the evolution equation of the level set method.

[0104] Preferably, S3 includes:

[0105] S31, setting a blank matrix A of a size of 2×(p+q), sorting the y components of the coordinates of the highest points of each obstacle and threat area, and placing the sorted coordinates into the matrix A in order;

[0106] S32, dividing the planning space into subspaces based on the matrix A and the upper and lower boundaries of the planning space;

[0107] S33. Update subspace based on commander's intention.

[0108] Preferably, S31 includes:

[0109] S311. The y component of the coordinates of the highest point among all vertices of each obstacle is:

[0110]

[0111] in, is the set of y-component coordinates of the highest point among all vertices of each obstacle, represents the longitudinal direction component of vertex j of the ith obstacle; n i is the number of vertices in the i-th obstacle;

[0112] The y component of the highest point of the kth threat zone is:

[0113]

[0114] in, is the y component of the highest point of the kth threat zone, is the y component of the center coordinate of the threat area; r k is the radius of the threat zone;

[0115] S312, let the number of obstacles between the source point and the end point be p, the number of threat areas be q, and q Sort them from large to small, where 1≤i≤p, 1≤k≤q;

[0116] S313, sort the p and q The y-component and x-component coordinate values ​​of the corresponding coordinate points are sequentially placed into the 2×(p+q) matrix A;

[0117] The x component is stored in the first row of matrix A, the y component is stored in the second row of matrix A, and the coordinate point of the i-th obstacle or threat area is (A(1, i), A(2, i)).

[0118] Preferably, S32 includes:

[0119] S321, such as Figure 4 As shown, the first sub-planning space is composed as follows:

[0120] The grid points between point (A(1,1), A(2,1)) and point (A(1,1),0) are set as virtual obstacle walls, and the grid points between point (A(1,1), A(2,1)) and point (A(1,1),0) are set as virtual obstacle walls;

[0121] Specifically, Figure 4 In the figure, the black solid polygon represents the obstacle, the black solid circle represents the threat area, the dotted line represents a virtual obstacle wall between the points (A(1, 1), A(2, 1)) and (A(1, 1), 0). The function of the obstacle wall is equivalent to that of the obstacle. The route must bypass the obstacle wall to reach the terminal position. The short arrow is the simulated ocean current.

[0122] S322, dividing the i-th sub-planning space:

[0123] The grid points between point (A(1, i-1), A(2, i-1)) and point (A(1, i-1), 100) are set as the i-th virtual obstacle wall, and the grid points between point (A(1, i), A(2, i)) and point (A(1, i), 0) are set as the i+1-th virtual obstacle wall;

[0124] The i-th sub-planning space is composed of the i-th virtual obstacle wall, the i+1-th virtual obstacle wall, obstacles, threat areas, simulated ocean currents and planning space boundaries; i is an integer greater than 1 and less than or equal to p+q;

[0125] Specifically, taking i=3 as an example, the third sub-planning space and its optimization path are shown as follows: Figure 5 As shown in the figure, each time a subspace planning is performed, each obstacle and threat area must be counted in the corresponding subplanning space.

[0126] S323, divide the p+qth sub-planning space:

[0127] The grid point between point (A(1, p+q), A(2, p+q)) and point (A(1, p+q), 100) is set as the final virtual obstacle wall;

[0128] The p+qth sub-planning space consists of the final virtual obstacle wall, obstacles, threat areas, simulated ocean currents and planning space boundaries.

[0129] like Figure 6 As shown, the final sub-planning space consists of a virtual obstacle wall, obstacles, threat areas, simulated ocean currents, and planning space boundaries between points (A(1, p+q), A(2, p+q)) and (A(1, p+q), 100).

[0130] Preferably, S33 includes:

[0131] S33, such as Figure 7 As shown, the updated subspace based on the commander's intention is:

[0132] Set the commander's preferred area, which refers to the area that the commander intends the ship to pass through. The commander's preferred area is

[0133] Where P i , P i+1 is the set preference point, and the preference area is the preference point P i and P i+1 The straight line area between.

[0134] Among them, P i x Represents P i The x-coordinate of the point, P i y Represents P i The y coordinate of the point, i∈N + ;

[0135] like Then the i-th preference subplanning space is updated from point P i and point (P i x , 100) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between them, as well as obstacles, threat areas, simulated ocean currents and planned space boundaries.

[0136] like Then the i-th preference subplanning space is updated from point P i and point (P i x , 0) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between them, as well as obstacles, threat areas, simulated ocean currents and planned space boundaries.

[0137] Specifically, in the actual underwater combat planning and ship route planning process, based on the current enemy and friendly situation and the commander's historical combat command experience, the commander often prefers to let the ship pass through a certain area or several areas and then safely and covertly reach the corresponding operation area to perform the mission. In two-dimensional space, the commander's preferred area can be represented by the interval between two points. Reaching the final position after intersection means the commander’s intention is achieved.

[0138] Preferably, S4 includes:

[0139] S41. According to the initial zero level set, the route planning evolution equation based on the level set method is established:

[0140]

[0141] Among them, V(X,t) is the ocean current velocity, and the ocean current velocity V(X,t) is V x (r), V y (r), V z (r) A vector with three components; X represents the instantaneous position of the ship, Y f represents the terminal position, φ is the level set function φ(X, t) constructed based on the ship motion and current velocity; is the partial derivative of the level set function, F AUV is the ship speed, F AUV To be set based on experience;

[0142] When the ship reaches the terminal position Y f When t=t(Y f ), the end point also falls within the zero level set, that is, φ(Y f ,t(Y f ))=0. t(Yf) is the optimal sailing time of the ship under the influence of ocean current.

[0143] Specifically, the basic idea of ​​the level set algorithm in the prior art to solve the problem is to construct a higher-dimensional level set function φ(X, t) in the space Ω, so that at any time t, the motion interface Γ(t) is the zero level set of φ(X, t), that is,

[0144]

[0145] For the convenience of expression, the level set function is abbreviated as φ; thus, the problem of physical quantities on the moving interface Γ, which is difficult to solve, is converted into first solving the level set function of a higher dimension that evolves with time, and then obtaining the zero level set curve, that is, the moving interface, according to the evolution time.

[0146] Taking two-dimensional space as an example, suppose l(t) is a smooth closed curve that changes with time and is equivalent to the interface Γ. The corresponding level set function is l(t), and the zero level set φ(l,t) represented by l(t) at time t is 0. Taking its derivative, we get:

[0147]

[0148] in, is the interface evolution speed. Assume the interface evolution rate is F and the direction is the normal direction of the curve Then the formula is obtained:

[0149]

[0150] When φ is the signed distance function, φ t =-F, when performing numerical iteration, the above partial differential equation is discretized to obtain φ n+1 =φ n -F·Δt, starting from the initial condition φ(X0,0)=0 and iterating step by step with the iteration step length Δl, the level set function and the zero level set function at each time step can be obtained.

[0151] The numerical iterative solution method is:

[0152]

[0153] In numerical calculation, the Cartesian grid spacing in two-dimensional space is assumed to be Δx, Δy, and the time step of evolution is Δt. represents φ(iΔx, jΔy, nΔt), that is, the value of the level set function at the coordinate (iΔx, jΔy) at time nΔt. Let v X 、v Y is the component of the ocean current velocity v(X, t) in the Cartesian coordinate system, then:

[0154]

[0155] Among them, φ X ,φ Y are the differential forms of the level set function in the X-axis and Y-axis directions respectively. The above formula can be discretized as:

[0156]

[0157] in,

[0158]

[0159] When the evolution term is numerically discretely solved, the evolution stability (CFL) condition that needs to be satisfied is: Generally have a is 0.5, where F X F AUV The component in the X-axis direction.

[0160] By backtracking through the following formula, we can get a navigation route with the best time.

[0161] S42, the backtracking equation is:

[0162]

[0163] The evolution equation is solved by continuous numerical iteration. When the iteration reaches the end position or near the end position, it is iterated back to the source position to obtain a theoretically time-optimal route.

[0164] S43, performing route planning on the entire planning space to obtain an optimal route;

[0165] S44. Perform route planning for all sub-planning spaces to obtain the p+q optimized routes, i.e., the suboptimal routes, of all sub-planning spaces.

[0166] The waypoints of the optimal route and p+q suboptimal routes are stored in the cell array Cell1 in sequence, and each cell is the waypoint value of the corresponding route; there are a total of p+q+1 cells in the array; the flight time and flight length of the p+q+1 routes are stored in the cell array Cell2; the lengths of Cell1 and Cell2 are both cell_num=p+q+1;

[0167] Preferably, S5 includes:

[0168] S51, set loop parameters i=1, j=1;

[0169] S52, compare whether the cell Cell1(i) and the cell Cell1(i+j) are equal,

[0170] If not, execute S53;

[0171] If yes, execute S55;

[0172] S53, add 1 to the value of j to determine whether the value of j is less than or equal to the value of cell_num.

[0173] If yes, execute S52;

[0174] If not, execute S54;

[0175] S54, add 1 to the value of i, j = i, and determine whether the value of i is less than or equal to the length cell_num of the cell array Cell1.

[0176] If yes, execute S52;

[0177] If not, execute S6;

[0178] S55, delete the Cell1(i+j) element in the cell array Cell1 and the Cell2(i+j) element in the cell array Cell2, reduce the value of cell_num by 1, increase the value of j by 1, and execute S56;

[0179] S56. Determine whether j is less than or equal to cell_num. If so, execute S52; if not, execute S54.

[0180] When the level set method is used for ship route planning, the movement of the ship is determined by the movement of the zero level set, and the evolution speed in the level set evolution equation is synthesized by the ocean current speed and the ship's own speed. The evolution equation is:

[0181]

[0182] The initial condition is: φ(X,0)=|XY s |. Among them, F AUV (X, t) is the ship's own speed, that is, the speed of the ship relative to the ocean current. In order to ensure that the desired route is time-optimal, F is required AUV The direction of (X, t) is the gradient direction of the level set function, so the above equation can be further transformed into the following Hamilton-Jacobi equation, which is the evolution equation of ship route planning based on the level set method:

[0183]

[0184] When the ship reaches the terminal position Y f When t=t(Y f ), the end point also falls within the zero level set, that is, φ(Y f ,t(Y f ))=0. At this time, t(Y f ) is the optimal sailing time for ships under the influence of ocean currents. Then, by backtracking through the following formula, we can get a sailing route with the best time. The backtracking equation is:

[0185]

[0186] From the above analysis, it can be seen that when the level set method is used for ship route planning, the ocean current factors are perfectly integrated into the partial differential equation in a very natural way. Combined with the narrowband reinitialization and fast marching method, the evolution equation is solved by continuous numerical iteration. When the iteration reaches the end position or near the end position, it is backtracked to the source position to obtain a theoretically optimal route in terms of time.

[0187] Example 2

[0188] This example compares the spatial partitioning method of the present invention with the spatial partitioning method using the ant colony algorithm and the quantum particle swarm algorithm in the prior art:

[0189] The source point position is set to (10,35), the end point is set to (65,70), the ship speed is set to 2.5m / s, and the ocean current is the superposition of three eddies and a steady ocean current, where the intensity and radius κ and ξ of each eddy are randomly selected from 5 to 10 and 3m to 6m, respectively. In the level set method, μ is 0.5, and Δx and Δy are both 1. In the localized equation, γ and β are set to 4 and 2, respectively. The initial level set curve is a circle with the center at the starting point of the voyage (10, 15) and a radius of 2m. The narrow band width is 6m during numerical calculation, and the step length for each journey is 1m. The number of iterations of the ant colony algorithm and the quantum particle swarm algorithm are both 200. In this embodiment, the first three optimized routes are selected for comparison during multi-route planning.

[0190] The results of multi-route planning based on the hybrid level set method of spatial partitioning of obstacles and threat areas are as follows: Figure 7 shown.

[0191] from Figure 8 It can be seen that along the three planned optimized routes, ships can better use the ocean current to navigate to the terminal position, and can successfully avoid obstacles and threat areas; at the same time, the three routes have low similarity and high smoothness, which can assist the commander's combat decision-making. The simulation results show that the algorithm can plan multiple optimized routes with large differentiation between a single source point and a single terminal position, which is reliable, effective and practical.

[0192] The multi-route planning results of the ant colony algorithm based on the spatial partitioning of obstacles and threat areas, such as Fig. 9 shown.

[0193] from Fig. 9It can be seen that along the three optimized routes planned by the ant colony algorithm, the ship can also use the ocean current to sail to the terminal position, and can successfully avoid obstacles and threat areas; however, the similarity between the best and the second best 1 routes among the three routes planned by the ant colony algorithm is relatively high, and they are only separated at the threat area 1, and there are many turns and the smoothness is not high. The simulation results show that the algorithm can plan multiple optimized routes with a certain degree of distinction between a single source point and a single terminal position, and can also play a certain auxiliary role in the commander's combat decision-making, but the route smoothness is poor.

[0194] The multi-route planning results of quantum particle swarm algorithm based on spatial segmentation of obstacles and threat areas, such as Fig.10 shown.

[0195] from Fig.10 It can be seen that, like the previous two algorithms, along the three optimized routes planned by the quantum particle swarm algorithm, the ship can also make better use of the ocean current to safely navigate to the terminal position while completely avoiding obstacles and threat areas; generally speaking, the smoothness of the planned route is better than that of the ant colony algorithm, but worse than that of the level set method.

[0196] To make a clearer comparison of the performance of the three algorithms for planning multiple routes based on the spatial segmentation of obstacles and threat areas, we put them in one figure for comparison, such as Fig.11 shown.

[0197] from Fig.11 It is easy to see that compared with the ant colony and particle swarm algorithms, the hybrid level set method based on the spatial division of obstacles and threat areas can more effectively utilize the ocean currents and the planned routes are smoother. In order to more clearly analyze the performance of the algorithms, this section simulates the three algorithms 100 times each, and takes the average of the navigation time, navigation length and total planning time of the three routes of each algorithm as shown in Table 1.

[0198] Table 1 Comparison of multi-route performance of three algorithms

[0199]

[0200] It can be seen from Table 1 that in terms of navigation time and navigation length, the three routes planned by the hybrid level set method based on spatial decomposition of obstacles and threat areas are the best, followed by the quantum particle swarm, and the ant colony is the worst; in terms of planning efficiency, the hybrid level set method is the fastest, followed by the ant colony, and the quantum particle swarm is the slowest. The planning efficiency of the hybrid level set method is about 9 times that of the ant colony algorithm and about 13 times that of the quantum particle swarm.

[0201] In addition to planning the optimal route, it is assumed that the commander intends to make the planned route pass through the following two areas: the area between point (56,50) and point (62,48), and the area between point (37,90) and point (40,85). Figure 2 The algorithm flow in the simulation experiment is to select the area that the route must pass through according to the commander's intention when dividing the space, and obtain the following Fig.12 result.

[0202] Fig.12 In , the area between the two * is the area that the commander intends the mine-laying system to pass through. Obviously, the two suboptimal routes planned by the algorithm implement the commander's intention, and the routes are relatively smooth. Therefore, the spatial subdivision level set method based on the commander's intention proposed in this section is reliable, effective, robust and practical.

[0203] The performance of the three optimized routes planned based on the commander’s intention is shown in Table 2 below.

[0204] Table 2 Multi-route performance based on commander's intention

[0205]

[0206] As can be seen from Table 2, of the two suboptimal routes that implement the commander's intention, the navigation time and navigation length of suboptimal route 1 are both shorter than those of suboptimal route 2. Obviously, the quality of suboptimal route 1 is better than that of suboptimal route 2. If the influence of other factors is no longer considered, the commander will choose from the optimal route and the suboptimal route 1 that implements the commander's intention. Obviously, the algorithm proposed in this section can not only implement the commander's intention well, but also assist the commander in better carrying out combat planning and decision-making through route performance comparison.

[0207] Beneficial effects:

[0208] The present invention realizes the efficient use of ocean currents to plan reliable and reference-worthy optimized routes with high differentiation for commanders to choose from, and the route quality is higher, the smoothness is better, and the planning speed is faster. It solves the problem of no effective alternative routes for commanders to choose in actual underwater route planning, and improves the auxiliary command decision-making ability of the mission planning system. In addition, the spatial subdivision level set method based on the commander's intention can well implement the commander's intention.

[0209] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-route planning method, characterized in that: The following steps are involved: S1. Initialize the planning space; S2, construct a two-dimensional simulated ocean current field model; S3, setting a virtual obstacle wall in the planning space to divide the planning space into p+q sub-planning spaces; The S3 includes: S31, setting a blank matrix A of a size of 2×(p+q), sorting the y components of the coordinates of the highest points of each obstacle and threat area, and placing the sorted coordinates into the matrix A in order; The S31 includes: S311. The y component of the coordinates of the highest point of each obstacle is: in, is the set of y components of the coordinates of the highest point of each obstacle, represents the longitudinal direction component of vertex j of the ith obstacle; n i is the number of vertices in the i-th obstacle; The y component of the highest point of the k-th circular threat area is: in, is the y component of the highest point of the kth threat zone, is the y component of the center coordinate of the threat area; r k is the radius of the threat zone; S312, let the number of obstacles between the source point and the end point be p, the number of threat areas be q, and q Sort them from large to small, where 1≤i≤p, 1≤k≤q; S313, sort the p and q The y-component and x-component coordinate values ​​of the corresponding coordinate points are sequentially placed into a 2×(p+q) blank matrix A; The x component is stored in the first row of matrix A, and the y component is stored in the second row of matrix A. The coordinate point of the i-th obstacle or threat area is (A(1, i), A(2, i)); S32, dividing the planning space into subspaces based on the matrix A and the upper and lower boundaries of the planning space; The S32 includes: S321, dividing the first sub-planning space: The grid point between point (A(1,1), A(2,1)) and point (A(1,1),0) is set as the first virtual obstacle wall; the first sub-planning space is composed of the first virtual obstacle wall, obstacles, threat areas, simulated ocean currents and planning space boundaries; S322, dividing the i-th sub-planning space: The grid points between point (A(1, i-1), A(2, i-1)) and point (A(1, i-1), 100) are set as the i-th virtual obstacle wall, and the grid points between point (A(1, i), A(2, i)) and point (A(1, i), 0) are set as the i+1-th virtual obstacle wall; The i-th sub-planning space is composed of the i-th virtual obstacle wall, the i+1-th virtual obstacle wall, obstacles, threat areas, simulated ocean currents and planning space boundaries; i is an integer greater than 1 and less than or equal to p+q; S323, divide the p+qth sub-planning space: The grid point between point (A(1, p+q), A(2, p+q)) and point (A(1, p+q), 100) is set as the final virtual obstacle wall; The p+qth sub-planning space consists of the final virtual obstacle wall, obstacles, threat areas, simulated currents, and planning space boundaries; S33, setting a commander's preferred area, and updating the subspace according to the commander's preferred area; the commander's preferred area is a straight line area that a ship needs to pass through when sailing, which is set according to experience; S4, using the level set method to perform route planning on the entire planning space processed by S1 to obtain the optimal route; at the same time, using the level set method to perform route planning on each of the p+q sub-planning spaces to obtain a total of p+q sub-optimal routes; storing the coordinate values ​​of the optimal route and the p+q sub-optimal routes in cell array Cell1 in sequence; storing the navigation duration and navigation length of the p+q+1 routes in cell array Cell2; the length cell_num of Cell1 and Cell2 are both p+q+1; S5, merge the suboptimal routes with the same sailing time and sailing length; S6, sorting the suboptimal routes in the cell arrays Cell1 and Cell2 from small to large according to the values ​​of the flight duration of the suboptimal routes, to obtain cell arrays Cell3 and Cell4; Cell3 is the sorted Cell1, and Cell4 is the sorted Cell2; The optimal route is stored in the first cell of Cell3, and the flight time and length of the optimal route are stored in the first cell of Cell4 to complete the route planning.

2. A multi-route planning method according to claim 1, characterized in that: The S1 planning space initialization includes: S11, rasterizing the planned space into a grid, setting the horizontal grid spacing to Δx and the vertical grid spacing to Δy; S12, setting obstacles and threat areas, and setting the areas where obstacles are located and threat areas as no-fly zones; setting the source point position and the end point position, and setting the initial zero level set to a circular area with the source point position as the center and Δl / 2 as the radius; S13, in the planning space after rasterization, select reference points based on experience and establish a two-dimensional coordinate system, where Δx is the unit length of the X axis and Δy is the unit length of the Y axis; S14. Let the number of polygon vertices corresponding to the i-th obstacle be n i , then the vertex coordinate set of the i-th obstacle is represents the horizontal component of vertex j of the ith obstacle, represents the longitudinal direction component of vertex j of the i-th obstacle; S15. Let the coordinates of the center of the kth threat zone be The radius of the kth threat zone is r k ; The coordinates of the highest point in the kth threat zone are The coordinates of the lowest point are 3. A multi-route planning method according to claim 1, characterized in that: The S2 constructs a two-dimensional simulated ocean current field model including: Where r is the position vector of the ship in the ocean current field, V x (r) is the horizontal component of the ocean current velocity V(X,t), V y (r) is the longitudinal component of the ocean current velocity V(X,t), V z (r) is the vertical component of the ocean current velocity V(X,t); κ is the eddy strength, ξ is the eddy radius coordinate, r i represents the position of the i-th vortex center, which is randomly generated, and n is the number of superimposed vortices; V cx V is a steady flow c The velocity component in the horizontal direction, V cy V is a steady flow c The velocity component in the longitudinal direction, V cz V is a steady flow c The velocity component in the vertical direction.

4. A multi-route planning method according to claim 1, characterized in that: The S33 includes: S331, set the commander's preference area, the commander's preference area is set to Among them, P i , P i+1 To set the preference point, preference area To set the preference point P i and P i+1 The straight line area between S332. Update all sub-planning spaces according to the commander's preferred area: like Then the i-th preference subplanning space is updated from point P i and point (P i x , 100) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between the obstacles, threat areas, simulated ocean currents in the two-dimensional simulated ocean current field model and the planning space boundary; like Then the i-th subplanning space is updated to point P i and point (P i x , 0) between the virtual obstacle wall and point P i+1 and Point The virtual obstacle wall between the obstacles, threat areas, simulated ocean currents in the two-dimensional simulated ocean current field model and the planning space boundary; Among them, P i x Represents P i The x-coordinate of the point, P i y Represents P i The y coordinate of the point, i∈N + .

5. A multi-route planning method according to claim 3, characterized in that: When performing route planning for the entire planning space and for all subspaces in S4, the following operation strategies are adopted: S41. According to the initial zero level set, the route planning evolution equation based on the level set method is established: Among them, V(X,t) is the ocean current velocity, and the ocean current velocity V(X,t) is V x (r), V y (r), V z (r) A vector with three components; X represents the instantaneous position of the ship, Y f represents the terminal position of the ship, φ is the level set function φ(X, t) constructed based on the ship motion and current velocity; is the partial derivative of the level set function, F AUV is the ship speed, F AUV To be set based on experience; S42, according to the iterative time step, numerically iterate and solve the evolution equation. When the iteration reaches any position point Y t When 0≤|Y t -Y f |<Δl, it is judged that the ship has reached the terminal position Y f , execute S43; the iteration time step is set based on experience; S43, when the ship reaches the terminal position Y f After that, the ship position is iterated back from the end point to the source point Y through the backtracking equation s , the optimal route within the current range can be obtained. The backtracking equation is: Where t represents the evolution time.

6. A multi-route planning method according to claim 1, characterized in that: The S5 includes: S51, set loop parameters i=1, j=1; S52, compare whether the cell Cell1(i) and the cell Cell1(i+j) are equal, If not, execute S53; If yes, execute S55; S53, add 1 to the value of j to determine whether the value of j is less than or equal to the value of cell_num. If yes, execute S52; If not, execute S54; S54, add 1 to the value of i, j = i, and determine whether the value of i is less than or equal to the length cell_num of the cell array Cell1. If yes, execute S52; If not, execute S6; S55, delete the Cell1(i+j) element in the cell array Cell1 and the Cell2(i+j) element in the cell array Cell2, reduce the value of cell_num by 1, increase the value of j by 1, and execute S56; S56. Determine whether j is less than or equal to cell_num. If so, execute S52; if not, execute S54.

Images