Path planning method based on Informed-RRT*

Abstract

The invention discloses a path planning method based on Informed-RRT *, and relates to the technical field of path planning. The invention aims to solve the problem that an existing path planning method cannot efficiently acquire a global path when passing through a narrow-distance area. The method comprises the following steps: identifying all narrow-distance areas in a research area by using a research area cost map, and generating offset points; establishing a forward and reverse bidirectional tree by using the coordinates of the starting node, the starting auxiliary node, the target node and the target auxiliary node, generating a new node to update the forward and reverse bidirectional tree, judging whether the updated forward and reverse bidirectional tree has a connection path, and obtaining a planned path of the mobile robot if the connection path exists; if not, judging whether the number of sampling points is greater than the total number of iterations; if the number of the sampling points is greater than the total number of iterations, determining that the output path planning fails; if the number of the sampling points is smaller than or equal to the total number of iterations, the bias nodes are used for guiding the node growth direction, and then the gravitation and repulsive force of the bias points are eliminated. The method is used for path planning.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning method based on Informed-RRT*. Background Art

[0002] Path planning of mobile robots refers to the robot planning a collision-free optimal path from the starting point to the end point after obtaining a map of the current environment. Path planning of mobile robots is particularly important in robot operations in various industries.

[0003] The current road planning is mainly based on the RRT algorithm. The RRT (Rapidly-exploring Random Treestar) algorithm is a traditional probability-based path search algorithm. It is often used in spaces with complex and irregular obstacles to find a path from the starting point to the end point, and after finding the path, it continues to iterate and optimize the branches to make the result as close to the optimal solution as possible. The RRT* algorithm has some unique advantages: the RRT* algorithm can search for paths in the entire configuration map, which is suitable for finding the global optimal path. Compared with the RRT algorithm, the RRT* algorithm adds a branch pruning process to achieve reconnected core optimization, which allows the RRT* algorithm to gradually find an approximate optimal path as the number of iterations increases, and can provide a relatively optimal solution. The RRT* algorithm is not only suitable for low-dimensional maps, but also has strong adaptability in high-dimensional configuration map space, and can adapt to environments with obstacles of various shapes and sizes. The Informed-RRT* algorithm is a branch of the RRT* algorithm. The difference is that the original irregular random sampling strategy is added with restrictions, with the starting point and the end point as the focus, to establish an elliptical area, so that the random points are sampled completely within the elliptical area, reducing a large number of invalid sampling points, and greatly increasing the efficiency of the algorithm on the basis of the RRT* algorithm. However, the path planning method based on the Informed-RRT* algorithm explores space through random sampling. When in a narrow area, due to limited space and densely distributed obstacles, the randomly sampled points are likely to fall into obstacles or fail to effectively guide the growth of the tree. The probability that the sampling point is exactly at the narrow distance position of the map is very low. Therefore, a large number of invalid sampling points are generated in the process of finding the optimal path, which ultimately leads to low path planning efficiency of the mobile robot when passing through the narrow distance area. The low probability that the random sampling point is exactly at the narrow distance position of the map will also lead to a small number of effective sampling points, making it impossible to effectively explore the global path. Summary of the invention

[0004] The purpose of the present invention is to solve the problem that the existing path planning method still cannot efficiently obtain the global path when passing through a narrow area, and proposes a path planning method based on Informed-RRT*.

[0005] A path planning method based on Informed-RRT*, specifically:

[0006] Step 1: Set the path planning parameters and initialize the starting node cost value to 0 and the target node cost value to 0;

[0007] The path planning parameters include: starting node coordinates, target node coordinates, extended node step size T, starting sub-node coordinates, target sub-node coordinates, sampling area and total number of iterations;

[0008] Step 2: Obtain a cost map of the study area, use the cost map of the study area to identify all narrow-distance areas in the study area, generate bias points, and obtain a cost map of the study area after generating the bias points;

[0009] Step 3: Use the starting node coordinates, starting sub-node coordinates, target node coordinates and target sub-node coordinates to establish a forward and reverse bidirectional tree, and generate a new node to update the forward and reverse bidirectional tree, and determine whether the updated forward and reverse direction tree has a connection path. If there is a connection path, execute step 5; if there is no connection path, determine whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than the total number of iterations, the output path planning fails; if the number of sampling points is less than or equal to the total number of iterations, execute step 4;

[0010] Step 4: Use the bias node generated in step 2 to guide the growth direction of the nodes in the positive and negative bidirectional trees updated in step 3, update the positive and negative direction trees again, then eliminate the gravitational force generated by the bias point and the repulsive force generated by the obstacle, and return to step 3;

[0011] Step 5: Use the forward and reverse direction trees obtained in step 3 to obtain the planned path of the mobile robot.

[0012] Furthermore, in the step 2, the cost map of the study area is obtained, and all narrow-distance areas in the study area are identified using the cost map of the study area, and bias points are generated to obtain the cost map of the study area after the bias points are generated, specifically:

[0013] Step 21. Obtain a map of the study area, divide the map into grids according to a preset number of grids, use the upper left corner of the map of the study area as the origin of the coordinate axis, and set the horizontal and vertical directions of the map of the study area as the x-axis and y-axis directions;

[0014] Step 22, set the cost value of the grid containing obstacles in the study area map to 1, and set the cost value of the free area grid without obstacles to 0, so as to obtain the study area cost map;

[0015] The grid containing the obstacle area is the obstacle area;

[0016] Step 2 and 3: Starting from the origin of the coordinate axis, traverse the study area cost map in the order of x-direction and y-direction to set the offset point.

[0017] Furthermore, in the steps 2 and 3, the study area cost map is traversed in the order of first traversing in the x direction and then traversing in the y direction from the origin of the coordinate axis, so as to set the bias point and obtain the study area cost map after generating the bias point, specifically:

[0018] Step 231: Taking each column of the cost map of the research area traversed in the x direction as an example, initialize the current column number i=1; initialize the element number j=1 in the i-th column row, and initialize l=1;

[0019] Step 232: Get the jth element value and j+1th element value of the i-th column in the cost map of the study area. If the jth element value is 1 and the j+1th element value is 0, go to step 233; otherwise, go to step 237;

[0020] Step 233: Get the value of the j+lth element in the i-th column. If the value of the j+lth element is 0 and the value of the j+l+1th element is 1, then record the position A corresponding to the j+1th element in the i-th column. i,j+1 The position A corresponding to the value of the j+lth element in the i-th column i,j+l , then execute step 234; otherwise, execute step 236;

