Underwater robot double-layer path planning system and method for underwater search and rescue tasks

Abstract

The invention provides an underwater robot double-layer path planning system and method for underwater search and rescue tasks. The system comprises an underwater robot system and a shore-based system. The shore-based system is used for providing a search and rescue task area and starting point position information, and determining whether to distribute a forcible entry machine to execute a forcible entry rescue task or not according to a forward-looking sonar image transmitted by the underwater robot system; and the underwater robot system is used for executing a wrecked ship search and rescue task according to the search and rescue task area and the starting point position information. The search and rescue task is executed in two steps, the performance characteristics and advantages of different sensors of the underwater robot can be brought into full play, the AUV can be helped to autonomously complete the detection task of the target area, and the search efficiency and intelligence of the AUV are effectively improved.

Description

Technical Field

[0001] The invention relates to a double-layer path planning system and method for an underwater robot used for underwater search and rescue missions, and belongs to the technical field of underwater vehicles. Background Art

[0002] With the recovery of global trade and the increase in cargo transportation in waters, people have found that shipwrecks have brought great harm to the safety of life and property of crew members. It is necessary to improve the efficiency of search and rescue missions after shipwrecks in the hope of effectively protecting the safety of people's lives and property. In shipwreck search and rescue missions, new mission characteristics are usually shown, such as a large search and rescue range, a high risk factor, and a high timeliness of search and rescue missions. This has brought huge technical challenges to the traditional solution of relying on divers to carry out search and rescue missions.

[0003] In recent years, with the development of technologies such as intelligent control, navigation and communication, underwater detection, and planning and decision-making, the application of various technologies in underwater robots has become increasingly mature, which has laid a technical foundation for underwater robots in detecting underwater environmental information and determining the location and identity of people in distress.

[0004] Most of the existing path planning methods for underwater vehicles are based on prior knowledge to obtain a preset path, which is no longer an optimal choice for underwater search and rescue missions, and reduces the efficiency of underwater vehicles in completing tasks. On the one hand, the preset path limits the performance of underwater vehicles. It makes it impossible for underwater robots to make adjustments based on environmental and mission information, and cannot meet the timeliness requirements of search and rescue missions; on the other hand, this traditional solution lacks sensor feedback, which makes the quality and effect of the final collected data poor, limiting the autonomy and intelligence of the underwater vehicle. Based on the needs of underwater search and rescue missions, the present invention proposes a double-layer path planning system and method for underwater robots for underwater search and rescue missions. Summary of the invention

[0005] The present invention aims to solve the technical problems in the prior art of low efficiency in completing underwater vehicle tasks and poor quality and effect of collected data, and further proposes a double-layer path planning system and method for underwater robots for underwater search and rescue tasks.

[0006] The technical solution adopted by the present invention to solve the above problems is: the present invention proposes a double-layer path planning system for underwater robots used for underwater search and rescue tasks, comprising:

[0007] underwater robotic systems and shore-based systems;

[0008] The shore-based system is used to provide information on the search and rescue mission area and starting point, and decide whether to deploy rescue machinery to perform the rescue mission based on the forward-looking sonar image transmitted by the underwater robot system;

[0009] The underwater robot system is used to perform shipwreck search and rescue missions based on the search and rescue mission area and starting point location information.

[0010] Optionally, the underwater robot system includes:

[0011] Dual-layer path planning module, control center, navigation / communication module, perception module, side-scan sonar and control module;

[0012] The navigation / communication module is used to receive the search and rescue mission area and starting point location information provided by the shore-based system;

[0013] The control center is used to receive the search and rescue mission area and starting point location information and transmit it to the double-layer path planning module;

[0014] The two-layer path planning module is used to calculate the re-planned path according to the search and rescue mission area and the starting point location information, and transmit the re-planned path to the control center;

[0015] The control module controls the heading according to the replanned path transmitted by the control center;

[0016] The perception module is used to track the replanned path and obtain the position of the underwater robot in real time during the search and rescue process;

[0017] Side-scan sonar is used to obtain forward-looking sonar images during the search and rescue process, and transmit the forward-looking sonar images to the shore-based system through the navigation / communication module.

[0018] Optionally, the two-layer path planning module includes a global planner and a local planner;

[0019] The global planner is used to rasterize the search and rescue mission area and calculate the global detection path;

[0020] The local planner retrieves the global grid information in real time and calculates the secondary exploration path with the best energy consumption.

[0021] A two-layer path planning method for underwater robots used in underwater search and rescue missions includes:

[0022] Step 1: Obtain the search and rescue mission area and starting point location information based on the shore-based system and deploy the underwater robot;

[0023] Step 2: The search and rescue mission area and the starting point location information are transmitted to the navigation / communication module. The central module receives the search and rescue mission area and the starting point location information and transmits it to the double-layer path planning module. The double-layer path planning module calculates the re-planned path and transmits it to the control center.

