A three-dimensional search and rescue ROV path planning system and method

Abstract

The present invention discloses a search and rescue ROV path planning system and method for a three-dimensional environment, wherein an environment modeling module is used to obtain underwater environment information of the ROV robot, abstracts a three-dimensional spatial model of the area where the ROV robot moves from the real environment, and constructs a three-dimensional grid model of the underwater environment map; an algorithm calls a path generation module to select a target point for path planning, and uses the current position of the ROV as the starting point for path planning and the target position as the end point; a three-dimensional path planning algorithm based on JPS-COSPS is used to input the three-dimensional grid model of the underwater environment map, the starting point, and the target point, perform path planning, and obtain a planned path; a path detection module is used to ensure path continuity by judging the completeness of the planned path. The present invention reduces the complexity of the three-dimensional environment and the difficulty of path finding; ensures path safety, path quality, and path smoothness; and ensures the smooth operation of the search and rescue ROV.

Description

Technical Field

[0001] The present invention relates to a search and rescue ROV path planning technology, and in particular to a search and rescue ROV path planning system and method in a three-dimensional environment. Background Art

[0002] Remotely Operated Underwater Vehicles (ROVs) have long been used in search and rescue operations. They can operate at a wide range of depths, from the surface to the seabed, offering irreplaceable advantages. However, ROV operation is still manual, requiring the operator to maintain extreme focus. This long workday inevitably places a significant strain on the operator's mind and body. To reduce operator stress, ROVs must move beyond manual operation and towards automation. Path planning is crucial to expanding ROV applications.

[0003] In the prior art, a quasi-three-dimensional path planning method based on an improved A* algorithm was written by Zhang Xuan, Pi Dawei, Wang Hongliang, Xie Boyuan, Wang Xianhui, Wang Erlie, and Sun Xiaowang. The method specifically includes the following steps: Step (1): pre-processing the original digital elevation model, i.e., DEM; Step (2): extracting and analyzing obstacle factors, and establishing an environmental map model: the established environmental map model is based on the concept of fuzzy obstacles, and the model includes the distribution of fuzzy obstacles; Step (3): optimizing the cost function of the improved A* algorithm and establishing a total cost function; Step (4): applying the improved A* algorithm to establish a better environmental map model and search for the optimal path. This method effectively represents environmental information only on a two-dimensional map, greatly reducing the amount of data storage; integrating the drivable area and the fuzzy obstacles, expanding the area of path planning, and providing feasibility for shortening the planning length.

[0004] Although the existing technical solutions have obvious advantages, such as integrating the drivable area and the fuzzy obstacles, simplifying the complex three-dimensional environment into two dimensions, reducing the amount of calculation, and expanding the path planning area, the two-dimensional map is difficult to represent the more complex underwater three-dimensional environment; and for ROV robots, the safety and smoothness of the path are higher. The above technical solutions do not have too many requirements in this regard, which is not conducive to the ROV search and rescue work. Summary of the Invention

[0005] Object of the invention: One object of the present invention is to provide a search and rescue ROV path planning system for a three-dimensional environment.

[0006] Another object of the present invention is to provide a three-dimensional search and rescue ROV path planning method, which realizes the three-dimensional path planning function based on the large-scale scanning requirements in underwater search and rescue missions, improves the search and rescue efficiency, and enables ROV to be better used in the rescue field.

[0007] Technical solution: The present invention provides a three-dimensional search and rescue ROV path planning system, comprising:

[0008] The environment modeling module is used to obtain the underwater environment information of the ROV robot, abstract the ROV robot's movement area from the real environment into a three-dimensional space model, and subdivide multiple three-dimensional areas, and each area is represented by three-dimensional information to construct the underwater environment map. Figure 3 dimensional grid model;

[0009] The algorithm calls the path generation module to select the target point for path planning, and uses the current position of the ROV as the starting point for path planning and the target position as the end point; it determines whether the selected target point is reasonable, and reselects if it is unreasonable; it uses the three-dimensional path planning algorithm based on JPS-COSPS, inputs the underwater environment map, and calculates the target point. Figure 3 Dimensional grid model, starting point, and target point, perform path planning, and obtain the planned path;

