Mobile Robot Path Planning Method and System Based on Water Flow Algorithm

Abstract

The present invention discloses a path planning method and system for a mobile robot based on a water flow algorithm. The method includes: using a mainstream point search model to determine mainstream points; determining path points between the mainstream points according to a virus-like algorithm and a water flow obstacle avoidance algorithm, where the virus-like algorithm is used for the water flow to identify the contours of relevant obstacles, and the water flow obstacle avoidance algorithm is used for the water flow to flow between the mainstream points; and determining a path based on the mainstream points and the path points between the mainstream points. The present invention can meet the performance requirements of the global path planning algorithm for a mobile robot, such as better path, strong timeliness, and good algorithm stability. In scenarios of processing complex maps or emphasizing real-time calculation, the water flow algorithm has strong application advantages. Further, through comparative analysis of simulation experiments with the ant colony algorithm, Dijkstra algorithm, and Floyd algorithm, it is verified that while ensuring the quality of the path solution, the search speed of the present invention is significantly improved.

Description

Technical Field

[0001] The present invention relates to a path planning method and system for a mobile robot based on a water flow algorithm, belonging to the fields of mobile robot navigation and path planning. Background Art

[0002] Mobile robot path planning is one of the important technologies in the field of robot research, and its research purpose is to find a safe, collision-free, optimal or approximately optimal path. According to the search method, current path planning algorithms are mainly divided into four categories: intelligent search algorithms, artificial intelligence-based algorithms, geometric model-based algorithms, and local obstacle avoidance algorithms. Intelligent search algorithms are currently relatively popular algorithms, which mainly include ant colony algorithms, Dijkstra algorithms, artificial potential field methods, and Floyd algorithms.

[0003] The ant colony algorithm (AC) is a stochastic heuristic search method based on group foraging, which is very effective in solving path planning problems and has strong robustness and adaptability. To solve the problems of local optimization, poor convergence, and low search efficiency in AC, many researchers have been committed to improving the initial search strategy, pheromone update rule, path selection rule, and deadlock penalty mechanism.

[0004] The Dijkstra algorithm is a typical shortest path algorithm, which expands layer by layer outward with the starting point as the center until the end point is reached to obtain the shortest path. The traditional Dijkstra algorithm has problems of slow path planning speed and low efficiency.

[0005] The Floyd algorithm is an algorithm for finding the shortest path between vertices in a given weighted path topology network. It has problems of high path planning time complexity and long path planning time. Summary of the Invention

[0006] In view of some deficiencies of traditional path planning methods in global static path planning, the present invention provides a path planning method and system for a mobile robot based on a water flow algorithm, which specifically includes a model and three sub-algorithms: a dynamic calculation mainstream point search model based on the current position and the end point, which determines feasible mainstream points through four steps of detection, search, detection, and transformation; a virus-like algorithm that simulates the virus diffusion and propagation process, which is used to identify obstacles during the flow process; a water-like obstacle avoidance algorithm that simulates the water flow trajectory to avoid obstacles; and a path optimization algorithm that optimizes the water-like path that has completed the planning.

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

[0008] According to one aspect of the present invention, there is provided a path planning method for a mobile robot based on a water flow algorithm, including: using a mainstream point search model to determine mainstream points; determining path points between mainstream points according to a virus-like algorithm and a pseudo water flow obstacle avoidance algorithm; wherein, the virus-like algorithm is used for the water flow to identify the contour of relevant obstacles; the pseudo water flow obstacle avoidance algorithm is used for the water flow to flow between mainstream points; determining a path based on the mainstream points and the path points between the mainstream points.

[0009] The step of using the mainstream point search model to determine mainstream points includes: starting from the path starting point, determining feasible mainstream points through detection, search, detection, and transformation steps until the end point is reached.

[0010] The detection step is used to determine a detection angle according to the current position and the end point position, and the detection angle expression is:

[0011]

[0012] Wherein, represents the detection angle at the k-th iteration, (i E , j E ) is the grid coordinate of the end point E, k is the current iteration number, (i k , j k ) represents the grid coordinate of the current position P k at the k-th iteration;

[0013] The search step is used to determine a search angle according to the detection angle, and the search angle expression is:

[0014]

[0015]

[0016] Wherein, represents the search angle at the k-th iteration; is the detection step length at the k-th iteration; r is the grid ratio;

[0017] The detection step is used to judge whether the search point determined according to the search angle and the detection step length is a mainstream point, and the judgment formula is:

[0018]

[0019] Wherein, F c (i k+1 , j k+1 ) is a search point inspection function used to check whether the search point is a mainstream point;

[0020] The transformation step is used to transform the mainstream point coordinates determined by the detection step into index values for storage.