[0021] Step 2, 3, 4, judge A i,j+1 With A i,j+l Are they the same position coordinates? If they are the same position coordinates, then at position A i,j+1 Set the bias point, then execute steps 2, 3, and 7; otherwise, obtain A i,j+1 and A i,j+l Distance L i , execute steps 235;

[0022] Step 235: L i Compared with the preset narrow distance parameter threshold Narrow, if L i >Narrow, then execute steps 237; if L i ≤Narrow, then A i,j+1 and A i,j+l The area between is the narrow spacing area. i,j+1 and Ai,j+l Generate an offset point at the midpoint position, and then execute steps 2, 3, and 7;

[0023] Step 236: Determine whether j+l+1 is less than the total number of elements J in the current column. If j+l+1<J, set l=l+1 and execute step 233; otherwise, execute step 238;

[0024] Step 237: Determine whether the current element index j is less than the total number of elements J in the current column. If j < J, set j = j + 1 and return to step 232; otherwise, execute step 238;

[0025] Step 238: Determine whether i is less than N. If i<N, set i=i+1 and return to step 232; otherwise, execute step 239;

[0026] Where N is the total number of columns in the costmap of the study area;

[0027] Step 239. Determine whether there are adjacent bias points in each column. If there are adjacent bias points, obtain the starting bias point coordinate a and the ending bias point coordinate b in the adjacent bias points, obtain the midpoint coordinates of a and b, delete the current adjacent bias points, and use the midpoint coordinates of a and b as the bias point, and output the study area cost map after generating the bias point; otherwise, directly output the study area cost map after generating the bias point.

[0028] Furthermore, in step 3, the forward and reverse bidirectional trees are established using the starting node coordinates, the starting sub-node coordinates, the target node coordinates and the target sub-node coordinates, and new nodes are generated to update the forward and reverse bidirectional trees, and it is determined whether the updated forward and reverse trees have new connection paths. If there are new connection paths, step 5 is executed; if there are no new connection paths, it is determined whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, the output path planning fails; if the number of sampling points is less than the total number of iterations, step 4 is executed, which is specifically:

[0029] Step 31: Use the starting node coordinates, the starting sub-node coordinates, the target node coordinates and the target sub-node coordinates to establish a positive and negative bidirectional tree;

[0030] Step 32: Generate a new random point in the sampling area;

[0031] Step 33, obtain the nodes m and n closest to the newly generated random point in the forward and reverse direction trees respectively, and update the forward and reverse direction trees using the nodes m, n and the newly generated random point;

[0032] Step 3 and 4: Determine whether there is a new connection path in the updated forward and reverse direction trees. If there is a new connection path, execute step 5; if there is no new connection path, determine whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, output path planning failure; if the number of sampling points is less than the total number of iterations, execute step 4;

[0033] The number of sampling points is the total number of random points generated in step 32 and the number of new nodes grown by the forward and reverse trees.

[0034] Furthermore, in step 33, the nodes m and n closest to the newly generated random point are obtained on the forward and reverse direction trees respectively, and the forward and reverse direction trees are updated using the nodes m, n and the newly generated random point, specifically:

[0035] Step 331. Obtain the nodes m and n closest to the newly generated random point in the forward and reverse direction trees respectively, and obtain the distance L between the newly generated random point and the node m m , the distance L between the newly generated random point and node n n , judge L m , L n The relationship between them and the step length T of the expansion node respectively;

[0036] If L m <T, the newly generated random point is used as a new node, and then step 332 is executed; otherwise, the positive direction tree grows according to the node expansion step T to obtain a new node, and then step 332 is executed;

[0037] If L n <T, the newly generated random point is used as a new node, and then step 332 is executed. Otherwise, the reverse tree grows according to the node expansion step T to obtain a new node, and then step 332 is executed;

[0038] Step 332: Determine whether there is an obstacle area between the new node and the nearest node in the forward direction tree. If so, return to step 32; otherwise, add the new node to the forward direction tree and update the forward direction tree;

[0039] Get whether there is an obstacle area between the new node and the nearest node in the reverse tree. If so, return to step 32; otherwise, add the new node to the reverse tree and update the reverse tree.

[0040] Furthermore, the determination of whether the updated forward and reverse direction trees have a new connection path in the steps three and four is specifically as follows: determining whether the distance between the node m' in the updated forward direction tree and the node n' in the updated reverse direction tree is less than or equal to the expansion node step length T; if the distance between m' and n' is less than or equal to T, it indicates that the path between the node m' and the node n' is a connection path; otherwise, it indicates that the path between the node m' and the node n' is not a connection path;

[0041] If the current connection path is newly appeared after the positive and negative direction trees are updated, the current connection path is the new connection path.

[0042] Furthermore, the bias node generated in step 2 in step 4 guides the growth direction of the nodes in the positive and negative bidirectional tree after the update in step 3, updates the positive and negative direction tree again, and then eliminates the gravitational force generated by the bias point and the repulsive force generated by the obstacle, and returns to step 3, specifically:

[0043] Step 41: Determine whether the new node q generated in step 332 falls on the offset point q s (x b ,y b ) is the center of the circle and Arrange is the radius of the gravitational region; if yes, execute step 42; otherwise, execute step 33;

[0044] Among them, Arrange is the preset gravitational area radius;

[0045] Step 42: Determine whether the current new node q is the first one to fall on the current bias point q s (x b ,y b ) and the current new node q is the same as the current offset point q s (x b ,y b ) is there any obstacle on the line connecting them, if so, record the coordinates (x a ,y a ) and execute step 43; otherwise, do not record the coordinates of the new node q and directly execute step 43;

[0046] Step 43: Get the bias point q s (x b ,y b )'s gravitational potential field function U att (q) and the repulsive potential field function U of the obstacle rep (q);

[0047] Step 4. Use bias point q s (x b ,y b ) to obtain the gravitational gradient function F at the bias point att (q) and the repulsive gradient function F of the obstacle rep (q);

[0048] Step 4 and 5: Use the gravitational gradient function of the offset point and the repulsive gradient function of the obstacle to obtain the resultant force F of the gravitational and repulsive forces on the new node q, and obtain the unit vector of the resultant force F:

[0049] First, the gravitational gradient function of the offset point and the repulsive gradient function of the obstacle are used to obtain the combined force F of the gravitational and repulsive forces on the new node q:

[0050] F=F att (q)+F rep (q)

[0051] Then, the Euclidean distance of the resultant force F of the gravitational force and the repulsive force on the new node q is obtained, and then the unit vector of the resultant force F is obtained using the Euclidean distance of the resultant force F;