[0024] Step 3: The control center transmits the replanned path to the control module for heading control, tracks the replanned path through the perception module, and obtains the position information of the underwater robot in real time during the search and rescue process;

[0025] Step 4: Use the forward-looking sonar image in the real-time search and rescue process of the scanning sonar to identify the identity of the target object based on the forward-looking sonar image;

[0026] Step 5: The target object identity and underwater robot position information are transmitted to the shore-based system through the navigation / communication module. The shore-based system determines whether to deploy the rescue machinery to perform the rescue mission based on the target object identity and position information;

[0027] Step 6: Continue to perform the search and rescue mission, compare the searched area with the mission area, and determine whether the search and rescue mission is completed. If not, repeat steps 3-5 until the search and rescue mission is completed.

[0028] Optionally, the step of obtaining the search and rescue mission area and the starting point location information and deploying the underwater robot in step 1 includes:

[0029] Step 1.1: Obtain the location information of the wrecked ship and the fixed island information by subscribing to the satellite;

[0030] Step 1.2: Based on prior knowledge, combine the location information of the wrecked ship and the fixed island information to determine the propagation crash location P start =[x strat ,y start ] and the flow field information of the accident area U i (X) = [U, θ].

[0031] Optionally, the shipwreck location P is determined in step 1.2 start =[x strat ,y start ] and the flow field information of the accident area U i The step of (X) = [U, θ] includes:

[0032] Step 1.2.1: Use the Lagrangian model distribution to predict the trajectory distribution of the target under the influence of the flow field and obtain the motion equation of the target;

[0033] Step 1.2.2: Consider the target object as a particle, and calculate the differential equations of the position and velocity of the target object based on the particle motion equation;

[0034] Step 1.2.3: Use the difference method to solve the differential equation, use the Euler method to obtain the solution formula, and obtain the predicted position of the target object;

[0035] Step 1.2.4: Based on the position vector P i =[x i ,y i ], mean vector μ = [μ1, μ2] T And the covariance matrix is Establish a two-dimensional distribution probability model of the target object, according to the target object position information X t =[x t ,y t ] Solve the probability E of the target object existing in all positions i ;

[0036] Step 1.2.5: Combine the predicted position of the target object and the probability of the target object existing in all positions E i Determine the location of the ship's crash and deploy an underwater robot to the corresponding location;

[0037] The motion equation of the target object is:

[0038]

[0039] In formula (1), X = [x, y] is the position vector of the target object, and V = [u, v] is the velocity vector of the target object itself under the action of the external flow field;

[0040] The differential equations for the position and velocity of the target are:

[0041]

[0042] The expression for solving the differential equation is:

[0043] X n+1 =X n +V n ·Δt (3);

[0044] The solution formula is:

[0045]

[0046] In formula (4), Δt is the time step;

[0047] The probability of the target object existing in all positions E i The calculation formula is:

[0048]

[0049] In formula (5), is the target object at P at time t i =[x i ,y i ] position probability distribution, mean μ = [μ1, μ2] using the current moment predicted target position X t =[x t ,y t ]assignment.

[0050] Optionally, the step of calculating the re-planned path by the two-layer path planning module in step 2 includes:

[0051] Step 2.1: The global planner subscribes to the boundary information of the search and rescue mission area and the underwater robot deployment position information, determines the global grid size according to the installation angle of the scanning sonar and the bottom depth of the underwater robot, rasterizes the search and rescue mission area, obtains the global detection path through the improved unit decomposition BINN fusion algorithm, transmits the global detection path to the control module, and updates the target position and local flow field information detected by the scanning sonar in real time to the global grid map;

[0052] Step 2.2: The local planner retrieves the global grid information in real time, determines whether there is target information to trigger the local planner, and calculates the re-planning path with optimal energy consumption through the improved ant colony algorithm.

[0053] Optionally, the step of obtaining the global detection path by using the improved unit decomposition-BINN fusion algorithm in step 2.1 includes:

[0054] Step 2.1.1: Initialize the grid map and place obstacles;

[0055] Step 2.1.2: Set the underwater robot's starting point and search direction;

[0056] Step 2.1.3: Control the underwater robot to move a distance W along the search direction and count the particle into the path segment;

[0057] Step 2.1.4: Determine whether the end position of the movement is one of the obstacle and the left boundary of the search task area. If not, repeat step 2.1.3. If yes, proceed to step 2.1.5.

[0058] Step 2.1.5: Execute the BINN algorithm to obtain the coverage path around the obstacle and determine whether the underwater robot has reached the target point. If so, connect the path segment and the path around the obstacle as the global detection path for output. If not, proceed to step 2.1.6;

[0059] Step 2.1.6: Determine whether the right boundary of the search task area has been reached. If not, move W distance in the opposite direction of the search direction and count the corresponding particle into the new path segment until the right boundary of the search task area has been reached. If yes, proceed to step 2.1.7;

