CN113934206B - Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium - Google Patents
Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium Download PDFInfo
- Publication number
- CN113934206B CN113934206B CN202110832083.0A CN202110832083A CN113934206B CN 113934206 B CN113934206 B CN 113934206B CN 202110832083 A CN202110832083 A CN 202110832083A CN 113934206 B CN113934206 B CN 113934206B
- Authority
- CN
- China
- Prior art keywords
- node
- point
- nearest
- path planning
- new
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000003860 storage Methods 0.000 title claims abstract description 12
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 136
- 230000002457 bidirectional effect Effects 0.000 claims abstract description 13
- 238000005070 sampling Methods 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 14
- 238000004519 manufacturing process Methods 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 9
- 230000008569 process Effects 0.000 abstract description 8
- 238000011156 evaluation Methods 0.000 abstract description 6
- 230000006870 function Effects 0.000 description 16
- 238000012360 testing method Methods 0.000 description 14
- 238000010586 diagram Methods 0.000 description 13
- 238000002474 experimental method Methods 0.000 description 13
- 238000005457 optimization Methods 0.000 description 6
- 230000006872 improvement Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000000750 progressive effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 235000015107 ale Nutrition 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
The application discloses a mobile robot path planning method, a mobile robot path planning device, computer equipment and a storage medium, wherein the mobile robot path planning method comprises the following steps: making a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively; selecting a point on the connection as a third node; and (4) carrying out path planning between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm. The method and the device ensure that the third node is positioned on the connecting line between the starting point and the target point, so that the calculation efficiency is improved while the path is prevented from being bent; by means of the target deflection strategy, the new node N is avoided new The cost evaluation calculation of the method is performed, and the calculation efficiency is further improved while the planning path is optimized; in the expansion process of two opposite search trees, when one search tree encounters an obstacle, the other search tree is expanded, and the search tree is quickly separated from the obstacle by exchanging expansion, so that the path tortuosity is avoided to the greatest extent.
Description
Technical Field
The present disclosure relates to the field of path planning technologies, and in particular, to a method, an apparatus, a computer device, and a storage medium for path planning of a mobile robot.
Background
Mobile robots have been widely used in various fields for decades. Path planning is a key technology and a significant challenge for mobile robots. Path planning requires searching for an optimal or sub-optimal path from a given starting point to a target point according to evaluation criteria (e.g. planning time, path length, etc.). The path planning may be divided into a global path planning where the environmental information is known and a local path planning where the environmental information is unknown or partially known.
Among the sampling-based path planning algorithms, the most widely used is the fast-explore search tree RRT (Rapidly exploring Random Trees) algorithm. The algorithm uniformly samples in the configuration space and has the characteristic of probability completeness. Because this approach samples randomly in space, the path generated by the algorithm tends to be less than optimal or suboptimal. Some studies have improved RRT algorithms based on their ideas.
In order to increase the search speed of the spanning tree, a two-way exploration RRT-Connect algorithm (shown in figure 1) is proposed in a high-efficiency method (RRT-Connect: an Efficient Approach to Single-Query Path Planning; proceedings) of RRT-Connect: single query path planning by LaValle et al, and the algorithm is respectively expanded from a starting point to an ending point and two random trees from the ending point to the starting point in a configuration space, so that the search efficiency is further accelerated. Kang Ge (Kang) et al proposed an Improved RRT-connection algorithm (Improved RRT-Connect Algorithm Based on Triangular Inequality for Robot Path Planning) for robot path planning based on triangle inequality by using the principle of triangle inequality, and experiments were performed in eight environments, which is 16% shorter than the path length of the original RRT-Connect algorithm.
The Chinese patent with publication number CN111238518A discloses an intelligent agricultural vehicle path planning method based on an improved RRT algorithm, a midpoint is calculated according to the positions of a starting point qstart and a target point qgol, and whether the midpoint belongs to a free area is judged; if the midpoint does not belong to the free region, the iteration number is set and a specific step length is set, and the iteration is circulated until the midpoint is determined. By constructing the search path, four random trees which are searched simultaneously are formed. The scheme uses the improved RRT-connect to obtain a search method of four trees, but the optimization degree of the path planning result achieved by the search mode is still not high enough, and the iterative operation is complex to influence the efficiency of the algorithm.
The Chinese patent with publication number of CN112904869A discloses a multi-tree RRT-based unmanned boat weighted iterative path planning method and equipment, the technical scheme is improved based on RRT, in the extending process, a node list of a random tree is used as a guide to reconstruct the RRT random tree, and meanwhile, a father node reconnection line is selected in consideration of angle cost and length cost. In the node searching process, the scheme fully considers the angle cost and the length cost, and influences the working efficiency of the algorithm.
In order to obtain a better path, the prior art is improved based on an RRT algorithm or an RRT-Connect algorithm, and as the technical schemes disclosed in the two patents, the former path planning result is not good enough and iterative calculation is adopted; the latter uses an angle cost and a length cost evaluation, and also performs iterative calculation. In order to obtain a better path, some technical solutions in the prior art change the selection mode of the parent node, and adopt a cost function to select the node with the minimum cost in the neighboring area of the expansion node as the parent node, so that the algorithm achieves progressive optimization. But the iteration speed is slow, sacrificing the planning time.
Disclosure of Invention
In view of the foregoing, it is desirable to provide a mobile robot path planning method.
The mobile robot path planning method comprises the following steps:
making a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively;
selecting a point on the connection as a third node;
and (4) carrying out path planning between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm.
Optionally, when no obstacle exists at the midpoint of the connection line, the third node is the midpoint of the connection line; and when an obstacle exists at the midpoint of the connecting line, the third node is a point which is close to the midpoint of the connecting line and does not exist an obstacle.
Optionally, the length of the connection line is set to be L, and the distance between the third node and the midpoint is not greater than L/4.
Optionally, when an obstacle exists at the midpoint of the connection line, a distance point L/2n from the midpoint is selected as the third node, n is a natural number not less than 2, and n is selected in a gradually increasing manner.
Optionally, the bidirectional fast random search tree algorithm generates a new node by adopting a target bias strategy, wherein the target bias strategy is as follows: from the current point N of the search tree nearest By connecting new nodes N new Toward target point X of search tree end Continuously expanding and extending; the current point N nearest And target point X end The angle formed by the connecting line and the transverse axis is used for correcting and determining the new node N new Is a position of (c).
Optionally, the new node N new The following formula is satisfied:
x＝N nearest (x)+Dcosθ+kcosα+(1-k)sinα；
y＝N nearest (y)+Dsinθ+ksinα+(1-k)cosα；
wherein:
(x, y) is a new node N new Coordinates of (c);
N nearest (x) For the current point N nearest Is the abscissa of (2);
N nearest (y) is the current point N nearest Is the ordinate of (2);
N rand random sampling points of a rapid random search tree algorithm;
d is N rand And N nearest A relatively small value of the two-point spacing and the fixed step E;
θ is the sampling point N rand And the current point N nearest An angle formed by the connecting line and the transverse shaft;
alpha is the current point N nearest And target point X end An angle formed by the connecting line and the transverse axis;
k is a constant.
Optionally, when expanding, any one search tree uses the current point N of another search tree nearest As the target point X end When one of the search trees encounters an obstacle, the other search tree is expanded.
The application also provides a mobile robot path planning device, which comprises:
the first module is used for manufacturing a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively;
a second module for selecting a point on the connection as a third node;
and the third module is used for planning paths between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm.
The application also provides a computer device comprising a memory and a processor, wherein the memory stores a computer program, and the processor realizes the steps of the mobile robot path planning method when executing the computer program.
A computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the mobile robot path planning method described herein.
The mobile robot path planning method has at least one of the following effects:
the third node is ensured to be positioned on a connecting line between the starting point and the target point, so that the calculation efficiency is improved while the path is prevented from being bent;
by means of the target deflection strategy, the new node N is avoided new The cost evaluation calculation of the method is performed, and the calculation efficiency is further improved while the planning path is optimized;
in the expansion process of two opposite search trees, when one search tree encounters an obstacle, the other search tree is expanded, and the search tree is quickly separated from the obstacle by exchanging expansion, so that the path tortuosity is avoided to the greatest extent.
Drawings
FIG. 1 is a schematic diagram of a prior art path plan for a bi-directional fast random search tree algorithm;
FIG. 2 is a schematic diagram of a path plan of a bi-directional fast random search tree algorithm in the prior art;
FIG. 3 is a schematic diagram illustrating a third node selection of a mobile robot path planning method according to an embodiment of the present application;
FIG. 4 is a schematic diagram illustrating a third node selection of a mobile robot path planning method according to an embodiment of the present application;
FIG. 5 is a schematic diagram of a third node selection flow of a mobile robot path planning method according to an embodiment of the present application;
FIG. 6 is a schematic diagram of a mobile robot path planning target bias strategy according to an embodiment of the present application;
FIG. 7 is a diagram showing the comparison test results of a path planning method of a mobile robot according to an embodiment of the present application and a path planning method according to the prior art under the condition of environment 1;
FIG. 8 is a diagram showing the comparison test results of a path planning method of a mobile robot according to an embodiment of the present application and a path planning method according to the prior art under the condition of environment 2;
FIG. 9 is a diagram showing the comparison test results of a path planning method of a mobile robot according to an embodiment of the present application and a path planning method according to the prior art under the condition of environment 3;
FIG. 10 is a diagram showing the comparison between the iteration number of the path planning method of the mobile robot in the present embodiment and the iteration number of the path planning method in the prior art in environments 1-3;
fig. 11 is an internal structural diagram of a computer device in an embodiment of the present application.
Detailed Description
In order to make the objects, technical solutions and advantages of the present application more apparent, the present application will be further described in detail with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the present application.
In the traditional technical scheme, a bidirectional rapid random search tree (RRT-Connect) algorithm is used for rapid detectionThe basis of the cable search tree RRT (Rapidly exploring Random Trees) algorithm makes two significant improvements: generating random trees from the starting state point and the ending state point respectively, and completing planning when the two trees are intersected, so that the searching speed of an algorithm is greatly improved; the node greedy expansion strategy is adopted, and when nodes are generated in each iteration, the algorithm tries to take the nearest node of another tree as the expansion direction of the tree, so that the two trees are intersected rapidly. Meanwhile, when the node is expanded, if no obstacle is encountered in the expansion direction, the algorithm continues to expand in the expansion direction. If an obstacle is encountered, the algorithm uses an exchange function to expand another random tree, so that the dilemma of the algorithm in local optimization is avoided to a great extent. Through the strategies, the search speed and the search efficiency of the RRT-Connect are greatly improved. FIG. 1 is a schematic diagram showing the RRT-Connect algorithm expansion process, referring to FIG. 1, wherein T1 and T2 are two random search trees of RRT-Connect, N earest A and N earest B is the current points of the two random search trees respectively; n (N) new A and N new B is the new node (next node) of two search trees respectively; n (N) rand A and N rand B are the sampling points of the two search trees respectively. The algorithm is initially that two trees taking a starting point and an ending point as root nodes are extended in a greedy mode, and the extension of the random tree to an unknown area is effectively reduced. However, the RRT-Connect is similar to the RRT algorithm, and although a feasible path can be obtained after multiple iterations, it cannot be guaranteed that the obtained path is optimal or suboptimal.
In the prior art, the above RRT-Connect is optimized, as shown in fig. 2, and an intermediate node is selected based on the expansion of the RRT-Connect algorithm, where the starting point of the RRT-Connect is q start Target point is q goal The intermediate selected node is q m . According to the technical scheme, four random trees are generated simultaneously to generate paths, and dynamic obstacle avoidance is performed based on a rolling optimization method, so that the path planning efficiency is improved. The technique preferentially selects q start And q goal The midpoint is q m . But when q start And q goal When an obstacle is present at the midpoint of (a),the technical proposal adopts the mode of adjusting step length to conduct iterative search until the midpoint q is determined m . But the technique first determines the midpoint q by iterative search m Resulting in reduced efficiency of iterative search; at the same time, the midpoint q determined by the technique m With a very high probability of not being at q start And q goal Resulting in a meandering of the resulting path.
In order to solve the above technical problems, referring to fig. 3, an embodiment of the present application provides a mobile robot path planning method, including: determining a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively; and making a connecting line of the starting point and the target point, and selecting a third node on the connecting line of the starting point and the target point. The first node is adjacent to the second node, the second node is adjacent to the third node, two opposite search trees are generated between every two adjacent nodes by adopting an RRT-Connect algorithm, four search trees are generated, the four search trees are continuously expanded, and mobile robot path planning is carried out, so that a final mobile robot planning path is obtained. In this embodiment, path meandering is avoided by ensuring that the third node is always in the connection of the starting point and the target point.
Referring to fig. 3-5, in one embodiment, a third node is selected on the start point and target point link. The method specifically comprises the following steps: the third node is selected at or near the midpoint of the connection between the starting point and the target point. Preferably, the midpoint of the connection between the starting point and the target point is the third node, and when an obstacle exists at the midpoint, a position close to the midpoint is selected as the third node.
Specifically, the length of the connection line between the starting point and the target point is L, the distance L/2n between the optional third node and the midpoint between the starting point and the connection line between the third node and the target point is a natural number not less than 2 (more than or equal to 2); the third node is selected in such a way that n gradually increases. When n takes any value, judging whether an obstacle exists in the area where the alternative third node is located, and if not, selecting the area as the third node; if present, incrementing the value of n again determines an alternative third node.
It will be appreciated that there are two alternatives for the third node when n takes on any value. If no obstacle exists in the area of the two alternative nodes, one of the nodes is randomly selected as a third node. For example, when n=2, the alternative third node is at a distance L/4 of the midpoint of the connection between the starting point and the target point.
In one embodiment, two candidate third nodes (alternatively, the distance L/2n between the third node and the midpoint of the connection between the starting point and the target point) are obtained based on the above method, and the obtained nodes are shown in fig. 4. The two nodes obtained are then judged, and if neither node is on an obstacle, one of them is randomly selected as the valid third node. If one of the two nodes is on the obstacle, the other node that is not on the obstacle is selected as the valid third node. If both nodes are generated on the obstacle, the third node of the two candidates is obtained by continuing to divide the two points toward the midpoint of the line between the starting point and the target point. Repeating the steps until an effective third node is obtained. The entire flow of generating the third node is shown in fig. 5.
In one embodiment, taking the configuration space as an example, two random trees are constructed from the starting point and the end point based on the RRT-Connect algorithm to optimize the path search efficiency. In the configuration space, the initial point (starting point) coordinates are Xstart (X1, y 1), and the final point (target point) coordinates are X end (x 2, y 2). When there is no obstacle between the starting point and the midpoint of the link of the target point, the midpoint of the link is taken as the third node, the midpoint of the link (x mid ,y mid ) The coordinates in the configuration space are obtained from the formula (1) as x mid Equation (2) obtains y mid The method is characterized by comprising the following steps:
x mid ＝(x 1 +x 2 )/2
y mid ＝(y 1 +y 2 )/2
wherein x1, x2, and y1, y2 are the abscissa of the start point and the end point in the configuration space and the ordinate of the start point and the end point in the configuration space, respectively;
generating the third node by the above method may be in two cases in the configuration space. The first case is that the generated position of the third node does not contain an obstacle, and the third node can be found by only performing one iteration, which is shown in fig. 3. After the third node is generated, the algorithm expands a total of four random trees including two random trees from the start point to the third node and two random trees from the third node to the end point. The searching speed of the proposed path planning algorithm is greatly improved.
The midpoint of the connection line is selected as the third node to be ideal. In practice, it is possible that the midpoint of the start to end link is just above the obstacle. In an embodiment, to continue to guarantee the iterative efficiency of the algorithm, we use the midpoint X of the above generation mid And on the basis of the above, the effective third node is continuously searched by adopting the bipartite idea. In particular, the starting point X is firstly taken start And a point X on the obstacle mid Approach on a wirePoint at (I)>As an alternative active node. At the same time in X mid And X end Is close to->Point at (I)>As a second alternative node. The generation formulas of the two alternative third nodes, such as formula (3)/(>And formula (4)/(>The following is shown:
wherein (x 1, y 1) and (x 2, y 2) are the start point coordinates and the end point coordinates, respectively, in the configuration space. Two candidate third nodes are obtained based on this method, the resulting nodes being shown in fig. 4. The candidate third node is selected as the third node as in the judgment method of fig. 5 described above.
In the prior art, the RRT-connect algorithm searches to obtain an optimized result, and the availability of the extended nodes of the search tree is evaluated by calculating the angle cost. For example, in order to obtain a better path, a parent node selection mode is changed, and a cost function is adopted to select a node with the minimum cost in the neighborhood of the expansion node as a parent node, so that the algorithm achieves progressive optimization. But these algorithms are slow to iterate, sacrificing planning time. For example, chinese patent publication No. CN112904869a, by calculating the weighted cost of the planned path, if it is higher than the preset expected cost, returning to step; if the cost is not higher than the preset expected cost or the preset maximum iteration number is reached.
Aiming at the technical problem of low iterative operation efficiency in the prior art, in one embodiment, referring to fig. 6, a target bias strategy for generating new nodes based on a bidirectional fast random search tree (RRT-Connect) algorithm is provided.
It will be appreciated that in the embodiment of the present application, there are two sets of four search trees that extend in opposite directions. For any search tree, the current point N is included in the expansion process nearest New node N new Target point X end . The target bias strategy described in this embodiment is based on the current point N nearest And target point X end The angle formed by the connecting line and the transverse axis (the angle smaller than or equal to 90 degrees) is corrected and determined to be a new node N new Is a position of (c).
It will be appreciated that in this embodiment, in determining the new node N new Previously, through the current point N nearest And target point X end The new node is re-planned by the formed angle so as to have bias to the end pointOrientation. Under ideal conditions, the search tree direction is continuously expanded and extended towards the target direction, and the prior art selects the iterative algorithm to evaluate the cost, so that the expansion and extension efficiency of the search tree is greatly reduced. In the present embodiment, however, the new node N is determined new So that N new The bias of the bias end point is provided, namely, the target bias strategy causes the random spanning tree to consider the direction of the target point every time a new node is generated, so that the overall growth direction of the spanning tree in the configuration space is always biased towards the target point. The improvement ensures that the searching efficiency of the path planning algorithm is stably improved (iterative operation is reduced as much as possible), and ensures the optimization degree of generating the planning path.
Specifically, taking the configuration space as an example, the current point N nearest And target point X end The angle formed by the connecting line and the transverse axis is used for correcting and determining the new node N new Comprises obtaining N new Is the new node N new The following formula is satisfied:
x＝N nearest (x) +Dcosθ+kcosα+ (1-k) sinα; (equation 5)
y＝N nearest (y) +Dsinθ+ksinα+ (1-k) cos α; (equation 6)
Wherein:
(x, y) is a new node N new Coordinates of (c);
N nearest (x) For the current point N nearest Is the abscissa of (2);
N nearest (y) is the current point N nearest Is the ordinate of (2);
N rand random sampling points of a rapid random search tree algorithm;
d is N rand And N nearest A relatively small value of the two-point spacing and the fixed step E;
θ is the sampling point N rand And the current point N nearest An angle formed by the connecting line and the transverse shaft;
alpha is the current point N nearest And target point X end An angle formed by the connecting line and the transverse axis;
k is a constant.
Specifically, k is a target deviation factor, and in order to obtain a better planned path, in one embodiment, the value of the constant k is 0.4-0.6; experiments prove that the effect of taking 0.4 from k is better.
In one embodiment, in order to make the RRT-Connect path planning algorithm search faster and search more efficient, the overall planning procedure of RRT-Connect path planning is improved by introducing the above-mentioned strategy of generating the third node and re-planning the new node. The random tree expansion generates an improved extension function pseudocode for the new node as shown in algorithm 1.
Wherein,
N rand sampling points for the search tree;
t is a random search tree;
the WithoutObstacle function is used for judging whether a point in the configuration space is at an obstacle or not;
the SampleFree function generates random points in the configuration space as sampling points;
the purpose of the search function is to find a distance sampling point N on the spanning tree rand The node closest to the node;
e is a fixed step length;
the Distance function is used for judging and generating Euclidean Distance between two nodes;
it is understood that D is N nearest And N rand Minimum value of distance and fixed step E and N nearest And N rand The distance between the two points is a first distance; the fixed step E is a second distance, and a value of which the first distance and the second distance are smaller is taken as a step D.
Min is a minimum function;
N new is new toThe formulas obtained by the horizontal and vertical coordinates of the nodes are shown in the embodiment columns and are not repeated;
add function is to Add new node N new To the search tree.
It is understood that the same parameters as those mentioned above related to the algorithm 1 may be further explained by the above embodiments, and other functions and parameters related to the algorithm 1 are code parameters commonly used by those skilled in the art, such as simple functions like if, ales, then, input, output, which are not described herein.
To obtain a more optimal planned path, in one embodiment, a switching expansion strategy is provided, comprising: when the search tree with opposite expansion is expanded, any one of the two search trees with the current point N of the other search tree is expanded in opposite directions nearest As the target point X end When one of the search trees encounters an obstacle, the other search tree is expanded.
It will be appreciated that the final goal of the search tree being expanded towards each other is to link the search trees being expanded towards each other, thereby forming a planned path. Therefore, the search trees expanding toward each other mutually correspond to the current point N of each other nearest As the target point X end . When any one search tree encounters an obstacle, the search tree encountering the obstacle is separated from the obstacle by expanding the other search tree, so that the path is further optimized, and the path is prevented from being bent.
In one embodiment, the algorithm for determining the third node, the target bias strategy (angle bias, or enhancement of the guiding force to the target point), and the exchange expansion strategy are applied simultaneously, and the total path planning flow of the improved algorithm is as shown in the algorithm 2 ImprovRRT-Connect pseudocode:
in the embodiment, the ImprovRRT-Connect is abbreviated as IRRT-Connect; a third node is first obtained by the function searchthird node, which is implemented by either equation (1) (2) or (3) (4). Then initializing four random spanning trees and adding believed initialization nodes, wherein T1 and T2 are two opposite random trees between the starting point and the third node, and T3 and T4 are two opposite random trees between the third node and the end point. In the expansion process, a ConnectWithoutObstacle function is used to judge whether an obstacle exists between the nodes on the spanning tree and the newly generated nodes, and if so, the new nodes are regenerated. In this algorithm, a greedy expansion strategy is also used, that is, when an obstacle is not encountered by expansion in a certain direction, expansion in the certain direction is continued until the obstacle is encountered or two trees are connected, and the greedy strategy is shown as 15 lines to 20 lines of pseudo code. In order to enhance the exploration efficiency of the algorithm, when two trees are expanded oppositely, if one tree obstacle meets the obstacle, the Swap function is used for exchange expansion, and the other tree is expanded to enable the algorithm to be quickly separated from the obstacle. When the set maximum iteration number N is exceeded, a feasible path is not found yet, namely the planning fails. Other functions and parameters involved in the algorithm 2 are code parameters commonly used by those skilled in the art, such as path, init initialization, variable i, return, and code parameters already explained in the algorithm 1, etc., and will not be described in detail herein.
In one embodiment, to demonstrate the effectiveness and superiority of the improved algorithm, the execution efficiency of RRT, RRT-Connect, and the improved RRT-Connect algorithm (IRRT-Connect) under three different circumstances is compared. Fig. 7a is a RRT algorithm test result, fig. 7b is a RRT-Connect algorithm test result, and fig. 7c is an IRRT-Connect test result (algorithm 2 described in an embodiment of the present application).
The algorithm language used for simulation is Python3.7, the hardware platform is a Window 10 operating system, the AMD Ryzen 4800U is 1.8GHz CPU, and the memory is 16GB. In the experimental process, the corresponding parameters of the three algorithms are kept the same, and the uniform fixed step length E is 0.8. In all three environments, the experimental simulated map has an abscissa range of (0, 50) meters and an ordinate range of (0, 30) meters. Fig. 7 shows the performance of three algorithms in environment 1. The set start point coordinates are (2, 2) in the configuration space, and the end point coordinates are (49,24). Since the path planning algorithm based on random sampling has randomness, we have performed 50 experiments on each algorithm, and the graph shows relatively good results for each algorithm in this environment. The original RRT algorithm shown in fig. 7 is executed in this environment for 0.062 seconds, with 405 iterations, and the resulting path length 65.39 meters. The original RRT-Connect algorithm shown in the figure has an execution time of 0.011 seconds, 83 iterations, and a path length of 59.68 meters in the modified environment. The last figure shows that the improved algorithm is performed in the environment, the whole path planning time is 0.007 seconds, the iteration number is only 15, and the path length is reduced to 54.84 meters. Table 1 shows the average index of three algorithm evaluations obtained after 50 experiments of the three algorithms in environment 1. By adding a reasonable third node algorithm, the iteration times of the RRT-Connect algorithm before improvement are reduced by 51%, the planning time is reduced by 36%, and the path length is shortened. By observing fig. 7 in combination with the experimental data, it can be seen that the iteration number of the improved algorithm is significantly reduced in the environment 1, and the path is smoother.
Table 1: average result of 50 experiments of three algorithms in environment 1
In one embodiment, to test the versatility of an algorithm in different environments, performance of the algorithm in different environments and at different coordinate points is attempted herein. The three algorithms behave in the environment 2 as shown in fig. 8. Fig. 8a is a RRT algorithm test result, fig. 8b is a RRT-Connect algorithm test result, and fig. 8c is an IRRT-Connect test result (algorithm 2 described in an embodiment of the present application).
In environment 2, the coordinates of the starting point and the target point are (2, 26) and (47,5), and the algorithm looks for a valid path from top left to bottom right. To illustrate the effectiveness of the third node presented herein, an obstacle is intentionally placed in environment 2 at the midpoint of the start coordinate to end coordinate connection such that the algorithm presented herein does not find an effective third node according to equations (1) (2) when first iterated, forcing the algorithm to continue iterating to find an effective third node according to equations (3) (4). As in environment 1, we also performed 50 experiments in environment 2, and the average results of the three algorithms obtained are shown in table 2. As can be seen from the data in table 2, the average planning time and the average iteration number of the improved algorithm are reduced by 36.8% and 66.8%, respectively, compared with the RRT-Connect, and the path length obtained on this basis is also reduced by about 4.84 meters. As can be seen from the average data in table 2 and the path planning effect graph of the three algorithms of fig. 8 in the second environment, the improved algorithm ultimately generates an effective third node and the resulting path is better than the other two algorithms.
Table 2: average result of 50 experiments of three algorithms in environment 2
In one embodiment, to test the effect of an algorithm executing in a complex environment, environment 3 is created that integrates the obstacle distribution and type of environments 1 and 2. The start point coordinates and the end point coordinates are set to (2, 2) and (49,24) in this environment. The experimental results under this environment are shown in fig. 9, where fig. 9a is the RRT algorithm test result, fig. 9b is the RRT-Connect algorithm test result, and fig. 9c is the IRRT-Connect test result (algorithm 2 described in an embodiment of the present application).
The RRT-Connect algorithm can not well smoothly transit past along the obstacle when encountering the obstacle, and the generated path is more tortuous after greedy expansion is carried out on tree exchange after the obstacle is encountered by the spanning tree. Although both the RRT-Connect algorithm and the improved algorithm use greedy expansion strategies, we introduce an expansion strategy of a new node for increasing the guiding force on the basis, so that the expansion direction is more biased towards the target point and has directivity. The resulting path is thus flatter and an effective path can be found quickly even after encountering an obstacle. The 50 average results of the three algorithms under environment 3 are shown in table 3. From the data, it can be seen that the improved algorithm presented herein is able to get a better path in a shorter time with a smaller number of iterations than the other two algorithms.
Table 3: average result of 50 experiments of three algorithms in environment 3
In one embodiment, to verify the stability of the algorithm, the results of the experiment were averaged 10 times, 20 times, 30 times, 40 times, 50 times for the three algorithms under three different environments, respectively. Three performance indexes including iteration times, planning time and path-finding length are compared to reflect the execution efficiency and quality of the algorithm. Fig. 10 (a) (b) shows the number of iterations of the three algorithms in three environments versus the seek time. The generation of an effective third node enables the algorithm to be greatly reduced in the number of iterations under both simple and complex environmental conditions, which also enables improved algorithm search speeds to be faster. Therefore, in order to cope with the road planning in a complex environment, such an improvement is necessary. Fig. 10 (c) shows that the path lengths of the three algorithms in three environments are compared, and the path lengths of the improved algorithm, whether the algorithm is found through a small number of experiments (10 times) or a large number of experiments (50 times), are shorter than those of the other two algorithms. This is possible thanks to our new node expanding method of re-planning the spanning tree, so that the new node generated by each iteration advances toward the target direction, and the searching speed of the algorithm is improved. With reference to fig. 10, it can be illustrated that the improved path planning algorithm has faster searching speed and better exploration efficiency in various environments.
Summarizing, path planning of mobile robots in complex environments is one of the key problems to be solved in the research field. The sampling-based path planning algorithm has the capabilities of simplicity and quick search and is good at solving the planning problem in a multidimensional space and a complex environment. Based on the RRT-Connect algorithm, a simple and efficient third node is generated in the configuration space through the concept of the dichotomy, and the third node is ensured to be on the connection line of the starting point and the target point, so that the algorithm is expanded by four trees, and the searching speed under the complex environment is accelerated. Redundant searching for reducing RRT-Connect algorithm; the generation of new nodes of the spanning tree is re-planned by increasing the guiding force (target deflection strategy), so that each expansion of the algorithm deflects to the direction of the target point, and the searching efficiency of the algorithm is accelerated. The method and the device ensure that a better planning path result is obtained (path tortuosity is avoided as much as possible), and meanwhile, the calculation efficiency is improved (iterative operation and cost evaluation are avoided as much as possible).
In order to verify the effectiveness and universality of the proposed algorithm, three environment maps with different complex conditions are constructed in the experiment, and the average performance index is obtained after 50 experiments are carried out. For the simpler environment 1, the number of iterations of the improved algorithm is reduced by 91% and 51% compared to the RRT and RRT-Connect algorithms, the seek time is reduced by 88% and 36%, and the resulting path length is reduced by 13% and 6%. The improved algorithm also exhibits excellent performance under more complex environmental 3 conditions, verifying the effectiveness of the improved algorithm herein.
The technical scheme provided by the application aims at the problem of path planning in a two-dimensional static environment, and it can be understood that the application scene of path planning can be a scene of planning of other plane paths such as land, ocean and the like; the path planning described in the application can be a simple planning route, is not limited to being used for mobile robot walking, and can also be used for other automatic or semi-automatic mechanical walking, such as agricultural machinery, marine unmanned boats and the like. It is understood that obstacle avoidance (including avoiding obstacles below or above the plane) path planning in or near a two-dimensional spatial plane (e.g., a plains zone) is within the scope of the present patent.
In one embodiment, a computer device is provided, which may be a terminal, and the internal structure thereof may be as shown in fig. 11. The computer device includes a processor, a memory, a network interface, a display screen, and an input device connected by a system bus. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface of the computer device is used for communicating with an external terminal through a network connection. The computer program is executed by a processor to implement a mobile robot path planning method. The display screen of the computer equipment can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer equipment can be a touch layer covered on the display screen, can also be keys, a track ball or a touch pad arranged on the shell of the computer equipment, and can also be an external keyboard, a touch pad or a mouse and the like.
In one embodiment, a computer device is provided comprising a memory and a processor, the memory having stored therein a computer program, the processor when executing the computer program performing the steps of: making a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively; selecting a point on the connection as a third node; and (4) carrying out path planning between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm.
It will be appreciated by those skilled in the art that the structure shown in fig. 11 is merely a block diagram of a portion of the structure associated with the present application and is not limiting of the computer device to which the present application applies, and that a particular computer device may include more or fewer components than shown, or may combine some of the components, or have a different arrangement of components.
In one embodiment, there is provided a mobile robot path planning apparatus including: a first module, a second module, and a third module, wherein: the first module is used for manufacturing a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively; a second module for selecting a point on the link as a third node; and the third module is used for planning paths between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm.
For specific limitations of the mobile robot path planning apparatus, reference may be made to the above limitations of the mobile robot path planning method, and no further description is given here. The above-described respective modules in the mobile robot path planning apparatus may be implemented in whole or in part by software, hardware, and a combination thereof. The above modules may be embedded in hardware or may be independent of a processor in the computer device, or may be stored in software in a memory in the computer device, so that the processor may call and execute operations corresponding to the above modules.
In one embodiment, a computer readable storage medium is provided having a computer program stored thereon, which when executed by a processor, performs the steps of: making a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively; selecting a point on the connection as a third node; and (4) carrying out path planning between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm.
Those skilled in the art will appreciate that implementing all or part of the above described methods may be accomplished by way of a computer program stored on a non-transitory computer readable storage medium, which when executed, may comprise the steps of the embodiments of the methods described above. Any reference to memory, storage, database, or other medium used in the various embodiments provided herein may include non-volatile and/or volatile memory. The nonvolatile memory can include Read Only Memory (ROM), programmable ROM (PROM), electrically Programmable ROM (EPROM), electrically Erasable Programmable ROM (EEPROM), or flash memory. Volatile memory can include Random Access Memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms such as Static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double Data Rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous Link DRAM (SLDRAM), memory bus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), among others.
The technical features of the above embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description. When technical features of different embodiments are embodied in the same drawing, the drawing can be regarded as a combination of the embodiments concerned also being disclosed at the same time.
The above examples merely represent a few embodiments of the present application, which are described in more detail and are not to be construed as limiting the scope of the invention. It should be noted that it would be apparent to those skilled in the art that various modifications and improvements could be made without departing from the spirit of the present application, which would be within the scope of the present application. Accordingly, the scope of protection of the present application is to be determined by the claims appended hereto.
Claims (8)
1. The mobile robot path planning method is characterized by comprising the following steps:
making a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively;
selecting a point on the connection as a third node;
and (3) carrying out path planning between every two adjacent nodes by adopting a bidirectional rapid random search tree algorithm, wherein the bidirectional rapid random search tree algorithm adopts a target deviation strategy to generate new nodes, and the target deviation strategy is as follows: from the current point N of the search tree nearest By connecting new nodes N new Toward target point X of search tree end Continuously expanding and extending; the current point N nearest And target point X end The angle formed by the connecting line and the transverse axis is used for correcting and determining the new node N new Is a position of (2);
the new node N new The following formula is satisfied:
x＝N nearest (x)+Dcosθ+kcosα+(1-k)sinα；
y＝N nearest (y)+Dsinθ+ksinα+(1-k)cosα；
wherein:
(x, y) is a new node N new Coordinates of (c);
N nearest (x) For the current point N nearest Is the abscissa of (2);
N nearest (y) is the current point N nearest Is the ordinate of (2);
N rand random sampling points of a rapid random search tree algorithm;
d is N rand And N nearest A relatively small value of the two-point spacing and the fixed step E;
θ is the sampling point N rand And the current point N nearest An angle formed by the connecting line and the transverse shaft;
alpha is the current point N nearest And target point X end An angle formed by the connecting line and the transverse axis;
k is a constant.
2. The mobile robot path planning method according to claim 1, wherein,
when no barrier exists at the midpoint of the connecting line, the third node is the midpoint of the connecting line;
and when an obstacle exists at the midpoint of the connecting line, the third node is a point which is close to the midpoint of the connecting line and does not exist an obstacle.
3. The mobile robot path planning method according to claim 2, wherein the length of the connection line is set to L, and the distance between the third node and the midpoint is not greater than L/4.
4. A mobile robot path planning method according to claim 3, characterized in that when there is an obstacle at the midpoint of the connection line, a distance point L/2n from the midpoint is selected as the third node, n is a natural number not less than 2, and n is selected in a gradually increasing manner.
5. The mobile robot path planning method according to claim 1, wherein when expanding, any one search tree uses the current point N of another search tree nearest As the target point X end When one of the search trees encounters an obstacle, the other search tree is expanded.
6. The mobile robot path planning device is characterized by comprising:
the first module is used for manufacturing a connecting line between a starting point and a target point, wherein the starting point and the target point are a first node and a second node respectively;
a second module for selecting a point on the connection as a third node;
the third module is used for planning paths between every two adjacent nodes by adopting a bidirectional fast random search tree algorithm, wherein the bidirectional fast random search tree algorithm adopts a target deviation strategy to generate new nodes, and the target deviation strategy is as follows: from the current point N of the search tree nearest By connecting new nodes N new Toward target point X of search tree end Continuously expanding and extending; the current point N nearest And target point X end The angle formed by the connecting line and the transverse axis is used for correcting and determining the new node N new Is a position of (2);
the new node N new The following formula is satisfied:
x＝N nearest (x)+Dcosθ+kcosα+(1-k)sinα；
y＝N nearest (y)+Dsinθ+ksinα+(1-k)cosα；
wherein:
(x, y) is a new node N new Coordinates of (c);
N nearest (x) For the current point N nearest Is the abscissa of (2);
N nearest (y) is the current point N nearest Is the ordinate of (2);
N rand random sampling points of a rapid random search tree algorithm;
d is N rand And N nearest A relatively small value of the two-point spacing and the fixed step E;
θ is the sampling point N rand And the current point N nearest An angle formed by the connecting line and the transverse shaft;
alpha is the current point N nearest And target point X end An angle formed by the connecting line and the transverse axis;
k is a constant.
7. Computer device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor, when executing the computer program, implements the steps of the mobile robot path planning method of any of claims 1 to 5.
8. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, realizes the steps of the mobile robot path planning method according to any one of claims 1 to 5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110832083.0A CN113934206B (en) | 2021-07-22 | 2021-07-22 | Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110832083.0A CN113934206B (en) | 2021-07-22 | 2021-07-22 | Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113934206A CN113934206A (en) | 2022-01-14 |
| CN113934206B true CN113934206B (en) | 2024-01-16 |
Family
ID=79274532
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110832083.0A Active CN113934206B (en) | 2021-07-22 | 2021-07-22 | Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113934206B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116652927A (en) * | 2022-02-21 | 2023-08-29 | 中兴通讯股份有限公司 | Path planning method and device and readable storage medium |
| CN114577217B (en) * | 2022-05-05 | 2022-07-29 | 季华实验室 | Route planning method, device, equipment and storage medium based on Von Lonouh graph |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109990796A (en) * | 2019-04-23 | 2019-07-09 | 成都信息工程大学 | Intelligent vehicle paths planning method based on two-way extension random tree |
| CN111735465A (en) * | 2019-04-24 | 2020-10-02 | 北京京东尚科信息技术有限公司 | Path planning method and device, computer system and computer readable medium |
| CN112462785A (en) * | 2020-12-04 | 2021-03-09 | 厦门大学 | Mobile robot path planning method and device and storage medium |
| CN112650256A (en) * | 2020-12-30 | 2021-04-13 | 河南大学 | Improved bidirectional RRT robot path planning method |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101667029B1 (en) * | 2009-08-10 | 2016-10-17 | 삼성전자 주식회사 | Method and apparatus of path planing for a robot |
-
2021
- 2021-07-22 CN CN202110832083.0A patent/CN113934206B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109990796A (en) * | 2019-04-23 | 2019-07-09 | 成都信息工程大学 | Intelligent vehicle paths planning method based on two-way extension random tree |
| CN111735465A (en) * | 2019-04-24 | 2020-10-02 | 北京京东尚科信息技术有限公司 | Path planning method and device, computer system and computer readable medium |
| CN112462785A (en) * | 2020-12-04 | 2021-03-09 | 厦门大学 | Mobile robot path planning method and device and storage medium |
| CN112650256A (en) * | 2020-12-30 | 2021-04-13 | 河南大学 | Improved bidirectional RRT robot path planning method |
Non-Patent Citations (4)
| Title |
|---|
| Chen JG,etal..Improved RRT-Connect Based Path Planning Algorithm for Mobile Robots.IEEE Access.2021,第9卷145988-145999. * |
| 一种改进RRT算法在路径规划中的应用研究;成怡 等;控制工程(03);161-165 * |
| 基于改进RRT的路径规划算法;刘晓倩;张辉;王英健;;自动化技术与应用(05);100-104 * |
| 基于改进渐进最优的双向快速扩展随机树的移动机器人路径规划算法;王坤 等;计算机应用;第39卷(第5期);1312-1317 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113934206A (en) | 2022-01-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113934206B (en) | Mobile robot path planning method, mobile robot path planning device, computer equipment and storage medium | |
| CN111504325B (en) | Global path planning method based on weighted A-algorithm of enlarged search neighborhood | |
| CN112677153B (en) | Improved RRT algorithm and industrial robot path obstacle avoidance planning method | |
| Akgun et al. | Sampling heuristics for optimal motion planning in high dimensions | |
| CN108413963B (en) | Self-learning ant colony algorithm-based strip robot path planning method | |
| CN112985408B (en) | Path planning optimization method and system | |
| CN112286202B (en) | Mobile robot path planning method for non-uniform sampling FMT | |
| CN113359746A (en) | Path planning method and device based on improved bidirectional RRT and Dijkstra fusion algorithm | |
| Peng et al. | Robot path planning based on improved A* algorithm | |
| CN110196602A (en) | The quick underwater robot three-dimensional path planning method of goal orientation centralized optimization | |
| CN109931943B (en) | Unmanned ship global path planning method and electronic equipment | |
| CN114721401A (en) | Efficient path planning method based on improved A-algorithm | |
| CN113485369A (en) | Indoor mobile robot path planning and path optimization method for improving A-x algorithm | |
| CN112902971B (en) | Robot motion trajectory calculation method based on Gaussian sampling and target deviation guidance and based on fast-expansion random tree algorithm, electronic equipment and storage medium | |
| CN112229419A (en) | Dynamic path planning navigation method and system | |
| Shivashankar et al. | Real-time planning for covering an initially-unknown spatial environment | |
| CN110954124A (en) | Adaptive path planning method and system based on A-PSO algorithm | |
| CN116203975A (en) | Robot path planning method | |
| CN113359775A (en) | Dynamic variable sampling area RRT unmanned vehicle path planning method | |
| Jiang et al. | Global path planning of mobile robot based on improved JPS+ algorithm | |
| CN115195706A (en) | Parking path planning method and device | |
| CN113325834A (en) | Path planning method of improved A-x algorithm based on graph preprocessing | |
| CN116698066A (en) | Robot path planning method and system based on neighborhood expansion and boundary point improvement A-star algorithm | |
| CN115741686A (en) | Robot path planning method based on variable probability constraint sampling | |
| CN116627132A (en) | Mobile robot path planning method based on RRT improvement |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |