CN111307159B - Multi-AUV three-dimensional collaborative route planning method - Google Patents

Abstract

The invention discloses a multi-AUV three-dimensional collaborative route planning method, which mainly comprises the following steps: three-dimensional environment abstract modeling and initialization of a level set function, level set function evolution, optimal route point selection, collaborative route scheme design, route conflict judgment and re-planning and planning result output. According to the collaborative planning method, a plurality of air routes can be planned in the air space at the same time by improving the level set algorithm, the planning efficiency of the algorithm is improved, all AUVs are required to reach the destination at the same time when the collaborative scheme is designed, each AUV needs to start one by one according to delay time, in addition, under the condition of considering rapidity and concealment, a collaborative planning scheme with optimum combination of concealment and air time is designed, the influence of ocean current and sound velocity factors is considered at the same time, and a conflict judgment and re-planning link is added, so that the collaborative air routes are safer; compared with two-dimensional route planning, the three-dimensional route planning has higher practicability and can better meet the actual navigation requirement.

Description

A 3D collaborative route planning method for multiple AUVs

technical field

The invention belongs to the technical field of three-dimensional path planning of underwater submersibles, and relates to a multi-AUV three-dimensional collaborative route planning method, in particular to a multi-AUV three-dimensional collaborative route planning method considering the influence of the marine environment.

Background technique

At present, most of the research is limited to the route planning problem of a single AUV, and there is a lack of research on the multi-route collaborative planning problem from multiple origins to multi-destinations under the influence of the marine environment. In the actual situation, in order to improve the completion rate of the task, all AUVs need to reach the multi-task point at the same time. At this time, how to generate an effective route for each AUV and coordinate the arrival time of each AUV is one of the prerequisites for completing the task. Due to the needs of practical applications, the concept of multi-AUV has emerged, that is, on the basis of the application of a single AUV, some complex underwater target tasks can be completed by coordinating multiple AUVs with each other. This not only makes up for the shortcomings of a single AUV in performing tasks, but also improves work efficiency and saves the cost of researching more complex AUVs, and multi-AUVs demonstrate the superiority in performing multi-cooperative tasks. In addition, the complex functions that can be realized by multiple AUVs cannot be possessed by a single AUV. Therefore, the research on multiple AUVs has increasingly attracted great attention from scholars all over the world.

The Level Set Method (Level Set Method) was proposed by American mathematicians Stanley Osher and James Sethian and developed from the research field of simulating interface evolution. It is an effective calculation tool for geometric topology in the process of interface evolution. The core of the improved level set algorithm of the present invention is to use the ocean current, the gravity of the concealed field and the speed of the AUV itself as the driving force for the evolution of the level set function, and the gradient direction of the zero level set curve is the optimal solution. At present, the level set algorithm has a complete mathematical theoretical basis, its concept is relatively simple, easy to program and realize, and it does not have high requirements for the speed and storage of computer hardware. As an emerging optimization algorithm, it has shown good application prospects in many fields. Patent CN201810519546.6 discloses a UUV path planning method based on multi-objective optimization of energy consumption and sampling volume under the influence of complex marine environments. The marine environmental information considered is only ocean currents, and marine environmental information such as temperature and salinity are not considered. Patent CN201610887874.2 discloses a time-optimal route planning method based on level set algorithm. It only designs a single route planning algorithm for a single AUV under the influence of ocean currents in the ocean environment, and does not consider multiple AUVs for collaborative route research. , there is no published literature that applies the level set algorithm to the study of 3D collaborative route planning considering the influence of the marine environment.

Contents of the invention

In view of the above-mentioned prior art, the technical problem to be solved by the present invention is to provide a multi-AUV three-dimensional collaborative route planning method that can simultaneously plan multiple optimal routes in an iterative cycle process and incorporate the influence of ocean current and sound velocity.

In order to solve the above technical problems, a multi-AUV three-dimensional collaborative route planning method of the present invention includes the following steps:

Step 1: Carry out three-dimensional environment modeling according to the electronic chart and ocean current, temperature, salinity and depth information, and establish terrain field, ocean current field and concealment field; perform level set function initialization, and initialize the level set function as a starting point The center of the circle, r is the circular function of the radius, and the appropriate narrow band width d is set to make the level set function evolve in the narrow band;

Step 2: Level set function evolution: The level set function considers the AUV's own speed, ocean current speed and concealed field to get an improved level set function evolution mathematical model, evolves according to the improved level set function evolution mathematical model, and reconstructs when it reaches the edge of the narrow band Level set function and narrow band width, reinitialize the level set function to a new distance function, and calculate the new narrow band width, judge whether to reach each end point, and save all zero level set curves before reaching this end point every time an end point is reached ;

Step 3: Optimal waypoint selection: From the end point of each zero level set curve, perform forward iterations to find the gradient direction point of each zero level set curve until the starting point, and sequentially connect all gradient direction points to obtain all optimal routes;

需要从n种航路组合中选出m个AUV到达m个任务点的m条航路，设定所有航路为H＝{H_1,1,H_1,2,…,H_1,m；H_2,1,H_2,2,…,H_2,m；…；H_n,1,H_n,2,…,H_n,m}，其中H_n,m代表第n种方案中第m艘AUV的航行路线，因此H表示n个方案中的所有AUV的航行路线；Step 4: Design of collaborative route allocation scheme: use the improved level set function evolution mathematical model obtained in step 2 to simultaneously plan all m×m routes from m origins to m destinations, and the planned m×m routes total There are n route combinations, among which

It is necessary to select m routes for m AUVs to reach m mission points from n route combinations, and set all routes as H={H _1,1 ,H _1,2 ,...,H _1,m ; H _{2, 1} ,H _2,2 ,…,H _2,m ;…;H _n,1 ,H _n,2 ,…,H _n,m }, where H _n,m represents the mth AUV in the nth scheme Navigation route, so H represents the navigation routes of all AUVs in n schemes;

AUV navigation energy consumption is set as:

Where k is the energy consumption coefficient, V _ax represents the X direction component in the AUV's own speed; V _ay represents the Y direction component in the AUV's own speed; L _x =|L _xi+1 -L _xi | represents the X direction component of the route Route length; L _y =|L _yi+1 -L _yi |represents the route length of the Y direction component of the route;

The energy loss N representing all routes in each combination is thus obtained:

N={N _1,1 ,N _1,2 ,...,N _1,m ; N _2,1 ,N _2,2 ,...,N _2,m ;...;N _n,1 ,N _n,2 ,... ,N _n,m };

Assuming that the selected route is H _i ={H _i,1 ,H _i,2 ,…H _i,m }, where the sailing time T _i ={T _i,1 ,T _i,2 ,…T _i,m }, the voyage Energy consumption N _i ={N _i,1 ,N _i,2 ,…N _i,m }, the concealment of the route, that is, the sum of all gravitational forces on the route nodes, is: M _i ={M _i,1 , M _i,2 ,...M _i,m }, route intersection D _i ={D _i,1 ,D _i,2 ,...D _i,m };

Constraints (1):

Max{N _i }≤N _max

Constraint condition (1) shows that: N _max represents the maximum energy carried by each AUV itself, that is, the energy consumption of all routes in the selected route must not exceed the maximum energy carried by the AUV itself;

Constraints (2):

Constraint condition (2) shows that: the arrival time of different AUVs at the intersection point of the route cannot be the same, that is, there is no time conflict point between different routes;

Constraint (3): Each task can only be assigned or executed once, that is, the task is unique;

)条协同航路规划方案，然后计算出n中每个方案的时间最优矩阵Ta＝{Ta₁,Ta₂,…,Ta_n}以及隐蔽性最优矩阵Ma＝{Ma₁,Ma₂,…,Ma_n}，利用公式C＝p·Ta+q·Ma计算每个方案的协同目标函数值C，其中p为设定的时间影响因子，q为设定的隐蔽性影响因子，并且根据目标函数值进行从小到大排列所有的航路方案，并存入数组S中，其中目标函数值最小值对应的航路方案为最优的路线方案；Put all the routes into a cell array A of m×m. According to the constraint condition (3), it can be seen that the AUV cannot reach the same mission point, so the combination of n (where

) collaborative route planning schemes, and then calculate the time optimal matrix Ta={Ta ₁ ,Ta ₂ ,…,Ta _n } and concealment optimal matrix Ma={Ma ₁ ,Ma ₂ ,… ,Ma _n }, use the formula C=p·Ta+q·Ma to calculate the collaborative objective function value C of each scheme, where p is the set time influence factor, q is the set concealment influence factor, and according to the target Arrange all the route schemes from small to large according to the function value, and store them in the array S, wherein the route scheme corresponding to the minimum value of the objective function value is the optimal route scheme;

Step 5: Select the optimal route plan from the array S for route conflict determination, including collision conflict determination and energy conflict determination. When there is a collision conflict or energy conflict, perform step 6, otherwise output the current route plan, multi-AUV three-dimensional coordination Route planning is completed;

Among them, the collision conflict judgment is specifically as follows:

(1) Express all routes H _i in the form of line segments, express H _i,1 as all discrete line segments in the i-th route, H _i,1 = (X _1,1 ,X _{1, 2} ,…,X _1,n1 ), where X _1,1 represents a line segment composed of two adjacent discrete points in this route, and n1 means that the route is composed of n1 line segments, so the route is re-expressed as H _i = {(X _1,1 ,X _1,2 ,…,X _1,n1 ), (X, _2,1 ,,X _2,2 ,…,X _2,n2 )…(X _m,1 ,X _m,2 ,...,X _m,nm )};

(2) The linear function expression of the line segment composed of adjacent discrete points in the known route is y=kx+b, so the route is re-expressed as H _i ={(K _1,1 ,K _1,2 ,…,K _1,n1 ),(K _2,1 ,K _2,2 ,…,K _2,n2 ),…,(K _m,1 ,K _m,2 ,…,K _m,nm )}, where K _{m, nm} represents the function expression of the line segment X _m,nm ;

(3) Then compare each line segment in each route with each line segment in other routes, and find all intersection points within the range of the line segment, that is, route intersection D _i ={D _i,1 ,D _i,2 ,...D _i,m }; Calculate the time difference between two AUVs arriving at the intersection point. When the time difference between arriving at the intersection point is less than the set safety time difference, there is a collision conflict;

The determination of energy conflict is specifically as follows: set navigation energy consumption N _i ={N _i,1 ,N _i,2 ,…N _i,m }, determine the maximum route energy consumption Max(N _i ) in the scheme and the rated energy consumption carried by the AUV The relationship between energy P, when Max(N _i )>P, there is an energy conflict;

