Path planning method and system based on RRT algorithm and grid map

Abstract

The present invention relates to the field of communication sensing, and more specifically, to a path planning method and system based on an RRT algorithm and a grid map. The method includes: obtaining a starting node, a target node, and initial information required for path planning; initializing and establishing a tree T and a grid map grid: generating sampling nodes according to the grid map grid when the probability value is less than a probability threshold; obtaining new nodes according to the sampling nodes, performing collision detection on the straight path between the adjacent nodes and the new nodes, and updating the grid map grid; updating the cost value according to the new node; judging whether the path planning is completed, if completed, adding the target node to the tree T, and optimizing the initial path according to the tree T, obtaining an optimized path, and completing the path planning. In the present invention, as the probability threshold increases in the later stage of planning, the probability of sampling in the blank enclosing grid also increases, reducing the randomness of sampling, so that the present invention not only ensures the speed of path planning, but also ensures the effect of path planning.

Description

Technical Field

[0001] The present invention relates to the field of unmanned vehicle path planning, and more specifically, to a path planning method and system based on an RRT algorithm and a grid map. Background Art

[0002] With the rapid development of industrial automation and intelligence, unmanned vehicles are increasingly used in the service field, medical field, etc. However, no matter what field, unmanned vehicles should have the ability to plan paths quickly and safely in complex working environments. The path planning algorithm is a key part of the operation of unmanned vehicles. Its main function is to plan a collision-free path from the starting point to the end point. Commonly used path planning algorithms include traditional path planning algorithms and intelligent path planning algorithms. The A* algorithm and the RRT algorithm are typical traditional algorithms. Based on the grid map, the algorithm evaluates each searched grid to find the best position, and then searches from this position until the target point is found. As a widely used path planning algorithm, the RRT* algorithm obtains obstacle information by collision detection of sampling nodes in the state space, avoiding accurate spatial modeling, and can effectively solve path planning problems with complex spaces and complex constraints. However, the RRT* algorithm also has some shortcomings. For example, the RRT* algorithm has too strong randomness in sampling nodes, which affects the path planning effect.

[0003] The prior art discloses a multi-agent path planning method based on improved RRT, including the following steps: constructing a grid map and dilating the grid map for obstacles, obtaining the starting point and end point of each agent, using the traditional RRT algorithm to plan paths for the multi-agents based on the dilated grid map, and obtaining the RRT search path corresponding to each agent; optimizing the RRT search path; and the multi-agents move at variable speeds along the optimized RRT search path according to the multi-agent collaborative strategy until the multi-agents all reach the end point from their respective starting points. This method still has the problem that the sampling nodes are too random, which affects the path planning effect. Summary of the invention

[0004] The purpose of the present invention is to disclose a path planning method and system based on RRT algorithm and grid map with better path planning effect.

[0005] In order to achieve the above object, the present invention provides a path planning method based on an RRT algorithm and a grid map, comprising:

[0006] S1: Obtain the starting node, target node and initial information required for path planning;

[0007] S2: Initialize the tree T and grid map grid according to the initial information required for path planning, and set the sampling probability threshold, where the nodes stored in the tree T are only the starting nodes;

[0008] S3: Randomly generate a sampling probability value, and generate a sampling node for expanding the tree T according to the sampling probability value, the probability threshold and the tree T: when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid;

[0009] S4: according to the sampling node, find an adjacent node in the tree T so that the Euclidean distance between the sampling node and the adjacent node is the smallest; the adjacent node is extended by a step length according to the direction of the sampling node to obtain a new node, and a collision detection is performed on the straight path between the adjacent node and the new node. If the straight path does not collide, the new node is added to the tree T and the grid map grid is updated; if the straight path collides with an obstacle, return to execute step S3;

[0010] S5: Obtain the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node;

[0011] S6: Determine whether the path planning is completed based on the Euclidean distance between the new node and the target node and the initial information required for path planning. If completed, add the target node to the tree T, and use the new node as the parent node of the target node. If not completed, update the grid map grid and increase the probability threshold to return to step S3.

[0012] S7: According to the tree T, an initial path is obtained; then the initial path is optimized to obtain an optimized path, and the path planning is completed.

[0013] Furthermore, in step S1, the initial information required for path planning includes the planned environment space, obstacle information inside the environment space, each expansion step, and a range threshold considered to have reached the target point.

[0014] Further, in step S2, each node in the tree T stores the node position information, the cost value of the node to the starting node, the parent node pointer of the node, and the set of all child node pointers of the node;

[0015] The grid map grid divides the map into N*M grids on average, where N*M is N columns and M rows. The index of the first grid from the upper left corner is 1, and the index increases to the right first, and then increases from left to right in the next row of grids. The index of the last grid is N*M. Grids are divided into two types: occupied grids and free grids. Initially, all grids are marked as free grids and the storage flag is 0.

[0016] Furthermore, in step S3, generating sampling nodes according to the grid map grid includes:

[0017] Select the free grid surrounding the occupied grid, record it as the surrounding grid GS, the center point of the surrounding grid GS is GSc, and store the index value and cost value of the surrounding grid GS at the same time. The surrounding grid GS is selected by searching each occupied grid in four directions: up, down, left, and right. Find the free grid with no obstacles between the occupied grid and the occupied grid as the surrounding grid GS. GS(i) represents the i-th surrounding grid. Find the surrounding grid with the smallest cost value and generate a sampling node in the surrounding grid with the smallest cost value.

[0018] The cost calculation formula of GS(i) is:

[0019] V GS(i) =min{V GO |GO∈Adjacent(GS(i))}+ED(GS(i) c ,P goal )

[0020] Adjacent(GS(i)) represents the neighboring grids in the four directions of GS(i), P goal represents the target node, ED(GS(i) c ,P goal ) represents the Euclidean distance between the target node and the i-th surrounding grid, GO is the occupied grid, V GO is the cost value of the data grid.

[0021] Furthermore, in step S4, updating the grid map grid includes that when a new node falls on a certain grid, the grid flag is 1, indicating that it is occupied, and the index value of the grid in the map, the location information of the new node, and the cost value of the grid are stored at the same time; the cost value of the occupied grid is the average path distance from all nodes in the grid to the starting node.

[0022] Furthermore, in step S5, it includes:

[0023] S5.1: Get all nodes in the circle tree T with the new node as the center and a radius of 1.5*eps as possible parent nodes; find a node A among all possible parent nodes, so that the cost value of the node A plus the Euclidean distance from the node A to the starting node is the minimum, and there is no obstacle between the node P and the new node, and take the node A as the parent node of the new node;

[0024] S5.2: Get all nodes in the circle tree T with the new node as the center and a radius of 1.5*eps. For each node except the parent node found in step S5.1, perform the following steps:

[0025] If the cost value of the node is greater than the cost value of the node after updating the parent node of the node to the new node, and there is no collision between the node and the new node, then the parent node of the node is set to the new node and the cost value of the node is updated. The node is used as the child node of the new node, and then the cost value of each node in the child node set with the new node as the root node is updated; if it is less than, no operation is performed.

[0026] Further, in step S6, judging whether the path planning is completed according to the Euclidean distance between the new node and the target node and the initial information required for the path planning includes: comparing the size relationship between the Euclidean distance between the new node and the target node and the range threshold; if the Euclidean distance between the new node and the target node is less than the range threshold, the path planning is determined to be completed; if the Euclidean distance between the new node and the target node is greater than the range threshold, the path planning is determined to be incomplete.

[0027] Further, in step S6, increasing the probability threshold includes: as the number of occupied grids increases, the probability value of the probability threshold gradually increases, and the specific value of the probability threshold is determined by the following formula:

[0028]

[0029] where p t is the probability threshold, N*M is the total number of grids, and num(GO) is the number of occupied grids GO.

[0030] Furthermore, in step S7, the initial path is optimized to obtain the optimized path including:

[0031] Starting from the starting node, search the grid where each node in the initial path is located in turn, where P i is the i-th node in the initial path, and the center point of the grid corresponding to the i-th node in the initial path is denoted as P ic ; Find the P that is farthest from the starting node and has no obstacles in the middle ic , as the second path point, denoted as P s1 , and then P s1 As a starting point, find P s1 The farthest path point P without collision s2 , as the third path point, until reaching the target node, the optimized path (P start ,…,P si ,…,P goal ).

[0032] In addition, the present invention also provides a path planning system based on the RRT algorithm and the grid map, comprising:

[0033] Acquisition module: obtains the starting node, target node and initial information required for path planning;

[0034] Initialization module: According to the initial information required for path planning, the tree T and grid map grid are initialized and the sampling probability threshold is set. The nodes stored in the tree T are only the starting nodes.

[0035] Sampling module: randomly generates sampling probability values, and generates a sampling node for expanding tree T according to the sampling probability value, probability threshold and tree T: when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid;

[0036] Update module: According to the sampling node, find an adjacent node in the tree T so that the Euclidean distance between the sampling node and the adjacent node is minimized; expand the adjacent node by a step length according to the direction of the sampling node to obtain a new node, and perform collision detection on the straight path between the adjacent node and the new node. If the straight path does not collide, the new node is added to the tree T and the grid map grid is updated; if the straight path collides with an obstacle, return to execute the sampling module;

[0037] Node module: obtain the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node;

[0038] Initial planning module: Determine whether the path planning is completed based on the Euclidean distance between the new node and the target node and the initial information required for path planning. If completed, add the target node to the tree T and use the new node as the parent node of the target node. If not completed, update the grid map and increase the probability threshold to return to the execution sampling module.

[0039] Optimization module: According to the tree T, the initial path is obtained; then the initial path is optimized to obtain the optimized path and complete the path planning.

[0040] Compared with the prior art, the technical solution of the present invention has the following beneficial effects:

[0041] In the present invention, when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid, and the probability threshold gradually increases during path planning, so that the probability of random sampling is relatively large in the early stage of path planning, thereby improving the planning speed. In the later stage of planning, as the probability threshold increases, the probability of sampling in the blank enclosing grid also increases, thereby reducing the randomness of sampling. Therefore, the present invention ensures both the speed of path planning and the effect of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 This is a flow chart of the path planning method based on the RRT algorithm and the grid map described in Example 1;

[0043] Figure 2 This is a schematic diagram of the grid map described in Example 2;

[0044] Figure 3 A block diagram of a path planning system based on an RRT algorithm and a grid map as described in Example 3; DETAILED DESCRIPTION

[0045] The drawings are for illustrative purposes only and should not be construed as limiting the present patent;

[0046] The technical solution of the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0047] Embodiment 1:

[0048] The present invention provides a path planning method based on an RRT algorithm and a grid map, comprising:

[0049] S1: Obtain the starting node, target node and initial information required for path planning;

[0050] S2: Initialize and establish the tree T and grid map grid according to the initial information required for path planning, and set the sampling probability threshold, where the nodes stored in the tree T are only the starting nodes;

[0051] S3: Randomly generate a sampling probability value, and generate a sampling node for expanding the tree T according to the sampling probability value, the probability threshold and the tree T: when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid;

[0052] S4: according to the sampling node, find an adjacent node in the tree T so that the Euclidean distance between the sampling node and the adjacent node is the smallest; the adjacent node is extended by a step length according to the direction of the sampling node to obtain a new node, and a collision detection is performed on the straight path between the adjacent node and the new node. If the straight path does not collide, the new node is added to the tree T and the grid map grid is updated; if the straight path collides with an obstacle, return to execute step S3;

[0053] S5: Obtain the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node;

[0054] S6: Determine whether the path planning is completed based on the Euclidean distance between the new node and the target node and the initial information required for path planning. If completed, add the target node to the tree T, and use the new node as the parent node of the target node. If not completed, update the grid map grid and increase the probability threshold to return to step S3.

[0055] S7: According to the tree T, an initial path is obtained; then the initial path is optimized to obtain an optimized path, and the path planning is completed.