[0052] Step 46: Determine whether the resultant force F is 0. If F is not equal to 0, generate a new node in the positive and negative direction tree in the direction of the unit vector of F, record it as q', and add q' to the positive and negative direction tree, and execute step 47; if the resultant force F is 0 and the unit vector cannot be generated, return to step 33;

[0053] Step 47: Determine the new node q' and the offset point q s (x b ,y b ) overlap; if so, execute step 49; otherwise, execute step 48;

[0054] Step 48: Get and The angle θ between them is used to obtain the current bias (x b ,y b ) to the edges of obstacles on both sides respectively, and compare θ with the larger value of θ1 and θ2. If θ is larger than the larger value of θ1 and θ2, execute step 49, otherwise execute step 46;

[0055] Step 49: Set the current offset point q s (x b ,y b ) and the potential field function of the obstacle are set to 0, then set node q = q' and return to step 43.

[0056] Furthermore, the step 43 of obtaining the bias point q s (x b ,y b )'s gravitational potential field function U att (q) and the repulsive potential field function U of the obstacle rep (q), specifically:

[0057]

[0058]

[0059] Among them, U att (q) is the potential field function of the sampling point q under the gravitational force, η is the gravitational potential field gain coefficient, qs is the bias point position, q is the sampling point position, ρ(q,q s ) is point q to point q s Euclidean distance, arrange is the preset bias point located at the boundary value of the gravitational region, k is the repulsive potential field gain coefficient, q o is an obstacle, ρ(q,q o ) is point q to point q o The Euclidean distance, ρ0 is the repulsive boundary value, U rep (q) is the potential field function of the repulsive force on the sampling point q.

[0060] Furthermore, the use of the bias point (x b ,y b ) to obtain the gravitational gradient function F at the bias point att (q) and the repulsive gradient function F of the obstacle rep (q), specifically:

[0061]

[0062]

[0063] Among them, F att (q) is the gradient function of the gravitational force on the sampling point q, F rep (q) is the gradient function of the repulsive force at sampling point q.

[0064] Furthermore, the forward and reverse direction trees obtained in step 3 are used in step 5 to obtain the planned path of the mobile robot, specifically:

[0065] Step 51: Obtain the new connection path R of the forward and reverse direction trees obtained in step 3 and the cost value of the current connection path;

[0066] Step 52: compare the cost value of the current connection path R with the saved connection path cost value. If the cost value of the current connection path R is less than the saved connection path cost value, update the saved connection path cost value to the cost value of the current connection path, and then execute step 53; otherwise, directly execute step 53;

[0067] The initial value of the saved connection path cost is positive infinity;

[0068] Step 53: Determine whether the number of sampling points of the forward and reverse direction trees outputted in step 3 is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, output the connection path corresponding to the recorded cost value as the planned path; if the number of sampling points is less than the total number of iterations, return to step 32.

[0069] The beneficial effects of the present invention are:

[0070] The present invention proposes a path planning method based on Informed-RRT*. First, a narrow-distance map recognition strategy is adopted. According to the variation law of the narrow-distance map cost value, the narrow-distance position is identified and a bias point is generated. Through the sampling guidance strategy, a potential field function is added to the bias point and the obstacle, and all the points generated in the area near the bias point are guided to the bias point. When the path passes through the bias point, the gravity function is initialized to zero through the gravity elimination strategy to avoid affecting further iterations. After the path is found, branch pruning optimization is performed, and the optimal path is selected as the path through the narrow-distance area, so as to achieve the purpose of quickly passing through the narrow-distance channel, improve the planning efficiency when passing through the narrow-distance map, and thus efficiently obtain the global path. In the operation process of the present invention, a bidirectional random tree is introduced to speed up the planning speed, further improving the efficiency of path planning. The present invention adopts a narrow-distance map recognition strategy combined with a bias point to introduce an artificial potential field method, which increases the number of effective sampling points, so that the global path can be effectively obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 It is a flow chart of the present invention;

[0072] Figure 2 Identify strategy flow chart for narrow-spacing maps;

[0073] Figure 3(a) is a map effect diagram before recognition by the narrow-distance map recognition strategy;

[0074] Figure 3(b) is a map effect diagram after the narrow-range map recognition strategy is used;

[0075] Figure 4(a) shows the map using the gravity elimination strategy;

[0076] Figure 4(b) is a schematic diagram of the gravitational elimination strategy;

[0077] Figure 5 Flowchart for sampling guidance and gravity elimination strategy;

[0078] Figure 6(a) is a graph showing the number of sampling points in the existing path planning method;

[0079] FIG6( b ) is a diagram showing the number of sampling points of the present invention;

[0080] Figure 7 Generate a path time comparison graph before and after optimization;

[0081] Figure 8 Compare the average time required to generate paths in different maps;

[0082] Fig. 9 This is a comparison curve of the average path length before and after optimization;

[0083] Fig.10It is a comparison curve of the best path length before and after optimization;

[0084] Figure 11(a) is a diagram showing the process of traversing each column of the cost map of the study area in the x direction to generate paranoid points;

[0085] Figure 11(b) shows the process of traversing each row of the cost map of the study area in the y direction to generate paranoid points. DETAILED DESCRIPTION

[0086] Specific implementation method 1: Figure 1 As shown, the specific process of a path planning method based on Informed-RRT* in this embodiment is as follows:

[0087] Step 1: Initialize path planning parameters:

[0088] The path planning parameters include: starting node coordinates, target node coordinates, starting node cost value, target node cost value, extended node step size T, starting sub-node coordinates, target sub-node coordinates, sampling area, and total number of iterations;

[0089] The cost value of the starting node is initialized to 0, and the cost value of the target node is initialized to 0;

[0090] The cost value includes 0 and 1; 0 means the current position is a free area, and 1 means the current position is an obstacle area;

[0091] Step 2: Figure 2 As shown, obtain the cost map of the study area, use the narrow distance map recognition strategy to traverse and identify all narrow distance areas in the study area, and generate bias points, specifically:

[0092] Step 21. Obtain a map of the study area, divide the map into grids, and use the upper left corner of the map as the origin of the coordinate axis, and set the horizontal and vertical directions of the map as the x-axis and y-axis directions;

[0093] Step 22, set the cost value of the grid containing obstacles in the study area map to 1, and set the cost value of the free area grid without obstacles to 0, so as to obtain the study area cost map;

[0094] The grid containing the obstacle area is the obstacle area;