[0010] The path detection module is used to ensure path continuity by judging the completeness of the planned path; to ensure the smooth operation of the ROV by detecting whether there is an identical path in the path; and to ensure the path safety by detecting whether the path is close to obstacles. If the detected planned path is not feasible, the feedback algorithm calls the path generation module to re-plan the path. If the detected planned path is feasible, the feasible planned path is output.

[0011] In another embodiment of the present invention, a method for search and rescue ROV path planning in a three-dimensional environment includes the following steps:

[0012] S1. Obtain the underwater environment of the ROV robot through laser, sonar scanning or external input, abstract the ROV robot's motion area from the real environment into a three-dimensional spatial model, and subdivide multiple three-dimensional regions. Each three-dimensional region is represented by three-dimensional information, and a three-dimensional coordinate system is established. The three-dimensional region is the three-dimensional path planning space. The coordinates are then divided into equal parts to form a three-dimensional grid map model of the underwater environment.

[0013] S2. Select the target point for path planning, and use the current position of the ROV robot as the starting point and the end point of the path planning;

[0014] S3, determine whether the selected target point is an obstacle, if yes, return to step S2 to reselect the target point; if no, execute step S4;

[0015] S4, using the three-dimensional path planning algorithm based on JPS-COSPS, input the underwater environment map Figure 3 Dimensional grid model, starting point, and target point, perform path planning, and obtain the planned path;

[0016] First, determine the critical obstacle and screen the point set around the critical obstacle;

[0017] Secondly, the filtered surrounding point set is set as the jump point, and the jump point is searched by the clipping plane method; the jump point that meets the requirements is added to the Openlist list, and the node with the smallest evaluation function F(n) in the list is selected as the parent node of the next search and added to the Closelist list;

[0018] Finally, delete the redundant turning points and directly connect the previous and next path points;

[0019] S5. Check the obtained planned path to ensure the continuity of the path by judging the completeness of the planned path; ensure the smooth operation of the ROV by checking whether there is an identical path in the path; ensure the safety of the path by checking whether the path is close to obstacles; if the detected planned path is not feasible, return to step S2; if the detected planned path is feasible, output the feasible planned path.

[0020] Furthermore, in step S1, the x-axis and y-axis of the three-dimensional coordinate system are the longitudinal and lateral increasing directions, respectively, and the z-axis is the height increasing direction, forming a three-dimensional space. The ROV robot is regarded as a particle in the three-dimensional grid map model of the underwater environment, and the position of the ROV in the model is represented by the x-axis, y-axis and z-axis. Each particle cannot exceed the boundary of the small three-dimensional grid after segmentation, and the ROV robot and obstacles cannot collide with each other when running in the three-dimensional space.

[0021] Furthermore, the rule for screening the surrounding point set of the critical obstacle in step S4 is:

[0022] (I) Determine whether there is an obstacle in the adjacent grid of the critical obstacle grid. If so, the adjacent grid is infeasible; if not, the critical obstacle adjacent grid is a feasible point.

[0023] (II) Determine whether there is an obstacle on the diagonal adjacent grid of the plane where the critical obstacle grid is located. If so, then this diagonal adjacent grid is infeasible; if not, then the diagonal adjacent grid of the critical obstacle is a feasible point;

[0024] (III) If an obstacle square has three adjacent squares with no obstacles, then the square opposite the angle formed by the three faces is a feasible point.

[0025] Furthermore, the method for screening the surrounding point set of the critical obstacle is:

[0026] (1) Determine the point set around the critical obstacle, the set length is length(SPS), and define identifiers i = 1, j = 1;

[0027] (2) The point numbered i in the point set is denoted as P i (x i ,y i ,z i );

[0028] (3) Calculation point P i The distance to the nearest obstacle point is Di;

[0029] (4) Determine Di 2 >=N, if yes, then record point P i (x i ,y i ,z i ), let P i (x i ,y i ,z i )=Q j (x j ,y j ,z j ), and j++; then i++; otherwise, i++;