[0021] Based on the viroid algorithm and the virtual water flow obstacle avoidance algorithm, the path points between the main points are determined, including: the virtual water flow starts to flow along the main points. During the flow process, it is judged whether an obstacle is encountered. If an obstacle is encountered, the viroid algorithm is used to identify the contour of the relevant obstacle. Based on the identified contour, the virtual water flow obstacle avoidance algorithm is used to avoid the obstacle to obtain the path points between the main points. If no obstacle is encountered, the water flow directly flows between the main points.

[0022] The viroid algorithm is used to take the grid coordinates of the obstacle recognized at the current position as the infection source. The infection source spreads the virus around with a radius of d. The obstacles infected with the virus are used as new infection sources and then spread the virus around with a radius of d until the infection ends, outputting the coordinates of the infected obstacles to determine the contour of the relevant obstacle. After the virtual water flow flows through the relevant obstacle, the virus will be automatically cleared.

[0023] The virtual water flow obstacle avoidance algorithm has two basic flow modes during obstacle avoidance: clockwise or counterclockwise. In the clockwise flow mode, it is composed of eight direction flow methods arranged according to the clockwise operation rule. Similarly, in the counterclockwise flow mode, it is composed of eight direction flow methods arranged according to the counterclockwise operation rule. If the path obtained by avoiding obstacles through the virtual water flow obstacle avoidance algorithm is one, the path points between the main points are directly determined according to this one path. If there are multiple paths, the path with the shortest length is selected to determine the path points between the main points. If there is none, it means there is no effective path.

[0024] The eight direction flow methods are determined according to three flow modes under the flow mode:

[0025] Flow mode 1 is used when the water flow is at the upper left, upper right, lower left, and lower right diagonal positions of the obstacle, and the virtual water flow adheres to the obstacle and flows.

[0026] Flow mode 2 is used when the water flow is not in the extreme position and adheres to the obstacle and flows according to the eight direction flow methods.

[0027] Flow mode 3 is used when the water flow is in the extreme position and adheres to the obstacle and flows according to the eight direction flow methods.

[0028] The extreme position refers to the current position being at the boundary of the map environment.

[0029] It also includes a path optimization step for optimizing the planned virtual water flow path according to the path optimization algorithm.

[0030] The path optimization algorithm is specifically: starting from the first path point Path(q1)=(i q1 ,j q1 ) and moving towards the second path point Path(q2)=(iq2 , j q2 ) Expand the search. If i q1 to i qn , j q1 to j qn The area composed of is a passable area, indicating that there are no obstacles in the area. Then delete the path point Path(q2); continue to search from Path(q1) to Path(q3)..., if there are no obstacles in the area composed of the points from Path(q1) to Path(qn), and there are obstacles in the area composed of the points from Path(q1) to Path(qn+1), then only keep the points q1 and qn, that is, change the path [Path(q1), Path(q2),..., Path(qn)] to the path [Path(q1), Path(qn)], and obtain the first segment of the path [Path(q1), Path(qn)]; then continue to search backward from the point Path(qn+1) until the last path point is searched, and determine the optimal path based on the retained points.

[0031] According to another aspect of the present invention, there is provided a mobile robot path planning system based on a water flow algorithm, including: a first determination module for determining the main flow points by using a main flow point search model; a second determination module for determining the path points between the main flow points according to a virus-like algorithm and a water flow obstacle avoidance algorithm, wherein the virus-like algorithm is used for the water flow to identify the contours of relevant obstacles; the water flow obstacle avoidance algorithm is used for the water flow to flow between the main flow points; a third determination module for determining the path according to the main flow points and the path points between the main flow points.

[0032] The beneficial effects of the present invention are: The present invention can meet the performance requirements of the global path planning algorithm of the mobile robot for better path, strong timeliness, and good algorithm stability; in the scenarios of dealing with complex maps or emphasizing real-time calculation, the water flow algorithm has strong application advantages. Further, through the simulation experiment comparison and analysis with the ant colony algorithm, Dijkstra algorithm, and Floyd algorithm, it is verified that the present invention has a significant improvement in the search speed while ensuring the quality of the path solution. Description of the Drawings

[0033] Figure 1 It is a schematic flow chart of the path planning and path optimization method according to the embodiment of the present invention;

[0034] Figure 2 It is a schematic diagram of the main flow point search model according to the embodiment of the present invention;

[0035] Figure 3 It is a schematic flow chart of the main flow point search process according to the embodiment of the present invention;

[0036] Figure 4Schematic diagram of virus diffusion and transmission in the embodiments of the present invention;

[0037] Figure 5 Flowchart of the virus-like algorithm in the embodiments of the present invention;

[0038] Figure 6 Schematic diagram of the simulation of the effect of the virus-like algorithm in the embodiments of the present invention;