Step 6: Carry out route re-planning, remove route schemes with collision conflicts or energy conflicts from the array S, and perform step 5.

The present invention also includes:

1. The 3D environment modeling in step 1 is specifically as follows:

First, divide the real ocean environment space for AUV route planning into lon×lat×depth grids. The grid division principle is: the grid spacing in the longitude direction and latitude direction is consistent with the resolution of the navigation area grid water depth data file , the grid spacing in the depth direction should not exceed 1/10 of the maximum depth value of the navigation area;

Modeling the terrain field of the AUV navigation area: it is realized by reading the grid water depth data file of the navigation area, and the area below the water depth value read from the file in the navigation space is set as a no-go area, no-go area The AUV speed data, ocean current data, and concealed field gravity data contained in the grid are set to 0, which is used to constrain the AUV's navigation depth;

The current field modeling of the AUV navigation area is realized by reading the grid current data file of the navigation area. If the resolution of the current data file is lower than the resolution of the water depth data file, the current data file is linearly Interpolation processing, on the contrary, the ocean current data file is sparsely processed until the resolution of the ocean current data file is the same as that of the water depth data file, so that each grid contains the ocean current information of the current position; the ocean current size in the depth direction is set to 0;

Carry out concealed field modeling in the AUV navigation area: first read the grid temperature, salinity, and depth data files of the navigation area, perform the same processing as the ocean current data file, and then use the temperature, salinity, and depth information in the AUV navigation area Calculate the sound velocity information c of different grid points in the navigation area, know the position of the detection sonar, and calculate the sound propagation loss under the influence of the sound velocity in the AUV navigation area, and the calculation of the sound propagation loss is specifically:

The sound transmission loss TL in the AUV navigation area is:

TL＝20logR+α·R

β为设定的声速影响系数；where R is the distance, α is the absorption coefficient,

β is the set sound velocity influence coefficient;

It is known that the figure of merit of the detection sonar is FOM, and according to the relationship between the sound propagation loss and the sonar figure of merit, the navigation space is divided into exposed areas, dangerous areas and concealed areas, specifically:

When TL>FOM, the area is a hidden area;

时，该区域为暴露区；when

, this area is the exposure area;

该区域为危险区；when

This area is a danger zone;

The hidden field modeling of the navigation area is carried out by calculating the gravitational value of the AUV in different regional grid points of the navigation area, specifically:

In the navigation space, the concealed field is represented by a gravitational field. The divided concealed area, dangerous area, and exposed area have different gravitational forces on the AUV. The gravitational force of the concealed area is set to a fixed value F=f, where f is the concealment effect on the AUV. The factor affected by the route planning algorithm is set to be twice the maximum current value in the navigation area, the gravity of the exposed area is 0, and the gravity of the dangerous area is set as:

Where R is the distance between the current position of the AUV and the sonar position, and R _TL is the position distance when TL=FOM.

2. The initialization of the level set function in step 1 is specifically:

Let the level set function be φ(X,t), where X represents a smooth closed curve, and at time t, use the evolution curve X(t) to represent the zero level set curve at the current moment φ(X,t)= 0, initialize the zero level set curve as a circle with the starting point as the center and r as the radius, set d as the narrow band width, where d>r, the level set function is a distance function, and the level set function in the zero level set curve The value of the level set function is negative, and the value of the level set function outside the zero level set curve is positive. In order to ensure that the zero level set curve can reach the end point, the level set function only evolves in the direction where the value of the level set function is positive. The level set function Initialized as:

φ(X,t)＝0:(xX ₁ ) ² +(yY ₁ ) ² +(zZ ₁ ) ² ＝r ²

Wherein x, y, z are grid point coordinates, (X ₁ , Y ₁ , Z ₁ ) are starting point coordinates.

3. The mathematical model of level set function evolution in step 2 is specifically:

为航行方向，

为吸引力的方向。Among them, V(H,t) is the ocean current, U is the speed of the AUV itself, M is the size of the concealment field, and H represents a position vector in the navigation space;

is the sailing direction,

the direction of attraction.

Beneficial effects of the present invention: In the multi-AUV three-dimensional collaborative route planning method proposed by the present invention, the level set algorithm is improved and applied to it, and the designed collaborative scheme first requires that all AUVs can make full use of the ocean current to reach the destination as quickly as possible, and In route planning, it can avoid sonar detection and plan a cooperative route with the best concealment. Compared with the traditional level set algorithm, it is more flexible. It can plan multiple optimal routes in an iterative cycle at the same time, and add ocean currents and sound speeds. The design considers the concealment link, improves the practicability in the actual marine environment, and realizes that the AUV three-dimensional collaborative route can carry out route re-planning in case of conflict.

Description of drawings

Fig. 1 is a flow chart of the multi-AUV three-dimensional cooperative route planning method considering the influence of the marine environment proposed by the present invention.

Fig. 2 is a flow chart of the improved level set algorithm used in the present invention.

Fig. 3 is a flowchart of the collaborative planning solution in the present invention.

Detailed ways

The specific embodiments of the present invention will be further described below in conjunction with the accompanying drawings.

A multi-AUV three-dimensional collaborative route planning method of the present invention, the steps mainly include:

Step 1: Construct the 3D route planning environment space based on the electronic chart and current, temperature, salinity and depth information, and establish the terrain field, current field and concealment field; carry out the abstract modeling of the 3D environment and the initialization of the level set function, and the level set The function is initialized as a circular function with the starting point as the center and r as the radius, and an appropriate narrow band width d is set to make the level set function evolve within the narrow band.

Step 2: Level set function evolution. The level set function evolves under the joint influence of the AUV's own speed, ocean current speed and concealment field, and evolves according to the improved level set function evolution equation. When reaching the edge of the narrow band, the level set function and narrow band width are reconstructed, and the level set function is reinitialized It is a new distance function, and calculate the new narrow band width, judge whether to reach each end point, and save all zero level set curves before reaching this end point every time an end point is reached.

Step 3: Optimal waypoint selection. From the end point of each zero level set curve, forward iterations are performed to find the gradient direction point of each zero level set curve until the starting point, and all the gradient direction points are sequentially connected to obtain all the optimal routes.

Step 4: Coordinate route allocation scheme design. In the present invention, the cooperative route scheme considers the optimal fusion of AUV flight time and concealment. Therefore, when designing the scheme, it is necessary to select the route combination scheme with the best sailing time and concealment among all AUV cooperative route schemes. In order to further consider the concealment issue, all AUVs are required to arrive at the destination at the same time, so the departure time of each AUV is different.

Step 5: Route conflict determination and re-planning. The route conflict determination includes the collision during the AUV route and the situation that the AUV cannot reach the target point due to energy problems. The collision problem is mainly caused by the simultaneous intersection of multiple AUVs in terms of position and time. The energy problem is mainly caused by the fact that the energy carried by the AUV cannot support the AUV to reach the target point. If any of the two situations occurs, this synergy combination will be invalid. Carry out route re-planning, re-planning is mainly to select the suboptimal route combination in the plan, and then perform conflict determination again.

Step 6: Output the currently selected collaborative route when the algorithm is satisfied to select a non-conflicting route combination plan, and the multi-AUV three-dimensional collaborative route planning ends.

In conjunction with accompanying drawing 1 to accompanying drawing 3, the concrete steps of the present invention are as follows:

Step 1: 3D environment modeling and level set function initialization.

Step 1.1 Construct three-dimensional route planning environment space based on electronic chart and marine environment information

Firstly, the real marine environment space for AUV route planning is divided into lon×lat×depth grids. The principle of grid division is: the grid spacing in the longitude direction and latitude direction is consistent with the resolution of the grid water depth data file in the navigation area, and the grid spacing in the depth direction should not exceed 1/10 of the maximum depth value in the navigation area. Modeling of terrain field, current field and concealment field in AUV navigation area.

The modeling of the terrain field in the AUV navigation area is realized by reading the grid water depth data file of the navigation area. The area below the water depth value read from the file in the navigation space is set as a no-navigation area. The AUV speed data, ocean current data and concealed field gravity data contained in the grid are set to 0, which is used to constrain the AUV's navigation depth and ensure the safety of the AUV.

The ocean current field modeling in the AUV navigation area is realized by reading the grid ocean current data file in the navigation area. If the resolution of the ocean current data file is lower than the resolution of the water depth data file, linear interpolation is performed on the ocean current data file , on the contrary, the current data file is sparsely processed until the resolution of the current data file is the same as that of the water depth data file, so that each grid contains the current information of the current position; the size of the current in the depth direction is set to 0.

Concealed field modeling in the AUV navigation area: first read the grid temperature, salinity, and depth data files in the navigation area, perform the same processing as the current data file, and then calculate based on the temperature, salinity, and depth information in the AUV navigation area The sound velocity information of the navigation area, the position of the detection sonar is known, and the sound propagation loss under the influence of the sound velocity of the AUV navigation area is calculated. It is known that the figure of merit of the detection sonar is FOM. The relationship between the navigation space is divided into exposed area, dangerous area and hidden area, and the hidden field modeling of the navigation area is carried out by calculating the gravitational value of the AUV in different areas of the navigation area.

(1) Sound velocity calculation

According to the temperature, salinity, and depth data of the AUV navigation area, the sound velocity c of different grid points in the AUV navigation area is obtained according to the conventional sound velocity formula.

(2) Calculation of sound propagation loss and concealed field gravity in navigation area

The sound transmission loss TL in the AUV navigation area is:

TL＝20logR+α·R

β为声速影响系数设为0.5。where R is the distance, α is the absorption coefficient,

β is the speed of sound influence coefficient set to 0.5.

When TL>FOM, the area is a hidden area;

时，该区域为暴露区；when

, this area is the exposure area;

该区域为危险区；when

This area is a danger zone;

In the navigation space, the gravitational field is used to represent the hidden field, and the divided hidden area, dangerous area and exposed area have different gravitational forces on AUV. The gravity of the hidden area is set to a fixed value F=f, where f is the factor that the concealment affects the AUV route planning algorithm, which is set to be twice the maximum current value in the navigation area, the gravity of the exposed area is 0, and the gravity of the dangerous area is 0. Set to:

Where R is the distance between the current position of the AUV and the sonar position, and R _TL is the position distance when TL=FOM.

The hidden area has the largest gravitational force on the AUV, and the AUV is the easiest to choose to enter the hidden area when planning the route; the gravitational force in the dangerous area increases with the increase of the distance between the AUV and the sonar; , generally do not choose to enter the exposed area, unless the starting point is in the exposed area.