[0095] Step 2 and 3: Starting from the origin of the coordinate axis, traverse the study area cost map in the order of x-direction traversal and then y-direction traversal to set the offset point:

[0096] Step 231: traverse each column of the study area cost map in the x direction, and then traverse each row of the study area cost map in the y direction; taking any traversal direction as an example, initialize the current column (row) number i = 1; initialize the element number j = 1 in the i-th column (row), and initialize l = 1;

[0097] This step can also be performed by first traversing each row of the cost map of the study area in the y direction, and then traversing each column of the cost map of the study area in the x direction. The reason why the map needs to be traversed in both the x and y directions is that the algorithm will identify all narrow channels in the x direction when traversing the map in the x direction; the present invention will identify all narrow channels in the y direction when traversing the map in the y direction. It is impossible to completely find all narrow channels in the map by traversing only one direction.

[0098] Step 232: Get the jth element value and j+1th element value of the i-th column (row) in the cost map of the study area. If the jth element value is 1 and the j+1th element value is 0, go to step 233; otherwise, go to step 237;

[0099] Step 233: Get the value of the j+lth element in the i-th column (row). If the value of the j+lth element is 0 and the value of the j+l+1th element is 1, then record the position A corresponding to the j+1th element in the i-th column (row) i,j+1 The position A corresponding to the value of the j+lth element in the i-th column (row) i,j+l , then execute step 234; otherwise, execute step 236;

[0100] Step 2, 3, 4, judge A i,j+1 With A i,j+l Are they the same position coordinates? If they are the same position coordinates, then they are at position A. i,j+1 Set the bias point, then execute steps 2, 3, and 7; otherwise, obtain A i,j+1 and A i,j+l Distance L i , execute steps 235;

[0101] Step 235: L i Compared with the preset narrow distance parameter threshold Narrow, if L i >Narrow, then execute steps 237; if L i ≤Narrow, then A i,j+1 and A i,j+l The area between is the narrow spacing area. i,j+1 and A i,j+l Generate an offset point at the midpoint position, and then execute steps 2, 3, and 7;

[0102] Step 236: Determine whether j+l+1 is less than the total number of elements J in the current column (row). If j+l+1<J, set l=l+1 and execute step 233; otherwise, execute step 238;

[0103] Step 237: Determine whether the current element index j is less than the total number of elements J in the current column (row); if j < J, set j = j + 1 and return to step 232; otherwise, execute step 238;

[0104] Step 238: Determine whether i is less than N. If i<N, set i=i+1 and return to step 232; otherwise, execute step 239;

[0105] If the current traversal is in the x direction, N represents the total number of columns in the cost map of the study area; if the current traversal is in the y direction, N represents the total number of rows in the cost map of the study area;

[0106] Step 239: Determine whether there are adjacent bias points in each column (row) such as Figure 11(a)-Figure 11(b) As shown by the blue dots, if adjacent bias points appear, obtain the starting bias point coordinate a and the ending bias point coordinate b in the adjacent bias points, obtain the midpoint coordinates of a and b, delete the current adjacent bias points, use the midpoint coordinates of a and b as the bias point, and output the cost map of the study area with the bias points set; otherwise, directly output the cost map of the study area with the bias points set.

[0107] In this step, since obstacles are represented by cost value 1 and free space is represented by cost value 0 in Matlab's cost map, a narrow-distance map recognition strategy is added to the algorithm. The specific principle is to traverse all columns (rows) in the x (y) direction of the grid map. Whenever a column (row) has a cost map value that changes from 1 to 0 and then from 0 to 1, the x and y coordinates of the two cost value changes are recorded respectively. Knowing the x and y coordinate values ​​of the two changes, a bias point can be set at the center of its coordinates. If multiple continuous bias points are generated in the x (y) direction of the bias point, the original bias point is cancelled, and the maximum and minimum values ​​of the continuously generated bias points in the x direction are screened. The middle value of x is taken as the newly generated new bias point to replace the previously generated continuous bias points. In the narrow-distance map recognition strategy, a parameter Narrow is also designed to measure whether the area here should be defined as narrow distance. The value of the parameter Narrow is the threshold size defined as the narrow distance area. The value of the specified parameter Narrow is used to distinguish whether a bias point should be generated in an area. The map before recognition is shown in Figure 3(a), and the map after recognition is shown in Figure 3(b).

[0108] Step 3: Use the starting node coordinates, starting sub-node coordinates, target node coordinates and target sub-node coordinates to establish a forward and reverse bidirectional tree, and generate a new node to update the forward and reverse bidirectional tree, and determine whether the updated forward and reverse trees have a new connection path. If there is a new connection path, execute step 5; if there is no new connection path, determine whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, the output path planning fails; if the number of sampling points is less than the total number of iterations, execute step 4, specifically:

[0109] Step 31: Use the starting node coordinates, the starting sub-node coordinates, the target node coordinates and the target sub-node coordinates to establish a positive and negative bidirectional tree;

[0110] Step 32: Generate a new random point in the sampling area;

[0111] Step 3. Obtain the nodes m and n closest to the newly generated random point in the forward and reverse direction trees respectively, and use the nodes m, n and the newly generated random point to update the forward and reverse direction trees, specifically:

[0112] Step 331. Obtain the nodes m and n closest to the newly generated random point in the forward and reverse direction trees respectively, and obtain the distance L between the newly generated random point and the node m m , the distance L between the newly generated random point and node n n , judge L m , L n Respectively with the size of the expansion node step T;

[0113] If L m <T, the newly generated random point is used as a new node, and then step 332 is executed; otherwise, the positive direction tree grows according to the node expansion step T to obtain a new node, and then step 332 is executed;

[0114] If L n <T, the newly generated random point is used as a new node, and then step 332 is executed. Otherwise, the reverse tree grows according to the node expansion step T to obtain a new node, and then step 332 is executed;

[0115] Step 332: Obtain whether there is an obstacle area between the new node and the nearest node in the forward direction tree. If so, return to step 32; otherwise, add the new node to the forward direction tree and update the forward direction tree.

[0116] Get whether there is an obstacle area between the new node and the nearest node in the reverse tree. If so, return to step 32; otherwise, add the new node to the reverse tree and update the reverse tree.

[0117] Step 3 and 4: Determine whether there is a new connection path in the updated forward and reverse direction trees. If there is a new connection path, execute step 5; if there is no new connection path, determine whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than the total number of iterations, output path planning failure; if the number of sampling points is less than or equal to the total number of iterations, execute step 4;