[0039] Figure 7 Schematic diagram of the virus-like water flow obstacle avoidance strategy in the embodiments of the present invention;

[0040] Figure 8 Schematic diagram of the limiting conditions of the virus-like water flow obstacle avoidance algorithm in the embodiments of the present invention

[0041] Figure 9 Schematic diagram of the virus-like water flow approaching an obstacle in the embodiments of the present invention;

[0042] Figure 10 Schematic diagram of the conventional flow pattern (clockwise) in the embodiments of the present invention;

[0043] Figure 11 Schematic diagram of the conventional flow pattern (counterclockwise) in the embodiments of the present invention;

[0044] Figure 12 Schematic diagram of the extreme flow pattern (clockwise) in the embodiments of the present invention;

[0045] Figure 13 Schematic diagram of the extreme flow pattern (counterclockwise) in the embodiments of the present invention;

[0046] Figure 14 Schematic diagram of the effect of the virus-like water flow obstacle avoidance in the embodiments of the present invention;

[0047] Figure 15 Schematic diagram of the two detection effects in the embodiments of the present invention;

[0048] Figure 16 Schematic diagram of the integration of the virus-like water flow obstacle avoidance algorithm and the virus-like algorithm in the embodiments of the present invention;

[0049] Figure 17 Schematic diagram of the path optimization process in the embodiments of the present invention;

[0050] Figure 18 Schematic diagram of the path optimization effect in the embodiments of the present invention;

[0051] Figure 19 Schematic diagram of the path planning process of the virus-like water flow algorithm in the embodiments of the present invention;

[0052] Figure 20 Schematic diagram of the comparison of the path planning of the virus-like water flow algorithm in the embodiments of the present invention;

[0053] Figure 21 Schematic diagram of the implementation framework of the pseudo-water flow algorithm according to an embodiment of the present invention;

[0054] Figure 22 Schematic diagram of the experimental scenario according to an embodiment of the present invention;

[0055] Figure 23 Schematic diagram of the experimental path planning result according to an embodiment of the present invention. Detailed implementation manners

[0056] The following will further describe the invention in conjunction with the accompanying drawings and embodiments, but the content of the present invention is not limited to the described scope.

[0057] Embodiment 1: As Figure 1 - 23 shown, according to one aspect of an embodiment of the present invention, a path planning method for a mobile robot based on a water flow algorithm is provided, including: using a mainstream point search model to determine mainstream points; determining path points between the mainstream points according to a pseudo-virus algorithm and a pseudo-water flow obstacle avoidance algorithm; wherein, the pseudo-virus algorithm is used to identify the contours of relevant obstacles by water flow; the pseudo-water flow obstacle avoidance algorithm is used for water flow to flow between the mainstream points; determining a path according to the mainstream points and the path points between the mainstream points.

[0058] Further, the using a mainstream point search model to determine mainstream points includes: starting from the path start point, determining feasible mainstream points through detection, search, detection, and conversion steps until the end point is reached.

[0059] Further, the detection step is used to determine a detection angle according to the current position and the end point position; the search step is used to determine a search angle according to the detection angle; the detection step is used to determine whether a search point determined according to the search angle and the detection step length is a mainstream point; the conversion step is used to convert the coordinates of the mainstream point determined by the detection step into an index value for storage.

[0060] Further, the determining path points between the mainstream points according to a pseudo-virus algorithm and a pseudo-water flow obstacle avoidance algorithm includes: the pseudo-water flow starts to flow along the mainstream points, and during the flow process, it is judged whether an obstacle is encountered: if an obstacle is encountered, the pseudo-virus algorithm is used to identify the contours of relevant obstacles, and according to the identified contours, the pseudo-water flow obstacle avoidance algorithm is used to avoid obstacles to obtain path points between the mainstream points; if no obstacle is encountered, the water flow directly flows between the mainstream points.

[0061] Further, the viroid algorithm is used to take the obstacle grid coordinates recognized at the current position as the infection source. The infection source spreads the virus around with a radius of d. The obstacles infected with the virus are used as new infection sources, and then the virus is spread around with a radius of d until the infection ends. The coordinates of the infected obstacles are output, and the contours of the relevant obstacles are determined. After the virtual water flow passes through the relevant obstacles, the virus will be automatically cleared. The end of the infection means that no new infection sources appear.

[0062] Further, there are two basic flow modes for the virtual water flow obstacle avoidance algorithm during obstacle avoidance: clockwise or counterclockwise. In the clockwise flow mode, it is composed of eight direction flow methods arranged according to the clockwise operation rule. Similarly, in the counterclockwise flow mode, it is composed of eight direction flow methods arranged according to the counterclockwise operation rule. If the path obtained by avoiding obstacles through the virtual water flow obstacle avoidance algorithm is one, the path points between the main flow points are directly determined according to this one path. If there are multiple paths, the path with the shortest length is selected to determine the path points between the main flow points. If there is no such path, it means that there is no valid path.

[0063] Further, the eight direction flow methods are determined according to three flow patterns under the flow mode:

[0064] Flow pattern 1 is used when the water flow is at the four diagonal positions of the upper left, upper right, lower left, and lower right of the obstacle, and the virtual water flow adheres to the obstacle and flows.

[0065] Flow pattern 2 is used when the water flow is not in the extreme position and adheres to the obstacle and flows according to the eight direction flow methods.

[0066] Flow pattern 3 is used when the water flow is in the extreme position and adheres to the obstacle and flows according to the eight direction flow methods.

[0067] The extreme position refers to the current position being at the boundary of the map environment.

[0068] Further, it also includes a path optimization step for optimizing the virtual water flow path that has been completed in planning according to the path optimization algorithm.

[0069] According to another aspect of the present invention, there is provided a mobile robot path planning system based on a water flow algorithm, including: a first determination module for determining main flow points by using a main flow point search model; a second determination module for determining path points between main flow points according to the viroid algorithm and the virtual water flow obstacle avoidance algorithm, wherein the viroid algorithm is used for the water flow to identify the contours of relevant obstacles, and the virtual water flow obstacle avoidance algorithm is used for the water flow to flow between main flow points; a third determination module for determining a path according to the main flow points and the path points between main flow points. For the parts not described in detail for each module above, reference can be made to the relevant descriptions in the embodiments.

[0070] Further, the optional specific embodiments of the present invention are described as follows in the order of steps shown in Figure 1 :

[0071] When performing path planning for a mobile robot, a virtual water flow is defined, called the virtual water flow, which flows from the starting point to the ending point. Obstacles are regarded as stones blocking the forward movement of the virtual water flow. By simulating the process of water flow in the map environment, a path from the ending point to the starting point that allows the robot to pass safely is finally obtained. The points in the environment where the virtual water flow can pass are defined as main flow points. By finding the main flow points, it is stipulated that the virtual water flow only flows between multiple main flow points to define the overall flow direction of the virtual water flow. In the map environment, except for obstacles, the rest are passable areas. To improve the flow efficiency of the virtual water flow, it is stipulated that the virtual water flow directly flows from the starting point to the ending point, and the relative relationship between the current position of the virtual water flow and the ending point is calculated in real time to determine the next flow direction. The specific calculation process is as follows:

[0072] 1. Use the main flow point detection model to determine the feasible main flow points through four steps: detection, search, detection, and transformation:

[0073] As shown in Figure 2 , first determine the detection angle according to the current position and the position of the ending point Then determine the search angle according to the detection angle Finally, determine the search step size with the search angle After the search is completed, use the detection function to detect the current position. After successful detection, use the transformation function to convert the coordinates of the current position into the serial number of this position and store it in the main flow point set {M}. After the main flow point set is determined, add the starting point S to the main flow point set as the first main flow point, and add the ending point E to the main flow point set as the last main flow point. If the detection angle is not within the detection range, the mirror image thinking is adopted, that is, the starting point and the ending point are exchanged, the starting point is detected from the ending point, and finally the obtained path is inverted, so that a path from the starting point to the ending point can be obtained. When finding the main flow points, refer to the main search model and perform update calculations according to formulas (1)-(8).

[0074]

[0075] Among them, (i n , j n ) is the grid coordinate represented by the nth main flow point M n , n represents the serial number of the current position, and D(M n , M n-1 ) is used to calculate the distance between two points M n and M n-1 .

[0076] ind(i k,j k ) transfer =R·(j k -1)+i k (2)

[0077] Among them, ind is used to calculate the index number of the grid, R is the total number of rows of the grid, and C is the total number of columns of the grid; that is, the index number of the grid obtained above is numbered from top to bottom and from left to right based on the grid. For example, for a 10*10 grid, for the coordinate (3,2), its index number is 13; for the coordinate (10,6), its index number is 60; for the coordinate (9,6), its index number is 59.

[0078]

[0079]

[0080] Among them, r is the ratio of the grid, which is determined by the geometric shape of the robot. In the embodiment of the present invention, the ratio of the grid is taken as 1, that is, the ratio of the length of the grid to the diameter of the circumscribed circle of the robot is 1:1; the mod() function is the remainder function, that is, it returns the remainder part of the division; the int() function is the integer function, that is, it returns the integer part of the division; R is the total number of rows.

[0081]

