Efficient path planning method for sea floating garbage salvage robot

Abstract

The invention provides an efficient path planning method for a sea floating garbage salvage robot, and the method comprises the steps: carrying out the traversal of all target positions of sea garbage through depth-first search, and generating a target position sequence; and calculating a target with the lowest total cost through a path planning algorithm, and setting the target as a next target position for traversing of the robot. According to the method, the optimal or suboptimal cleaning path can be planned for the robot based on the real-time environment information, and unnecessary path repetition and invalid movement are reduced, so that the cleaning efficiency is remarkably improved.

Description

Technical Field

[0001] The invention belongs to the field of marine salvage, and in particular relates to an efficient path planning method for a marine floating garbage salvaging robot. Background Art

[0002] SMURF is a path planning method improved from SCAN, which traverses the entire area based on full coverage. However, in a dynamic marine environment, traversing the entire area will be a waste of time and electricity costs. In addition, marine debris is dynamic, and some marine debris may not be salvaged during the movement of the robot. Therefore, SMURF assumes that the marine environment is a static environment, which is not in line with the actual situation. Summary of the invention

[0003] In order to solve the above problems, the present invention provides an efficient path planning method for a marine debris salvaging robot.

[0004] The technical solution of the present invention is as follows:

[0005] An efficient path planning method for a marine debris salvaging robot, the method comprising:

[0006] Traverse all target locations of marine debris through depth-first search to generate a target location sequence;

[0007] The target with the lowest total cost is calculated through the path planning algorithm and set as the next target position for the robot to traverse.

[0008] Furthermore, when traversing all target locations of marine debris, a threshold is set, that is, the distance between the next traversed target location and the location of the robot must be less than or equal to td.

[0009] Furthermore, the path planning algorithm formula is:

[0010] f(G i )=g(G i )+h(G i )+p(G i )

[0011] Among them, f(G i ) is from the current position G i The overall priority is the total cost, g(G i ) represents the current position G i To the starting point G s The actual cost, h(G i ) represents the current position G i To the target position G k The estimated cost, p(Gi ) is the current position G i To the target position G k adaptation cost.

[0012] Furthermore, the actual cost is calculated by the robot from the starting position G s Go to the current location of the floating garbage G i The distance cost;

[0013]

[0014] In the formula, G s is the starting position, G i is the target location.

[0015] Furthermore, the estimated cost is calculated as follows:

[0016]

[0017] where dist(G i ,G j )≤td

[0018] Among them, G k is the target position, G i is the current location.

[0019] Furthermore, the calculation method of the adaptation cost is:

[0020] p(G i ) = t i,k

[0021] Among them, t i,k The robot moves from its current position G i To target position G k travel time.

[0022] Furthermore, the travel time is determined according to the direction and velocity of the water flow. If the water flow is downstream, the salvage time and energy consumption are in a better state; on the contrary, upstream is in the worst state.

[0023] Furthermore, if the water flow and the robot’s heading angle are the same, the robot’s travel time t i,k Calculated by the following formula:

[0024]

[0025] where T e =v+w×cos(θ)

[0026] Among them, v (distance / unit time) is the movement speed of the robot, w (unit is distance / unit time) is the speed of the water flow, the distance of the robot from grid a to grid b is d, and the heading angle is θ (unit is degree).

[0027] Furthermore, if the water flow is opposite to the robot's heading angle (countercurrent), the robot's travel time t i,k Calculated by the following formula:

[0028]

[0029] where T e =vw×cos(θ)

[0030] Among them, v (distance / unit time) is the movement speed of the robot, w (unit is distance / unit time) is the speed of the water flow, the distance of the robot from grid a to grid b is d, and the heading angle is θ (unit is degree).

[0031] Compared with the prior art, the present invention has the following advantages:

[0032] The present invention can plan an optimal or suboptimal cleaning path for the robot based on real-time environmental information, reduce unnecessary path repetition and invalid motion, and thus significantly improve cleaning efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] The accompanying drawings generally illustrate various embodiments by way of example and not limitation, and together with the description and claims, serve to illustrate the embodiments of the invention. Where appropriate, the same reference numerals are used throughout the drawings to refer to the same or similar parts. Such embodiments are illustrative and are not intended to be exhaustive or exclusive embodiments of the present apparatus or method.

