Path planning method of underwater vehicle in complex environment based on foresight sonar

Abstract

The invention discloses a forward-looking sonar-based path planning method for an underwater vehicle in a complex environment, and the method comprises the following steps: 1, constructing a path planning model which comprises a YOLO target detection network, an A * algorithm, a DWA algorithm, and a BoT-SORT algorithm; 2, optimizing a YOLO target detection network structure: adding a decoupling head; optimizing an A * algorithm: constructing a segmentation weight, and performing weighted summation of the cost and the estimated cost through the segmentation weight; step 3, constructing a boundary restriction mechanism and a leak detection prevention mechanism; 4, coordinate conversion and preprocessing are carried out on the front sonar image set; and 5, training a path planning model through the front sonar image set processed in the step 4, and performing path planning by using the trained path planning model. According to the invention, a detection-tracking-planning integrated obstacle avoidance strategy is realized, and analysis and planning can be carried out according to three characteristics of target characteristics, position moving directions and speed and distance changes of dynamic obstacles.

Description

Technical Field

[0001] The invention relates to the technical field of path planning, and in particular to a path planning method for an underwater vehicle in a complex environment based on forward-looking sonar. Background Art

[0002] Ensuring the safe development of underwater professions is of great value to all fields, and efficient path planning is one of the key technologies for underwater professional safety. For unmanned underwater vehicles equipped with forward-looking sonar, combining deep learning, path planning algorithms and the advantages of fast imaging speed and low power consumption of forward-looking sonar can effectively achieve safe avoidance of underwater static, emerging and dynamic obstacles and optimal path planning.

[0003] At present, mainstream path planning research mainly uses ant colony algorithm, neural network method and free space method, but the ant colony algorithm has a slow convergence speed in complex environments; the neural network law is difficult to express using the corresponding planning formula; for the free space method, the huge amount of calculation is a problem that it is difficult to overcome, making it difficult for this method to effectively cope with real-time planning of complex underwater scenes. At the same time, most traditional path planning algorithms can only show better avoidance capabilities for static known obstacles, lack the ability to avoid sudden obstacles and dynamic obstacles, and traditional algorithms are also difficult to effectively overcome the situation of being easily trapped in local optimal solutions. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a path planning method for an underwater vehicle in a complex environment based on forward-looking sonar, so as to solve the technical problems in the prior art of poor obstacle avoidance performance for static, emerging, dynamic obstacles and a mixture of multiple obstacles in a complex underwater environment, as well as low efficiency of the path planning algorithm and easy trapping in a local optimal solution.

[0005] The present invention provides a path planning method for an underwater vehicle in a complex environment based on a forward-looking sonar, comprising the following steps:

[0006] Step 1: Build a path planning model, which includes: the YOLO target detection network for target recognition in sonar images, the A* algorithm for global path planning, the DWA algorithm for local obstacle avoidance, and the BoT-SORT algorithm for dynamic target tracking during local obstacle avoidance;

[0007] Step 2: Optimize the YOLO target detection network structure: add a decoupling head for small target detection;

[0008] Optimize the A* algorithm: construct segment weights and use the segment weights to perform weighted summation of cost and estimated cost;

[0009] Step 3: Build boundary restriction mechanism and anti-missing detection mechanism;

[0010] Step 4: Obtain a front sonar image set of a complex underwater environment, and perform coordinate transformation and preprocessing on the front sonar image set;

[0011] Step 5: Train the path planning model using the front sonar image set processed in step 4, and use the trained path planning model for path planning.

[0012] Furthermore, in step 2, the specific method of adding a decoupling head is: performing fusion feature extraction and EMA processing on the feature maps merged in the first upsampling in sequence to output scale features, and convolving the EMA processing results with the features used as the first upsampling input for the next scale feature output.

[0013] Furthermore, the optimal cost function formula in the A* algorithm in step 2 is:

[0014] f(n)=g(n)+w(t)*h(n)

[0015] Where g(n) is the cost function; h(n) is the heuristic function; w(t) is the segment weight.

[0016] Furthermore, in the A* algorithm, when the distance between the planning node and the obstacle is greater than 10 nodes, the Euclidean distance formula is selected to calculate the cost and the estimated cost; when the distance between the planning node and the obstacle is less than or equal to 10 nodes, the Manhattan distance formula is selected to calculate the cost and the estimated cost.

[0017] Furthermore, in the A* algorithm, when the detection distance can cover the end point but the end point cannot be searched, the several nodes before the current node are traced back to determine whether there is a large displacement angle change. When a large displacement angle change occurs, a waiting program is started. The waiting program is specifically as follows: wait for the restart time and then search again. When the end point can be searched again, the waiting program is stopped and the subsequent steps are continued; when the end point cannot be searched, the waiting process is repeated until the number of repetitions reaches the upper limit.