[0060] Step 2.1.7: Execute the BINN algorithm to obtain the coverage path around the obstacle and determine whether it has reached the target point. If not, repeat step 2.1.6 until the underwater robot reaches the target point. If so, connect the path segment and the path around the obstacle as the global detection path for output.

[0061] Optionally, the step of calculating the replanning path with optimal energy consumption by using the improved ant colony algorithm in step 2.2 includes:

[0062] Step 2.2.1: Initialize basic parameters and create an environment model;

[0063] Step 2.2.2: Place the ants at the starting point and calculate the energy consumption during the ant colony exploration process using the ant colony algorithm;

[0064] Step 2.2.3: Set the heuristic function to guide the ant colony to move towards the target point;

[0065] Step 2.2.4: Calculate the transfer probability of the ant colony, use the roulette method to select the next node, and update the taboo table;

[0066] Step 2.2.5: Determine whether ant K has completed an exploration. If not, repeat step 2.2.4 until ant K has completed an exploration. If yes, proceed to step 2.2.6.

[0067] Step 2.2.6: Update ants: K = K + 1;

[0068] Step 2.2.7: Determine whether all ants have completed one exploration. If not, repeat steps 2.2.4 to 2.2.6 until all ants have completed one exploration. If yes, proceed to step 2.2.8;

[0069] Step 2.2.8: Global update of pheromones, update of optimal path, and determination of whether the taboo table satisfies NC=NC max , if yes, then output the updated optimal path as the replanned path, if not, proceed to step 2.2.9;

[0070] Step 2.2.9: Clear the taboo table again, and repeat steps 2.2.4 to 2.2.8 until the taboo table satisfies NC = NC max , and output the replanning path;

[0071] The expression to clear the taboo table is:

[0072] NC = NC + 1 (6);

[0073] In formula (6), NC is the number of iterations;

[0074] The expression of the heuristic function is:

[0075]

[0076] In formula (7), t ij is the time required for navigation along the path, θ ijis the angle between the navigation direction of the path and the flow field direction, ω3 is the first weight, and ω4 is the second weight.

[0077] Optionally, the step of calculating the energy consumption during the ant colony exploration process by using the ant colony algorithm in step 2.2.2 includes:

[0078] Step 2.2.2.1: Decompose the area of ​​the underwater robot affected by the water flow into projections in the X-axis and Y-axis directions and calculate the projection area;

[0079] Step 2.2.2.2: Calculate the energy consumption and total energy consumption of the underwater robot in the X-axis and Y-axis directions based on the projection areas of the underwater robot in the X-axis and Y-axis directions;

[0080] Step 2.2.2.3: Substitute the yaw angle into the cost function, and combine it with the total energy consumption of the underwater robot to obtain an improved cost function. Based on the improved cost function, the path smoothness during the exploration process is guaranteed;

[0081] The projection area expressions of the underwater robot in the X-axis and Y-axis directions are:

[0082] S x =d(Lsinθ+Bcosθ) (8);

[0083] S y =d(Lcosθ+Bsinθ)(9);

[0084] In formula (8) and formula (9), L, B and d are the length, width and height of AUV respectively, θ is the angle between the moving direction and the X-axis, S x is the projection area of ​​AUV on the X axis, S y is the projection area of ​​AUV on the Y axis;

[0085] The expression of total energy consumption of underwater robot is:

[0086]

[0087]

[0088] E=E y -E x (12);

[0089] In formula (10)-formula (12), C c is the seawater resistance coefficient, ρ w is the density of seawater, E x is the energy consumed by the AUV in the X-axis direction, E y is the energy consumed by the AUV in the Y-axis direction, E is the total energy consumption of the AUV, t is the movement time of the AUV in each path, V w is the ocean current speed;

[0090] The expression of the improved cost function is:

[0091] cost = ω1E-ω2cosγ (13);

[0092] In formula (13), ω1 and ω2 are the third weight and the fourth weight respectively, and cosγ is the yaw angle;

[0093] The yaw angle expression is:

[0094]

[0095] In formula (14), points J-1, J, and J+1 are three adjacent target points. is the vector from point J-1 to J; is the vector from point J to J+1, For vector Model; For vector The modulus of , γ is the angle between the two vectors, and n is the number of target points.

[0096] The beneficial effects of the present invention are:

[0097] 1. The present invention can help underwater robots to accurately detect the location and identity information of search and rescue targets, and can complete the detection task of the search and rescue mission area with an efficient search and rescue path, effectively improving the efficiency of underwater search and rescue missions.

[0098] 2. The present invention divides the search and rescue mission into two steps, which can give full play to the performance characteristics and advantages of different sensors of the underwater robot, and can help the AUV to autonomously complete the detection mission of the target area, effectively improving the search efficiency and intelligence of the AUV.