[0082] Among them, represents the detection angle in the k-th iteration, (i E ,j E ) is the grid coordinate of the end position E, k is the current iteration number, and (i k ,j k ) represents the current position P k of the grid in the k-th iteration;

[0083]

[0084]

[0085]

[0086] Among them, represents the search angle in the k-th iteration; is the detection step length in the k-th iteration. Based on the current position in the k-th iteration, determine the direction according to the search angle and determine the search point with the detection step length as the current position (i k+1 ,j k+1), that is, the point at the k-th iteration is the current position for the next iteration; r is the ratio of the grid, which is determined by the geometry of the robot. In the embodiment of the present invention, the ratio of the grid is taken as 1, that is, the ratio of the length of the grid to the diameter of the circumscribed circle of the robot is 1:1; F c (i k+1 ,j k+1 ) is a search point inspection function, used to check whether the search point is a mainstream point. That is, if (i k+1 ,j k+1 ) represents a passable area, then F c (i k+1 ,j k+1 ) takes a value of 1, indicating that the search point is a mainstream point; otherwise, the detection fails. The flowchart of the process of finding the mainstream point is as shown in Figure 3 .

[0087] 2. Use the virus-like algorithm to let the water flow identify the contours of relevant obstacles:

[0088] Specifically, the present invention proposes a virus-like algorithm that simulates the process of virus diffusion and propagation, enabling the virtual water flow to identify the contours of obstacles. The first obstacle group that the virtual water flow is about to pass through is defined as the relevant obstacle, and the obstacle groups outside the relevant obstacle are defined as irrelevant obstacles.

[0089] Implementation of the virus-like algorithm: As shown in Figure 4 , when the virtual water flow is flowing, it carries a kind of virtual virus. When the virtual water flow encounters the first obstacle group, it will spread the virus to the obstacle grid closest to the virtual water flow. The virus will spread and infect in all directions (eight directions) from the grid carrying the virus, and the infected obstacle grids will continue to spread and infect until all the obstacles in this area are infected. In this way, the virtual water flow can completely identify the contours of the current relevant obstacles. To ensure that the entire relevant obstacles in this area can be accurately covered by the virus without affecting other obstacles in this area, the virus diffusion radius r is the ratio of the grid, which is determined by the geometry of the robot. After the virus is injected, it will infect the obstacles within the diffusion range, and the infected obstacles within the diffusion range will then immediately spread and infect until all the relevant obstacles are infected. The flowchart of the virus-like algorithm is as shown in Figure 5 . The simulation schematic diagram of the virus-like algorithm is as shown in Figure 6 . Inject the virus at the position shown in the figure, and it will quickly spread and propagate within the obstacle group. After several iterations, the obstacle group will be marked by the virtual virus and thus be identified by the virtual water flow.

[0090] 3. Use the virtual water flow obstacle avoidance algorithm to let the water flow flow between the mainstream points:

[0091] S3. The present invention proposes a method for simulating the flow trajectory of water to avoid obstacles, called the pseudo-water flow obstacle avoidance algorithm. The pseudo-water flow obstacle avoidance strategy is as follows Figure 7 shown. When avoiding obstacles with the pseudo-water flow, an additional limiting condition is added: as Figure 8 shown, the obstacle is regarded as a stone that can support the flow of the pseudo-water flow, and the pseudo-water flow must flow along the obstacle and cannot break away from the obstacle. Generally speaking, there are two basic flow modes for the pseudo-water flow when avoiding obstacles - clockwise or counterclockwise. In the clockwise flow mode, it consists of a total of eight flow methods arranged according to the clockwise operation rule. Similarly, in the counterclockwise flow mode, it consists of eight flow methods arranged according to the counterclockwise operation rule, and its flow conditions are different under different flow modes. After selecting the flow mode, the pseudo-water flow will continue to flow according to the three flow patterns under this flow mode.

[0092] Flow Pattern 1: Determine the positional relationship between the pseudo-water flow and the obstacle. If the pseudo-water flow is located at the four diagonal positions of the upper left, upper right, lower left, and lower right of the obstacle, the pseudo-water flow needs to fit the obstacle, as Figure 9 shown. The obstacle is represented by the gray area surrounded by a black line, and the feasible area is represented by the white area surrounded by a dotted line. Table 1 details the meanings corresponding to different flow conditions, and Table 2 introduces the corresponding relationship between different flow methods and flow conditions under Flow Pattern 1.

[0093] Table 1 Corresponding Relationship Table of Flow Conditions

[0094] Flow conditions Meaning field[i,j+1] Search to the right field[i+1,j] Search downwards field[i,j-1] Search to the left field[i-1,j] Search upwards field[i+1,j+1] Search to the lower right field[i-1,j+1] Search to the upper right field[i+1,j-1] Search to the lower left field[i-1,j-1] Search to the upper left ＝0 The search result is a passable area ＝1 The search result is an obstacle ＝3 The search result is a relevant obstacle