[0018] Furthermore, the calculation formula of the restart time is:

[0019]

[0020] Where, T is the restart time; The reciprocal of the distance between the dynamic obstacle and the current position; Box obstacle is the size characteristic of the dynamic obstacle; is the speed of the dynamic obstacle; σ represents the linear proportional mapping relationship between the weighted score and time, which can be adjusted according to the actual task requirements.

[0021] Furthermore, each time the restart time is calculated, the last dynamic obstacle related data is cleared, and the dynamic obstacle related data is reacquired through the optimized YOLO target detection network and BoT-SORT algorithm.

[0022] Furthermore, when the target distance is greater than 20 nodes, w(t) is set to 2; when the target distance is less than 20 nodes, w(t) is set to 0.5.

[0023] Furthermore, the step 2 also includes: optimizing the DWA algorithm: performing a second sampling around the optimal speed combination obtained by the first sampling.

[0024] Furthermore, in step 2, optimizing the DWA algorithm further includes: introducing an endpoint guidance mechanism in the process of using the DWA algorithm for local obstacle avoidance.

[0025] Furthermore, the overall evaluation function of the DWA is:

[0026] G(v,w)=σ(α·target(v,w)+β·dist(v,w)+γ·velocity(v,w))+δ·(final(v,w))

[0027] Where target(v,w) is the evaluation function of the distance to the target point; dist(v,w) is the evaluation function of the trajectory distance to obstacles; velocity(v,w) is the speed evaluation function.

[0028] Furthermore, in step 4, the specific formula for coordinate transformation of the front sonar image set is as follows:

[0029]

[0030] Where x, y, l, h represent the center position coordinate information (x, y) and length and height information of the obstacle area prediction box detected from the YOLO target detection network; w0, Indicates the resolution of the image detected by the YOLO target detection network; w1, Indicates the size of the path planning background box of the YOLO target detection network detection image.

[0031] Furthermore, in step 5, the obstacle setting method in the path planning model training process includes: randomly adding emergent obstacles.

[0032] Furthermore, in step 1, it is set that when the A* algorithm globally plans the path, the first 180° method is used to search the path.

[0033] Beneficial effects of the present invention:

[0034] The path planning model constructed by the present invention realizes an integrated obstacle avoidance strategy of detection-tracking-planning, and can analyze and plan for the three characteristics of dynamic obstacles: target characteristics, position movement direction, and speed and distance changes. At the same time, the algorithms in the path planning model are optimized, which can provide the path planning algorithm with real-time and accurate obstacle area size and location information, greatly simplifying the path planning steps and shortening the detection time, thereby improving the real-time response speed of the underwater vehicle.

[0035] The present invention combines and optimizes the A* algorithm for global planning and the DWA algorithm for local obstacle avoidance, which can better solve local emergencies, further improve the efficiency of path planning, and solve the possible problems of global algorithm failure and falling into local dead loops.

[0036] The present invention adopts the setting of boundary limitation mechanism and anti-omission mechanism, which can prevent the simulated aircraft from mistakenly entering the visual blind spot and realize error correction of obstacle information.

[0037] The present invention uses a method of randomly adding emergent obstacles in the path planning model, which can effectively improve the efficiency and accuracy of the path planning model in dealing with sudden obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The features and advantages of the present invention will be more clearly understood by referring to the accompanying drawings, which are schematic and should not be construed as limiting the present invention in any way. In the accompanying drawings:

[0039] Figure 1 is a flow chart of a specific embodiment of the present invention;

[0040] Figure 2 It is a schematic diagram of the algorithm flow of a specific embodiment of the present invention;

[0041] Figure 3 This is a diagram of the optimized YOLOv8n target detection network structure in a specific embodiment of the present invention;

[0042] Figure 4 This is a visual test effect diagram of the optimized YOLOv8n target detection network in a specific embodiment of the present invention;

[0043] Figure 5 This is a test effect diagram of the PR curve of the optimized YOLOv8n target detection network in a specific embodiment of the present invention;

[0044] Figure 6 A is the optimized A in the specific embodiment of the present invention * Algorithm flow chart;

[0045] Figure 7This is a flow chart of the optimized DWA algorithm in a specific embodiment of the present invention;

[0046] Figure 8 This is the test effect diagram before A* algorithm optimization;

[0047] Fig. 9 This is the test effect diagram after A* algorithm optimization;