[0118] Determine whether there is a connection path between the forward and reverse trees, and determine whether the distance between the updated node m' in the forward tree and the updated node n' in the reverse tree is less than or equal to the expansion node step T. If the distance between m' and n' is less than or equal to T, it means that the path between the node m' and the node n' is a connection path; otherwise, it means that the path between the node m' and the node n' is not a connection path;

[0119] If the current connection path is newly appeared after the forward and reverse trees are updated, then the current connection path is a new connection path. The number of sampling points is the total number of random points generated in step 32 and new nodes grown by the forward and reverse trees;

[0120] Step 4: Figure 4(a), Figure 4(b) and Figure 5 As shown in the figure, using the bias node generated in step 2, the sampling guidance strategy is used to guide the growth direction of the nodes in the positive and negative bidirectional trees after the update in step 3, and the positive and negative direction trees are updated again. The gravity elimination strategy is used to eliminate the gravity and repulsion generated by the bias point and obstacles, and then return to step 3, specifically:

[0121] Step 41: Determine whether the new node q generated in step 332 falls on the offset point q s (x b ,y b ) is the center of the circle and Arrange is the radius of the gravitational region; if yes, execute step 42; otherwise, execute step 33;

[0122] Among them, Arrange is the preset gravitational area radius;

[0123] Step 42: Determine whether the current new node q is the first one to fall on the current bias point q s (x b ,y b ) and the current new node q is the same as the current offset point q s (x b ,y b ) is there any obstacle on the line connecting them, if so, record the coordinates (x a ,y a ) execute step 43; otherwise, do not record the coordinates of the new node q and directly execute step 43;

[0124] Step 43: Get the bias point q s(x b ,y b )'s gravitational potential field function U att (q) and the repulsive potential field function U of the obstacle rep (q):

[0125]

[0126]

[0127] Among them, U att (q) is the potential field function of the new node q under the gravitational force of the offset point, η is the gravitational potential field gain coefficient, q s is the offset point position, q is the new node position, ρ(q,q s ) is point q to point q s Euclidean distance, arrange is the preset bias point located at the boundary value of the gravitational region, k is the repulsive potential field gain coefficient, q o is an obstacle, ρ(q,q o ) is point q to point q o The Euclidean distance, ρ0 is the repulsive boundary value, U rep (q) is the potential field function of the new node q subjected to the repulsive force of the obstacle.

[0128] Step 4. Use bias point q s (x b ,y b ) to obtain the gravitational gradient function of the bias point and the repulsive gradient function of the obstacle, specifically:

[0129]

[0130]

[0131] Among them, F att (q) is the gradient function of the gravitational force of the new node q on the bias point, F rep (q) is the gradient function of the repulsive force of the obstacle on the new node q;

[0132] Step 4 and 5: Use the gravitational gradient function of the offset point and the repulsive gradient function of the obstacle to obtain the resultant force F of the gravitational and repulsive forces on the new node q, and obtain the unit vector of the resultant force F:

[0133] First, the gravitational gradient function of the offset point and the repulsive gradient function of the obstacle are used to obtain the combined force F of the gravitational and repulsive forces on the new node q:

[0134] F=F att (q)+F rep (q)

[0135] Then, the Euclidean distance of the resultant force F of the gravitational force and the repulsive force on the new node q is obtained, and then the unit vector of the resultant force F is obtained using the Euclidean distance of the resultant force F;

[0136] Step 46: Determine whether the resultant force F is 0. If F is not equal to 0, generate a new node in the positive and negative direction tree in the direction of the unit vector of F, record it as q', and add q' to the positive and negative direction tree, and execute step 47; if the resultant force F is 0 and the unit vector cannot be generated, return to step 33;

[0137] Step 47: Determine the new node q' and the bias point (x b ,y b ) overlap; if so, execute step 49; otherwise, execute step 48;

[0138] Step 48: Get and The angle θ between them is used to obtain the current bias (x b ,y b ) to the edges of obstacles on both sides, respectively, and compare θ with the larger value of θ1 and θ2. If θ is larger than the larger value of θ1 and θ2, it means that node q' has passed the current offset point (x b ,y b ), then execute step 49, otherwise execute step 46;

[0139]

[0140] in, is the current bias point (x b ,y b ) to (x a ,y a ), is the current bias point (x b ,y b ) to the vector of the newly generated node, is the current bias point (x b ,y b ) to one of the edge points of the obstacle on the W side, is the current bias point (x b ,y b ) to the other edge point of the obstacle on the W side, is the current bias point (x b ,y b ) to one of the edge points of the obstacle on the W' side, is the current bias point (x b ,y b ) to the other edge point of the obstacle on the W' side, where W and W' are the directions of the obstacles on both sides of the offset point;

[0141] In this step, after generating the bias point in the map, it is necessary to further guide the node to generate at the bias point. To achieve this effect, the present invention introduces and improves the potential field function of the artificial potential field, assigning low potential energy to the bias point and high potential energy to the obstacle. Due to the limitations of the artificial potential field itself, multiple gravitational forces may occur during the generation of the node, and the resultant force of the repulsive force is zero, resulting in the node being unable to move and always being at the potential energy equilibrium point. To solve this problem, the potential field function is introduced into a region restriction. Only when the random node falls into the region, the obstacles and bias points corresponding to the region will generate high and low potential energy, so that the node can quickly find the bias point under the action of the resultant force.

[0142] This step is to assign gravity to the low potential field function and repulsion to the high potential field function, and to deduce the gradient functions of gravity and repulsion respectively. In the gravitational potential field function, the farther the random sampling point is from the bias point, the higher its potential energy, and the closer the distance is, the lower its potential energy is. Under the action of the potential energy difference, the newly generated node will gradually iterate towards the bias point. In the repulsive potential field function, the farther the random sampling point is from the obstacle, the lower its potential energy is, and the closer the distance is, the higher its potential energy is. Under the action of the potential energy difference, the newly generated node will iterate in the direction away from the obstacle. Under the action of the combined force of gravity and repulsion, the node can successfully find the position of the bias point. Since the map narrow distance recognition strategy may generate multiple bias points in the map, each bias point has a gravitational function of the artificial potential field, which greatly increases the possibility of the node falling at the potential field equilibrium point where the combined force of gravity and repulsion is equal to zero, making the node unable to iterate further. In order to prevent this from happening, some modifications were made to the original gravitational potential field function. The original gravitational potential field function did not have random sampling points q and bias points q s Distance restriction: If multiple bias points are generated in the cost map, each bias point will exert gravitational force on the node, and the node is likely to fall into a local optimal situation. Therefore, the present invention adds a constraint on |ρ(q,q s )|restrictions.