[0056] In this embodiment, when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid, and the probability threshold gradually increases during path planning, so that the probability of random sampling is relatively large in the early stage of path planning, thereby improving the planning speed. In the later stage of planning, as the probability threshold increases, the probability of sampling in the blank enclosing grid also increases, thereby reducing the randomness of sampling. Therefore, the present invention ensures both the speed of path planning and the effect of path planning.

[0057] Embodiment 2:

[0058] This embodiment further discloses on the basis of the first embodiment:

[0059] In step S1, the initial information required for path planning includes the planned environment space, obstacle information inside the environment space, each expansion step, and a range threshold considered to have reached the target point.

[0060] In step S2, each node in the tree T stores the node position information, the cost value from the node to the start node, the parent node pointer of the node, and the set of all child node pointers of the node;

[0061] The grid map grid divides the map into N*M grids on average, where N*M is N columns and M rows. The index of the first grid from the upper left corner is 1, and the index increases to the right first, and then increases from left to right in the next row of grids. The index of the last grid is N*M. Grids are divided into two types: occupied grids and free grids. Initially, all grids are marked as free grids and the storage flag is 0.

[0062] In step S3, generating sampling nodes according to the grid map grid includes:

[0063] Select the free grid surrounding the occupied grid, record it as the surrounding grid GS, the center point of the surrounding grid GS is GSc, and store the index value and cost value of the surrounding grid GS at the same time. The surrounding grid GS is selected by searching each occupied grid in four directions: up, down, left, and right. Find the free grid with no obstacles between the occupied grid and the occupied grid as the surrounding grid GS. GS(i) represents the i-th surrounding grid. Find the surrounding grid with the smallest cost value and generate a sampling node in the surrounding grid with the smallest cost value. At this time, the grid map grid is as follows Figure 2 As shown;

[0064] The cost calculation formula of GS(i) is:

[0065] V GS(i) =min{V GO |GO∈Adjacent(GS(i))}+ED(GS(i) c ,P goal )

[0066] Adjacent(GS(i)) represents the neighboring grids in the four directions of GS(i), P goal represents the target node, ED(GS(i) c ,P goal ) represents the Euclidean distance between the target node and the i-th surrounding grid, GO is the occupied grid, V GO is the cost value of the data grid.

[0067] In step S4, updating the grid map grid includes that when a new node falls on a certain grid, the grid flag is 1, indicating that it is occupied, and the index value of the grid in the map, the location information of the new node, and the cost value of the grid are stored at the same time; the cost value of the occupied grid is the average path distance from all nodes in the grid to the starting node.

[0068] In step S5, it includes:

[0069] S5.1: Get all nodes in the circle tree T with the new node as the center and a radius of 1.5*eps as possible parent nodes; find a node A among all possible parent nodes, so that the cost value of the node A plus the Euclidean distance from the node A to the starting node is the minimum, and there is no obstacle between the node P and the new node, and take the node A as the parent node of the new node;

[0070] S5.2: Get all nodes in the circle tree T with the new node as the center and a radius of 1.5*eps. For each node except the parent node found in step S5.1, perform the following steps:

[0071] If the cost value of the node is greater than the cost value of the node after updating the parent node of the node to the new node, and there is no collision between the node and the new node, then the parent node of the node is set to the new node and the cost value of the node is updated. The node is used as the child node of the new node, and then the cost value of each node in the child node set with the new node as the root node is updated; if it is less than, no operation is performed.

[0072] In step S6, judging whether the path planning is completed according to the Euclidean distance between the new node and the target node and the initial information required for the path planning includes: comparing the size relationship between the Euclidean distance between the new node and the target node and the range threshold; if the Euclidean distance between the new node and the target node is less than the range threshold, the path planning is determined to be completed; if the Euclidean distance between the new node and the target node is greater than the range threshold, the path planning is determined to be incomplete.

[0073] In step S6, increasing the probability threshold includes: as the number of occupied grids increases, the probability value of the probability threshold gradually increases, and the specific value of the probability threshold is determined by the following formula:

[0074]

[0075] where p t is the probability threshold, N*M is the total number of grids, and num(GO) is the number of occupied grids GO.

[0076] In step S7, the initial path is optimized to obtain the optimized path including:

[0077] Starting from the starting node, search the grid where each node in the initial path is located in turn, where P i is the i-th node in the initial path, and the center point of the grid corresponding to the i-th node in the initial path is denoted as P ic ; Find the P that is farthest from the starting node and has no obstacles in the middle ic , as the second path point, denoted as P s1 , and then P s1 As a starting point, find P s1 The farthest path point P without collision s2 , as the third path point, until reaching the target node, the optimized path (P start ,…,P si ,…,P goal ).

[0078] In this embodiment, when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid, and the probability threshold gradually increases during path planning, so that the probability of random sampling is relatively large in the early stage of path planning, thereby improving the planning speed. In the later stage of planning, as the probability threshold increases, the probability of sampling in the blank enclosing grid also increases, thereby reducing the randomness of sampling. Therefore, the present invention ensures both the speed of path planning and the effect of path planning.

[0079] Embodiment three:

[0080] This embodiment provides Figure 3 A path planning system based on an RRT algorithm and a grid map is shown, comprising:

[0081] Acquisition module: obtains the starting node, target node and initial information required for path planning;

[0082] Initialization module: According to the initial information required for path planning, the tree T and grid map grid are initialized and the sampling probability threshold is set. The nodes stored in the tree T are only the starting nodes.

[0083] Sampling module: randomly generates sampling probability values, and generates a sampling node for expanding tree T according to the sampling probability value, probability threshold and tree T: when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid;

[0084] Update module: According to the sampling node, find an adjacent node in the tree T so that the Euclidean distance between the sampling node and the adjacent node is minimized; expand the adjacent node by a step length according to the direction of the sampling node to obtain a new node, and perform collision detection on the straight path between the adjacent node and the new node. If the straight path does not collide, the new node is added to the tree T and the grid map grid is updated; if the straight path collides with an obstacle, return to execute the sampling module;

[0085] Node module: obtain the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node;

[0086] Initial planning module: Determine whether the path planning is completed based on the Euclidean distance between the new node and the target node and the initial information required for path planning. If completed, add the target node to the tree T and use the new node as the parent node of the target node. If not completed, update the grid map and increase the probability threshold to return to the execution sampling module.

[0087] Optimization module: According to the tree T, the initial path is obtained; then the initial path is optimized to obtain the optimized path and complete the path planning.

[0088] In this embodiment, when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid, and the probability threshold gradually increases during path planning, so that the probability of random sampling is relatively large in the early stage of path planning, thereby improving the planning speed. In the later stage of planning, as the probability threshold increases, the probability of sampling in the blank enclosing grid also increases, thereby reducing the randomness of sampling. Therefore, the present invention ensures both the speed of path planning and the effect of path planning.

[0089] In summary, an embodiment of the present invention provides a path planning method and system based on an RRT algorithm and a grid map. The method includes: obtaining a starting node, a target node, and initial information required for path planning; initializing and establishing a tree T and a grid map grid according to the initial information required for path planning, and setting a sampling probability threshold, wherein the node stored in the tree T is only the starting node; randomly generating a sampling probability value, and generating a sampling node for expanding the tree T according to the sampling probability value, the probability threshold, and the tree T: randomly generating a sampling node when the probability value is greater than the probability threshold; generating a sampling node according to the grid map grid when the probability value is less than the probability threshold; searching for an adjacent node in the tree T according to the sampling node so that the Euclidean distance between the sampling node and the adjacent node is minimized; and expanding the adjacent node by a step length according to the direction of the sampling node. , get the new node, perform collision detection on the straight path between the adjacent node and the new node, if the straight path does not collide, add the new node to the tree T, and update the grid map grid;; get the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node; judge whether the path planning is completed according to the Euclidean distance between the new node and the target node, and the initial information required for path planning. If it is completed, add the target node to the tree T, and the new node is used as the parent node of the target node. If it is not completed, update the grid map grid and increase the probability threshold; according to the tree T, get the initial path; then optimize the initial path to get the optimized path and complete the path planning. In an embodiment of the present invention, when the probability value is greater than the probability threshold, a sampling node is randomly generated; when the probability value is less than the probability threshold, a sampling node is generated according to the grid map grid, and the probability threshold gradually increases during path planning, so that the probability of random sampling is relatively large in the early stage of path planning, thereby improving the planning speed. In the later stage of planning, as the probability threshold increases, the probability of sampling in the blank enclosing grid also increases, thereby reducing the randomness of sampling. Therefore, the present invention ensures both the speed of path planning and the effect of path planning.

[0090] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A path planning method based on RRT algorithm and grid map, characterized in that: The following steps are involved: S1: Obtain the starting node, target node and initial information required for path planning; S2: Initialize the tree T and grid map grid according to the initial information required for path planning, and set the sampling probability threshold, where the nodes stored in the tree T are only the starting nodes; S3: randomly generate a sampling probability value, and generate a sampling node for expanding the tree T according to the sampling probability value, the probability threshold and the tree T: when the probability value is greater than the probability threshold, a sampling node is randomly generated; When the probability value is less than the probability threshold, a sampling node is generated according to the grid map; S4: according to the sampling node, find an adjacent node in the tree T so that the Euclidean distance between the sampling node and the adjacent node is the smallest; the adjacent node is extended by a step length according to the direction of the sampling node to obtain a new node, and a collision detection is performed on the straight path between the adjacent node and the new node. If the straight path does not collide, the new node is added to the tree T and the grid map grid is updated; if the straight path collides with an obstacle, return to execute step S3; S5: Obtain the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node; S6: Determine whether the path planning is completed based on the Euclidean distance between the new node and the target node and the initial information required for path planning. If completed, add the target node to the tree T, and use the new node as the parent node of the target node. If not completed, update the grid map grid and increase the probability threshold to return to step S3. Increasing the probability threshold includes: as the number of occupied grids increases, the probability value of the probability threshold gradually increases, and the specific value of the probability threshold is determined by the following formula: where p t is the probability threshold, N*M is the total number of grids, and num(GO) is the number of occupied grid GOs; S7: According to the tree T, an initial path is obtained; then the initial path is optimized to obtain an optimized path, and the path planning is completed.

2. The path planning method based on the RRT algorithm and the grid map according to claim 1, characterized in that: In step S1, the initial information required for path planning includes the planned environment space, obstacle information inside the environment space, each expansion step, and a range threshold considered to have reached the target node.

3. The path planning method based on RRT algorithm and grid map according to claim 1, characterized in that: In step S2, each node in the tree T stores the node position information, the cost value from the node to the start node, the parent node pointer of the node, and the set of all child node pointers of the node; The grid map grid divides the map into N*M grids on average, where N*M is N columns and M rows. The index of the first grid from the upper left corner is 1, and the index increases to the right first, and then increases from left to right in the next row of grids. The index of the last grid is N*M. Grids are divided into two types: occupied grids and free grids. Initially, all grids are marked as free grids and the storage flag is 0.

4. The path planning method based on the RRT algorithm and the grid map according to claim 3, characterized in that: In step S3, generating sampling nodes according to the grid map grid includes: Select the free grid surrounding the occupied grid, record it as the surrounding grid GS, the center point of the surrounding grid GS is GSc, and store the index value and cost value of the surrounding grid GS at the same time. The surrounding grid GS is selected by searching each occupied grid in four directions: up, down, left, and right. Find the free grid with no obstacles between the occupied grid and the occupied grid as the surrounding grid GS. GS(i) represents the i-th surrounding grid. Find the surrounding grid with the smallest cost value and generate a sampling node in the surrounding grid with the smallest cost value. The cost calculation formula of GS(i) is: V GS(i) =min{V GO |GO∈Adjacent(GS(i))}+ED(GS(i) c ,P goal ) Adjacent(GS(i)) represents the neighboring grids in the four directions of GS(i), P goal represents the target node, ED(GS(i) c ,P goal ) represents the Euclidean distance between the target node and the i-th surrounding grid, GO is the occupied grid, V GO is the cost value of the occupied grid.

5. The path planning method based on RRT algorithm and grid map according to claim 1, characterized in that: In step S4, updating the grid map grid includes that when a new node falls on a certain grid, the grid flag is 1, indicating that it is occupied, and the index value of the grid in the map, the location information of the new node, and the cost value of the grid are stored at the same time; the cost value of the occupied grid is the average path distance from all nodes in the grid to the starting node.

6. The path planning method based on RRT algorithm and grid map according to claim 1, characterized in that: In step S5, it includes: S5.1: Get all nodes in the circle tree T with the new node as the center and a radius of 1.5*eps as possible parent nodes; find a node A among all possible parent nodes, so that the cost value of the node A plus the Euclidean distance from the node A to the starting node is the minimum, and there is no obstacle between the node A and the new node, and take the node A as the parent node of the new node; S5.2: Get all nodes in the circle tree T with the new node as the center and a radius of 1.5*eps. For each node except the parent node found in step S5.1, perform the following steps: If the cost value of the node is greater than the cost value of the node after updating the parent node of the node to the new node, and there is no collision between the node and the new node, then the parent node of the node is set to the new node and the cost value of the node is updated. The node is used as the child node of the new node, and then the cost value of each node in the child node set with the new node as the root node is updated; if it is less than, no operation is performed.

7. The path planning method based on RRT algorithm and grid map according to claim 2, characterized in that: In step S6, judging whether the path planning is completed according to the Euclidean distance between the new node and the target node and the initial information required for the path planning includes: comparing the size relationship between the Euclidean distance between the new node and the target node and the range threshold; if the Euclidean distance between the new node and the target node is less than the range threshold, the path planning is determined to be completed; if the Euclidean distance between the new node and the target node is greater than the range threshold, the path planning is determined to be incomplete.

8. The path planning method based on RRT algorithm and grid map according to claim 1, characterized in that: In step S7, the initial path is optimized to obtain the optimized path including: Starting from the starting node, search the grid where each node in the initial path is located in turn, where P i is the i-th node in the initial path, and the center point of the grid corresponding to the i-th node in the initial path is denoted as P ic ; Find the P that is farthest from the starting node and has no obstacles in the middle ic , as the second path point, denoted as P s1 , and then P s1 As a starting point, find P s1 The farthest path point P without collision s2 , as the third path point, until reaching the target node, the optimized path (P start ,…,P si ,…,P goal ).

9. A path planning system based on RRT algorithm and grid map, characterized in that: include: Acquisition module: obtains the starting node, target node and initial information required for path planning; Initialization module: According to the initial information required for path planning, the tree T and grid map grid are initialized and the sampling probability threshold is set. The nodes stored in the tree T are only the starting nodes. Sampling module: randomly generates a sampling probability value, and generates a sampling node for expanding the tree T according to the sampling probability value, the probability threshold and the tree T: when the probability value is greater than the probability threshold, a sampling node is randomly generated; When the probability value is less than the probability threshold, a sampling node is generated according to the grid map; Update module: According to the sampling node, find an adjacent node in the tree T so that the Euclidean distance between the sampling node and the adjacent node is minimized; expand the adjacent node by a step length according to the direction of the sampling node to obtain a new node, and perform collision detection on the straight path between the adjacent node and the new node. If the straight path does not collide, the new node is added to the tree T and the grid map grid is updated; if the straight path collides with an obstacle, return to execute the sampling module; Node module: obtain the parent node of the new node and the child node set of the new node according to the new node, and update the cost value of the child node set of the new node according to the new node; Initial planning module: Determine whether the path planning is completed based on the Euclidean distance between the new node and the target node and the initial information required for path planning. If completed, add the target node to the tree T and use the new node as the parent node of the target node. If not completed, update the grid map and increase the probability threshold to return to the execution sampling module. Increasing the probability threshold includes: as the number of occupied grids increases, the probability value of the probability threshold gradually increases, and the specific value of the probability threshold is determined by the following formula: where p t is the probability threshold, N*M is the total number of grids, and num(GO) is the number of occupied grid GOs; Optimization module: According to the tree T, the initial path is obtained; then the initial path is optimized to obtain the optimized path and complete the path planning.

Images