Step 1.2 Level set function initialization

Let the level set function be φ(X,t), where X represents a smooth closed curve, and at time t, use the evolution curve X(t) to represent the zero level set curve at the current moment φ(X,t)= 0. Initialize the zero-level-set curve as a circle centered at the starting point and radius r, and set d to the width of the narrow band, where d>r. The level set function is a distance function, the value of the level set function inside the zero level set curve is negative, and the value of the level set function outside the zero level set curve is positive, in order to ensure that the zero level set curve can reach the end point, the level set The function evolves only in the direction where the level set function value is positive. The level set function is initialized as:

φ(X,t)＝0:(xX ₁ ) ² +(yY ₁ ) ² +(zZ ₁ ) ² ＝r ²

Among them, x, y, z are grid point coordinates, and X ₁ , Y ₁ , Z ₁ are starting point coordinates.

Step 2: Level set function evolution.

The evolution of the level set function value is mainly affected by the ocean current V(H,t), the AUV's own velocity U, and the concealment field M, where H represents a position vector in the navigation space, and the level set evolution function under the influence of the AUV's own velocity U for:

Under the influence of ocean currents, the level set evolution function is:

引力方向为零水平集曲线指向航行空间内的不同隐蔽区以及危险区的和方向，因此在隐蔽场作用下的水平集演化函数为：Adding the influence of the hidden field, the present invention adopts the method of combining the artificial potential field method and the level set algorithm to deal with the influence of the hidden field when the zero level set curve evolves, that is, the attraction of the area with strong concealment to the zero level set curve The force is greater, the size of the hidden field is M, and the direction of attraction is

The gravitational direction is the sum direction of the zero level set curve pointing to different hidden areas and dangerous areas in the navigation space, so the level set evolution function under the action of the hidden field is:

综合可得，水平集函数演化数学模型为：In addition, the direction of travel is

Comprehensively, the mathematical model of level set function evolution is:

The level set function evolves in a narrow band according to the above equation, and the evolution curve is composed of multi-point sets. When the level set curve evolves to the edge of the narrow band, because the value of the level set function outside the narrow band is not updated, it is necessary to periodically calculate the value of the level set function outside the narrow band by calculating the distance between the points outside the narrow band and the current zero level set curve .

When the improved level set algorithm proposed in the present invention is performing level set function evolution, whenever the zero level set curve reaches an end point, it saves all zero level set curves from the current end point to the starting point, when the level set function at all end points When the value is zero or negative, the evolution of the level set function is complete.

Step 3: Optimal waypoint selection.

In order to obtain the optimal route of time and concealment, the sum speed of AUV during navigation is composed of AUV's own speed U, ocean current speed and the gravity of concealment field. When the AUV's sailing direction is perpendicular to the gradient direction of the level set function and sails at the speed U, the AUV is always on the evolution zero level set curve. Due to the arbitrariness of the gradient direction of the level set function, if from the starting point along the zero level set The gradient direction of the curve is used to solve the optimal route, and there will be multiple routes, so it is necessary to find the gradient direction point on each zero level set curve from the end point to the starting point as the optimal route point.

Step 4: Collaborative route scheme design.

需要从n种航路组合中选出m个AUV到达m个任务点的m条航路。设定所有航路为H＝{H_1,1,H_1,2,…,H_1,m；H_2,1,H_2,2,…,H_2,m；…；H_n,1,H_n,2,…,H_n,m}，其中H_n,m代表第n种方案中第m艘AUV的航行路线，因此H表示n个方案中的所有AUV的航行路线。Using the improved level set algorithm, all m×m routes from m origins to m destinations can be planned at the same time, so it is necessary to select m cooperative routes from m×m routes. There are n route combinations in the planned m×m routes, among which

It is necessary to select m routes for m AUVs to reach m mission points from n route combinations. Set all routes as H={H _1,1 ,H _1,2 ,…,H _1,m ; H _2,1 ,H _2,2 ,…,H _2,m ;…;H _n,1 ,H _n,2 ,...,H _n,m }, where H _n,m represents the navigation route of the m-th AUV in the n-th scheme, so H represents the navigation routes of all AUVs in the n-th scheme.

The energy consumption of AUV is related to the speed of AUV itself and the length of the route, so the energy consumption of AUV navigation is set as:

Among them, k represents the energy consumption coefficient, a constant determined by the AUV's own parameters, V _ax represents the X-direction component of the AUV's own speed; V _ay represents the Y-direction component of the AUV's own speed; L _x =|L _xi+1 -L _xi |represents the route length of the X-direction component of the route; L _y =|L _yi+1 -L _yi |represents the route length of the Y-direction component of the route.

The energy loss N representing all routes in each combination is thus obtained.

N={N _1,1 ,N _1,2 ,...,N _1,m ; N _2,1 ,N _2,2 ,...,N _2,m ;...;N _n,1 ,N _n,2 ,... ,N _n,m }.

Assuming that the selected route is H _i ={H _i,1 ,H _i,2 ,…H _i,m }, where the sailing time T _i ={T _i,1 ,T _i,2 ,…T _i,m }, the voyage Energy consumption N _i ＝{N _i,1 ,N _i,2 ,…N _i,m }, the concealment of the route (the sum of all gravitational forces on the route nodes) is M _i ＝{M _i,1 ,M _{i ,2} ,...M _i,m }, route intersection D _i ={D _i,1 ,D _i,2 ,...D _i,m }.