[0095] Table 2 Corresponding Relationship Table of Mode1 Flow Conditions

[0096]

[0097] Flow Pattern 2: After the pseudo-water flow approaches the obstacle, it will flow over the obstacle. Generally, a conventional flow pattern is adopted. The eight flow methods and conditions under the conventional clockwise and counterclockwise flow patterns are as Figure 10 , 11 shown. Tables 3 and 4 introduce the corresponding relationship between different flow methods and flow conditions under Flow Pattern 2.

[0098] Table 3 Corresponding Relationship Table of Flow Conditions for Mode2 Conventional Flow Pattern (Clockwise)

[0099]

[0100] Table 4 Corresponding Relationship Table of Flow Conditions for Mode2 Conventional Flow Pattern (Counterclockwise)

[0101]

[0102] Flow pattern 3: Define the position where the pseudo-water flow is about to flow out of the grid map (beyond the index value of the map) as the extreme position. When the pseudo-water flow flows at the extreme position, it is very likely to overflow the map area and exceed the index value of the map. At this time, the extreme flow pattern needs to be adopted. The flow method and conditions of the extreme flow pattern are as follows Figure 12 , 13 shown. Tables 5 and 6 introduce the corresponding relationships between different flow methods and flow conditions in flow pattern 2

[0103] Table 5 Corresponding relationship table of flow conditions for Mode 3 extreme flow pattern (clockwise)

[0104]

[0105] Table 6 Corresponding relationship table of flow conditions for Mode 3 extreme flow pattern (counterclockwise)

[0106]

[0107] As Figure 14 shown, when avoiding obstacles for the pseudo-water flow, start from both the clockwise and counterclockwise directions simultaneously. After reaching the target point, compare the paths in the two directions and select the shorter path to retain. If the pseudo-water flow is blocked by the boundary or obstacles during the flow process, then directly discard the path on this side. When avoiding obstacles for the pseudo-water flow, in order to avoid the path points successfully searched being repeated with the previously searched path points, the pseudo-water flow is reversed in this way. Define the path points successfully searched each time as P(k), define the set of path points successfully searched clockwise as temp_path_left, and define the set of path points successfully searched counterclockwise as temp_path_right. If then store the searched path point P(k) in temp_path_left. Similarly, if then store the searched path point P(k) in temp_path_right

[0108] After the main flow points are determined, the pseudo-water flow starts from the first main flow point and flows towards the second main flow point; the second main flow point then flows towards the third main flow point until it reaches the last main flow point. As Figure 15 shown, when flowing between the main flow points, the pseudo-water flow will detect the position of the next main flow point M i based on its own position, and the detection direction is towards M iIn the direction, if an obstacle is detected, the first detected obstacle grid is used as the infection source. During the detection process, if the mainstream point is found first, it will flow directly to the mainstream point. If an obstacle is found first and then the mainstream point is found, the virus-like algorithm is first called to identify the contour of the obstacle, and then the virtual water flow obstacle avoidance algorithm is called to flow to the next mainstream point. At this time, change u = 1 to u = 3.

[0109] As Figure 16 shown, when the virtual water flow encounters multiple obstacles, it will be diverted or cause the virtual water flow to collide and lock, which will affect the search efficiency and even prevent successful path planning. By integrating the intelligent virtual water flow algorithm and the virus-like algorithm, the virtual water flow can first completely identify the contour of the current relevant obstacles, and then smoothly and efficiently bypass the obstacles without the interference of irrelevant obstacles. In order not to affect the next planning, the virus will be automatically cleared after the virtual water flow passes through the relevant obstacles.

[0110] 4. Use the path optimization algorithm to optimize the virtual water flow path that has completed the planning:

[0111] After the virtual water flow successfully reaches the last mainstream point, a complete passable path can be obtained, but sometimes there will be many unnecessary redundant turns in the path. Therefore, the present invention proposes a path optimization algorithm to optimize the virtual water flow path that has completed the preliminary path planning after the preliminary path planning. The specific steps are as follows: Starting from the first path point Path(q1) = (i q1 , j q1 ), expand the search to the second path point Path(q2) = (i q2 , j q2 ). If the field(i, j) is always equal to 0 during the process of i q1 → i q2 , j q1 → j q2 , that is, it indicates that there are no obstacles in the area, then delete the path point Path(q2); continue to search from Path(q1) to Path(q3)... If there are no obstacles in the area composed of the Path(q1) point to the Path(qn) point, and there are obstacles in the area composed of the Path(q1) point to the Path(qn + 1) point, then only retain the q1 and qn points, that is, change the path [Path(q1), Path(q2),..., Path(qn)] to the path [Path(q1), Path(qn)], and obtain the first section of the path [Path(q1), Path(qn)]; then continue to search backward from the Path(qn + 1) point until the last path point is searched, and determine the optimal path based on the retained points; The schematic diagram of the virtual water flow path optimization process and the simulation diagram of the virtual water flow path optimization effect are as Figure 17 、18 as shown