[0048] Fig.10 The figure is a schematic diagram of path planning according to a specific embodiment of the present invention. DETAILED DESCRIPTION

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

[0050] The present invention is further illustrated below in conjunction with specific embodiments. Those skilled in the art should understand that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention, and modifications to various equivalent forms of the present invention fall within the scope defined by the appended claims of this application.

[0051] like Figure 1 , 2 As shown, a specific embodiment of the present invention provides a path planning method for an underwater vehicle in a complex environment based on a forward-looking sonar, comprising the following steps:

[0052] Step 1: Build a path planning model, which includes: the YOLOv8n target detection network for target recognition in sonar images, the A* algorithm for global path planning, the DWA algorithm for local obstacle avoidance, and the BoT-SORT algorithm for dynamic target tracking during local obstacle avoidance.

[0053] Step 2: Optimize the YOLOv8n target detection network structure and add a decoupling head for small target detection based on the existing structure, such as Figure 3 Specifically, the output of the Concat layer in the first upsampling is processed by the C2f layer and the EMA layer in turn to output the scale feature of 160*160*128; and the EMA layer processing result is passed through the Conv layer and then merged with the output of the C2f_kw layer in the backbone network through the Concat layer feature for the next scale feature output. Among them, the added C2f layer, EMA layer, Conv layer, Concat layer, and Detect layer constitute the newly added decoupling head.

[0054] In order to verify the improvement effect of the YOLOv8n model and better carry out the subsequent path planning steps, the detection performance of the improved YOLOv8n was evaluated, such as Figure 4 , 5 As shown in the figure, the visualization effect and detection accuracy data results show that the improved YOLOv8n detection model has excellent target detection performance and can fully meet the needs of subsequent path planning.

[0055] When setting the A* algorithm to globally plan the path, the first 180° method is used for path search; based on the cost optimization principle in the global planning process, the optimal cost acquisition method in the A* algorithm is optimized: segment weights are constructed, and the weighted sum of the cost and the estimated cost is performed through the weights. Considering the existence of obstacles, in order to further improve the reliability of subsequent planning costs, a distance calculation formula optimization strategy is constructed. When the planned path is far from the obstacle (greater than 10 nodes), the Euclidean distance formula is used as the cost evaluation benchmark; when the planned path is close to the obstacle (less than or equal to 10 nodes), it automatically switches to use the Manhattan distance formula as the cost evaluation benchmark. Use such Figure 6 The optimized A* algorithm shown in the figure performs global obstacle avoidance and path planning on the acquired obstacle area and coordinate information, which can improve the flexibility and efficiency of planning. In the later stage of planning, the timing waiting program is started and judgment is made. The specific process is as follows:

[0056] Step A1: Initialize the algorithm, set the starting point, and store it in the current list;

[0057] Step A2: Find surrounding nodes and store them in the current list, remove the starting point from the current list and store it in the storage list, which stores the path nodes that have been reached;

[0058] Step A3: Evaluate the nodes in the current list and move the nodes that meet the minimum cost into the storage list, where the optimal cost is given by the formula:

[0059] f(n)=g(n)+w(t)*h(n)

[0060] g(n new )=g(n current )+1

[0061] h1(n)=|x1-x2|+|y1-y2|

[0062]

[0063] 0°<θ new-＞current <180°

[0064] In the formula, f(n) is the optimal cost function that needs to be satisfied; g(n) is the cost function; h(n) is the heuristic function; w(t) is the segment weight. When the distance d to the target is large, w(t) is increased to 2. At this time, the algorithm searches faster. When the distance d to the target is small, w(t) is reduced to 0.5. That is, when approaching the end point, the optimal path is given priority. The overall effect is better and more flexible than a single weight.

[0065] Step A4: Continue searching for the surrounding nodes of the node with the minimum cost. When the searched node is not in the current list, add the searched node to the current list and return to step 3 until no surrounding nodes are found.

[0066] Step A5: When the detection distance can cover the end point but the end point cannot be found, trace back to several nodes before the current node to determine whether there is a large displacement angle change. When a large displacement angle change occurs, it means that a dynamic obstacle has entered between the aircraft and the end point, causing a blockage. Start the waiting program and wait for the dynamic obstacle to move away. The waiting program is specifically as follows: wait for the restart time and search again. When the end point can be searched again, stop the waiting program and continue with the subsequent steps; when the end point cannot be searched, repeat the waiting process. When the number of repetitions reaches the upper limit, stop planning and return to status -1 in the program.

[0067] The calculation formula for the restart time is:

[0068]