Constraints (1):

Max{N _i }≤N _max

Constraint condition (1) shows that: N _max represents the maximum energy carried by each AUV itself, indicating that the energy consumption of all routes in the selected route must not exceed the maximum energy carried by the AUV itself.

Constraints (2):

Constraint condition (2) shows that the arrival time of different AUVs at the intersection of routes cannot be the same, indicating that there is no time conflict between different routes ^[7] .

Constraint (3): Each task can only be assigned or executed once, indicating that the task is unique.

)条协同航路规划方案，然后计算出n中每个方案的时间最优矩阵Ta＝{Ta₁,Ta₂,…,Ta_n}以及隐蔽性最优矩阵Ma＝{Ma₁,Ma₂,…,Ma_n}，利用公式C＝p·Ta+q·Ma计算每个方案的协同目标函数值C，其中p为时间影响因子设为0.4，q为隐蔽性影响因子设为0.6，并且根据目标函数值进行从小到大排列所有的航路方案，并存入数组S中，从数组S中选择最优的方案进行冲突判定，其中目标函数值最小值对应的航路方案为最优的路线方案。Put all the routes into a cell array A of m×m. According to the constraint condition (3), it can be seen that the AUV cannot reach the same mission point, so the combination of n (where

) collaborative route planning schemes, and then calculate the time optimal matrix Ta={Ta ₁ ,Ta ₂ ,…,Ta _n } and concealment optimal matrix Ma={Ma ₁ ,Ma ₂ ,… ,Ma _n }, use the formula C=p·Ta+q·Ma to calculate the collaborative objective function value C of each scheme, where p is the time influence factor and is set to 0.4, q is the concealment influence factor and is set to 0.6, and according to the objective Arrange all the route plans from small to large according to the function value, and store them in the array S, select the optimal plan from the array S for conflict determination, and the route plan corresponding to the minimum value of the objective function value is the optimal route plan.

Step 5: Route conflict determination and re-planning.

Step 5.1 Airway Conflicts

Since the air route planned by the improved level set algorithm is composed of unequal discrete points, and the air route is composed of the connection lines of all adjacent discrete points, so the present invention can adopt section-by-section comparison on the problem of judging the intersecting points of air routes Law.

(1) Express all routes H _i in the form of line segments, express H _i,1 as all discrete line segments in the i-th route, H _i,1 = (X _1,1 ,X _{1, 2} ,…,X _1,n1 ), where X _1,1 represents a line segment composed of two adjacent discrete points in this route, and n1 means that the route is composed of n1 line segments, so the route is re-expressed as H _i = {(X _1,1 ,X _1,2 ,…,X _1,n1 ), (X, _2,1 ,,X _2,2 ,…,X _2,n2 )…(X _m,1 ,X _m,2 ,...,X _m,nm )}.

(2) The linear function expression of the line segment composed of adjacent discrete points in the known route is y=kx+b, so the route is re-expressed as H _i ={(K _1,1 ,K _1,2 ,…,K _1,n1 ),(K _2,1 ,K _2,2 ,…,K _2,n2 ),…,(K _m,1 ,K _m,2 ,…,K _m,nm )}, where K _{1, 1} represents the function expression of the line segment X _1,1 ;

(3) Then compare each line segment in each route with each line segment in other routes, and find all intersection points within the range of the line segment, that is, route intersection D _i ={D _i,1 ,D _i,2 ,...D _i,m }; In addition, assuming navigation energy consumption N _i ={N _i,1 ,N _i,2 ,...N _i,m }, it is only necessary to determine the maximum route energy consumption Max(N _i ) It should be less than the rated energy P carried by the AUV.

Step 5.2 Route re-planning

Calculate whether the two AUVs intersect in time at the intersection point; if there is, the probability of the two AUVs colliding here is extremely high, choose the suboptimal plan in the plan, and then judge the conflict until the AUV is selected not to happen Conflicting schemes; in addition, when there is an energy conflict problem, that is, the energy carried by the AUV cannot support the AUV to reach the target point, the suboptimal scheme in the scheme is also selected, and then the conflict judgment is made until the AUV is selected without conflict scheme.

Step 6: Output the currently selected collaborative route when the route combination scheme that does not conflict with the algorithm is selected, and the multi-AUV three-dimensional collaborative route planning ends.

The invention improves the level set algorithm so that multiple air routes can be planned simultaneously in the navigation space, thereby improving the planning efficiency of the algorithm. At the same time, the AUV three-dimensional collaborative route planning scheme is proposed, and the improved level set algorithm is applied to it, and all AUVs are required to arrive at the destination at the same time when designing the collaborative scheme. Each AUV needs to start one by one according to the delay time. In addition, considering the speed and concealment In the case of , a collaborative planning scheme with the optimal combination of concealment and navigation time is designed. The steps mainly include: three-dimensional environment abstract modeling and level set function initialization, level set function evolution, optimal waypoint selection, collaborative route scheme design, route conflict determination and re-planning, and output planning results. Compared with the traditional level set algorithm, the collaborative route planning method proposed in this patent takes into account the influence of ocean currents and sound velocity factors, and adds conflict determination and re-planning links, making the collaborative route safer; at the same time, the realization of three-dimensional route planning is compared to two The dimensional route planning is more practical and can better meet the actual navigation needs.

Claims (3)