[0112] As Figure 19 shown, the path planning of the present invention is completed by the above 4 main steps, and the specific implementation is as follows:

[0113] Step 5.1, initialize the map environment;

[0114] Step 5.2, determine the main flow points using the mainstream point model;

[0115] Step 5.3, the virtual water flow starts to flow along the main flow points;

[0116] Step 5.4, determine whether an obstacle is encountered. If an obstacle is encountered, enter Step 5.5. If no obstacle is encountered, then go to Step 5.6;

[0117] Step 5.5, depict the contour of the relevant obstacle using the virtual virus algorithm and perform obstacle avoidance using the virtual water flow obstacle avoidance algorithm;

[0118] Step 5.6, determine whether the end point is reached and meet the exit conditions. If the conditions are met, end. If not, return to Step 5.3.

[0119] As Figure 20 shown in and Table 7 below, it is a schematic comparison diagram of an indoor mobile robot path planning and path optimization method based on the virtual water flow algorithm (SA) of the present application with the traditional Dijkstra algorithm, Floyd algorithm, and ant colony algorithm. The planned path obtained by the present application has a shorter search time, fewer collision points with obstacles, and can better meet the trajectory requirements of the robot.

[0120] Table 7 Comparison between the water flow algorithm SA and traditional algorithms

[0121] Algorithm Floyd Dijkstra AC SA Path length 30.9706 30.9706 30.9706 31.5563 Collision point 10 10 11 9 Turning point 15 15 15 17 Search time 0.7256 0.1368 6.1287 0.0541

[0122] To verify the effectiveness and feasibility of the algorithm proposed by the present invention, the virtual water flow algorithm is applied to the actual ROS-based mobile robot Turtlebot2. Turtlebot2 uses the lidar A2 to obtain the experimental scene information and then uses the amcl and gmapping modules for positioning and mapping. Then, the algorithm proposed in this paper is transplanted into the move_base module by registering through the plugin to replace the global_planner for global path planning. The specific implementation framework is as Figure 21 shown

[0123] To verify the effectiveness and feasibility of the algorithm proposed in the present invention, the pseudo-water flow algorithm is applied to the actual ROS-based mobile robot Turtlebot2. After Turtlebot2 uses the lidar A2 to obtain the experimental scene information, it uses the amcl and gmapping modules for positioning and mapping. Then, the algorithm proposed in this paper is transplanted into the move_base module by registering it through a plugin to replace the global_planner for global path planning. The specific implementation framework is as shown in Figure 22 shown.

[0124] The experimental scene of this experiment is as shown in Figure 22 shown. Some obstacles are placed in the stairs of the laboratory. During the experiment, first start the keyboard control node to move Turtlebot2 to construct a 2D map of the experimental scene, then start the map_server module to save the map, then start the navigation node program to import the map, and use the 2D_Nav_Estimate function in rviz to calibrate the position of Turtlebot2. Then, with the current position as the starting point, the starting point is set as a yellow point, and the red point is specified as the target point for path planning. Since the A* algorithm has good performance in actual scene applications, in the experiment, the algorithm in this paper is compared with the improved A* algorithm and the traditional A* algorithm. As shown in Figure 23 shown, from left to right are the pseudo-water flow algorithm, the improved A* algorithm, and the traditional A* algorithm of the present invention. From the path-finding results, it can be seen that the algorithm in this paper has fewer path turning points and a shorter path in actual applications. The experimental results prove that the pseudo-water flow algorithm proposed in the present invention can effectively complete the path planning task of the mobile robot.

[0125] The specific implementation manners of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited to the above implementation manners. Within the scope of knowledge possessed by those of ordinary skill in the art, various changes can be made without departing from the gist of the present invention.

Claims