[0034] Figure 1 A schematic diagram of the method flow of the present invention is shown;

[0035] Figure 2 A schematic diagram of a water surface garbage cleaning robot and path planning of the present invention is shown;

[0036] Figure 3 A schematic diagram showing the comparison of the amount of marine garbage salvaged by five algorithms of the present invention;

[0037] Figure 4 The effect of the amount of marine debris on the total path length according to the present invention is shown;

[0038] Figure 5 The effect of the average moving speed of the robot of the present invention on the average amount of marine garbage collected is shown;

[0039] Figure 6 The effect of the movement rate of the robot of the present invention on the average path length is shown;

[0040] Figure 7 A schematic diagram showing the effect of the obstacle size of the present invention on the average amount of collected marine debris;

[0041] Figure 8 A schematic diagram showing the effect of obstacle size on average path length of the present invention is shown;

[0042] Fig. 9 A schematic diagram showing the effect of the ocean velocity on the average amount of collected marine debris according to the present invention;

[0043] Fig.10 A schematic diagram showing the effect of ocean velocity on average path length according to the present invention is shown. DETAILED DESCRIPTION

[0044] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0045] The embodiment of the present invention provides an efficient path planning method for a marine debris salvaging robot, comprising:

[0046] 1 Definition and Assumptions

[0047] In MA*, it is assumed that the robot can obtain the location of all marine debris through satellites. The present invention frames the obstacles with a rectangular frame, such as Figure 1 As shown. Therefore, after obtaining the coordinate positions of the four corners of all obstacles and the positions of the floating garbage, an undirected graph can be established. The distance matrix MAT∈R can be established through the undirected graph ((M+1)×(M+1)) , where MAT is defined as the Euclidean distance between any two point pairs. The diagonal distances of the matrix MAT are all 0. In addition, the satellite will update the location information of the floating garbage at regular intervals. In the present invention, every time the robot collects a piece of marine garbage, it will request the satellite for the location information of the marine garbage.