[0030] Among them, Q j (x j ,y j ,z j ) is the feasible point after screening; N = 2 or 3, if the screening rule (II) is selected, then N is 2, that is, the judgment statement is Di 2 >=2; If the screening rule (III) is selected, then N is 3, and the judgment statement is Di 2 >=3, if the screening rule (I) is selected, step (4) is not executed;

[0031] (5) Determine whether i <= length (SPS). If so, return to step (2) to execute; otherwise, end.

[0032] Furthermore, the jump point search method in step S4 is:

[0033] (1) During the point search process, the jump points that meet the requirements are added to the Openlist list, and the node with the smallest evaluation function F(n) in the list is selected as the parent node of the next search;

[0034] (2) Add the jump points that can be reached without obstacles in the three planes where the parent node is located to the Openlist list;

[0035] (3) Determine whether the target point exists in the list. If so, pathfinding ends; if not, extend the parent node in the current direction to the next grid point;

[0036] (4) Update the extension point to the parent node and determine whether the parent node is an obstacle point. If not, jump to step (2); if yes, jump to step (1).

[0037] Furthermore, the inflection point deletion method in step S4 is:

[0038] Randomly select inflection points in the path as deletable points, attempt to delete the points, and directly connect the nodes before and after the deletable points. Determine whether the connected path touches an obstacle. If so, cancel the deletion operation, restore the original path, and find another inflection point. If not, retain the deletion operation and generate a new path. To avoid unnecessary operations, set the nodes to path points with integer coordinates.

[0039] In yet another embodiment of the present invention, a device includes a memory and a processor, wherein:

[0040] a memory for storing computer programs capable of running on the processor;

[0041] The processor is configured to execute the steps of the above-mentioned method for search and rescue ROV path planning in a three-dimensional environment when running the computer program.

[0042] In another embodiment of the present invention, a storage medium stores a computer program, and when the computer program is executed by at least one processor, the steps of the above-mentioned search and rescue ROV path planning method in a three-dimensional environment are implemented.

[0043] Beneficial Effects: Compared with existing technologies, this invention offers significant advantages: It incorporates a path planning function into the ROV control system, utilizing the JPS algorithm derived from the A* algorithm to perform path planning in a three-dimensional grid environment. By introducing COSPS (Critical Obstacle and Around Point Set), the complexity of the three-dimensional environment is reduced, thus reducing the difficulty of pathfinding. Path safety is ensured by setting a method for selecting the surrounding point set. Path quality is guaranteed by improving the algorithm's cost function and establishing a total cost function. Path smoothness is ensured by removing and optimizing inflection points. The result is an excellent, short, safe, and smooth path, ensuring the smooth operation of the search and rescue ROV. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a structural block diagram of the search and rescue ROV path planning system in a three-dimensional environment of the present invention;

[0045] Figure 2It is a flow chart of the search and rescue ROV path planning method for a three-dimensional environment of the present invention;

[0046] Figure 3 It is a constructed underwater environment Figure 3 Schematic diagram of the dimensional grid model;

[0047] Figure 4 It is the flowchart of surrounding point set screening;

[0048] Figure 5 is a schematic diagram of the critical obstacle and the surrounding point set;

[0049] Figure 6 It is a jump point search flow chart;

[0050] Figure 7 It is a schematic diagram of inflection point deletion;

[0051] Figure 8 It is a planning path detection flow chart. DETAILED DESCRIPTION