[0143] Step 49: Set the current offset point q s (x b ,y b ) and the potential field function of the obstacle are initialized to 0, then set node q = q' and return to step 43.

[0144] Whenever a node passes through a bias point, the artificial potential field function is initialized through the gravity elimination strategy to eliminate the gravitational repulsion and the bias point, thus preventing the sampling guidance strategy from affecting the further iteration of subsequent nodes.

[0145] Step 5: Use the forward and reverse direction trees obtained in step 3 to obtain the planned path of the mobile robot, specifically:

[0146] Step 51: Obtain the new connection path R of the forward and reverse direction trees obtained in step 3 and the cost value of the current connection path;

[0147] Step 52: compare the cost value of the current connection path R with the saved connection path cost value. If the cost value of the current connection path R is less than the saved connection path cost value, update the saved connection path cost value to the cost value of the current connection path, and then execute step 53; otherwise, directly execute step 53;

[0148] If the current connection path is the first connection path, the previously saved connection path cost value is the initial value and is set to positive infinity;

[0149] Step 53: Determine whether the number of sampling points of the forward and reverse direction trees outputted in step 3 is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, output the connection path corresponding to the recorded cost value as the planned path; if the number of sampling points is less than the total number of iterations, return to step 32.

[0150] In this step, it is determined whether the bidirectional random tree is connected. If no connection path is found, the iteration continues. If one or more paths are found, the sum of the cost values ​​of all paths is calculated to find a path with the minimum cost value for branch pruning optimization. This method not only solves the problem that the artificial potential field is prone to fall into the local optimum, but also speeds up the efficiency of the Informed-RRT* algorithm through narrow channels and greatly increases the number of effective sampling points.

[0151] Embodiment: In order to verify the beneficial effects of the present invention, the following experiments were carried out in this embodiment:

[0152] This embodiment uses a grid map model to study the path planning of a mobile robot. The simulation environment of this embodiment is a 100×100 grid map, where white represents the free area and black represents the obstacle area. The green hollow circle is the starting point, which is located in the upper left corner of the map. The red hollow circle is the end point, which is located in the lower right corner of the map. The present invention aims to improve the efficiency of the Informed-RRT* algorithm when passing through narrow channels, and to show the improvement effect of the algorithm before and after the improvement by comparing a variety of data. Table 3 shows the description of the parameters of this embodiment.

[0153] Table 3 Parameter description

[0154]

[0155] Comparison of the operation efficiency of the proposed algorithm IRRT-APF and the existing Informed-RRT* path:

[0156] Matlab simulation is used to compare the paths before and after improvement. Figure 7 The figure shows the path planned by the existing Informed-RRT*. The pink sampling points in Figure 6(a) represent the forward RRT random tree, and the yellow sampling points represent the reverse RRT tree. Figure 6(b) shows the path planned by the present invention. The pink sampling points in the figure represent the forward RRT random tree, the green sampling points represent the reverse RRT tree, and the black sampling points represent the sampling points guided to the bias point under the influence of the artificial potential force. By comparing the two simulation figures, it can be clearly seen that the optimized algorithm is significantly more efficient than before optimization, the number of sampling points is sharply reduced, and the path planning efficiency is effectively improved. In order to avoid the randomness of the simulation results, the time consumed by the algorithm to generate the path is recorded multiple times for narrow-distance maps as shown in Table 4. The comparison of the path generation time before and after optimization is shown in Table 4. Figure 7 As shown;

[0157] Table 4 Time consumed for generating paths (s)

[0158]

[0159] Before the algorithm was improved, the average time was 116.02 seconds, the variance between data was 195.85, and the standard deviation was 13.99. After the algorithm was improved, the average time was 19.72 seconds, the variance between data was 59.51, and the standard deviation was 7.714. Through the above data analysis, the running time of the algorithm was shortened by 86.38%, the running speed was increased by 573.98%, and the stability of the algorithm operation was improved by 39.35%.

[0160] In order to avoid the factors of map specificity, six different types of narrow-distance maps were reselected, and the map types were from simple to complex. Each map was run nine times to compare the average operating efficiency of the algorithm before and after optimization. The average time for generating a path is shown in Table 5. The average time required to generate a path in different maps is compared in Table 5. Figure 8 As shown;

[0161] Table 5 Average time to generate a path (s)

[0162]

[0163] Through the above data analysis, in different narrow-distance maps, the running time of the improved algorithm is shortened by an average of about 84.31%, and the running speed is increased by an average of 557.27%. According to the results of the above data analysis, the running efficiency of the improved algorithm is significantly higher than that of the algorithm before the improvement, which proves the feasibility of the improved Informed_RRT* algorithm of the present invention.

[0164] This embodiment is simulated in six different types of narrow-distance maps from simple to complex, and the average path lengths of the paths found in different maps are recorded to detect the improvement effect before and after the algorithm is improved. Fig. 9 The figure shows the average path lengths in six different narrow-spacing maps. Table 6 provides detailed data on the average path lengths.

[0165] Table 6 Average path length of paths

[0166]

[0167]

[0168] from Fig.10 It can be clearly seen from Table 6 that the solid red line is always below the dotted blue line, and the path of the improved algorithm is shorter than that of the original algorithm in each narrow-distance map. This shows that no matter what kind of narrow-distance map, the average path length of the optimized algorithm is always shorter than that of the algorithm before optimization, which proves the universality of the optimized Informed_RRT* algorithm of the present invention.

[0169] In order to compare the iterative convergence ability of the algorithm before and after optimization, the algorithms before and after optimization are placed in the same map to record the number of iterations and the generated path length data. The algorithms before and after optimization are observed. As the number of iterations increases, the convergence of the algorithm length is shown in Table 7. Fig.10 As shown;

[0170] Table 7 Comparison of path lengths before and after algorithm improvement

[0171]

[0172] From the above data, it can be easily seen that the optimized algorithm can generate a path for the first time when the 65th sampling is performed, and because the generation of sampling points is manually guided before, the invalid sampling points on the map are reduced. During the pruning process, the path length converges faster. When iterating to the 109th sampling, the path at this time is the best path with a length of 88. The method of the present invention generates a path for the first time when the 111th sampling is performed, and until iterating to the 173rd sampling, the path at this time is the best path with a length of 133. The present invention not only has a significant optimization effect on the running time, but also has further enhanced the ability to search for the optimal path length and path convergence.