[0048] MAT=MAT T . (

[0049] Diag(MAT)=0.

[0050] 2 Candidate target locations

[0051] First, the present invention must first determine the target location that the robot is going to. In the present invention, it is assumed that there are N marine debris in the environment, and the grid where the marine debris is located is called the target location. The present invention traverses all the target locations where marine debris is located through depth-first search (DFS). In order to avoid traversing marine debris farther away during the traversal process, a threshold is set, that is, the distance between the next traversed target location and the location of the robot must be less than or equal to td.

[0052]

[0053] Assume that the first traversed target position G i , the next target position to be traversed is G i+1 ,The distance between two target positions must be less than or equal to td. The reason for this design is to avoid the robot traversing the farther target position first and then traversing the closer target position, which will generate redundant paths, resulting in excessive path length and waste of time and electricity costs.

[0054] 3. Path Planning

[0055] In order to cope with the dynamic environment of the ocean, when the robot is moving, the floating garbage is also in a moving state. The present invention must consider the moving state of the floating garbage and find the next target position to be traversed. After the DFS traversal, the present invention will obtain a set of candidate sequences. Each target element in the sequence will be individually calculated by the path planning program, and the target with the lowest total cost will be selected and set as the next target position to be traversed by the robot.

[0056] The present invention designs a path planning algorithm based on the A* algorithm and improves the heuristic function, which is called MA*. In the MA* algorithm, the heuristic function is defined as shown in the following formula.

[0057] f(G i )=g(G i )+h(G i )+p(G i )(

[0058] Where f(G i ) is from the current position G i The robot will select the marine debris with the best comprehensive priority (the lowest total cost) to traverse when it is time to traverse the next marine debris. i ) represents the current position G i To the starting point G s The actual cost, h(G i ) represents the current position G i To the target position G kThe estimated cost, p(G i ) is the current position G i To the target position G k adaptation cost.

[0059] Define the calculation method of the actual cost, that is, the robot starts from the starting position G s Go to the current location of the floating garbage G k The distance cost.

[0060]

[0061] Define the calculation method of the estimated cost, where G k is the target position, G i is the current location.

[0062]

[0063] Considering the time and power costs, the last item in the calculation of the comprehensive priority is the adaptation cost, where t i,k The robot moves from its current position G i To target position G k The travel time is determined by the direction and velocity of the water flow. If the water flow is downstream, the salvage time and energy loss are in a better state; conversely, upstream is in the worst state.

[0064] p(G i ) = t i,k (

[0065] The following formulas are the calculation methods of the downstream and upstream travel time when the robot is heading to the target grid. v (distance / unit time) is the robot's moving speed, w (unit is distance / unit time) is the speed of the water flow, the distance from grid a to grid b is d, and the heading angle is θ (unit is degree).

[0066] If the water flow and the robot's heading angle are the same (belonging to the current), the robot's travel time t i,k It can be calculated by the following formula:

[0067]

[0068] If the water flow is opposite to the robot's heading angle (countercurrent), the robot's travel time t i,k It can be calculated by the following formula:

[0069]

[0070] Example 1

[0071] The present invention uses a Windows 11 operating system equipped with 16GB DDR4-3000 memory. In terms of simulation development, the present invention uses C / C++ language, MATLAB and Gnuplot as auxiliary tools for drawing and complex calculations. The present invention conducted four groups of experiments to compare the performance of five algorithms, MA*, Greedy, RRT, SMURF and U*, in terms of average path length and average amount of recovered marine garbage. Each group of experiments was conducted as follows: (A) Changing the total amount of marine garbage. (B) Changing the moving speed of the robot. (C) Changing the size of the obstacle. (D) Changing the speed of the ocean current. The parameters of the experimental settings are listed in Table 1.

[0072] Table 1. Parameter settings

[0073]

[0074] The average path length Avg is defined as length , where exet is the total number of runs.

[0075] If there is an edge E between mesh i and mesh j, then e ij =1, otherwise e ij =0.

[0076]

[0077] where e ij ={0,1}.

[0078] The following formula defines the average amount of collected marine debris, GN avg , where GN m represents the amount of marine debris collected in the mth experiment. In addition, when the distance between the robot and the marine debris is less than 1 meter, it is considered to be collected successfully, otherwise, the collection fails, where exet is the total number of runs.

[0079]

[0080] In order to make the experiment closer to reality, in addition to using orcaflex software to simulate the marine environment, the ocean movement model used in the present invention is the Ocean Current Model (kinematic model)

[24] . The kinematic model of the Ocean Current Model can be written as:

[0081] Vx=k1λV sin(k2x)cos(k3y)+k1λcos(2k1t)+k4

[0082] Vy=-λV cos(k2x)sin(k3y)+k5

[0083] Among them, Vx is the speed on the x-axis, Vy is the speed on the y-axis, and k1, k2, k3, λ, and V are variables closely related to environmental factors such as tide and water depth. These parameters will change in different environments, as shown in Table 2.

[0084] Table 2. Ocean current model parameter settings

[0085]

[0086]

[0087] Impact of the amount of marine debris

[0088] This set of experiments compared the effects of five methods on the total path length of the robot and the efficiency of marine debris collection by changing the total amount of marine debris. The robot started from the upper left corner of the area, the speed of the robot was set to 3 m / s, and each experiment ran for 3000 seconds.

[0089] Figure 3 and Figure 4 Results for the amount of marine debris collected and the average path length, respectively.

[0090] exist Figure 3 In the results, the MA* algorithm showed a relatively stable fishing efficiency under different total amounts of garbage, and the amount of garbage caught increased with the increase of the total amount of garbage, showing a good effect. Its total path length is relatively short, showing that more garbage can be caught under a shorter path, which may be due to the high efficiency of the MA* algorithm in path planning and search. The Greedy algorithm is relatively stable in terms of the amount of garbage captured, but slightly inferior to the MA*. Its total path length is slightly longer, which may be due to its greedy selection strategy that leads to relatively less optimized path planning. The RRT algorithm is relatively unstable in terms of fishing efficiency and path length, with large fluctuations, showing the uncertainty brought by its randomness.

[0091] exist Figure 4 In the analysis of average path length, the MA* algorithm shows a relatively short path length under each total amount of garbage, indicating that it has a certain efficiency advantage in path planning. The Greedy algorithm is second, but still stable and more reliable than the RRT algorithm, SMURF algorithm and U* algorithm. The path length of the RRT algorithm fluctuates greatly, and a more stable path planning strategy may be needed to improve efficiency. The SMURF algorithm and U* algorithm perform averagely in path length, and the path planning strategy may need to be further optimized to improve efficiency and stability.

[0092] Effect of robot movement rate

[0093] This set of experiments compared the effects of three methods on the robot's total path length and marine debris collection efficiency when the robot's speed changes. 50 marine debris were randomly generated, and the robot started from the upper left corner of the area. The robot's movement speed increased from 5m / s to 14m / s, and the execution time of each experiment was 3000 seconds. Figure 5 and Figure 6 Results are given for the average path length and amount of marine debris collected at different moving speeds of the robot.

[0094] exist Figure 5 In the present invention, the performance of each algorithm under different robot speeds was observed. The MA* algorithm showed relatively stable results in terms of the amount of garbage caught and the total path length, demonstrating good fishing efficiency and path planning capabilities. The Greedy algorithm performed second best in terms of the amount of garbage caught, but the total path length was short, indicating that it was relatively efficient in path planning. The RRT algorithm and the SMURF algorithm performed generally in terms of fishing efficiency, but the path length was relatively long, and a more optimized path planning strategy may be required. The U* algorithm performed relatively generally in terms of fishing efficiency and path length, and may require further improvement to improve the effect.

[0095] exist Figure 6 In the results, the MA* algorithm showed a shorter path length, showing its high efficiency in path planning. The Greedy algorithm was second, but it was stable at different robot speeds. The average path lengths of the RRT algorithm and the SMURF algorithm were longer, probably because their path planning strategies needed further optimization. The U* algorithm performed averagely in terms of average path length, and may need to be improved to improve the path planning effect. Different algorithms showed their own characteristics in terms of marine debris salvage efficiency and average path length. The MA* algorithm was relatively excellent in both efficiency and path length, while other algorithms needed further improvement to improve the effect.

[0096] Effect of obstacle size

[0097] This group of experiments compares the effect of obstacle size on marine debris. The present invention randomly generates 50 marine debris, and the obstacle size increases from 10% to 55%. The robot starts from the upper left corner of the area, and the robot movement speed is set to 3 meters per second. Each experiment lasts for 3000 seconds.

[0098] Figure 7 and Figure 8 The relationship between detection frequency, average path length and the amount of marine debris collected is shown. Based on the experimental data provided, the present invention focuses on the impact of the total obstacle area ratio on the marine debris fishing efficiency and the average path length. As the obstacle area ratio increases, the performance of different algorithms also changes. Figure 7Among them, the MA* algorithm performs better when the obstacle ratio is low, but its efficiency decreases as the obstacle ratio increases; the Greedy algorithm performs well at a high obstacle ratio, but its fishing efficiency is low at a low ratio; the RRT and SMURF algorithms perform relatively stably at different ratios, but the fishing efficiency and path length need to be improved; the U* algorithm performs generally well overall, and its adaptability to different obstacle ratios is weak.

[0099] exist Figure 8 In the results, as the obstacle ratio increases, the path length of each algorithm also shows different changes. The path length of the MA* algorithm is short when the obstacle ratio is low, but as the ratio increases, the path length increases; the path length of the Greedy algorithm is relatively short at a high ratio; the RRT and SMURF algorithms perform generally at different ratios, and the path length fluctuates greatly; the U* algorithm performs relatively smoothly in path length, but the overall performance is general. The total area ratio of obstacles has a significant impact on the efficiency of marine garbage fishing and path length. Different algorithms show their own characteristics at different ratios. The MA* and Greedy algorithms perform well in some cases, but other algorithms need to be further improved to improve adaptability and efficiency. The change in path length shows the difference in path planning among algorithms. It is necessary to select a suitable algorithm according to the specific situation to improve the efficiency of marine garbage fishing and the accuracy of path planning.

[0100] This set of experiments compared the efficiency of collecting marine debris when the current speed changes. 50 pieces of marine debris were randomly generated, the current speed increased from 2.2 m / s to 4 m / s, the robot started from the upper left corner of the area, the detection radius was set to 50 meters, the detection frequency was once every ten seconds, the robot movement speed was set to 3 m / s, the current speed increased from 1 m / s to 2.8 m / s, and each experiment was performed for 3000 seconds.

[0101] Fig. 9 and Fig.10 The relationship between the average path length and the amount of marine debris collected under different current speeds is given. Based on the experimental data provided, the present invention focuses on the impact of ocean current speed on the efficiency of marine debris fishing and the average path length. As the ocean current speed changes, different algorithms show different performances in fishing efficiency and path length. Fig. 9 Among them, the MA* algorithm performs relatively stably at low ocean current speeds, but its efficiency decreases as the speed increases; the Greedy algorithm performs well at high speeds, but its fishing efficiency is low at low speeds; the RRT and SMURF algorithms have large fluctuations at different speeds, and there is room for improvement in fishing efficiency and path length; the U* algorithm performs generally well overall, and has weak adaptability to changes in different ocean current speeds.

[0102] exist Fig.10In the results, as the ocean current speed increases, the path length of each algorithm also shows different changes. The path length of the MA* algorithm is relatively short at low speed, but as the speed increases, the path length increases; the path length of the Greedy algorithm is relatively short at high speed; the RRT and SMURF algorithms perform generally at different speeds, and the path length fluctuates greatly; the U* algorithm performs relatively smoothly in path length, but the overall performance is general. The ocean current speed has a significant impact on the efficiency of marine garbage fishing and path length. Different algorithms show their own characteristics at different speeds. The MA* and Greedy algorithms perform well in some cases, but other algorithms need to be further improved to improve adaptability and efficiency. The change in path length shows the difference in path planning among algorithms. It is necessary to select a suitable algorithm according to the specific situation to improve the efficiency of marine garbage fishing and the accuracy of path planning.

[0103] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. An efficient path planning method for a marine debris salvaging robot, characterized in that: The method comprises: Traverse all target locations of marine debris through depth-first search to generate a target location sequence; The target with the lowest total cost is calculated through the path planning algorithm and set as the next target position for the robot to traverse.

2. The efficient path planning method for a marine debris salvaging robot according to claim 1 is characterized in that: When traversing the target locations of all marine debris, a threshold is set, that is, the distance between the next traversed target location and the location of the robot must be less than or equal to td.

3. The efficient path planning method for a marine debris salvaging robot according to claim 1 is characterized in that: The path planning algorithm formula is: f(G i )=g(G i )+h(G i )+p(G i ) Among them, f(G i ) is from the current position G i The overall priority is the total cost, g(G i ) represents the current position G i To the starting point G s The actual cost, h(G i ) represents the current position G i To the target position G k The estimated cost, p(G i ) is the current position G i To the target position G k adaptation cost.

4. The efficient path planning method for a marine debris salvaging robot according to claim 3 is characterized in that: The actual cost is calculated by the robot from the starting position G s Go to the current location of the floating garbage G i The distance cost; In the formula, G s is the starting position, G i is the target location.

5. The efficient path planning method for a marine debris salvaging robot according to claim 3 is characterized in that: The estimated cost is calculated as follows: where dist(G i ,G j )≤td Among them, G k is the target position, G i is the current location.

6. The efficient path planning method for a marine debris salvaging robot according to claim 3 is characterized in that: The calculation method of the adaptation cost is: p(G i )=t i，k Among them, t i，k The robot moves from its current position G i To target position G k travel time.

7. The efficient path planning method for a marine debris salvaging robot according to claim 6 is characterized in that: The travel time is determined by the direction and velocity of the water flow. If the water flow is downstream, the salvage time and energy consumption are in a better state; conversely, upstream is in the worst state.

8. The efficient path planning method for a marine debris salvaging robot according to claim 7 is characterized in that: If the water flow has the same angle as the robot’s heading, the robot’s travel time t i，k Calculated by the following formula: where T e =v+w×cos(θ) Among them, v is the moving speed of the robot, w is the speed of the water flow, the distance from grid a to grid b of the robot is d, and the heading angle is θ.

9. The efficient path planning method for a marine debris salvaging robot according to claim 7 is characterized in that: If the water flow is opposite to the robot's heading angle (countercurrent), the robot's travel time t i，k Calculated by the following formula: where T e =v-w×cos(θ) Among them, v is the moving speed of the robot, w is the speed of the water flow, the distance from grid a to grid b of the robot is d, and the heading angle is θ.

Images