[0052] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0053] like Figure 1 As shown in the figure, the three-dimensional environment search and rescue ROV path planning system of the present invention includes an environment modeling module, an algorithm call path generation module and a path detection module. The environment modeling module obtains the underwater environment information of the ROV robot based on sonar, laser scanning data, and external environment input information, abstracts the ROV robot's movement area from the real environment into a three-dimensional space model, and subdivides multiple three-dimensional areas, and each area is represented by three-dimensional information to construct an underwater environment map. Figure 3 The algorithm calls the path generation module to select the target point for path planning, and uses the current position of the ROV as the starting point for path planning and the target position as the end point; it determines whether the selected target point is reasonable, and reselects if it is unreasonable; it uses the three-dimensional path planning algorithm based on JPS-COSPS, inputs the underwater environment map, and generates a 3D grid model; the ... inputs the underwater environment map, Figure 3 The system uses a three-dimensional grid model, a starting point, and a target point to perform path planning and obtain a planned path. The path detection module is used to ensure the continuity of the path by judging the completeness of the planned path. The system ensures the smooth operation of the ROV by detecting whether there is an identical path in the path. The system ensures the safety of the path by detecting whether the path is close to obstacles. If the detected planned path is not feasible, the feedback algorithm calls the path generation module to re-plan the path. If the detected planned path is feasible, the feasible planned path is output.

[0054] like Figure 2 As shown, a search and rescue ROV path planning method for a three-dimensional environment of the present invention includes the following steps:

[0055] S1. Acquire and process environmental information, obtain underwater environment through laser, sonar scanning or external input, and establish a three-dimensional grid model of the underwater environment;

[0056] The obtained search and rescue ROV working environment is converted into a mathematical model and a three-dimensional grid model of the underwater environment is established;

[0057] The ROV robot's motion area is abstracted from the real environment into a three-dimensional space model, and subdivided into multiple three-dimensional areas, and each three-dimensional area is represented by three-dimensional coordinate information. Figure 3 As shown, a three-dimensional coordinate system is established with the lower left vertex as the origin of the three-dimensional coordinates. The x-axis and y-axis represent the longitudinal and lateral directions, respectively, and the z-axis represents the height direction, thus forming a three-dimensional space. The three-dimensional space (i.e., the three-dimensional region) represented by the three-dimensional coordinate information is the three-dimensional path planning space. The coordinates are then divided equally to form a three-dimensional grid model of the underwater environment. In the grid model, the mobile robot is regarded as a point mass, and its position in the model is represented by the x-axis, y-axis, and z-axis. Each point mass cannot exceed the boundary of the divided small three-dimensional grid. In the three-dimensional grid model of the underwater environment, black squares represent obstacles. Assuming that the scale and coordinates of the three-dimensional grid map remain unchanged and each three-dimensional grid is also static, it is required that the robot and obstacles cannot collide with each other while moving in space.

[0058] S2. Select the target point for path planning, and use the current position of the ROV as the starting point of the path planning and the target point as the end point.

[0059] S3, judging the legality of the set point, if it is illegal, resetting the starting point and the end point; if it is legal, executing step S4;

[0060] This step is used to determine whether the selected target point is an obstacle. If it is, a new one will be selected; if it is not an obstacle, step S4 will be executed;

[0061] S4. Call the path planning interface of the improved algorithm (i.e., call the algorithm to call the path generation module), input the map model, starting point, and target point, perform path planning, and output the planned path.

[0062] A 3D path planning algorithm based on JPS-COSPS is proposed. By introducing and improving COSPS (critical obstacles and surrounding point sets), the complexity of the 3D environment is reduced, which reduces the pathfinding difficulty of the JPS algorithm in the 3D environment. By screening the surrounding point sets, the distance between the path and obstacles is guaranteed, ensuring the path safety. By improving the cost function of the algorithm and establishing a total cost function, the path quality is guaranteed. By optimizing the inflection point removal, the path smoothness is guaranteed.

[0063] 1) Critical obstacle and around Point Set COSPS (Critical obstacle and around Point Set):

[0064] The COSPS method reduces the complexity of 3D path planning by identifying critical obstacles based on their importance to the planned path and finding surrounding points that surround these obstacles in a 3D environment. With fewer obstacles and points, the path is constructed using fewer points, allowing for more efficient path generation. The details are as follows:

[0065] Research on path planning reveals that shortest paths are often constructed around obstacles that directly intersect the line between the starting and destination points. The line connecting the starting and destination points is the lower bound of the shortest path. When there are no obstacles or the obstacles are too remote, the lower bound is feasible. However, obstacles that render the lower bound infeasible often result in feasible paths being constructed around them. Therefore, we define two types of obstacles: critical obstacles (CO) and non-critical obstacles (NCO).

[0066] After determining the critical obstacle (CO), the surrounding point set (SPS) can be determined by screening the non-obstacle points around it. However, not all surrounding points are reasonable path points. In the grid model, the present invention treats the target point and the mobile robot as a mass point, and uses the x-axis, y-axis, and z-axis to represent its position in the model. However, in actual operation, the ROV robot may have volume collisions, so the present invention further screens the surrounding point set to ensure path safety. The three-level screening rules are as follows:

[0067] (I) Determine whether there is an obstacle in the adjacent grid of the critical obstacle grid. If so, the adjacent grid is infeasible; if not, the critical obstacle adjacent grid is a feasible point.

[0068] (II) Determine whether there is an obstacle on the diagonal adjacent grid of the plane where the critical obstacle grid is located. If so, then this diagonal adjacent grid is infeasible; if not, then the diagonal adjacent grid of the critical obstacle is a feasible point;

[0069] (III) If an obstacle square has three adjacent squares with no obstacles, then the square opposite the angle formed by the three faces is a feasible point.

[0070] The above screening rules can filter out safe feasible points. If an obstacle-free grid is judged to be infeasible, it will not be added to the feasible point set. In order to avoid excessive calculation complexity and reduce the difficulty of screening, the obstacle environment will not have a concave center of the plane. Figure 4 As shown in Figure 2, the specific steps for filtering the surrounding point set are:

[0071] (1) Determine the point set around the critical obstacle, the set length is length(SPS), and define identifiers i = 1, j = 1;

[0072] (2) The point numbered i in the point set is denoted as P i (x i ,y i ,z i );

[0073] (3) Calculation point P i The distance to the nearest obstacle point is Di;

[0074] (4) Determine Di 2 >=3 (or 2), if yes, then record point P i (x i ,y i ,z i ), let P i (x i ,y i ,z i )=Q j (x j ,y j ,z j ), and j++; then i++; otherwise, i++;

[0075] Among them, Q j (x j ,y j ,z j ) is the jth feasible point after screening, and the set of feasible points is the set of feasible points after screening;

[0076] (5) Determine whether i <= length (SPS). If so, return to step (2) to execute; otherwise, end.

[0077] If different screening rules (I), (II) or (III) are selected, the above steps (1), (2), (3) and (5) are the same, and only step (4) will be different according to different screening rules; specifically: if screening rule (II) is selected, the judgment statement is Di 2 >=2; If the screening rule (III) is selected, the judgment statement is Di 2 >=3, if the screening rule (I) is selected, then step (4) is not required. To simplify the surrounding point set, select rule (3) for screening, such as Figure 5 As shown in the figure, the black squares are critical obstacles, and the light squares are the filtered surrounding point sets. By connecting the surrounding point sets, a safe path to cross the obstacle can be obtained.

[0078] 2) Jump Point Search Algorithm JPS (Jump Point Search)

[0079] The JPS algorithm, also known as the Jump Point Search algorithm, is a heuristic path planning algorithm proposed by scholars such as Harabor based on the A* algorithm. The main difference between JPS and the A* algorithm lies in the successor node expansion strategy. Unlike the A* algorithm, which directly obtains all non-closed reachable neighbor nodes of the current node for expansion, JPS expands the successor node according to the direction of the current node and based on the strategy of searching for jump points, which effectively improves the path planning speed. In a three-dimensional environment, the A* algorithm expands the eight-neighborhood to 23-grids, and the expansion strategy does not change much; but for the JPS algorithm, the strategy of searching for jump points in a three-dimensional environment is difficult to define. In order to preserve the timeliness of path planning, the present invention introduces the method of critical obstacles and surrounding point sets shown in 1) above, sets the filtered surrounding point set as jump points, and the JPS algorithm searches for jump points by clipping planes. The search steps are summarized as follows: 1. In the process of point search, add the jump points that meet the requirements to the Openlist list, and select the node with the smallest evaluation function F(n) in the list as the parent node for the next search; 2. Add the jump points that can be reached without obstacles in the three planes where the parent node is located to the Openlist list; 3. Determine whether there is a target point in the list. If so, the path search ends; if not, extend the parent node in the current direction to the next grid point; 4. Update the extension point to the parent node and determine whether the parent node is an obstacle point. If not, jump to step 2, otherwise jump to step 1. Figure 6 The specific steps are as follows:

[0080] (1) Divide the search area into appropriate grids, determine the starting point and target point, and put the starting point into the Openlist;

[0081] (2) Find the point in the Openlist with the smallest evaluation function F value and set it as the parent node current;

[0082] (3) Delete the parent node current in the Openlist list and add the parent node current to the Closelist list;

[0083] (4) Find a feasible jump point that exists in the three planes where the parent node current is located and can be reached without obstacles;

[0084] (5) Determine whether the feasible jump point point is in the Openlist list. If not, add the feasible jump point point to the Openlist list; if so, jump to step (6);

[0085] (6) Determine whether the target point exists in the Openlist list. If so, jump to step (8); if not, extend the parent node current in the current direction to the next grid point, and obtain the new parent node current as the current parent node current;

[0086] (7) Determine whether the current parent node is the target point. If so, jump to step (8); otherwise, determine whether the current parent node is an obstacle point. If so, jump to step (2); if not, jump to step (3);

[0087] (8) Connect the path according to the starting point, end point and Closelist list, and the path finding is completed.

[0088] During the point search process, the jump points that meet the requirements are added to the Openlist list. The node with the smallest evaluation function F(n) in the list is selected as the parent node of the next search and added to the Closelist list. The evaluation function formula of the jump point is consistent with the A* algorithm, as follows:

[0089] F(N)=G(n)+H(n)

[0090] Where G(n) is the actual cost function, which represents the actual distance of the path search to this node; H(n) is the estimated cost function, which represents the estimated distance from this node to the end point, usually calculated using Euclidean distance or Manhattan distance. In three-dimensional space, G(n) is calculated using Euclidean distance:

[0091]

[0092] Among them, (x n ,y n , z n ) is the location of the current jump point n, (x n-1 ,y n-1 , z n-1 ) is the location of the previous hop. For H(n), common minimum cost function calculation methods include Manhattan distance, Chebyshev distance, and Euclidean distance. The present invention uses Euclidean distance to calculate H(n), that is:

[0093]

[0094] 3) Inflection point deletion

[0095] The path planned by the JPS-COSPS-based three-dimensional path planning algorithm can obtain a relatively safe and complete path from the starting point to the target point, but the path length is slightly longer and has more turns. Due to the definition of the cost function, the algorithm will give priority to points closer to the line connecting the starting point and the target point, so it is necessary to delete and optimize the turning points. Figure 7As shown in the figure, by deleting redundant inflection points and directly connecting the preceding and following path points, the path length can be effectively shortened and the number of path turns can be reduced. Deletion steps: Randomly select inflection points in the path as deletable points, attempt to delete the points, and directly connect the nodes before and after the deletable points. Determine whether the connected path touches an obstacle. If so, cancel the deletion operation, restore the original path, and find another inflection point. If not, retain the deletion operation and generate a new path. To avoid unnecessary operations, all nodes are set to path points with integer coordinates.

[0096] S5. Obtain the planned path and perform a path detection on the planned path to determine its feasibility.

[0097] The obtained planned path is tested to ensure the continuity of the path by judging the completeness of the planned path; the smooth operation of the ROV is ensured by detecting whether there is an identical path in the path; the path safety is ensured by detecting whether the path is close to obstacles; if the detected planned path is not feasible, return to step S2; if the detected planned path is feasible, output a feasible planned path.

[0098] The feasibility of the planned path is determined by the planned path detection module to ensure the smooth progress of path tracking. Figure 8 As shown, the specific process is:

[0099] Ensure path continuity by judging whether the path is complete;