[0173] The present invention verifies the remarkable effect of the algorithm before and after the improvement by comparing the path image, the number of effective sampling points, the optimal path length and the average path length before and after the improvement. The present invention not only effectively improves the ability of random sampling points to pass through narrow-distance areas and improves the operating efficiency of path planning, but also further strengthens the ability to find the optimal solution and the path convergence effect.

Claims

1. A path planning method based on Informed-RRT*, characterized in that: The specific process of the method is: Step 1: Set the path planning parameters and initialize the starting node cost value to 0 and the target node cost value to 0; The path planning parameters include: starting node coordinates, target node coordinates, extended node step size T, starting sub-node coordinates, target sub-node coordinates, sampling area and total number of iterations; Step 2: Obtain a cost map of the study area, use the cost map of the study area to identify all narrow-distance areas in the study area, generate bias points, and obtain a cost map of the study area after generating the bias points; Step 3: Use the starting node coordinates, starting sub-node coordinates, target node coordinates and target sub-node coordinates to establish a forward and reverse bidirectional tree, and generate a new node to update the forward and reverse bidirectional tree, and determine whether the updated forward and reverse direction tree has a new connection path. If there is a new connection path, execute step 5; if there is no new connection path, determine whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, output path planning failure; if the number of sampling points is less than the total number of iterations, execute step 4; Step 4: Use the bias node generated in step 2 to guide the growth direction of the nodes in the positive and negative bidirectional trees updated in step 3, update the positive and negative direction trees again, then eliminate the gravitational force generated by the bias point and the repulsive force generated by the obstacle, and return to step 3; Step 5: Use the forward and reverse direction trees obtained in step 3 to obtain the planned path of the mobile robot.

2. The path planning method based on Informed-RRT* according to claim 1, characterized in that: The step 2 of obtaining the study area cost map, using the study area cost map to identify all narrow-distance areas in the study area, and generating bias points, to obtain the study area cost map after generating the bias points, is specifically as follows: Step 21. Obtain a map of the study area, divide the map into grids according to a preset number of grids, use the upper left corner of the map of the study area as the origin of the coordinate axis, and set the horizontal and vertical directions of the map of the study area as the x-axis and y-axis directions; Step 22, set the cost value of the grid containing obstacles in the study area map to 1, and set the cost value of the free area grid without obstacles to 0, so as to obtain the study area cost map; The grid containing the obstacle area is the obstacle area; Step 2 and 3: Starting from the origin of the coordinate axis, traverse the study area cost map in the order of x-direction and y-direction to set the offset point.

3. The path planning method based on Informed-RRT* according to claim 2, characterized in that: In the steps 2 and 3, the study area cost map is traversed in the order of first traversing in the x direction and then traversing in the y direction from the origin of the coordinate axis, so as to set the bias point and obtain the study area cost map after generating the bias point, specifically: Step 231: Taking each column of the cost map of the research area traversed in the x direction as an example, initialize the current column number i=1; initialize the element number j=1 in the i-th column row, and initialize l=1; Step 232: Get the jth element value and j+1th element value of the i-th column in the cost map of the study area. If the jth element value is 1 and the j+1th element value is 0, go to step 233; Otherwise, execute step 237; Step 233: Get the value of the j+lth element in the i-th column. If the value of the j+lth element is 0 and the value of the j+l+1th element is 1, then record the position A corresponding to the j+1th element in the i-th column. i,j+1 The position A corresponding to the value of the j+lth element in the i-th column i,j+l , then execute steps 2, 3 and 4; Otherwise, execute steps 2, 3, and 6; Step 2, 3, 4, judge A i,j+1 With A i,j+l Are they the same position coordinates? If they are the same position coordinates, then at position A i,j+1 Set the bias point, then execute steps 2, 3, and 7; otherwise, obtain A i,j+1 and A i,j+l Distance L i , execute steps 235; Step 235: L i Compared with the preset narrow distance parameter threshold Narrow, if L i >Narrow, then execute steps 2, 3, and 7; if L i ≤Narrow, then A i,j+1 and A i,j+l The area between is the narrow spacing area. i,j+1 and A i,j+l Generate an offset point at the midpoint position, and then execute steps 2, 3, and 7; Step 236: Determine whether j+l+1 is less than the total number of elements J in the current column. If j+l+1<J, set l=l+1 and execute step 233; Otherwise, execute step 238; Step 237: Determine whether the current element index j is less than the total number of elements J in the current column. If j < J, set j = j + 1 and return to step 232; Otherwise, execute step 238; Step 238: Determine whether i is less than N. If i<N, set i=i+1 and return to step 232; Otherwise, execute step 239; Where N is the total number of columns in the costmap of the study area; Step 239. Determine whether there are adjacent bias points in each column. If there are adjacent bias points, obtain the starting bias point coordinate a and the ending bias point coordinate b in the adjacent bias points, obtain the midpoint coordinates of a and b, delete the current adjacent bias points, and use the midpoint coordinates of a and b as the bias point, and output the study area cost map after generating the bias point; otherwise, directly output the study area cost map after generating the bias point.

4. The path planning method based on Informed-RRT* according to claim 3, characterized in that: In the step 3, the forward and reverse bidirectional trees are established by using the starting node coordinates, the starting sub-node coordinates, the target node coordinates and the target sub-node coordinates, and a new node is generated to update the forward and reverse bidirectional trees, and it is determined whether the updated forward and reverse direction trees have a new connection path. If there is a new connection path, step 5 is executed; if there is no new connection path, it is determined whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, the output path planning fails; if the number of sampling points is less than the total number of iterations, step 4 is executed, which is specifically: Step 31: Use the starting node coordinates, the starting sub-node coordinates, the target node coordinates and the target sub-node coordinates to establish a positive and negative bidirectional tree; Step 32: Generate a new random point in the sampling area; Step 33, obtain the nodes m and n closest to the newly generated random point in the forward and reverse direction trees respectively, and update the forward and reverse direction trees using the nodes m, n and the newly generated random point; Step 3 and 4: Determine whether there is a new connection path in the updated forward and reverse direction trees. If there is a new connection path, execute step 5; if there is no new connection path, determine whether the number of sampling points is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, output path planning failure; if the number of sampling points is less than the total number of iterations, execute step 4; The number of sampling points is the total number of random points generated in step 32 and the number of new nodes grown by the forward and reverse trees.

5. The path planning method based on Informed-RRT* according to claim 4, characterized in that: In the step 33, the nodes m and n closest to the newly generated random point are obtained on the forward and reverse direction trees respectively, and the forward and reverse direction trees are updated using the nodes m, n and the newly generated random point, specifically: Step 331. Obtain the nodes m and n closest to the newly generated random point in the forward and reverse direction trees respectively, and obtain the distance L between the newly generated random point and the node m m , the distance L between the newly generated random point and node n n , judge L m , L n The relationship between them and the step length T of the expansion node respectively; If L m <T, the newly generated random point is used as a new node, and then step 332 is executed; otherwise, the positive direction tree grows according to the node expansion step T to obtain a new node, and then step 332 is executed; If L n <T, the newly generated random point is used as a new node, and then step 332 is executed. Otherwise, the reverse tree grows according to the node expansion step T to obtain a new node, and then step 332 is executed; Step 332: Obtain whether there is an obstacle area between the new node and the nearest node in the positive direction tree. If so, return to step 32; Otherwise, add the new node to the forward direction tree and update the forward direction tree; Get whether there is an obstacle area between the new node and the nearest node in the reverse direction tree. If so, return to step 32; Otherwise, add the new node to the inverse tree and update the inverse tree.

6. The path planning method based on Informed-RRT* according to claim 5, characterized in that: Determining whether there is a new connection path in the updated forward and reverse direction trees in step three or four is specifically: determining whether the distance between the node m' in the updated forward direction tree and the node n' in the updated reverse direction tree is less than or equal to the expansion node step length T, if the distance between m' and n' is less than or equal to T, it means that the path between the node m' and the node n' is a connection path; otherwise, it means that the path between the node m' and the node n' is not a connection path; If the current connection path is newly appeared after the positive and negative direction trees are updated, the current connection path is the new connection path.

7. The path planning method based on Informed-RRT* according to claim 6, characterized in that: In step 4, the offset node generated in step 2 is used to guide the growth direction of the nodes in the positive and negative bidirectional trees after the update in step 3, and the positive and negative direction trees are updated again, and then the gravitational force generated by the offset point and the repulsive force generated by the obstacle are eliminated, and then the process returns to step 3, specifically: Step 41: Determine whether the new node q generated in step 332 falls on the offset point q s (x b ,y b ) is the center of the circle and Arrange is the radius of the gravitational region; if yes, execute step 42; otherwise, execute step 33; Among them, Arrange is the preset gravitational area radius; Step 42: Determine whether the current new node q is the first one to fall on the current bias point q s (x b ,y b ) and the current new node q is the same as the current offset point q s (x b ,y b ) is there any obstacle on the line connecting them, if so, record the coordinates (x a ,y a ) and execute step 43; otherwise, do not record the coordinates of the new node q and directly execute step 43; Step 43: Get the bias point q s (x b ,y b )'s gravitational potential field function U att (q) and the repulsive potential field function U of the obstacle rep (q); Step 4. Use bias point q s (x b ,y b ) to obtain the gravitational gradient function F at the bias point att (q) and the repulsive gradient function F of the obstacle rep (q); Step 4 and 5: Use the gravitational gradient function of the offset point and the repulsive gradient function of the obstacle to obtain the resultant force F of the gravitational and repulsive forces on the new node q, and obtain the unit vector of the resultant force F: First, the gravitational gradient function of the offset point and the repulsive gradient function of the obstacle are used to obtain the combined force F of the gravitational and repulsive forces on the new node q: F=F att (q)+F rep (q) Then, the Euclidean distance of the resultant force F of the gravitational force and the repulsive force on the new node q is obtained, and then the unit vector of the resultant force F is obtained using the Euclidean distance of the resultant force F; Step 46: Determine whether the resultant force F is 0. If F is not equal to 0, generate a new node in the positive and negative direction tree in the direction of the unit vector of F, record it as q', and add q' to the positive and negative direction tree, and execute step 47; if the resultant force F is 0 and the unit vector cannot be generated, return to step 33; Step 47: Determine the new node q′ and the bias point q s (x b ,y b ) overlap; if so, execute step 49; otherwise, execute step 48; Step 48: Get and The angle θ between them is used to obtain the current bias (x b ,y b ) to the edges of obstacles on both sides respectively, and compare θ with the larger value of θ1 and θ2. If θ is larger than the larger value of θ1 and θ2, execute step 49, otherwise execute step 46; Step 49: Set the current offset point q s (x b ,y b ) and the potential field function of the obstacle are set to 0, then set the node q = q′ and return to step 43.

8. The path planning method based on Informed-RRT* according to claim 7, characterized in that: Obtaining the bias point q in step 43 s (x b ,y b )'s gravitational potential field function U att (q) and the repulsive potential field function U of the obstacle rep (q), specifically: Among them, U att (q) is the potential field function of the sampling point q under the gravitational force, η is the gravitational potential field gain coefficient, q s is the bias point position, q is the sampling point position, ρ(q,q s ) is point q to point q s Euclidean distance, arrange is the preset bias point located at the boundary value of the gravitational region, k is the repulsive potential field gain coefficient, q o is an obstacle, ρ(q,q o ) is point q to point q o The Euclidean distance, ρ0 is the repulsive boundary value, U rep (q) is the potential field function of the repulsive force on the sampling point q.

9. The path planning method based on Informed-RRT* according to claim 8, characterized in that: The use of the bias point (x b ,y b ) to obtain the gravitational gradient function F at the bias point att (q) and the repulsive gradient function F of the obstacle rep (q), specifically: Among them, F att (q) is the gradient function of the gravitational force on the sampling point q, F rep (q) is the gradient function of the repulsive force at sampling point q.

10. The path planning method based on Informed-RRT* according to claim 9, characterized in that: The forward and reverse direction trees obtained in step 3 are used in step 5 to obtain the planned path of the mobile robot, specifically: Step 51: Obtain the new connection path R of the forward and reverse direction trees obtained in step 3 and the cost value of the current connection path; Step 52: compare the cost value of the current connection path R with the saved connection path cost value. If the cost value of the current connection path R is less than the saved connection path cost value, update the saved connection path cost value to the cost value of the current connection path, and then execute step 53; otherwise, directly execute step 53; The initial value of the saved connection path cost is positive infinity; Step 53: Determine whether the number of sampling points of the forward and reverse direction trees outputted in step 3 is greater than the total number of iterations; if the number of sampling points is greater than or equal to the total number of iterations, output the connection path corresponding to the recorded cost value as the planned path; if the number of sampling points is less than the total number of iterations, return to step 32.

Images