1. A path planning method for a mobile robot based on a water flow algorithm, characterized in that, Including: Using the mainstream point search model to determine the mainstream points; According to the virus-like algorithm and the virus-like water flow obstacle avoidance algorithm, determining the path points between the mainstream points; wherein, the virus-like algorithm is used for the water flow to identify the outline of relevant obstacles; the virus-like water flow obstacle avoidance algorithm is used for the water flow to flow between the mainstream points; Determining the path according to the mainstream points and the path points between the mainstream points; The virus-like algorithm is used to take the grid coordinates of the obstacle recognized at the current position as the infection source, and the infection source spreads the virus around with a radius of d. The obstacles infected by the virus are used as new infection sources, and then spread the virus around with a radius of d until the infection ends, outputting the coordinates of the infected obstacles to determine the outline of relevant obstacles; after the virus-like water flow passes through the relevant obstacles, the virus will be automatically cleared; The using the mainstream point search model to determine the mainstream points includes: starting from the path start point, determining the feasible mainstream points through the detection, search, detection and transformation steps until the end point is searched; The detection step is used to determine the detection angle according to the current position and the end point position, and the expression of the detection angle is: Among them, represents the detection angle at the k-th iteration, (i E , j E ) are the grid coordinates of the end point E, k is the current iteration number, (i k , j k ) represents the current position P k at the k-th iteration; The search step is used to determine the search angle according to the detection angle, and the expression of the search angle is: Among them, represents the search angle under the k-th iteration; is the detection step size under the k-th iteration; r is the ratio of the grid; The detection step is used to judge whether the search point determined according to the search angle and the detection step length is a mainstream point, and the judgment formula is: Among them, F c (i k+1 , j k+1 ) is a search point checking function used to check whether the search point is a mainstream point; The transformation step is used to transform the coordinates of the mainstream point determined by the detection step into an index value for storage; The determining the path points between the mainstream points according to the virus-like algorithm and the virus-like water flow obstacle avoidance algorithm includes: the virus-like water flow starts to flow along the mainstream points. During the flowing process, it is judged whether an obstacle is encountered: if an obstacle is encountered, the virus-like algorithm is used to identify the outline of the relevant obstacle, and according to the identified outline, the virus-like water flow obstacle avoidance algorithm is used to avoid the obstacle to obtain the path points between the mainstream points; if no obstacle is encountered, the water flow directly flows between the mainstream points; The virus-like water flow obstacle avoidance algorithm has two basic flow modes during obstacle avoidance: clockwise or counterclockwise; in the clockwise flow mode, it is composed of eight direction flow methods arranged according to the clockwise operation rule. Similarly, in the counterclockwise flow mode, it is composed of eight direction flow methods arranged according to the counterclockwise operation rule; if the path obtained by avoiding the obstacle through the virus-like water flow obstacle avoidance algorithm is one, directly determine the path points between the mainstream points according to this one path; if there are multiple paths, select the path with the shortest path to determine the path points between the mainstream points; if not, it means that there is no effective path.

2. The path planning method of the mobile robot based on the water flow algorithm according to claim 1, characterized in that The eight direction flow methods are determined according to three flow modes under the flow mode: Flow mode 1 is used for the water flow to be at the upper left, upper right, lower left, and lower right diagonal positions of the obstacle, and the virus-like water flow adheres to the obstacle and flows; Flow mode 2 is used for the water flow to adhere to the obstacle according to the eight direction flow methods when it is not in the extreme position; Flow mode 3 is used for the water flow to adhere to the obstacle according to the eight direction flow methods when it is in the extreme position; The extreme position refers to the current position being at the boundary of the map environment.

3. The path planning method for a mobile robot based on a water flow algorithm according to claim 1, wherein It also includes a path optimization step for optimizing the planned virus-like water flow path according to the path optimization algorithm.

4. The path planning method for a mobile robot based on a water flow algorithm according to claim 3, wherein, The path optimization algorithm is specifically as follows: Starting from the first path point Path(q1) = (i q1 , j q1 ), expand the search to the second path point Path(q2) = (i q2 , j q2 ). If the area formed by i q1 to i qn , j q1 to j qn is a passable area, that is, it indicates that there are no obstacles in the area, then delete the path point Path(q2); continue to search from Path(q1) to Path(q3)... If there are no obstacles in the area formed by the Path(q1) point to the Path(qn) point, and there are obstacles in the area formed by the Path(q1) point to the Path(qn+1) point, then only retain the q1 and qn points, that is, change the path [Path(q1), Path(q2),..., Path(qn)] to the path [Path(q1), Path(qn)], and obtain the first segment of the path [Path(q1), Path(qn)]; then continue to search backward from the Path(qn+1) point until the last path point is searched, and determine the optimal path based on the retained points.

5. A path planning system for a mobile robot based on a water flow algorithm, characterized in that, Implement the path planning method for a mobile robot based on the water flow algorithm as described in claim 1, including: A first determination module for determining main flow points by using a mainstream point search model; A second determination module for determining path points between main flow points according to the virus-like algorithm and the water flow obstacle avoidance algorithm; wherein, the virus-like algorithm is used for the water flow to identify the contours of relevant obstacles; the water flow obstacle avoidance algorithm is used for the water flow to flow between main flow points; A third determination module for determining a path according to the main flow points and the path points between the main flow points.

Images