[0100] By detecting whether there is an identical path in the path, the smooth operation of the ROV is ensured;

[0101] Ensure path safety by detecting whether the path is close to obstacles.

[0102] The path detected by the path is used as the tracking path for the control system path tracking.

[0103] In yet another embodiment of the present invention, a device includes a memory and a processor, wherein:

[0104] a memory for storing computer programs capable of running on the processor;

[0105] The processor is used to execute the steps of the above-mentioned search and rescue ROV path planning method in a three-dimensional environment when running the computer program, and achieve the same technical effect as the above-mentioned method.

[0106] In another embodiment of the present invention, a storage medium stores a computer program, which, when executed by at least one processor, implements the steps of the above-mentioned search and rescue ROV path planning method for a three-dimensional environment and achieves the same technical effect as the above-mentioned method.

[0107] This invention addresses the shortcomings of existing technologies and their deficiencies in search and rescue ROV applications. It utilizes the JPS algorithm, derived from the A* algorithm, to perform path planning in a three-dimensional grid environment. By introducing COSPS (Critical Obstacle and Around Point Set), the complexity of the three-dimensional environment is reduced, thus lowering the difficulty of pathfinding. A method for selecting the surrounding point set is used to ensure path safety. Path quality is guaranteed by improving the algorithm's cost function and establishing a total cost function. Path smoothness is ensured by removing optimized inflection points. The result is an excellent, short, safe, and smooth path, ensuring smooth operation of the search and rescue ROV.

Claims

1. A search and rescue ROV path planning method in a three-dimensional environment, characterized in that: The following steps are involved: S1. Obtain the underwater environment of the ROV robot through laser, sonar scanning or external input, abstract the ROV robot's motion area from the real environment into a three-dimensional spatial model, and subdivide multiple three-dimensional regions. Each three-dimensional region is represented by three-dimensional information, and a three-dimensional coordinate system is established. The three-dimensional region is the three-dimensional path planning space. The coordinates are then divided into equal parts to form a three-dimensional grid map model of the underwater environment. S2. Select the target point for path planning, and use the current position of the ROV robot as the starting point and the end point of the path planning; S3, determine whether the selected target point is an obstacle, if yes, return to step S2 to reselect the target point; if no, execute step S4; S4. Using a three-dimensional path planning algorithm based on JPS-COSPS, inputting a three-dimensional grid model of the underwater environment map, a starting point, and a target point, performing path planning, and obtaining a planned path; First, determine the critical obstacle and filter the point set around the critical obstacle. The rules for filtering the point set around the critical obstacle are: (I) Determine whether there is an obstacle in the adjacent grid of the critical obstacle grid. If so, the adjacent grid is infeasible; if not, the critical obstacle adjacent grid is a feasible point. (II) Determine whether there is an obstacle on the diagonal adjacent grid of the plane where the critical obstacle grid is located. If so, then this diagonal adjacent grid is infeasible; if not, then the diagonal adjacent grid of the critical obstacle is a feasible point; (III) If an obstacle square has three adjacent squares with no obstacles, then the square opposite the angle formed by the three faces is a feasible point; The method for screening the surrounding point set of critical obstacles is: (1) Determine the point set around the critical obstacle, the set length is length(SPS), and define identifiers i = 1, j = 1; (2) The point numbered i in the point set is denoted as P i (x i ,y i ,z i ); (3) Calculation point P i The distance to the nearest obstacle point is Di; (4) Determine Di 2 >=N, if yes, then record point P i (x i ,y i ,z i ), let P i (x i ,y i ,z i )=Q j (x j ,y j ,z j ), and j++; then i++; otherwise, i++; Among them, Q j (x j ,y j ,z j ) is the feasible point after screening; N = 2 or 3, if the screening rule (II) is selected, then N is 2, that is, the judgment statement is Di 2 >=2; If the screening rule (III) is selected, then N is 3, and the judgment statement is Di 2 >=3, if the screening rule (I) is selected, step (4) will not be executed; (5) Determine if i <= length(SPS). If so, return to step (2). Otherwise, end. Secondly, the filtered surrounding point set is set as the jump point, and the jump point is searched by the clipping plane method; the jump point that meets the requirements is added to the Openlist list, and the node with the smallest evaluation function F(n) in the list is selected as the parent node of the next search and added to the Closelist list; Finally, delete the redundant turning points and directly connect the previous and next path points; S5. Check the obtained planned path to ensure the continuity of the path by judging the completeness of the planned path; ensure the smooth operation of the ROV by checking whether there is an identical path in the path; ensure the safety of the path by checking whether the path is close to obstacles; if the detected planned path is not feasible, return to step S2; if the detected planned path is feasible, output the feasible planned path.