[0069] Where, T is the restart time; The reciprocal of the distance between the dynamic obstacle and the current position; Box obstacle is the size characteristic of the dynamic obstacle; is the speed of the dynamic obstacle; σ represents the linear proportional mapping relationship between the weighted score and time, which can be adjusted according to the actual task requirements.

[0070] Taking into account the efficiency and accuracy of the algorithm, each time the restart time is calculated, the last dynamic obstacle-related data is cleared, and the dynamic obstacle-related data is re-acquired through the optimized YOLO target detection network and BoT-SORT algorithm.

[0071] Optimize the DWA algorithm: Perform a second sampling around the optimal speed combination obtained in the first sampling, and introduce an endpoint guidance mechanism in the process of using the DWA algorithm for local obstacle avoidance. This can increase the detection range of the DWA local path planning algorithm and further enhance the obstacle avoidance capability of DWA, which can effectively overcome the occurrence of local dead loops.

[0072] After introducing the endpoint guidance mechanism, the overall evaluation function is:

[0073] G(v,w)=σ(α·target(v,w)+β·dist(v,w)+γ·velocity(v,w))+δ·(final(v,w))

[0074] Where target(v,w) is the evaluation function of the distance to the target point; dist(v,w) is the evaluation function of the trajectory distance to obstacles; velocity(v,w) is the speed evaluation function; v is the speed; w is the angular velocity; α, β, γ, δ are the corresponding weight coefficients, which belong to the hyper-parameter range and can be flexibly adjusted according to task requirements.

[0075] like Figure 7 As shown in the figure, during the operation of the DWA algorithm, a timing loop algorithm is used for real-time planning and synchronous update of the dynamic obstacle area. The current distance, speed and direction of the dynamic obstacle can be analyzed through the real-time changes in the above dynamic obstacle occlusion situation, and the above analyzed data information can be transmitted to the reward and punishment mechanism in real time for deceleration, local obstacle avoidance distance planning and optimal path selection.

[0076] Step 3: Construct boundary restriction mechanism and anti-missing detection mechanism to limit the path planning range of the underwater vehicle to the effective visual range of the forward-looking sonar image, and at the same time avoid affecting the subsequent path planning accuracy due to misdetection or missed detection;

[0077] The constructed boundary restriction mechanism is defined as temporarily blocking the current planned route when the simulated vehicle is about to cross the boundary. When the underwater vehicle reaches the boundary area, the underwater vehicle is set to navigate along the boundary. When the pre-planned route is re-detected in the forward-looking sonar observation area, it continues to travel along the original planned path.

[0078] Step 4: Obtain a front sonar image set of a complex underwater environment, and perform coordinate transformation and preprocessing on the front sonar image set;

[0079] The coordinate transformation is carried out based on the characteristics of forward-looking sonar vision, which uses the midpoint directly below the image as the observation starting point, visual symmetry, and the fact that the YOLO detection coordinates are the starting point of the upper left corner. The specific formula for coordinate transformation of the forward sonar image set is as follows:

[0080]

[0081] Where x, y, l, h represent the center position coordinate information (x, y) and length and height information of the obstacle area prediction box detected from the YOLO target detection network; w0, Indicates the resolution of the image detected by the YOLO target detection network; w1, Indicates the size of the path planning background box of the YOLO target detection network detection image;

[0082] At the same time, mean filtering and grayscale image processing methods are used to filter out a large number of noise points in the original sonar image to facilitate subsequent path planning.

[0083] Step 5: The path planning model is trained using the front sonar image set processed in step 4. The obstacle setting method during the path planning model training process includes: randomly adding emergent obstacles, and finally using the trained path planning model for path planning.

[0084] In order to verify the effectiveness and comprehensive performance of the present invention, MATLAB was used to perform grayscale processing and mean filtering denoising on the image. The simulation used Python 3.8 programming language, Window 10 operating system, PyTorch 1.11.2 programming environment, and 300 training times.

[0085] like Figure 8 , 9 As shown in the figure, when the A* algorithm before and after the improvement obtains the same optimal solution, the improved A* algorithm has improved both the planning time and the search range compared to the original A* planning algorithm, saving computing resources and further improving the operating efficiency of the subsequent local avoidance algorithm.

[0086] The comparison results of different planning algorithms are shown in Table 1 below:

[0087]

[0088] Table 1

[0089] It can be seen from the table that, compared with other planning algorithms, the planning speed and computational complexity of the A* algorithm are generally better than those of other algorithms. At the same time, the planning path of the A* algorithm is also optimal, which shows that the A* algorithm has better planning efficiency and can better meet the real-time performance requirements of the present invention.