[0099] 3. The present invention can help underwater robots improve the efficiency of underwater search and rescue missions. During the execution of this path, the underwater robot can obtain sensor information online, independently determine the location and identity information of underwater targets, and complete underwater search and rescue missions. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] Figure 1 A structural block diagram of a double-layer path planning system for an underwater robot used for underwater search and rescue missions provided by the present invention;

[0101] Figure 2 A flow chart of a double-layer path planning method for an underwater robot for underwater search and rescue missions provided by the present invention;

[0102] Figure 3 A flow chart of obtaining a global path provided by the present invention;

[0103] Figure 4 A flow chart of the global path planning algorithm provided by the present invention;

[0104] Figure 5 A flow chart of obtaining a replanning path provided by the present invention;

[0105] Figure 6 A speed decomposition diagram of the underwater robot provided by the present invention;

[0106] Figure 7 A schematic diagram of the yaw angle provided by the present invention;

[0107] Figure 8 This is a flow chart of the improved ant colony algorithm provided by the present invention. DETAILED DESCRIPTION

[0108] Specific implementation method 1: Combination Figure 1 This embodiment describes the structure of the underwater robot double-layer path planning system for underwater search and rescue missions described in this embodiment, which includes:

[0109] underwater robotic systems and shore-based systems;

[0110] The shore-based system is used to provide information on the search and rescue mission area and starting point, and decide whether to deploy rescue machinery to perform the rescue mission based on the forward-looking sonar image transmitted by the underwater robot system;

[0111] The underwater robot system is used to perform ship search and rescue missions based on the search and rescue mission area and starting point location information.

[0112] The underwater robot system includes:

[0113] Dual-layer path planning module, control center, navigation / communication module, perception module, side-scan sonar and control module;

[0114] The navigation / communication module is used to receive the search and rescue mission area and starting point location information provided by the shore-based system;

[0115] The control center is used to receive the search and rescue mission area and starting point location information and transmit it to the double-layer path planning module;

[0116] The two-layer path planning module is used to calculate the re-planned path according to the search and rescue mission area and the starting point location information, and transmit the re-planned path to the control center;

[0117] The two-layer path planning module includes a global planner and a local planner;

[0118] The global planner is used to rasterize the search and rescue mission area and calculate the global detection path;

[0119] The local planner retrieves the global grid information in real time and calculates the secondary exploration path with the best energy consumption.

[0120] The control module controls the heading according to the replanned path transmitted by the control center;

[0121] The perception module is used to track the replanned path and obtain the position of the underwater robot in real time during the search and rescue process;

[0122] Side-scan sonar is used to obtain forward-looking sonar images during the search and rescue process, and transmit the forward-looking sonar images to the shore-based system through the navigation / communication module.

[0123] Specific implementation method 2: Combination Figure 2-7 This embodiment is described. In this embodiment, the AUV is an underwater robot. Figure 2 As shown, the steps of the underwater robot double-layer path planning method for underwater search and rescue missions described in this embodiment include:

[0124] S1: Subscribe to the location information of the wrecked ship obtained by satellite, determine the optimal mission area based on the location and prior information, and deploy the underwater robot to the corresponding search and rescue mission area through the search and rescue ship.

[0125] S101: The shore-based end processes the location of the wrecked ship and the fixed island information obtained by the satellite, and can determine the wreck location P based on prior knowledge. start =[x strat ,y start ] and the flow field information of the accident area U i (X) = [U, θ], U i (X) = [U, θ] represents a velocity vector field in space, indicating the magnitude and direction of the flow field at position X = [x, y]. ;

[0126] S102: Use the Lagrangian model to predict the trajectory distribution of the target under the influence of the flow field, assuming that the motion of the crashed target satisfies the following motion equation:

[0127]

[0128] In formula (1), X = [x, y] is the position vector of the target object, and V = [u, v] is the velocity vector of the target object itself under the action of the external flow field;

[0129] S103: Compared with the vast waters, the size of the target is small enough and can be simplified as a particle. In the flow field, the particle is affected by the water velocity field, and the force on the target in the flow field satisfies Newton's second law, as shown in the following formula:

[0130]

[0131] In formula (2), is the potential energy gradient of the velocity field, which can be expressed as Substituting formula (2) into formula (1) can obtain the differential equations of the position and velocity of the target object;

[0132] The differential equations for the position and velocity of the target are:

[0133]

[0134] S104: Solve the differential equation using a difference method, obtain a solution formula using the Euler method, and obtain a predicted position of the target object;

[0135] The expression for solving the differential equation is:

[0136] X n+1 =X n +V n ·Δt (4);

[0137] The solution formula is:

[0138]

[0139] In formula (5), Δt is the time step;

[0140] S105: Assuming that the target object satisfies the Gaussian distribution model in the water area, a two-dimensional Gaussian distribution target object distribution probability model is established, assuming that the target existence probability E i From the position vector P i =[x i ,y i ], mean vector μ = [μ1, μ2] T And the covariance matrix is The two-dimensional Gaussian distribution of the composition is determined according to the target position information X t =[x t ,y t ] Solve the existence probability of the target object in all positions;