1. A multi-AUV three-dimensional collaborative route planning method is characterized by comprising the following steps:

step 1: performing three-dimensional environment modeling according to the electronic chart, the ocean current, the temperature, the salinity and the depth information, and establishing a terrain field, an ocean current field and a hidden field; initializing a level set function, initializing the level set function into a circular function with a starting point as a circle center and r as a radius, and setting a proper narrow band width d to enable the level set function to evolve in a narrow band; the three-dimensional environment modeling specifically comprises the following steps:

firstly, dividing a real marine environment space subjected to AUV route planning into lon x lat x depth grids, wherein the grid division principle is as follows: the grid spacing in the longitude direction and the latitude direction is consistent with the resolution of a grid water depth data file in a navigation area, and the grid spacing in the depth direction should not exceed 1/10 of the maximum depth value of the navigation area;

and (3) carrying out terrain field modeling of an AUV navigation area: the method is realized by reading a grid water depth data file of a navigation area, wherein an area below a water depth value read from the file in a navigation space is set as a no-navigation area, and AUV speed data, ocean current data and hidden field gravity data contained in a grid of the no-navigation area are set as 0 to be used for restricting the navigation depth of an AUV;

carrying out ocean current field modeling of an AUV navigation area: the method is realized by reading a grid ocean current data file of a navigation area, if the resolution of the ocean current data file is lower than that of a water depth data file, linear interpolation processing is carried out on the ocean current data file, otherwise, the ocean current data file is subjected to thinning processing until the resolution of the ocean current data file is the same as that of the water depth data file, so that each grid contains ocean current information of the current position; the size of the ocean current in the depth direction is set to 0;

carrying out hidden field modeling of an AUV navigation area: firstly, reading a grid temperature, salinity and depth data file of a navigation area, performing the same processing as a sea current data file, then calculating sound velocity information c of different grid points of the navigation area according to the temperature, salinity and depth information in the AUV navigation area, knowing the position of a detected sonar, and calculating sound propagation loss under the influence of the sound velocity of the AUV navigation area, wherein the calculation of the sound propagation loss specifically comprises the following steps:

the acoustic propagation loss TL in the AUV navigation region is:

TL＝20log R+α·R

wherein R is the distance, alpha is the absorption coefficient,

beta is a set sound velocity influence coefficient;

the known high-quality factor of detection sonar is FOM, and a navigation space is divided into an exposed area, a dangerous area and a hidden area according to the relationship between sound propagation loss and sonar quality factor, and the method specifically comprises the following steps:

when TL > FOM, the area is a hidden area;

when the temperature is higher than the set temperature

When the area is the exposed area;

when in use

The area is a hazardous area;

the hidden field modeling of the navigation area is carried out by calculating the gravity value of the AUV through grid points in different areas of the navigation area, which specifically comprises the following steps:

in the navigation space, a hidden field is represented by a gravitational field, the gravitations of the divided hidden area, dangerous area and exposed area to the AUV are different in size, the gravitation of the hidden area is set to be a fixed value F = F, wherein F is a factor influencing the AUV route planning algorithm by the concealment and is set to be 2 times of the maximum ocean current value of the navigation area, the gravitation of the exposed area is 0, and the gravitation of the dangerous area is set as:

wherein R is the distance between the current position of AUV and sonar position, R _TL Distance at TL = FOM;

and 2, step: evolution of a level set function: the level set function considers the AUV self speed, the ocean current speed and the hidden field to obtain an improved level set function evolution mathematical model, the evolution is carried out according to the improved level set function evolution mathematical model, when the narrow band edge is reached, the level set function and the narrow band width are reconstructed, the level set function is reinitialized into a new distance function, the new narrow band width is calculated, whether each terminal point is reached or not is judged, and all zero level set curves before the terminal point are stored when each terminal point is reached;

and step 3: selecting an optimal waypoint: carrying out forward iteration on the end point of each zero level set curve to find out the gradient direction point of each zero level set curve until the end point is reached, and sequentially connecting all the gradient direction points to obtain all the optimal air routes;

and 4, step 4: designing a collaborative route allocation scheme: simultaneously planning all m multiplied by m routes from m starting points to m end points by using the improved level set function evolutionary mathematical model obtained in the step 2, wherein n route combinations coexist in the planned m multiplied by m routes, wherein

