Global static and local dynamic-based underwater tunnel path planning method
By combining A* and DWA algorithms for real-time obstacle detection and avoidance, the global and local path planning problems in underwater tunnel robot navigation are solved, achieving safe and effective underwater tunnel path planning.
Patent Information
- Application Number
- PCT/CN2023/102647
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2023-05-22
- Filing Date
- 2023-06-27
- Publication Date
- 2024-11-28
AI Technical Summary
In existing technologies, global path planning cannot handle dynamic obstacles in real time, and local path planning is prone to getting stuck in local optima, leading to navigation direction deviations and collision risks for underwater tunnel robots.
Combining A* global path planning and DWA local path planning algorithms, obstacles are detected in real time by sensors. During the global path planning process, the algorithm switches to local path planning to bypass obstacles and returns to the global path after the obstacles disappear. The velocity vector is optimized using an evaluation function to avoid collisions.
This technology enables underwater tunnel robots to avoid obstacles in real time during global path planning, ensuring safe and efficient arrival at the target point and avoiding collision risks.
Smart Images

Figure CN2023102647_28112024_PF_FP_ABST
Abstract
Description
Underwater tunnel path planning method based on global static and local dynamics Technical Field
[0001] The present invention relates to the technical field of underwater robot automatic navigation, and in particular to an underwater tunnel path planning method based on global static and local dynamic methods. Background Art
[0002] Water diversion tunnels are common in large-scale water conservancy and hydropower projects, and can reach tens of kilometers in length. Their structural health is crucial to the safety of these facilities, necessitating regular inspection and maintenance of underwater tunnels. Due to the confined underwater environment, manual inspections are time-consuming, labor-intensive, and unsafe. Therefore, robots are being used to inspect underwater tunnels. These robots need to determine their position and plan their paths, a process known as underwater navigation and positioning.
[0003] Global path planning is divided into two main approaches: graph search methods and sampling methods. Graph search methods include A* and Dijkstra, while sampling methods include PRM and RRT algorithms. Local path planning primarily includes artificial potential field methods, dynamic window methods, and TEB methods. Global path planning cannot be performed in real time and must be based on a pre-existing map. Local path planning is prone to local optimality and lacks a comprehensive view of the overall situation, which can lead to deviations in the planned direction. Therefore, combining global path planning with local optimality has become a pressing issue.
[0004] Summary of the Invention
[0005] The present invention proposes an underwater tunnel path planning method based on global static and local dynamic, comprising the following steps:
[0006] Step 1: Global path planning: Use the A* global path planning algorithm to perform overall global path planning based on the cost value in the grid map to obtain the first route from the starting point to the end point;
[0007] Step 2: While moving along the first route, the ROV uses sensors to obtain information about the surrounding environment and forms a warning area around the ROV;
[0008] Step 3: When an obstacle appears in the warning area, a second route is obtained by DWA local path planning to bypass the obstacle.
[0009] Step 4: When the obstacle in the warning area disappears, return to the first route from the second route;
[0010] Step 5: Repeat steps 2 to 4 until the target point is reached.
[0011] In step 4, the ROV is controlled to maintain a safe distance from the obstacle and advance toward the nearest waypoint in the first route in the forward direction.
[0012] Preferably, in step 1, the A* global path planning algorithm formula includes: F(n)=G(n)+H(n);
[0013] G(n) is the actual cost of moving from the starting node to the current node, H(n) is the estimated cost of moving from the current node to the target node, and F(n) is the sum of the costs of G(n) and H(n).
[0014] Preferably, in step 2, the sensor includes sonar or underwater laser radar.
[0015] Preferably, in step 2, the point cloud data obtained by the sensor is used to create a grid map through SLAM technology. The grid map distinguishes between idle areas and obstacles by cost value. When the cost value exceeds a preset value, it is determined to be an obstacle.
[0016] Preferably, in step 3, the velocity vector space of the ROV is determined, and all possible velocity vectors are denoted as v s , the velocity vector of all avoidable obstacles is v a , the velocity vector that meets all acceleration limits is v d , then the space of all feasible velocity vectors v r It can be expressed as: r =v s ∩v a ∩v d
[0017] Secondly, in the velocity vector space v r The appropriate speed is selected so that the ship can avoid obstacles and move forward, so the evaluation function is established: G(v,w)=σ(α.heading(v,w)+β·dist(v,w)+γ·vel(v,w))
[0018] Where heading(v, w) is the degree of similarity between the ship's attitude and the expected attitude when arriving at the predicted point at this speed, expressed as: heading(v, w) = 1-|θ| / π
[0019] Where θ represents the angle between the ship's attitude and the expected attitude when the ship reaches the predicted position at this speed;
[0020] dist(v, w) represents the distance between the current position of the ship and the nearest obstacle. The greater the distance between the ship and the obstacle, the higher the evaluation of the function. Specifically, it is expressed as:
[0021] dist(v, w) = rγ
[0022] In the formula is the predicted trajectory radius, γ is the rotation angle;
[0023] vel(v, w) is the speed of the ship. The greater the speed, the higher the evaluation. It is specifically expressed as: vel(v, w) = v / v max
[0024] Where v is the linear velocity, v max is the maximum expected linear velocity.
[0025] Preferably, in step 4, when the underwater ROV returns from the second route to the first route, the end of the second route is used as the starting point, and the waypoint closest to the starting point in the positive direction of the first route is used as the end point.
[0026] Preferably, in step 1, the first route is located in the center of the underwater tunnel.
[0027] Compared with the prior art, the advantages of the present invention are:
[0028] The invention plans the driving path of an ROV in an underwater tunnel based on global static and local dynamic algorithms. The invention plans the static path of the entire map based on the A* global path planning algorithm. During the process of following the static path, obstacles are continuously detected. When an obstacle is detected, the DWA local path planning algorithm is switched to perform path planning in a local range to bypass the obstacle. When there are no obstacles in the warning area around the ROV, the ROV returns to the predetermined route by selecting a waypoint in the forward direction that is closest to the current position. The waypoint is generated by the global path planning, and the ROV is driven to the waypoint. In this way, the ROV can travel from the starting point to the end point without collision in the intermediate process. BRIEF DESCRIPTION OF THE DRAWINGS
[0029] The accompanying drawings are not intended to be drawn to scale. In the drawings, each identical or nearly identical component shown in various figures may be represented by the same reference numeral. For the sake of clarity, not every component is labeled in every figure. Embodiments of various aspects of the present invention will now be described by way of example and with reference to the accompanying drawings, in which:
[0030] FIG1 is a schematic diagram of path planning for obstacles appearing in the ROV warning area shown in the present invention;
[0031] FIG2 is a schematic diagram of path planning in which there are no obstacles in the ROV warning area shown in the present invention. DETAILED DESCRIPTION
[0032] In order to better understand the technical content of the present invention, specific embodiments are given and described below with reference to the accompanying drawings.
[0033] The present invention is applicable to underwater tunnel path planning for underwater ROVs (Remote Operated Vehicles). Currently, path planning algorithms generally include global and local path planning algorithms. Specifically, the A* global path planning algorithm has the advantage of being applicable to static path planning for the entire map. It can plan a suitable and safe track from the starting point to the end point for the vessel to navigate based on the existing raster map information. The DWA local path planning algorithm has the advantage of being able to cope with dynamically appearing obstacles. Secondly, the DWA algorithm has velocity vector space limitations, that is, it considers the characteristics of the vessel during path planning.
[0034] The disadvantages of the A* global path planning algorithm are that it cannot handle obstacles that are not recorded in the grid map or dynamic obstacles. Once the ship encounters such obstacles, it will inevitably collide. Secondly, the A* algorithm cannot plan in accordance with the limitations of the ship itself, such as the acceleration limit of the ship (it does not consider the constraints of the robot itself and only searches for paths based on the cost value in the grid map). The disadvantage of the DWA local path planning algorithm is that it is only suitable for small-scale path planning.
[0035] 1-2 , the present invention proposes a method for underwater tunnel path planning based on global static and local dynamic methods, comprising the following steps:
[0036] Step 1: Global path planning: Use the A* global path planning algorithm to perform overall global path planning based on the cost value in the grid map to obtain the first route from the starting point to the end point;
[0037] Step 2: While moving along the first route, the ROV uses sensors to obtain information about the surrounding environment and forms a warning area around the ROV;
[0038] Step 3: When an obstacle appears in the warning area, a second route is obtained by DWA local path planning to bypass the obstacle.
[0039] Step 4: When the obstacle in the warning area disappears, return to the first route from the second route;
[0040] Step 5: Repeat steps 2 to 4 until the target point is reached.
[0041] In step 4, the ROV is controlled to maintain a safe distance from the obstacle and advance toward the nearest waypoint in the first route in the forward direction.
[0042] Wherein, in step 1, the A* global path planning algorithm formula includes: F(n)=G(n)+H(n);
[0043] G(n) is the actual cost of moving from the starting node to the current node, H(n) is the estimated cost of moving from the current node to the target node, and F(n) is the sum of the costs of G(n) and H(n).
[0044] Specific search process:
[0045] First, there are two set priority queues openList and closeList
[0046] 1. Put the start node into the priority queue
[0047] 2. If there are still elements in the priority queue
[0048] 3. Take an element from the priority queue (the element with the smallest F(n) value)
[0049] 4. Traverse the adjacent elements of the element (up, down, left, and right, provided that the node has not been traversed, that is, it is not in the openList or closeList), calculate the values of its fn, gn, and hn, and add it to the priority queue.
[0050] 5. If the element is the target node, the algorithm ends and the shortest path is found (output)
[0051] 6. If the element is not the target node, add the element to closeList and continue with step 2.
[0052] Through the above steps, a first path can be planned and formed.
[0053] Preferably, in step 2, the sensor includes sonar or underwater laser radar.
[0054] Preferably, in step 2, the point cloud data obtained by the sensor is used to create a grid map through SLAM technology. The grid map distinguishes between idle areas and obstacles by cost value. When the cost value exceeds a preset value, it is determined to be an obstacle.
[0055] Preferably, in step 3, the principle of the DWA local path planning algorithm is to sample multiple groups of speeds in the speed space (v, w), simulate the motion trajectories of these speeds within a certain period of time, and then score these trajectories through an evaluation function. The optimal speed is selected and sent to the lower computer.
[0056] Specifically, to determine the velocity vector space of ROV, let all possible velocity vectors be v s , the velocity vector of all avoidable obstacles is v a , the velocity vector that meets all acceleration limits is v d , then the space of all feasible velocity vectors v r It can be expressed as:r =v s ∩v a ∩v d
[0057] Secondly, in the velocity vector space v r The appropriate speed is selected so that the ship can avoid obstacles and move forward, so the evaluation function is established: G(v,w)=σ(α.heading(v,w)+β·dist(v,w)+γ·vel(v,w))
[0058] Where heading(v, w) is the degree of similarity between the ship's attitude and the expected attitude when arriving at the predicted point at this speed, expressed as: heading(v, w) = 1-|θ| / π
[0059] Where θ represents the angle between the ship's attitude and the expected attitude when the ship reaches the predicted position at this speed;
[0060] dist(v, w) represents the distance between the current position of the ship and the nearest obstacle. The greater the distance between the ship and the obstacle, the higher the evaluation of the function. Specifically, it is expressed as: dist(v, w) = rγ
[0061] In the formula is the predicted trajectory radius, γ is the rotation angle;
[0062] vel(v, w) is the speed of the ship. The greater the speed, the higher the evaluation. It is specifically expressed as: vel(v, w) = v / v max
[0063] Where v is the linear velocity, v max is the maximum expected linear velocity.
[0064] Preferably, in step 4, when the underwater ROV returns from the second route to the first route, the end of the second route is used as the starting point, and the waypoint closest to the starting point in the positive direction of the first route is used as the end point.
[0065] Preferably, in step 1, the first route is located in the center of the underwater tunnel.
[0066] In combination with the above embodiments, the present invention plans the ROV driving path in an underwater tunnel based on global static and local dynamic algorithms, plans the static path of the entire map based on the A* global path planning algorithm, and continuously detects obstacles along the static path. When an obstacle is detected, the DWA local path planning algorithm is switched to perform path planning in a local range to bypass the obstacle. When there are no obstacles in the warning area around the ROV, the ROV returns to the predetermined route by selecting the waypoint closest to the current position in the forward direction. The waypoint generated by the global path planning is selected and the ROV is driven to the point. In this way, the ROV can travel from the starting point to the end point without collision in the intermediate process.
[0067] While the present invention has been disclosed above with reference to preferred embodiments, this is not intended to limit the present invention. Persons skilled in the art will readily appreciate that various modifications and variations can be made without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention shall be determined by the claims.
Claims
1. A method for underwater tunnel path planning based on global static and local dynamic, characterized in that: The steps include: Step 1: Global path planning: Use the A* global path planning algorithm to perform overall global path planning based on the cost value in the grid map to obtain the first route from the starting point to the end point; Step 2: While moving along the first route, the ROV uses sensors to obtain surrounding environmental information and forms an early warning area around the ROV; Step 3: When an obstacle appears in the warning area, a second route that bypasses the obstacle is obtained through DWA local path planning; Step 4: When the obstacle in the warning area disappears, return to the first route from the second route; Step 5: Repeat steps 2 to 4 until the target point is reached; In step 4, the ROV is controlled to maintain a safe distance from the obstacles that appear, and to move toward the nearest waypoint in the first route in the forward direction.
2. The underwater tunnel path planning method based on global static and local dynamic according to claim 1 is characterized in that: In step 1, the A* global path planning algorithm formula includes: F(n)=G(n)+H(n); G(n) is the actual cost of moving from the starting node to the current node, H(n) is the estimated cost of moving from the current node to the target node, and F(n) is the sum of the costs of G(n) and H(n).
3. The underwater tunnel path planning method based on global static and local dynamic according to claim 1 is characterized in that: In step 2, the sensor includes sonar or underwater laser radar.
4. The underwater tunnel path planning method based on global static and local dynamic according to claim 3 is characterized in that: In step 2, the point cloud data obtained by the sensor is used to create a grid map through the SLAM technology. The grid map will distinguish between idle areas and obstacles by cost value. When the cost value exceeds the preset value, it is determined to be an obstacle.
5. The underwater tunnel path planning method based on global static and local dynamic according to claim 1 is characterized in that: In step 3, determine the velocity vector space of the ROV, assuming that all possible velocity vectors are v s , the velocity vector of all avoidable obstacles is v a , the velocity vector that meets all acceleration limits is v d , then the space of all feasible velocity vectors v r It can be expressed as: r =v s ∩v a ∩v d Secondly, in the velocity vector space v r Select a suitable speed so that the ship can avoid obstacles and move forward, so the evaluation function is established: G(v,w)=σ(α.heading(v,w)+β·dist(v,w)+γ·vel(v,w)) Among them, heading(v, w) is the similarity between the ship's attitude and the expected attitude when it reaches the predicted point at this speed, expressed as: heading(v,w)=1-|θ| / π Where θ represents the angle between the ship's attitude and the expected attitude when the ship reaches the predicted position at this speed; dist(v, w) represents the distance between the current position of the ship and the nearest obstacle. The greater the distance between the ship and the obstacle, the higher the evaluation of the function. Specifically, it is expressed as: dist(v,w)=rγ In the formula is the predicted trajectory radius, γ is the rotation angle; vel(v, w) is the speed of the ship. The greater the speed, the higher the evaluation. It is specifically expressed as: vel(v, w)=v / v max Where v is the linear velocity, v max is the maximum expected line speed.
6. The underwater tunnel path planning method based on global static and local dynamic according to claim 1 is characterized in that: In step 4, when the underwater ROV returns from the second route to the first route, the end of the second route is used as the starting point, and the waypoint closest to the starting point in the positive direction of the first route is used as the end point.
7. The underwater tunnel path planning method based on global static and local dynamic according to claim 1 is characterized in that: In step 1, the first route is located at the center of the underwater tunnel.
Citation Information
Patent Citations
Hybrid path planning method for unmanned surface vehicle in complex environment
CN110703762A
ROS-based mobile robot autonomous navigation method in indoor environment
CN114510057A
Cited By
Power transformation equipment inspection robot path planning method, equipment and storage medium
CN115933637A
Self-adaptive planning unmanned navigation path planning system and method
CN119356369A
Sanitation vehicle road sweeping operation method and system based on big data
CN119476672A
Cable type underwater robot path planning system and method
CN119596997A
Improved dynamic window collision avoidance method, program and equipment for ocean robot to quickly approach target point and storage medium
CN119717810A
