CN113204238B - Path planning method, equipment and storage medium for mobile robot - Google Patents
Path planning method, equipment and storage medium for mobile robot Download PDFInfo
- Publication number
- CN113204238B CN113204238B CN202110435985.0A CN202110435985A CN113204238B CN 113204238 B CN113204238 B CN 113204238B CN 202110435985 A CN202110435985 A CN 202110435985A CN 113204238 B CN113204238 B CN 113204238B
- Authority
- CN
- China
- Prior art keywords
- random
- tree
- node
- obstacle
- random tree
- 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 63
- 238000005070 sampling Methods 0.000 claims abstract description 57
- 238000001514 detection method Methods 0.000 claims abstract description 18
- 238000004364 calculation method Methods 0.000 claims description 10
- 230000006872 improvement Effects 0.000 claims description 3
- 230000007547 defect Effects 0.000 abstract description 6
- 201000004569 Blindness Diseases 0.000 abstract description 3
- 238000004422 calculation algorithm Methods 0.000 description 56
- 230000002457 bidirectional effect Effects 0.000 description 10
- 230000008901 benefit Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 238000004088 simulation Methods 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 229930192334 Auxin Natural products 0.000 description 1
- PGLIUCLTXOYQMV-UHFFFAOYSA-N Cetirizine hydrochloride Chemical compound Cl.Cl.C1CN(CCOCC(=O)O)CCN1C(C=1C=CC(Cl)=CC=1)C1=CC=CC=C1 PGLIUCLTXOYQMV-UHFFFAOYSA-N 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 239000002363 auxin Substances 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- SEOVTRFCIGRIMH-UHFFFAOYSA-N indole-3-acetic acid Chemical compound C1=CC=C2C(CC(=O)O)=CNC2=C1 SEOVTRFCIGRIMH-UHFFFAOYSA-N 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
- 238000010845 search algorithm Methods 0.000 description 1
- 238000002922 simulated annealing 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
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)
Abstract
The application discloses a path planning method, equipment and a storage medium of a mobile robot, wherein the method comprises the following steps: s1, acquiring a starting point and a target point of a mobile robot, and establishing a first random tree and a second random tree; s2, collision detection is carried out on the first random tree and the second random tree respectively, and a first node to be grown and a second node to be grown which are nearest to the obstacle are determined; s3, acquiring random probability values of random sampling of the two random trees, and acquiring random sampling points according to the random probability values of the two random trees and preset fixed probability; s4, acquiring a new node according to the random sampling points, improving the new node, and adding the improved new node to a corresponding random tree; s5, repeating the steps S3 to S4 until the latest nodes on the first random tree and the second random tree meet, and selecting an optimal path generated after the latest nodes on the two random trees are communicated. The application solves the defects of blindness, strong randomness and poor stability in path planning.
Description
Technical Field
The present application relates to the field of path planning technologies, and in particular, to a path planning method, apparatus, and storage medium for a mobile robot.
Background
With the continuous development of the robot field, mobile robots have been closely related to human life and are widely used in a variety of fields. Path planning is one of the core problems in the field of mobile robot research, and is an important basis for the mobile robot to complete various tasks. The path planning is to make the mobile robot find a safe, collision-free, optimal or suboptimal path from the starting point to the end point under a specified and known environment. According to the grasping degree of the environment information, the path planning can be divided into two types of global path planning and local path planning. The global path planning is classified according to a searching algorithm and mainly comprises a graph searching type algorithm, a group intelligent optimization algorithm, a random sampling type algorithm and the like. The local path planning includes artificial potential field method, simulated annealing algorithm, neural network algorithm, etc.
In 1998, laValle et al proposed a random sampling based path planning algorithm (RRT algorithm) which has the advantage that it can effectively take incomplete constraints into account inside the algorithm, thereby avoiding the consideration of complex kinematic constraints and thus simplifying the path planning problem. The algorithm is a path planning algorithm with perfect probability but non-optimal. The probability is complete, that is, if an effective path exists between the starting point and the end point of the planned path, the effective path can be found as long as the sampling point and the planning time are enough; and the non-optimal algorithm can find the effective path, but the optimal path is not guaranteed. The RRT (classical rapid exploration random tree) path planning algorithm has the advantages of no need of modeling or preprocessing the space, strong searching and obstacle avoidance capability, capability of rapidly exploring unknown areas on a map and the like, but has the defects of blindness, strong randomness, poor stability and the like in searching.
Disclosure of Invention
The application aims to overcome the technical defects, and provides a mobile robot path planning method, equipment and a storage medium, which solve the technical problems of blindness, strong randomness and poor stability in searching during mobile robot path planning in the prior art.
In order to achieve the technical purpose, the application adopts the following technical scheme:
in a first aspect, the present application provides a path planning method for a mobile robot, including the steps of:
s1, acquiring a starting point and a target point of a mobile robot, and establishing a first random tree and a second random tree, wherein the first random tree takes the starting point as a root node, and the second random tree takes the target point as a root node;
s2, respectively performing collision detection on the first random tree and the second random tree, and respectively adding the first node to be grown and the second node to be grown into the first random tree and the second random tree after determining the first node to be grown and the second node to be grown which are nearest to the obstacle;
s3, performing node expansion random sampling on the first random tree and the second random tree respectively to obtain random probability values of random sampling of the two random trees, and obtaining random sampling points of the two random trees according to the random probability values of the two random trees and preset fixed probability;
s4, acquiring new nodes of the two trees according to random sampling points of the two trees, performing collision detection on the new nodes and the nearest tree nodes on the corresponding random trees, adding the new nodes to the corresponding random trees when no obstacle is detected, improving the coordinate positions of the new nodes when the obstacle is detected, and adding the improved new nodes to the corresponding random trees after the improved new nodes are obtained;
s5, repeating the steps S3 to S4 until the distance between the latest nodes on the first random tree and the second random tree is within a preset range, and selecting an optimal path generated after the latest nodes on the two random trees are communicated as a planning path of the mobile robot.
Preferably, in the path planning method of a mobile robot, the step S2 specifically includes:
and respectively taking the target point and the starting point as the end points, performing collision detection on the first random tree and the second random tree, stopping when an obstacle is detected, determining a first node to be grown and a second node to be grown which are nearest to the obstacle according to the detected obstacle, and adding the first node to be grown and the second node to be grown into the first random tree and the second random tree respectively.
Preferably, in the path planning method of a mobile robot, the random probability value is set to be P, and the preset fixed probability is P 0 In the step S3, the method for acquiring the random sampling points of the two random trees specifically includes:
when P 0 <P<1, randomly generating the random sampling points to guide the random tree to grow;
when 0 is<P≤P 0 When the probability ratio coefficient P is calculated according to the Euclidean distance between the last growing node on the first random tree and the last growing node on the second random tree and the Euclidean distance between the starting point and the end point var Then according to the probability proportion coefficient P var Random probability value P and preset fixed probability P 0 And acquiring the random sampling points.
Preferably, in the path planning method of a mobile robot, the calculating method of the probability scaling factor is as follows:
wherein ,Pvar As probability scale factor, X T1end and XT2end The last growing node, X, of the first random tree and the second random tree, respectively start and Xgoal Respectively a starting point and a target point.
Preferably, in the path planning method of a mobile robot, the probability scaling factor P is used for var Random probability value P and preset fixed probability P 0 The acquiring the random sampling points specifically comprises the following steps:
when P 0 P var <P<P 0 When the first random tree performs random sampling, the other random tree is a second random tree, and when the second random tree performs random sampling, the other random tree is a first random tree;
when 0 is<P≤P 0 P var And taking the end point of each random tree as a random sampling point, wherein the end point of the first random tree is a target point, and the end point of the second random tree is a starting point.
Preferably, in the path planning method of a mobile robot, in S4, the improving the coordinate position of the new node specifically includes:
obtaining a tree node closest to an obstacle, and calculating attractive force applied by an end point to the tree node closest to the obstacle, wherein the end point is a target point when the new node is a node on a first random tree, and the end point is a starting point when the new node is a node on a second random tree;
acquiring coordinates of an obstacle point closest to the tree node closest to the obstacle, and calculating repulsive force exerted on the tree node closest to the obstacle by the obstacle point closest to the tree node closest to the obstacle;
and calculating the coordinates of the new improved node based on the coordinates, attraction and repulsion of the obstacle point nearest to the tree node nearest to the obstacle.
Preferably, in the path planning method of a mobile robot, the method for calculating the attractive force includes:
wherein ,Fatt Represents gravitational force, p represents basic step length of single growth of random tree, X finish Indicating the coordinates of the end point, X closest Coordinates representing a tree node nearest to the obstacle;
the method for calculating the repulsive force comprises the following steps:
wherein ,Frep Represents repulsive force, p represents the basic step length of single growth of random tree, X closest Representing the coordinates of the tree node nearest to the obstacle, X obstacle And coordinates of the nearest obstacle point to the nearest tree node to the obstacle are represented.
Preferably, in the path planning method of a mobile robot, the improved method for calculating coordinates of a new node includes:
X new ＝X closest +F att +F rep ，
wherein ,Xnew Representing coordinates of the new node after improvement, X closest Representing the coordinates of the tree node nearest to the obstacle, F att Representing gravitational force, F rep Indicating repulsive force.
In a second aspect, the present application also provides a path planning apparatus for a mobile robot, including: a processor and a memory;
the memory has stored thereon a computer readable program executable by the processor;
the processor, when executing the computer readable program, implements the steps in the path planning method of the mobile robot as described above.
In a third aspect, the present application also provides a computer-readable storage medium storing one or more programs executable by one or more processors to implement the steps in the path planning method of a mobile robot as described above.
Compared with the prior art, the path planning method, the equipment and the storage medium of the mobile robot provided by the application aim at the defects of no purpose, low convergence speed and the like of path growth in the RRT algorithm, and a growth pretreatment algorithm is added in the early stage of path planning, so that the unnecessary path growth calculated amount is reduced, and the path planning efficiency is improved; the growth directions of the two growth trees are adaptively adjusted, different target points are selected as the growth directions at different periods of path growth with different probabilities, and the path convergence speed is accelerated; and the growth mode of the growth tree after touching the obstacle is improved, the growth efficiency is improved, the ineffective growth is avoided again, and the growth efficiency is obviously improved especially in a narrow-channel environment. The randomness is smaller, the convergence speed is faster, the required path nodes are fewer, and the obstacle avoidance capability is stronger.
Drawings
FIG. 1 is a flow chart of a path planning method for a mobile robot according to a preferred embodiment of the present application;
fig. 2 is a schematic diagram of a growth pretreatment of a random tree in the path planning method of the mobile robot provided by the application;
FIG. 3 is a schematic diagram of an improved new node growth in the path planning method of the mobile robot provided by the application;
FIG. 4a is a simulated view of a preferred embodiment of path planning using a conventional RRT algorithm in a generally complex environment;
FIG. 4b is a simulated view of a preferred embodiment of path planning using artificial potential field methods in a generally complex environment;
FIG. 4c is a simulated diagram of a preferred embodiment of path planning using a bi-directional target bias RRT algorithm in a generally complex environment;
FIG. 4d is a schematic diagram of a preferred embodiment of path planning using the path planning method of the present application in a generally complex environment;
FIG. 5a is a simulated view of a preferred embodiment of path planning using conventional RRT algorithm in a narrow channel trap environment;
FIG. 5b is a simulated view of a preferred embodiment of path planning using artificial potential field method in a narrow channel trap environment;
FIG. 5c is a simulated view of a preferred embodiment of path planning using a bi-directional target bias RRT algorithm in a narrow channel trap environment;
FIG. 5d is a simulated view of a preferred embodiment of path planning using the path planning method of the present application in a narrow channel trap environment.
Detailed Description
The present application will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present application more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application.
Referring to fig. 1, the path planning method for a mobile robot according to the embodiment of the present application includes the following steps:
s1, acquiring a starting point and a target point of a mobile robot, and establishing a first random tree and a second random tree, wherein the starting point is taken as a root node by the first random tree, and the target point is taken as the root node by the second random tree.
Specifically, the conventional RRT algorithm is a search algorithm with incremental sampling, and the main idea is to take the path starting point as the root node of the tree, set a new node by random sampling in space, and continuously expand the random tree until the random tree expands to the step detection range of the target point, thereby generating a feasible path. The main steps of the algorithm are as follows:
(1) Setting a starting point as a root node X of a random tree in a known map start ；
(2) Generating a random sampling point X rand Traversing all nodes of the random tree and finding the distance X rand Nearest node X closest 。
(3) Judgment of X rand Whether or not it is at node X closest Within the step size limit of (2), otherwise calculate the intermediate point X between them new Replacement X rand 。
(4) For X new and Xclosest Collision detection is performed, i.e., it is determined whether an obstacle exists between the two.
(5) If the above conditions are satisfied, X is new Adding to a random tree, and adding X closest Set as X new Is a parent node of (c).
(6) Judgment of X new Whether the collision detection is within the step size limit range of the end point or not, and performing collision detection on the two.
(7) If the above conditions are satisfied, X will be new Setting a father node as a destination, adding the destination into a random tree, searching the father node reversely from the destination, and planning a feasible path from the starting point to the destination. Otherwise, repeating the steps (2) - (6) until the condition is met.
The conventional RRT algorithm has only one random tree, so that the operation efficiency is low, and therefore, the random exploration tree is added on the basis of the conventional RRT algorithm, and the two growth trees are respectively explored and expanded outwards from the starting point and the end point until the two trees meet, so that the algorithm converges, and the operation efficiency of the RRT algorithm is further improved.
S2, collision detection is carried out on the first random tree and the second random tree respectively, and after a first node to be grown and a second node to be grown closest to the obstacle are determined, the first node to be grown and the second node to be grown are added into the first random tree and the second random tree respectively.
Specifically, the step S2 specifically includes:
and respectively taking the target point and the starting point as the end points, performing collision detection on the first random tree and the second random tree, stopping when an obstacle is detected, determining a first node to be grown and a second node to be grown which are nearest to the obstacle according to the detected obstacle, and adding the first node to be grown and the second node to be grown into the first random tree and the second random tree respectively.
Referring to fig. 2, since the RRT algorithm has strong randomness, many redundant paths are generated in the non-obstacle area during the path growth process, so that the convergence speed is slow due to unnecessary searching and calculation. Therefore, the embodiment of the application performs pretreatment on the random exploration tree in the earlier stage of path growth. Firstly, respectively taking the root nodes of two random trees as targets of respective endpoints, respectively performing primary collision detection, and stopping when an obstacle is detected; then, respectively calculating a node to be grown which is at a proper distance from the obstacle, and avoiding collision with the obstacle when the node to be grown is too close to the obstacle and grows next time; and finally, respectively connecting the two root nodes and the respective nodes to be grown to preliminarily obtain two growth trees. In a special case, if there is no obstacle in the straight line connecting the start point and the end point, the final path can be obtained directly.
Therefore, the number of sampling points of the random sampling points can be reduced to a certain extent, so that the path convergence speed is increased, the random auxin can pass through the non-obstacle area in the early growth stage of the path at one time, and unnecessary growth and calculation are reduced.
And S3, respectively carrying out node expansion random sampling on the first random tree and the second random tree, obtaining random probability values of random sampling of the two random trees, and obtaining random sampling points of the two random trees according to the random probability values of the two random trees and preset fixed probability.
Specifically, in the step S3, the method for acquiring the random sampling points of the two random trees specifically includes:
setting the random probability value as P, and setting the preset fixed probability as P 0 In the step S3, the method for acquiring the random sampling points of the two random trees specifically includes:
when P 0 <P<1, randomly generating the random sampling point X rand To guide random tree growth;
when 0 is<P≤P 0 When the probability ratio coefficient P is calculated according to the Euclidean distance between the last growing node on the first random tree and the last growing node on the second random tree and the Euclidean distance between the starting point and the end point var Then according to the probability proportion coefficient P var Random probability value P and preset fixed probability P 0 And acquiring the random sampling points.
Wherein the probability scaling factor P var The calculation method of (1) is as follows:
wherein ,Pvar As probability scale factor, X T1end and XT2end The last growing node, X, of the first random tree and the second random tree, respectively start and Xgoal Respectively a starting point and a target point.
Further, the probability scale factor P var Random probability value P and preset fixed probability P 0 The acquiring the random sampling points specifically comprises the following steps:
when P 0 P var <P<P 0 When the first random tree performs random sampling, the other random tree is a second random tree, and when the second random tree performs random sampling, the other random tree is a first random tree;
when 0 is<P≤P 0 P var And taking the end point of each random tree as a random sampling point, wherein the end point of the first random tree is a target point, and the end point of the second random tree is a starting point.
Specifically, in the Goal-bias (target bias) RRT algorithm, a target end point is selected with a certain probability as a sampling point during each random sampling, and a growth tree is guided to grow towards the end point, so that the problem of strong randomness of the RRT algorithm is solved to a certain extent. However, the probability of selecting the target point each time and the coordinate position of the target point are fixed by the golal-bias RRT algorithm, and when two growing trees are about to meet, the final convergence speed may be affected if the respective end points are still used as random sampling points.
Therefore, the application adopts a bidirectional self-adaptive gold-bias algorithm, firstly, a fixed probability P0 is set, and the probability of each sampling of the random sampling points is a random value P between 0 and 1; when P 0 <P<1, randomly generating sampling points Xrand in a map to guide the random tree to grow; when 0 is<P≤P 0 When the method is used, the Euclidean distance between the last growing node of the first random tree and the last growing node of the second random tree is calculated, and the value of the Euclidean distance between the starting point and the target point is divided by the value to obtain a value P which continuously changes along with the growth of the two growing trees var . In general P var Is a value greater than 0 and less than 1, and can be converted into 1/P if special conditions greater than 1 occur var Or not processed. When P 0 P var <P<P 0 When the method is used, the last growing node of the growing tree is selected to guide another growing tree to grow; when 0 is<P<P 0 P var When selecting the target endpoint X of the growing tree start Or X goal As sampling points to guide its growth.
In the early stage of path planning, two random trees are far apart, and the target bias probability can more select a target endpoint to guide the random tree to grow; as the random trees continue to grow closer to each other, the target bias probability progressively more selects the last growing node of another random tree to direct its growth, thus increasing the final convergence rate of the two random trees.
S4, acquiring new nodes of the two trees according to random sampling points of the two trees, performing collision detection on the new nodes and the nearest tree nodes on the corresponding random trees, adding the new nodes to the corresponding random trees when no obstacle is detected, improving the coordinate positions of the new nodes when the obstacle is detected, and adding the improved new nodes to the corresponding random trees after the improved new nodes are obtained.
Specifically, in the process of guiding the growth tree to expand outwards by the random sampling points, the generated node to be grown may fall into an obstacle area, and the idea is that for the RRT algorithm, the node is eliminated and then resampled randomly. However, the newly generated node to be grown after re-random sampling still may fall in the obstacle or cannot be close to the target point away from the obstacle, which consumes a lot of time and calculation.
Therefore, the concept of attraction and repulsion is integrated into the RRT algorithm, and the attraction and repulsion are firstly realized through the random sampling point X rand Calculating to-be-grown node X new Then treat the growing node X new And the nearest tree node X on the growth tree closest Collision detection is performed, and if an obstacle is detected, the path growth of the growing tree is guided using attraction and repulsion.
Specifically, in S4, the improving the coordinate position of the new node specifically includes:
obtaining a tree node closest to an obstacle, and calculating attractive force applied by an end point to the tree node closest to the obstacle, wherein the end point is a target point when the new node is a node on a first random tree, and the end point is a starting point when the new node is a node on a second random tree;
acquiring coordinates of an obstacle point closest to the tree node closest to the obstacle, and calculating repulsive force exerted on the tree node closest to the obstacle by the obstacle point closest to the tree node closest to the obstacle;
and calculating the coordinates of the new improved node based on the coordinates, attraction and repulsion of the obstacle point nearest to the tree node nearest to the obstacle.
Further, the method for calculating the gravitation comprises the following steps:
wherein ,Fatt Represents gravitational force, p represents basic step length of single growth of random tree, X finish Indicating the coordinates of the end point, X closest Coordinates representing a tree node nearest to the obstacle;
the method for calculating the repulsive force comprises the following steps:
wherein ,Frep Represents repulsive force, p represents the basic step length of single growth of random tree, X closest Representing the coordinates of the tree node nearest to the obstacle, X obstacle And coordinates of the nearest obstacle point to the nearest tree node to the obstacle are represented.
Further, the improved calculation method of the coordinates of the new node comprises the following steps:
X new ＝X closest +F att +F rep ，
wherein ,Xnew Representing coordinates of the new node after improvement, X closest Representing the coordinates of the tree node nearest to the obstacle, F att Representing gravitational force, F rep Indicating repulsive force.
Specifically, referring to FIG. 3, the nearest tree node X is identified by the destination endpoint closest Applying attraction force to calculate distance X closest Nearest obstacle point coordinate X obstacle Tree node X by the nearest obstacle point closest Exerting repulsive force effect, tree node X closest And the obstacle is bypassed to grow to the target end point under the action of the resultant force of the attractive force and the repulsive force.
When the new node generated by using the random sampling mode does not pass the collision detection, the generation mode of the new node is improved by using the ideas of attraction and repulsion, so that the growing tree can smoothly grow to the target point along the edge of the obstacle, especially when the new node passes a narrow channel, the new node growing next time can be effectively prevented from falling into the obstacle again, and the convergence speed of two growing trees is greatly improved.
S5, repeating the steps S3 to S4 until the distance between the latest nodes on the first random tree and the second random tree is within a preset range, and selecting an optimal path generated after the latest nodes on the two random trees are communicated as a planning path of the mobile robot.
Specifically, after the new node is obtained, repeating steps S3 to S4, sequentially obtaining the new node, and finally, when the distance between the latest nodes on the first random tree and the second random tree is within a preset range, indicating that the path searching is completed, and at the moment, selecting an optimal path generated after the latest nodes on the two random trees are communicated as a planning path of the mobile robot.
In order to verify the effectiveness of the improved algorithm, the improved algorithm is compared and analyzed with three algorithms, namely an artificial potential field method, an RRT algorithm and a bidirectional target bias RRT algorithm in two map environments of a general complex environment and a narrow channel trap environment. The simulation experiment is realized by using a compiling tool MATLAB R2020a in a Windows10 environment, and is run on a notebook computer, wherein a processor is AMD (Ryzen 7) 2.90Ghz, and a memory is 16G. Setting the map environment size to 500×500, the starting point coordinate to (10,490), the end point coordinate to (490,10), the basic step size p=10, the number of experiments of each algorithm to 20, assuming that the mobile robot is an ideal circular point, the simulation diagrams on the general complex environment map are shown in fig. 4a to 4d, and the simulation results are shown in the following table:
from the above, in a general complex environment, the RRT algorithm generates a large number of random tree nodes in the map due to strong randomness and no purpose of searching, consumes a large amount of computation time, and makes the final path more tortuous; the path generated by the artificial potential field method is smoother, but the path is very easy to sink into local optimum and is difficult to jump out; the bidirectional target bias RRT algorithm grows towards the target point with a certain probability due to the bidirectional growth characteristic of the bidirectional target bias RRT algorithm, so that the number of the random tree nodes and the calculation time are reduced to a certain extent, but the growth directivity of the bidirectional target bias RRT algorithm is unstable, and the obstacle avoidance effect is not obvious. The improved algorithm is superior to the three algorithms in various indexes, and compared with a bidirectional target bias RRT algorithm, the improved algorithm has the advantages that the average iteration number is reduced by 60.2%, the average path point number is reduced by 47.2%, the average path length is reduced by 9.8%, and the average planning time is reduced by 62%.
Simulation results in a narrow channel trap environment map are shown in fig. 5a to 5d and the following table.
5 a-5 d and Table 2, in a narrow channel trap environment map, the path planned by the artificial potential field method falls into a local optimum and cannot reach the end point; because the passage through the barrier is narrow and a trap environment exists, the bidirectional target bias RRT algorithm and the classical RRT algorithm both need to consume a large amount of sampling points and calculation time to pass through the barrier, so that compared with the classical RRT algorithm, the bidirectional target bias RRT algorithm only has larger advantages in iteration times and planning time, and the optimization is not obvious in the number of path points and the path length; the improved algorithm effectively reduces the node number in the early stage of path growth through a growth pretreatment step in the environment map, and uses an artificial potential field method idea to guide nodes which do not pass collision detection to grow in a narrow-channel environment, so that the growth advantage is obvious, and the path growth efficiency is greatly improved; the adaptive target bias algorithm is used, the defect that an artificial potential field method cannot pass through a trap area is overcome, and the meeting of two growth trees is quickened. Compared with the bidirectional target bias RRT algorithm, the average iteration number of the improved algorithm is reduced by 94.7%, the number of average path points is reduced by 67.5%, the average path length is reduced by 13.4%, and the average planning time is reduced by 97.3%.
Based on the path planning method of the mobile robot, the application further provides a path planning device of the mobile robot, which comprises the following steps: a processor and a memory;
the memory has stored thereon a computer readable program executable by the processor;
the processor, when executing the computer readable program, implements the steps in the path planning method of the mobile robot according to the above embodiments.
Since the path planning method of the mobile robot has been described in detail above, it is not described in detail herein.
Based on the path planning method of the mobile robot, the application further provides a corresponding computer readable storage medium, wherein the computer readable storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to realize the steps in the path planning method of the mobile robot according to the embodiments.
Since the path planning method of the mobile robot has been described in detail above, it is not described in detail herein.
In summary, the path planning method, the device and the storage medium for the mobile robot provided by the application aim at the defects of no purpose, low convergence speed and the like of path growth in the RRT algorithm, and a growth preprocessing algorithm is added in the early stage of path planning, so that unnecessary path growth calculation amount is reduced, and path planning efficiency is improved; the growth directions of the two growth trees are adaptively adjusted, different target points are selected as the growth directions at different periods of path growth with different probabilities, and the path convergence speed is accelerated; the method has the advantages that the growth mode of the growth tree after touching the obstacle is improved by means of the artificial potential field method, the growth efficiency is improved, the ineffective growth is avoided, and the growth efficiency is remarkably improved particularly in a narrow-channel environment. The randomness is smaller, the convergence speed is faster, the required path nodes are fewer, and the obstacle avoidance capability is stronger.
The above-described embodiments of the present application do not limit the scope of the present application. Any other corresponding changes and modifications made in accordance with the technical idea of the present application shall be included in the scope of the claims of the present application.
Claims (7)
1. The path planning method of the mobile robot is characterized by comprising the following steps of:
s1, acquiring a starting point and a target point of a mobile robot, and establishing a first random tree and a second random tree, wherein the first random tree takes the starting point as a root node, and the second random tree takes the target point as a root node;
s2, respectively performing collision detection on the first random tree and the second random tree, and respectively adding the first node to be grown and the second node to be grown into the first random tree and the second random tree after determining the first node to be grown and the second node to be grown which are nearest to the obstacle;
s3, performing node expansion random sampling on the first random tree and the second random tree respectively to obtain random probability values of random sampling of the two random trees, and obtaining random sampling points of the two random trees according to the random probability values of the two random trees and preset fixed probability;
s4, acquiring new nodes of the two trees according to random sampling points of the two trees, performing collision detection on the new nodes and the nearest tree nodes on the corresponding random trees, adding the new nodes to the corresponding random trees when no obstacle is detected, improving the coordinate positions of the new nodes when the obstacle is detected, and adding the improved new nodes to the corresponding random trees after the improved new nodes are obtained;
s5, repeating the steps S3 to S4 until the distance between the latest nodes on the first random tree and the second random tree is within a preset range, and selecting an optimal path generated after the latest nodes on the two random trees are communicated as a planning path of the mobile robot;
setting random probability valuesP, the preset fixed probability is P 0 In the step S3, the method for acquiring the random sampling points of the two random trees specifically includes:
when P 0 <P<1, randomly generating the random sampling points to guide the random tree to grow;
when 0 is<P≤P 0 When the probability ratio coefficient P is calculated according to the Euclidean distance between the last growing node on the first random tree and the last growing node on the second random tree and the Euclidean distance between the starting point and the end point var Then according to the probability proportion coefficient P var Random probability value P and preset fixed probability P 0 Acquiring the random sampling points;
the calculation method of the probability proportionality coefficient comprises the following steps:
，
wherein ,Pvar As probability scale factor, X T1end and XT2end The last growing node, X, of the first random tree and the second random tree, respectively start and Xgoal Respectively a starting point and a target point;
said scaling factor P according to probability var Random probability value P and preset fixed probability P 0 The acquiring the random sampling points specifically comprises the following steps:
when P 0 P var <P<P 0 When the first random tree performs random sampling, the other random tree is a second random tree, and when the second random tree performs random sampling, the other random tree is a first random tree;
when 0 is<P≤P 0 P var And taking the end point of each random tree as a random sampling point, wherein the end point of the first random tree is a target point, and the end point of the second random tree is a starting point.
2. The path planning method of a mobile robot according to claim 1, wherein the S2 specifically includes:
and respectively taking the target point and the starting point as the end points, performing collision detection on the first random tree and the second random tree, stopping when an obstacle is detected, determining a first node to be grown and a second node to be grown which are nearest to the obstacle according to the detected obstacle, and adding the first node to be grown and the second node to be grown into the first random tree and the second random tree respectively.
3. The path planning method of a mobile robot according to claim 1, wherein in S4, the improving the coordinate position of the new node specifically includes:
obtaining a tree node closest to an obstacle, and calculating attractive force applied by an end point to the tree node closest to the obstacle, wherein the end point is a target point when the new node is a node on a first random tree, and the end point is a starting point when the new node is a node on a second random tree;
acquiring coordinates of an obstacle point closest to the tree node closest to the obstacle, and calculating repulsive force exerted on the tree node closest to the obstacle by the obstacle point closest to the tree node closest to the obstacle;
and calculating the coordinates of the new improved node based on the coordinates, attraction and repulsion of the obstacle point nearest to the tree node nearest to the obstacle.
4. A path planning method of a mobile robot according to claim 3, characterized in that the method of calculating the attractive force is:
，
wherein ,indicate gravitation, & lt & gt>Basic step size representing single growth of random tree, < ->Indicating the coordinates of the end point, X closest Coordinates representing a tree node nearest to the obstacle;
the method for calculating the repulsive force comprises the following steps:
，
wherein ,representing repulsive force, < > on->Represents the basic step length, X of single growth of random tree closest Representing the coordinates of the tree node nearest to the obstacle, X obstacle And coordinates of the nearest obstacle point to the nearest tree node to the obstacle are represented.
5. The path planning method of a mobile robot according to claim 4, wherein the improved calculation method of coordinates of the new node is:
，
wherein ,representing coordinates of the new node after improvement, X closest Representing the coordinates of the tree node nearest to the obstacle,indicate gravitation, & lt & gt>Indicating repulsive force.
6. A path planning apparatus of a mobile robot, comprising: a processor and a memory;
the memory has stored thereon a computer readable program executable by the processor;
the processor, when executing the computer readable program, implements the steps in the path planning method of a mobile robot according to any one of claims 1-5.
7. A computer readable storage medium storing one or more programs executable by one or more processors to implement the steps in the path planning method of a mobile robot according to any one of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110435985.0A CN113204238B (en) | 2021-04-22 | 2021-04-22 | Path planning method, equipment and storage medium for mobile robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110435985.0A CN113204238B (en) | 2021-04-22 | 2021-04-22 | Path planning method, equipment and storage medium for mobile robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113204238A CN113204238A (en) | 2021-08-03 |
| CN113204238B true CN113204238B (en) | 2023-09-12 |
Family
ID=77027907
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110435985.0A Active CN113204238B (en) | 2021-04-22 | 2021-04-22 | Path planning method, equipment and storage medium for mobile robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113204238B (en) |
Families Citing this family (1)
| 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 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108681787A (en) * | 2018-04-28 | 2018-10-19 | 南京航空航天大学 | Based on the unmanned plane method for optimizing route for improving the two-way random tree algorithm of Quick Extended |
| CN111650941A (en) * | 2020-06-10 | 2020-09-11 | 湖南爱米家智能科技有限公司 | Medical mobile robot path planning method, device, medium and equipment based on RRT-Connect improvement |
| CN111678523A (en) * | 2020-06-30 | 2020-09-18 | 中南大学 | Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization |
| CN111897328A (en) * | 2020-07-17 | 2020-11-06 | 武汉理工大学 | Path planning method, device and equipment based on improved artificial potential field method |
-
2021
- 2021-04-22 CN CN202110435985.0A patent/CN113204238B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108681787A (en) * | 2018-04-28 | 2018-10-19 | 南京航空航天大学 | Based on the unmanned plane method for optimizing route for improving the two-way random tree algorithm of Quick Extended |
| CN111650941A (en) * | 2020-06-10 | 2020-09-11 | 湖南爱米家智能科技有限公司 | Medical mobile robot path planning method, device, medium and equipment based on RRT-Connect improvement |
| CN111678523A (en) * | 2020-06-30 | 2020-09-18 | 中南大学 | Rapid BI _ RRT obstacle avoidance trajectory planning method based on STAR algorithm optimization |
| CN111897328A (en) * | 2020-07-17 | 2020-11-06 | 武汉理工大学 | Path planning method, device and equipment based on improved artificial potential field method |
Non-Patent Citations (1)
| Title |
|---|
| 基于RRT改进的智能车辆路径规划算法;施杨洋;《计算技术与自动化》;第38卷(第4期);81-86 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113204238A (en) | 2021-08-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110083165B (en) | Path planning method of robot in complex narrow environment | |
| CN107234617B (en) | Obstacle avoidance path planning method guided by artificial potential field irrelevant to obstacle avoidance task | |
| CN108444489A (en) | A kind of paths planning method improving RRT algorithms | |
| US10365110B2 (en) | Method and system for determining a path of an object for moving from a starting state to an end state set avoiding one or more obstacles | |
| CN112926139B (en) | Improved intelligent sparrow optimization method based on chaotic mapping and golden sine strategy | |
| CN113867368B (en) | Robot path planning method based on improved gull algorithm | |
| Yang | Anytime synchronized-biased-greedy rapidly-exploring random tree path planning in two dimensional complex environments | |
| CN112013846A (en) | Path planning method combining dynamic step RRT algorithm and potential field method | |
| Zhang et al. | Reinforcement Learning in Robot Path Optimization. | |
| CN111290390A (en) | Intelligent ship path planning method based on longicorn stigma search | |
| CN113204238B (en) | Path planning method, equipment and storage medium for mobile robot | |
| CN110632922A (en) | Path planning method based on bat algorithm and reinforcement learning | |
| CN113296496A (en) | Multi-sampling-point-based gravitational adaptive step size bidirectional RRT path planning method | |
| Li et al. | Application of improved ant colony optimization in mobile robot trajectory planning | |
| CN115248592A (en) | Multi-robot autonomous exploration method and system based on improved rapid exploration random tree | |
| Huang | Path planning based on mixed algorithm of RRT and artificial potential field method | |
| CN116038688A (en) | Mechanical arm joint space path planning method based on probability virtual potential field guided bidirectional RRT algorithm | |
| Yin et al. | Reinforcement learning path planning based on step batch Q-learning algorithm | |
| CN114911233A (en) | Football robot path planning method based on multi-optimization rapid expansion random tree | |
| CN115167388A (en) | RRT multi-robot formation path planning algorithm based on target guidance | |
| Liu et al. | UUV path planning method based on QPSO | |
| Li et al. | A novel path planning algorithm based on Q-learning and adaptive exploration strategy | |
| Xu et al. | A path planning method of logistics robot based on improved ant colony algorithm | |
| Xu et al. | Research on Path Planning Algorithm of Bidirectional Rapidly Exploring Random Tree Improved by Artificial Potential Field | |
| US11969892B2 (en) | Method, system, and storage medium for selecting grasp posture of dual-arm robot |
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 |