M routes from m AUVs to m task points need to be selected from n route combinations, and all routes are set to be H = { H = _1,1 ,H _1,2 ,…,H _1,m ；H _2,1 ,H _2,2 ,…,H _2,m ；…；H _n,1 ,H _n,2 ,…,H _n,m H wherein H _n,m Representing navigation routes of m AUVs in the n schemes, so that H represents navigation routes of all AUVs in the n schemes;

AUV navigation energy consumption is set as:

where k is the coefficient of energy consumption, V _ax Represents the X-direction component in the speed of the AUV; v _ay Represents the Y-direction component in the speed of the AUV; l is _x ＝|L _xi+1 -L _xi L represents the route length of the X-direction component of the route; l is _y ＝|L _yi+1 -L _yi | represents the route length of the Y-direction component of the route;

thus, an energy loss N is obtained that represents all the routes in each combination:

N＝{N _1,1 ,N _1,2 ,…,N _1,m ；N _2,1 ,N _2,2 ,…,N _2,m ；…；N _n,1 ,N _n,2 ,…,N _n,m }；

suppose that the selected route is H _i ＝{H _i,1 ,H _i,2 ,…H _i,m Therein, the time of flight T _i ＝{T _i,1 ,T _i,2 ,…T _i,m }, navigation energy consumption N _i ＝{N _i,1 ,N _i,2 ,…N _i,m And the hiding degree of the air route, namely the sum of all gravitation degrees on the air route nodes, is as follows: m is a group of _i ＝{M _i,1 ,M _i,2 ,…M _i,m }, intersection D of air paths _i ＝{D _i,1 ,D _i,2 ,…D _i,m }；

Constraint (1):

Max{N _i }≤N _max

constraint (1) indicates that: n is a radical of _max Representing the maximum energy carried by each AUV, namely the energy consumption of all the routes in the selected routes cannot exceed the maximum energy carried by the AUV;

constraint (2):

constraint (2) states that: the time of different AUVs reaching the intersection point of the routes cannot be the same, namely, no time conflict point exists between different routes;

constraint condition (3): each task can be distributed or executed only once, namely the task has uniqueness;

putting all routes into an m multiplied by m cellular array A, knowing that the AUV can not reach the same task point according to the constraint condition (3), and combining n collaborative route planning schemes, wherein

Then, a time optimal matrix Ta = { Ta ] of each scheme in n is calculated ₁ ,Ta ₂ ,…,Ta _n And a concealment optimization matrix Ma = { Ma = } ₁ ,Ma ₂ ,…,Ma _n Calculating a cooperative objective function value C of each scheme by using a formula C = p · Ta + q · Ma, wherein p is a set time influence factor, q is a set hiding influence factor, arranging all the route schemes from small to large according to the objective function values, and storing the route schemes into an array S, wherein the route scheme corresponding to the minimum value of the objective function value is an optimal route scheme;

and 5: selecting an optimal route scheme from the array S to perform route conflict judgment, wherein the route conflict judgment comprises collision conflict judgment and energy conflict judgment, executing the step 6 when collision conflict or energy conflict exists, otherwise, outputting the current route scheme, and finishing the multi-AUV three-dimensional collaborative route planning;

wherein the collision conflict determination specifically comprises:

(1) All the routes H _i Each route is represented by a line segment, and H _i,1 Represented as all discrete segments, H, in the ith flight path _i,1 ＝(X _1,1 ,X _1,2 ,…,X _1,n1 ) Wherein X is _1,1 Representing a line segment formed by two adjacent discrete points in the route, n1 represents that the route is formed by combining n1 line segments, so that the route is represented as H again _i ＝{(X _1,1 ,X _1,2 ,…,X _1,n1 )，(X _2,1 ,X _2,2 ,…,X _2,n2 )，…，(X _m,1 ,X _m,2 ,…,X _m,nm )}；

(2) The linear function expression of a segment made up of adjacent discrete points in the route is known as y = kx + b, so the route is re-represented as H _i ＝{(K _1,1 ,K _1,2 ,…,K _1,n1 ),(K _2,1 ,K _2,2 ,…,K _2,n2 )，…，(K _m,1 ,K _m,2 ,…,K _m,nm ) In which K is _m,nm Represents X _m,nm A functional expression of this line segment;

(3) Then comparing each line segment in each route with each line segment in other routes to obtain all intersection points in the range of the line segments, namely route intersection points D _i ＝{D _i,1 ,D _i,2 ,…D _i,m }; calculating the time difference of the two AUVs reaching the intersection point, and if the time difference of the two AUVs reaching the intersection point is smaller than the set safe time difference, collision conflict exists;

the energy conflict judgment specifically comprises the following steps: energy consumption N for navigation _i ＝{N _i,1 ,N _i,2 ,…N _i,m Judge maximum route energy consumption Max (N) in the scheme _i ) Relationship to the nominal energy P carried by the AUV, when Max (N) _i )>P, there is an energy conflict;

and 6: and (5) carrying out route re-planning, removing the route schemes with collision conflict or energy conflict from the array S, and executing the step 5.

2. The multi-AUV three-dimensional collaborative route planning method according to claim 1, characterized in that: the initialization of the level set function in the step 1 specifically comprises the following steps:

setting a level set function as phi (X, t), wherein X represents a smooth closed curve, at the time of t, using an evolution curve X (t) to represent a zero level set curve phi (X, t) =0 at the current time, initializing the zero level set curve as a circle taking a starting point as a circle center and r as a radius, setting d as a narrow band width, wherein d is larger than r, the level set function is a distance function, a level set function value in the zero level set curve is a negative value, a level set function value outside the zero level set curve is a positive value, and in order to ensure that the zero level set curve can reach an end point, the level set function only evolves towards the direction that the level set function value is a positive value, and the level set function is initialized as follows:

φ(X,t)＝0:(x-X ₁ ) ² +(y-Y ₁ ) ² +(z-Z ₁ ) ² ＝r ²

where X, y, z are grid point coordinates, (X) ₁ ，Y ₁ ，Z ₁ ) As the starting point coordinates.

3. The multi-AUV three-dimensional collaborative route planning method according to claim 1, characterized in that: the level set function evolution mathematical model in the step 2 is specifically as follows:

v (H, t) is ocean current, U is the speed of the AUV, M is the size of a hidden field, and H represents a position vector in a navigation space;

is the direction of the flight of the ship,

which is the direction of the attractive force.

Images