[0141] The probability of the target object existing in all positions E i The calculation formula is:

[0142]

[0143] In formula (5), is the target object at P at time t i =[x i ,y i ] position probability distribution, mean μ = [μ1, μ2] using the current moment predicted target position X t =[x t ,yt ]assignment.

[0144] S2: The shore-based end sends the search and rescue mission area and the starting point location information communication module to the AUV, and the global planner calculates the global coverage detection path and sends the global path to the path tracking module of the underwater vehicle. The target position and local flow field information detected by the sensor in real time are updated to the global grid map.

[0145] S201: Figure 3 As shown, the global planner on the AUV subscribes to the boundary information and deployment point location information of the search and rescue mission area, determines the global grid size according to the side scan sonar installation angle and the AUV bottom depth, and then rasterizes the search and rescue mission area. The global detection path is obtained through the improved unit decomposition-BINN fusion algorithm. The algorithm consists of two parts. First, the unit decomposition method is used to solve the global path based on the improved path point selection method. When there are obstacles in the global path, the BINN algorithm is started to calculate the path points, and finally an efficient coverage path is obtained. In addition, considering the characteristics of SSS sonar imaging, it is stipulated that the coverage path movement only moves in four directions: up, down, left, and right;

[0146] S202: Figure 4 As shown, by connecting the path segments obtained based on the unit decomposition method and the coverage path near the obstacle obtained based on the BINN algorithm, the complete coverage path is finally output;

[0147] S20201: Initialize the grid map and place obstacles;

[0148] S20202: Set the underwater robot starting point and search direction;

[0149] S20203: Control the underwater robot to move a distance W along the search direction and count the mass points into the path segment;

[0150] S20204: Determine whether the end position of the movement is one of an obstacle and the left boundary of the search task area. If not, perform S20203 again. If yes, perform S20205.

[0151] S20205: Execute the BINN algorithm to obtain the coverage path around the obstacle, and determine whether the underwater robot has reached the target point. If so, connect the path segment and the path around the obstacle as a global detection path for output. If not, proceed to S20206;

[0152] S20206: Determine whether the right boundary of the search task area has been reached. If not, move W distance in the opposite direction of the search direction and count the corresponding particle into the new path segment until the right boundary of the search task area has been reached. If yes, proceed to S20207;

[0153] S20207: Execute the BINN algorithm to obtain the coverage path around the obstacle and determine whether the target point has been reached. If not, repeat S20206 until the underwater robot reaches the target point. If so, connect the path segment and the path around the obstacle as a global detection path for output.

[0154] S3: The local planner will retrieve the global grid information in real time, trigger the re-planning module by determining whether there is target information, and calculate a secondary search path with optimal energy consumption through the re-planning algorithm.

[0155] like Figure 5 As shown in the figure, the local planner subscribes to the target position information of the side scan sonar and the local flow field information calculated by sensors such as ADCP, and designs an online re-planning scheme by improving the ant colony algorithm, taking into account the impact of the flow field on the path, which can efficiently solve the above problems and plan the secondary search path process:

[0156] The traditional ant colony algorithm does not consider the impact of ocean currents in the region on the path planning problem. It calculates the energy consumption of the AUV affected by the ocean currents when it passes through each path, and introduces the energy consumption into the cost function. The ant colony algorithm selects the optimal path according to the cost at each iteration, such as Figure 8 As shown, the steps of improving the ant colony algorithm include:

[0157] S301: Initialize basic parameters and create an environment model;

[0158] S302: placing ants at the starting point and calculating the energy consumption during the ant colony exploration process using an ant colony algorithm;

[0159] S30201: Figure 6 As shown, the area of ​​the underwater robot affected by the water flow is decomposed into projections in the X-axis and Y-axis directions and the projection area is calculated;

[0160] The projection area expressions of the underwater robot in the X-axis and Y-axis directions are:

[0161] S x =d(Lsinθ+Bcosθ) (7);

[0162] S y =d(Lcosθ+Bsinθ) (8);

[0163] In formula (7) and formula (8), L, B and d are the length, width and height of AUV respectively, θ is the angle between the moving direction and the X-axis, S x is the projection area of ​​AUV on the X axis, S y is the projection area of ​​AUV on the Y axis;

[0164] S30202: Calculate the AUV combined speed corresponding to the path, and calculate the energy consumption and total energy consumption of the underwater robot in the X-axis and Y-axis directions based on the projection areas of the underwater robot in the X-axis and Y-axis directions;

[0165] V w sinθ=V s sinδ(9);

[0166] V a =V s cosδ+V w cosθ (10);

[0167] In formula (9) and formula (10), V w is the ocean current velocity, V s is the maximum still water speed of the AUV, θ is the angle between the combined speed and the ocean current speed, V a is the AUV total speed corresponding to this path;

[0168] The expression of total energy consumption of underwater robot is:

[0169]

[0170]

[0171] E=E y -E x (13);

[0172] In formula (11)-formula (13), C c is the seawater resistance coefficient, ρ w is the density of seawater, E x is the energy consumed by the AUV in the X-axis direction, E y is the energy consumed by the AUV in the Y-axis direction, E is the total energy consumption of the AUV, t is the movement time of the AUV in each path, V w is the ocean current speed;

[0173] S30203: If Figure 7 As shown, the yaw angle is brought into the cost function, and the improved cost function is obtained by combining the total energy consumption of the underwater robot. The path smoothness in the exploration process is guaranteed based on the improved cost function.

[0174] The expression of the improved price change function is:

[0175] cost = ω1E-ω2cosγ (14);

[0176] In formula (14), ω1 and ω2 are the third weight and the fourth weight respectively, and cosγ is the yaw angle;

[0177] The expression for the yaw angle is:

[0178]

[0179] In formula (15), points J-1, J, and J+1 are three adjacent target points. is the vector from point J-1 to J; is the vector from point J to J+1, For vector Model; For vector The modulus of , γ is the angle between the two vectors, and n is the number of target points.

[0180] S303: Setting a heuristic function to guide the ant colony to move toward the target point;

[0181] The expression of the heuristic function is:

[0182]

[0183] In formula (16), t ij is the time required for navigation along the path, θ ij is the angle between the navigation direction of the path and the flow field direction, ω3 is the first weight, and ω4 is the second weight;

[0184] S304: Calculate the transfer probability of the ant colony, select the next node using the roulette method, and update the taboo table;

[0185] S305: Determine whether ant K has completed one exploration. If not, repeat S304 until ant K has completed one exploration. If yes, proceed to S306.

[0186] S306: Update ants: K=K+1;

[0187] S307: Determine whether all ants have completed one exploration. If not, repeat S304-S306 until all ants have completed one exploration. If yes, proceed to S308;

[0188] S308: Global update of pheromones, update of optimal path, and determination of whether the taboo table satisfies NC=NC max If yes, the updated optimal path is output as the replanned path. If no, proceed to S309;

[0189] S309: Clear the taboo table repeatedly, and repeat S304-S308 until the taboo table satisfies NC=NC max , and output the replanning path;

[0190] The expression to clear the taboo table is:

[0191] NC = NC + 1 (17);

[0192] In formula (17), NC is the number of iterations;

[0193] S4: By tracking the replanned path, the forward-looking sonar detects the target identity information in real time, and uploads the target identity and location information to the shore-based end. The shore-based end determines whether to deploy demolition machinery to perform demolition and rescue tasks based on the target identity and location information.

[0194] By replanning the search and rescue path, the present invention can give full play to the performance characteristics and advantages of different sensors of the underwater robot, can help the AUV to autonomously complete the detection task of the target area, and effectively improve the search efficiency and intelligence of the AUV.

[0195] The present invention can help underwater robots to accurately detect the location and identity information of search and rescue targets, and can complete the detection task of the search and rescue mission area with an efficient search and rescue path, thus effectively improving the efficiency of underwater search and rescue missions.

[0196] S5: Continue to perform the search and rescue mission, and compare the searched area with the search and rescue mission area to determine whether the search and rescue mission has been completed. If not, repeat S4 until the search and rescue mission is completed.

[0197] The above is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technician familiar with this profession can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any simple modification, equivalent replacement and improvement made to the above embodiments without departing from the content of the technical solution of the present invention, based on the technical essence of the present invention, within the spirit and principles of the present invention, still fall within the protection scope of the technical solution of the present invention.

Claims

1. A two-layer path planning system for underwater robots used for underwater search and rescue missions, characterized in that: The structure of the underwater robot double-layer path planning system for underwater search and rescue missions includes: underwater robotic systems and shore-based systems; The shore-based system is used to provide search and rescue mission area and starting point location information, and decide whether to deploy demolition machinery to perform demolition and rescue missions based on the forward-looking sonar image transmitted by the underwater robot system; The underwater robot system is used to perform the search and rescue mission for the wrecked ship according to the search and rescue mission area and the starting point location information.

2. The underwater robot double-layer path planning system for underwater search and rescue missions according to claim 1 is characterized in that: The underwater robot system comprises: Dual-layer path planning module, control center, navigation / communication module, perception module, side-scan sonar and control module; The navigation / communication module is used to receive the search and rescue mission area and starting point location information provided by the shore-based system; The control center is used to receive the search and rescue mission area and the starting point location information and transmit it to the double-layer path planning module; The dual-layer path planning module is used to calculate the re-planned path according to the search and rescue mission area and the starting point location information, and transmit the re-planned path to the control center; The control module controls the heading according to the re-planned path transmitted by the control center; The perception module is used to track the replanned path and obtain the position of the underwater robot in real time during the search and rescue process; The side scan sonar is used to obtain forward-looking sonar images during the search and rescue process, and transmit the forward-looking sonar images to the shore-based system through the navigation / communication module.

3. The underwater robot double-layer path planning system for underwater search and rescue missions according to claim 2 is characterized in that: The two-layer path planning module includes a global planner and a local planner; The global planner is used to perform rasterization processing on the search and rescue mission area and calculate the global detection path; The local planner retrieves global grid information in real time and calculates a secondary exploration path with optimal energy consumption.

4. A double-layer path planning method for underwater robots used for underwater search and rescue missions, applied to the double-layer path planning system for underwater robots used for underwater search and rescue missions as claimed in any one of claims 1 to 3, characterized in that: include: Step 1: Based on the shore-based system, the search and rescue mission area and the starting point location information are obtained and the underwater robot is deployed; Step 2: The search and rescue mission area and the starting point location information are transmitted to the navigation / communication module, the central module receives the search and rescue mission area and the starting point location information and transmits it to the double-layer path planning module, and the double-layer path planning module calculates the re-planned path and transmits it to the control center; Step 3: The control center transmits the replanned path to the control module for heading control, tracks the replanned path through the perception module, and obtains the position information of the underwater robot in the search and rescue process in real time; Step 4: Use the forward-looking sonar image in the real-time search and rescue process of the scanning sonar to identify the identity of the target object based on the forward-looking sonar image; Step 5: The target object identity and underwater robot position information are transmitted to the shore-based system through the navigation / communication module. The shore-based system determines whether to deploy the rescue machinery to perform the rescue mission according to the target object identity and position information; Step 6: Continue to perform the search and rescue mission, compare the searched area with the mission area, and determine whether the search and rescue mission is completed. If not, repeat steps 3-5 until the search and rescue mission is completed.

5. The underwater robot double-layer path planning method for underwater search and rescue missions according to claim 4 is characterized in that: The steps of obtaining the search and rescue mission area and the starting point location information and deploying the underwater robot in step 1 include: Step 1.1: Obtain the location information of the wrecked ship and the fixed island information by subscribing to the satellite; Step 1.2: Based on prior knowledge, combine the location information of the wrecked ship and the fixed island information to determine the propagation crash location P start =[x strat ,y start ] and the flow field information of the accident area U i (X)=[U,θ].

6. The double-layer path planning method for underwater robots used for underwater search and rescue missions according to claim 5, characterized in that: Determine the shipwreck location P in step 1.2 start =[x strat ,y start ] and the flow field information of the accident area U i The steps of (X) = [U, θ] include: Step 1.2.1: Use the Lagrangian model distribution to predict the trajectory distribution of the target under the influence of the flow field and obtain the motion equation of the target; Step 1.2.2: Consider the target object as a particle, and calculate the differential equations of the position and velocity of the target object based on the particle motion equation; Step 1.2.3: Use the difference method to solve the differential equation, use the Euler method to obtain the solution formula, and obtain the predicted position of the target object; Step 1.2.4: Based on the position vector P i =[x i ,y i ], mean vector μ = [μ1, μ2] T And the covariance matrix is Establish a two-dimensional distribution probability model of the target object, according to the target object position information X t =[x t ,y t ] Solve the probability E of the target object existing in all positions i ; Step 1.2.5: Combine the predicted position of the target object and the probability of the target object existing in all positions E i Determine the location of the ship's crash and deploy an underwater robot to the corresponding location; The motion equation of the target object is: In formula (1), X = [x, y] is the position vector of the target object, and V = [u, v] is the velocity vector of the target object itself under the action of the external flow field; The differential equations for the position and velocity of the target are: The expression for solving the differential equation is: X n+1 =X n +V n ·Δt(3); The solution formula is: In formula (4), Δt is the time step; The probability of the target object existing in all positions E i The calculation formula is: In formula (5), is the target object at P at time t i =[x i ,y i ] position probability distribution, mean μ = [μ1, μ2] using the current moment predicted target position X t =[x t ,y t ]assignment.

7. The underwater robot double-layer path planning method for underwater search and rescue missions according to claim 4, characterized in that: The steps of calculating the re-planned path by the two-layer path planning module in step 2 include: Step 2.1: The global planner subscribes to the boundary information of the search and rescue mission area and the underwater robot deployment position information, determines the global grid size according to the installation angle of the scanning sonar and the bottom depth of the underwater robot, rasterizes the search and rescue mission area, obtains the global detection path through the improved unit decomposition BINN fusion algorithm, transmits the global detection path to the control module, and updates the target position and local flow field information detected by the scanning sonar in real time to the global grid map; Step 2.2: The local planner retrieves the global grid information in real time, determines whether there is target information to trigger the local planner, and calculates the re-planning path with optimal energy consumption through the improved ant colony algorithm.

8. The underwater robot double-layer path planning method for underwater search and rescue missions according to claim 7, characterized in that: The steps of obtaining the global detection path by the improved unit decomposition-BINN fusion algorithm in step 2.1 include: Step 2.1.1: Initialize the grid map and place obstacles; Step 2.1.2: Set the underwater robot's starting point and search direction; Step 2.1.3: Control the underwater robot to move a distance W along the search direction and count the particle into the path segment; Step 2.1.4: Determine whether the end position of the movement is one of the obstacle and the left boundary of the search task area. If not, repeat step 2.1.

3. If yes, proceed to step 2.1.

5. Step 2.1.5: Execute the BINN algorithm to obtain the coverage path around the obstacle and determine whether the underwater robot has reached the target point. If so, connect the path segment and the path around the obstacle as the global detection path for output. If not, proceed to step 2.1.6; Step 2.1.6: Determine whether the right boundary of the search task area has been reached. If not, move W distance in the opposite direction of the search direction and count the corresponding particle into the new path segment until the right boundary of the search task area has been reached. If yes, proceed to step 2.1.7; Step 2.1.7: Execute the BINN algorithm to obtain the coverage path around the obstacle and determine whether it has reached the target point. If not, repeat step 2.1.6 until the underwater robot reaches the target point. If so, connect the path segment and the path around the obstacle as the global detection path for output.

9. The underwater robot double-layer path planning method for underwater search and rescue missions according to claim 7, characterized in that: The steps of calculating the replanning path with optimal energy consumption by improving the ant colony algorithm in step 2.2 include: Step 2.2.1: Initialize basic parameters and create an environment model; Step 2.2.2: Place the ants at the starting point and calculate the energy consumption during the ant colony exploration process using the ant colony algorithm; Step 2.2.3: Set the heuristic function to guide the ant colony to move towards the target point; Step 2.2.4: Calculate the transfer probability of the ant colony, use the roulette method to select the next node, and update the taboo table; Step 2.2.5: Determine whether ant K has completed an exploration. If not, repeat step 2.2.4 until ant K has completed an exploration. If yes, proceed to step 2.2.

6. Step 2.2.6: Update ants: K = K + 1; Step 2.2.7: Determine whether all ants have completed one exploration. If not, repeat steps 2.2.4 to 2.2.6 until all ants have completed one exploration. If yes, proceed to step 2.2.8; Step 2.2.8: Global update of pheromones, update of optimal path, and determination of whether the taboo table satisfies NC=NC max , if yes, then output the updated optimal path as the replanned path, if not, proceed to step 2.2.9; Step 2.2.9: Clear the taboo table again, and repeat steps 2.2.4 to 2.2.8 until the taboo table satisfies NC = NC max , and output the replanning path; The expression to clear the taboo table is: NC = NC + 1 (6); In formula (6), NC is the number of iterations; The expression of the heuristic function is: In formula (7), t ij is the time required for navigation along the path, θ ij is the angle between the navigation direction of the path and the flow field direction, ω3 is the first weight, and ω4 is the second weight.

10. The underwater robot double-layer path planning method for underwater search and rescue missions according to claim 9, characterized in that: The steps of calculating the energy consumption during the ant colony exploration process by using the ant colony algorithm in step 2.2.2 include: Step 2.2.2.1: Decompose the area of ​​the underwater robot affected by the water flow into projections in the X-axis and Y-axis directions and calculate the projection area; Step 2.2.2.2: Calculate the energy consumption and total energy consumption of the underwater robot in the X-axis and Y-axis directions based on the projection areas of the underwater robot in the X-axis and Y-axis directions; Step 2.2.2.3: Bring the yaw angle into the cost function, combine it with the total energy consumption of the underwater robot to obtain an improved cost function, and ensure the path smoothness during the exploration process based on the improved cost function; The projection area expressions of the underwater robot in the X-axis and Y-axis directions are: S x =d(Lsinθ+Bcosθ) (8); S y =d(Lcosθ+Bsinθ) (9); In formula (8) and formula (9), L, B and d are the length, width and height of AUV respectively, θ is the angle between the moving direction and the X-axis, S x is the projection area of ​​AUV on the X axis, S y is the projection area of ​​AUV on the Y axis; The expression of total energy consumption of underwater robot is: E=E y -AND x (12); In formula (10)-formula (12), C c is the seawater resistance coefficient, ρ w is the density of seawater, E x is the energy consumed by the AUV in the X-axis direction, E y is the energy consumed by the AUV in the Y-axis direction, E is the total energy consumption of the AUV, t is the movement time of the AUV in each path, V w is the ocean current speed; The improved cost function expression is: cost = ω1E-ω2cosγ (13); In formula (13), ω1 and ω2 are the third weight and the fourth weight respectively, and cosγ is the yaw angle; The yaw angle expression is: In formula (14), points J-1, J, and J+1 are three adjacent target points. is the vector from point J-1 to J; is the vector from point J to J+1, For vector Model; For vector The modulus of , γ is the angle between the two vectors, and n is the number of target points.

Images