2. A three-dimensional environment search and rescue ROV path planning method according to claim 1, characterized in that: In step S1, the x-axis and y-axis of the three-dimensional coordinate system are longitudinal and lateral increasing directions, respectively, and the z-axis is the height increasing direction, forming a three-dimensional space. The ROV robot is regarded as a particle in the three-dimensional grid map model of the underwater environment, and the position of the ROV in the model is represented by the x-axis, y-axis and z-axis. Each particle cannot exceed the boundary of the small three-dimensional grid after segmentation, and the ROV robot and obstacles cannot collide with each other when running in the three-dimensional space.

3. The method for searching and rescuing ROV in a three-dimensional environment according to claim 1, wherein: The jump point search method in step S4 is: (1) During the point search process, the jump points that meet the requirements are added to the Openlist list, and the node with the smallest evaluation function F(n) in the list is selected as the parent node of the next search; (2) Add the jump points that can be reached without obstacles in the three planes where the parent node is located to the Openlist list; (3) Determine whether the target point exists in the list. If so, pathfinding ends; if not, extend the parent node in the current direction to the next grid point; (4) Update the extension point to the parent node and determine whether the parent node is an obstacle point. If not, jump to step (2); if yes, jump to step (1).

4. The method for searching and rescuing ROV in a three-dimensional environment according to claim 1, wherein: The inflection point deletion method in step S4 is: Randomly select the path inflection point in the path as the deletable point, try to delete the point, and directly connect the nodes before and after the deletable point to determine whether the connected path touches an obstacle. If so, cancel the deletion operation, restore the original path, and find another path inflection point; if not, retain the deletion operation and generate a new path; to avoid unnecessary operations, set the nodes to path points with integer coordinates.

5. A system for the search and rescue ROV path planning method in a three-dimensional environment according to any one of claims 1 to 4, characterized in that: include: The environment modeling module is used to obtain underwater environment information of the ROV robot, abstract the ROV robot's movement area from the real environment into a three-dimensional spatial model, and subdivide multiple three-dimensional areas, each of which is represented by three-dimensional information to construct a three-dimensional grid model of the underwater environment map; The algorithm calls the path generation module to select the target point for path planning, and uses the current position of the ROV as the starting point of the path planning and the target position as the end point; Determine whether the selected target point is reasonable. If it is unreasonable, reselect it. Use the three-dimensional path planning algorithm based on JPS-COSPS, input the three-dimensional grid model of the underwater environment map, the starting point, and the target point, perform path planning, and obtain the planned path. The path detection module is used to ensure the continuity of the path by judging the completeness of the planned path; By detecting whether there is an identical path in the path, the smooth operation of the ROV is ensured; by detecting whether the path is close to obstacles, the path safety is ensured; if the detected planned path is not feasible, the feedback algorithm calls the path generation module to re-plan the path; if the detected planned path is feasible, the feasible planned path is output.

6. A device, characterized in that: comprising a memory and a processor, wherein: a memory for storing computer programs capable of running on the processor; A processor is configured to execute the steps of a search and rescue ROV path planning method for a three-dimensional environment as described in any one of claims 1 to 4 when running the computer program.

7. A storage medium, characterized in that: The storage medium stores a computer program, which, when executed by at least one processor, implements the steps of the search and rescue ROV path planning method for a three-dimensional environment as described in any one of claims 1 to 4.

Images