[0090] like Fig.10 As shown in the figure, the present invention can still effectively avoid and reach the destination in various situations such as multiple obstacles, narrow sections, and sudden obstacles. The obstacle avoidance function adopted by the present invention is realized by the improved DWA algorithm avoidance function. Fig.10 It can be seen that there are multiple simulated routes in front of the underwater vehicle. What makes these simulated routes more efficient than other avoidance algorithms is that they are updated in real time. When encountering obstacles, some simulated routes will be shortened or completely lost due to occlusion. At this time, the underwater vehicle will make real-time reward and punishment evaluations based on the changes in the simulated routes and update the navigation direction and speed in a timely manner. Both flexibility and real-time performance are difficult to match by other planning algorithms.

[0091] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A path planning method for an underwater vehicle in a complex environment based on forward-looking sonar, characterized in that: The steps include: Step 1: Build a path planning model, which includes: the YOLO target detection network for target recognition in sonar images, the A* algorithm for global path planning, the DWA algorithm for local obstacle avoidance, and the BoT-SORT algorithm for dynamic target tracking during local obstacle avoidance; Step 2: Optimize the YOLO target detection network structure: add a decoupling head for small target detection; Optimize the A* algorithm: construct segment weights and use the segment weights to perform weighted summation of cost and estimated cost; Step 3: Build boundary restriction mechanism and anti-missing detection mechanism; Step 4: Obtain a front sonar image set of a complex underwater environment, and perform coordinate transformation and preprocessing on the front sonar image set; Step 5: Train the path planning model using the front sonar image set processed in step 4, and use the trained path planning model for path planning.

2. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 1, characterized in that: In step 2, the specific method of adding a decoupling head is: extracting fused features and performing EMA processing on the feature maps merged in the first upsampling in sequence to output scale features, and merging the EMA processing results with the features used as the first upsampling input after convolution for the next scale feature output.

3. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 1, characterized in that: The optimal cost function formula in the A* algorithm in step 2 is: f(n)=g(n)+w(t)*h(n) Where g(n) is the cost function; h(n) is the heuristic function; w(t) is the segment weight.

4. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 1, characterized in that: In the A* algorithm, when the distance between the planning node and the obstacle is greater than 10 nodes, the Euclidean distance formula is selected to calculate the cost and estimated cost; when the distance between the planning node and the obstacle is less than or equal to 10 nodes, the Manhattan distance formula is selected to calculate the cost and estimated cost.

5. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 1, characterized in that: In the A* algorithm, when the detection distance can cover the end point but the end point cannot be searched, the algorithm backtracks to several nodes before the current node to determine whether there is a large displacement angle change. When a large displacement angle change occurs, the waiting program is started. The waiting program is specifically as follows: wait for the restart time and search again. When the end point can be searched again, the waiting program is stopped and the subsequent steps are continued. When the end point cannot be searched, the waiting process is repeated until the number of repetitions reaches the upper limit.

6. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 5, characterized in that: The calculation formula of the restart time is: Where T is the restart time; The reciprocal of the distance between the dynamic obstacle and the current position; Box obstacle is the size characteristic of the dynamic obstacle; is the speed of the dynamic obstacle; σ represents the linear proportional mapping relationship between the weighted score and time, which can be adjusted according to the actual task requirements.

7. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar according to claim 5 or 6, characterized in that: Each time the restart time is calculated, the last dynamic obstacle related data is cleared, and the dynamic obstacle related data is re-acquired through the optimized YOLO target detection network and BoT-SORT algorithm.

8. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 1, characterized in that: The step 2 also includes: optimizing the DWA algorithm: performing a second sampling around the optimal speed combination obtained by the first sampling.

9. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar as claimed in claim 8, characterized in that: In step 2, optimizing the DWA algorithm further includes: introducing an end-point guidance mechanism in the process of using the DWA algorithm for local obstacle avoidance, and the overall evaluation function of the DWA is: G(v,w)=σ(α·target(v,w)+β·dist(v,w)+γ·velocity(v,w))+δ·(final(v,w)) Where target(v,w) is the evaluation function of the distance to the target point; dist(v,w) is the evaluation function of the trajectory distance to obstacles; velocity(v,w) is the speed evaluation function.

10. The path planning method for an underwater vehicle in a complex environment based on forward-looking sonar according to claim 1, characterized in that: In step 4, the specific formula for coordinate transformation of the front sonar image set is as follows: Where x, y, l, h represent the center position coordinate information (x, y) and length and height information of the obstacle area prediction box detected from the YOLO target detection network; w0, Indicates the resolution of the image detected by the YOLO target detection network; w1, Indicates the size of the path planning background box of the YOLO target detection network detection image.

Images