CN111610786B - Mobile robot path planning method based on improved RRT algorithm - Google Patents
Mobile robot path planning method based on improved RRT algorithm Download PDFInfo
- Publication number
- CN111610786B CN111610786B CN202010466195.4A CN202010466195A CN111610786B CN 111610786 B CN111610786 B CN 111610786B CN 202010466195 A CN202010466195 A CN 202010466195A CN 111610786 B CN111610786 B CN 111610786B
- Authority
- CN
- China
- Prior art keywords
- grid
- parent
- path
- node
- representing
- 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
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 78
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005070 sampling Methods 0.000 claims abstract description 60
- 238000013138 pruning Methods 0.000 claims abstract description 10
- 238000002474 experimental method Methods 0.000 claims abstract description 9
- 238000009499 grossing Methods 0.000 claims abstract description 7
- 238000005516 engineering process Methods 0.000 claims description 5
- 230000004927 fusion Effects 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 abstract description 3
- 238000003708 edge detection Methods 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 19
- 238000010586 diagram Methods 0.000 description 15
- 238000004364 calculation method Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 2
- 239000011664 nicotinic acid Substances 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 101150014751 map4 gene Proteins 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
Images
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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0242—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using non-visible light signals, e.g. IR or UV signals
-
- 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- 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/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Acoustics & Sound (AREA)
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a mobile robot path planning method based on an improved RRT algorithm in an indoor environment. Firstly, a high-precision grid map of a robot working environment is established, map segmentation is carried out, then a neighborhood expansion strategy is adopted to determine the next grid to be searched, a random sampling experiment is used to determine the type of the searched grid and the number of effective sampling points, then the effective sampling points are added into a random tree by adopting a neighborhood optimal principle, the grid is updated based on a memory strategy, and finally, a pruning algorithm and a Bezier curve are adopted to carry out smoothing treatment on the obtained planning path. The introduction of the map segmentation and obstacle edge detection ideas improves the sampling efficiency of the RRT algorithm in complex maps such as narrow channels and the like; the memory mechanism and the neighborhood expansion strategy are introduced, so that the planning success rate of the algorithm in a complex environment is improved; the parent node is selected according to the optimal neighborhood mode, so that the problems of strong algorithm randomness, non-optimal path and the like are solved.
Description
Technical Field
The invention relates to the technical field of mobile robot path planning, in particular to a mobile robot path planning method based on an improved RRT algorithm.
Background
Along with development and progress of science and technology, intelligent robots are increasingly widely applied to human life. The path planning is used as a key technology of the intelligent robot, is a foundation for executing various advanced tasks and realizing autonomous navigation, and aims to find a collision-free path from a starting point to a target point in an environment with obstacles. The current commonly used path planning algorithms mainly comprise a rapid expansion random tree (RRT), a random road map method (PRM), an A-star, dijkstra, an artificial potential field, a fuzzy logic algorithm, a particle swarm algorithm, an ant colony algorithm and the like. Wherein, the A-star and Dijkstra algorithm is not suitable for large-scale or high-precision grid maps, and the running time is increased sharply along with the increase of the size of the map; the artificial potential field and the fuzzy logic algorithm are influenced by the environment structure and are easy to be trapped in a local minimum value, so that path planning failure is caused; the intelligent bionic algorithms such as particle swarm and ant colony have the problems of large calculation amount and poor real-time performance, and the setting of related parameters in the algorithm seriously affects the success rate of planning, so that the intelligent bionic algorithm cannot be effectively adapted to various different types of environments; RRT and PRM can only guarantee completeness in probability, and the defects of inconsistent multi-time planning results, non-optimal paths, low success rate in complex environments and the like exist.
The rapid-expansion random tree (RRT) is a planning algorithm based on sampling, overcomes the defect of high time complexity of other planning algorithms in a high-dimensional space, and can also be used for path planning by combining the kinematic or dynamic constraint of a mobile robot. However, the planning success rate of RRT algorithms is affected by the environmental structure or obstacle shape: when a narrow channel does not exist in the environment and all barriers are convex, the algorithm can plan a path with a higher success rate; conversely, if some concave obstacles exist in the environment, such as a wall in an indoor scene, the algorithm is very easy to sink into a local minimum, so that the planning success rate of the algorithm is low. Meanwhile, the RRT algorithm has strong randomness, and the planned path is often far away from the optimal solution. Although RRT algorithms enable paths to reach progressive optimality by introducing a "rerouting" process, such optimality sacrifices the performance of RRT algorithms for fast convergence and is not suitable for use as real-time path planning.
Disclosure of Invention
Aiming at the problems of non-optimal path, low planning success rate in a complex environment, poor real-time performance of the RRT algorithm and the like existing in the RRT algorithm, the invention provides a mobile robot path planning method based on the improved RRT algorithm, which overcomes the defects of the prior art, improves the planning success rate and the real-time performance of the algorithm in the complex environment, reduces the randomness of the path and ensures that the planned path is close to the optimal as much as possible.
In order to achieve the technical effects, the invention provides a mobile robot path planning method based on an improved RRT algorithm, which comprises the following steps:
step 1: acquiring working environment information of the robot through a vision camera, a laser radar sensor, an ultrasonic sensor and an infrared sensor which are equipped with the mobile robot, and then establishing a Map of the working environment of the mobile robot by utilizing information fusion and SLAM technology;
step 2: defining a current position point of the mobile robot as a starting point Q start The task place of the mobile robot is a target point Q goal ；
Step 3: dividing the Map into a number of m×n grids according to a preset search step S, namely map= { R (i, j) }, R (i, j) representing the grid located in the ith row and jth column of the Map, wherein i=0, 1, …, m, j=0, 1, …, n, and then starting the Map at a point Q start The grid at which this is located is marked as the starting grid R (X start ,Y start ) Will target point Q goal The grid at which is located is marked as target grid R (X goal ,Y goal ) Wherein X is start 、Y start Respectively represents the horizontal and vertical coordinates, X of the initial grid on the Map goal 、Y goal Respectively representing the horizontal and vertical coordinates of the target grid on the Map;
wherein r is max Representing a maximum radius of the mobile robot;
step 4: determining the next grid R to be searched by adopting a neighborhood expansion strategy next ；
Step 5: to grid R next Random sampling experiment is carried out to determine grid R next The number num of valid sampling points in (1);
step 6: searching a parent node for each effective sampling point according to a neighborhood optimal principle, and adding the effective sampling point of the found parent node into a random tree T;
step 7: grid R using memory strategy next Status update is performed while updating grid R next Path cost of (a);
step 8: repeating the steps 4-7 until the target point Q goal Successfully adding the target point to the random tree T, and then searching a planning path to be smoothed from the target point to the starting point by using a backtracking method;
step 9: and deleting redundant nodes in the planned path to be smoothed by adopting a pruning algorithm, and smoothing the planned path to be smoothed after deleting the redundant nodes by adopting a Bezier curve to obtain a motion curve which can be used for track tracking and is used as a planned path for tracking the mobile robot.
The step 4 is specifically expressed as:
step 4.1: set L of searched grids past Initializing, denoted as L past ＝{R(X start ,Y start )}；
Step 4.2: calculating a grid set L to be searched in the next step by using a formula (2) to a formula (3) next ；
L next ＝{∪N(x,y)-L past |R(x,y)∈L past } (2)
N(x,y)＝{R(i,j)|R(i,j)∈Map,0<|x-i|≤1,0<|y-j|≤1} (3)
Wherein R (x, y) represents a searched grid, and N (x, y) represents a neighborhood of the searched grid R (x, y);
step 4.3: estimating the set L by using the formula (4) next Each grid of (a)
To the starting point Q start Path cost of (2)
Then selecting the grid corresponding to the minimum path cost as the grid R to be searched in the next step next ；
In the method, in the process of the invention,
representing the grid that may be searched next, C (R (i, j)) represents the grid R (i, j) to the starting point Q start Path cost of->
Representing grid->
Neighborhood of->
Respectively represent grid->
The abscissa in Map.
The step 5 is specifically expressed as:
step 5.1: to grid R next Performing N times of random sampling experiments, and calculating a grid R by using a formula (6) next The area ratio P occupied by the free space in (c),
P＝M/N (6)
wherein M represents the number of sample points falling in the free space;
step 5.2: judging grid R according to P next When p=0, represents the grid R next Is an obstacle grid; when p=1, it represents the grid R next Is a free grid; when 0 is<P<1, represents a grid R next Is an obstacle edge grid;
step 5.3: determining grid R using equation (7) next The number num of effective sampling points in the system, wherein the effective sampling points are random sampling realSample points in the experiment that fall within free space,
wherein ρ represents the grid R next Broadcast density of the effective sampling points in the (a), k represents a density weighting factor;
step 5.4: representing num valid sample points as a set L sample ＝{Q h |h＝1,2,…,num}，Q h Indicating the h valid sample point.
The step 6 is specifically expressed as:
step 6.1: at the effective sampling point Q h At grid R next In the neighborhood of (2), Q is calculated by using the formulas (8) to (9) h Node with minimum path cost and no collision is taken as Q h Parent node Q of (1) parent ，
C(Q h )＝min{C(Q parent )+w L ||Q h -Q parent ||+w s α(Q ancestor ,Q parent ,Q h )},Q parent ∈T∩N(R next ) (8)
Wherein C (Q) h ) Representation node Q h To the starting point Q start Is equal to the path cost of N (R next ) Representing a grid R next Neighborhood of Q parent Is represented as falling within N (R next ) Nodes, C (Q parent ) Representation node Q parent To the starting point Q start Path cost, w L Weights representing path length, w s Weights representing path smoothness, |Q h -Q parent I represents the valid sampling point Q h To node Q parent Euclidean distance, Q ancestor Representation node Q parent Is a parent node of alpha (Q) ancestor ,Q parent ,Q h ) Representing the path at node Q parent Steering angle at v 1 Representing the node Q ancestor Pointing to node Q parent Direction vector v of (v) 2 Representing the node Q parent Pointing to the effective sampling point Q h Is a vector of the direction of (2);
step 6.2: adding the effective sampling point for finding the father node into the random tree T, and if the effective sampling point Q is in the adding process h And Q is equal to parent Is connected with the line of (a)
Collision with an obstacle causes no effective sampling point Q h Adding to a random tree;
step 6.3: and (6) repeating the steps 6.1-6.2, calculating the father node of each effective sampling point and adding the father node into the random tree T.
The step 7 is specifically expressed as: according to grid R next Whether num effective sampling points in the tree can be successfully added into the random tree T, and the grid R next The status update of (2) is divided into three cases:
1) If num valid sample points can be successfully added to the random tree T, then the grid R is applied next Mark as searched grid and store grid R next Added to L past In (2), then update R using equation (10) next Path cost C (R) next ) Finally, turning to the step 4.1 to continue execution;
C(R next )＝(∑C(Q h ))/num (10)
2) If none of the num valid sample points is successfully added to the random tree T, then the grid R is applied next Marked as unsearched grid, and from L next R is removed from next Finally, turning to the step 4.3 to continue execution;
3) If a part of the num valid sampling points is successfully added to the random tree T, the grid R is used for next Marked as unsearched grid, and from L next R is removed from next Then based on the valid sample points successfully added to the random tree T, update with equation (10)R next Path cost C (R) next ) Finally, the process goes to step 4.3 to continue execution.
The beneficial effects of the invention are as follows:
the invention provides a mobile robot path planning method based on an improved RRT algorithm, which aims at solving the problems of a basic RRT algorithm and an RRT algorithm and improves the following three aspects:
1) The map segmentation and obstacle edge detection ideas are introduced into the RRT basic frame, so that the sampling efficiency of the algorithm in complex maps such as narrow channels is improved;
2) The memory mechanism and the neighborhood expansion strategy are introduced into the RRT basic framework, so that the problem of excessive searching of the algorithm in a local trap area is avoided, and the planning success rate of the algorithm in a complex environment is improved;
3) The cost function is designed by combining various performance indexes such as path length, path smoothness and the like, and the father node is selected according to a neighborhood optimal mode, so that the problems of strong randomness, non-optimal path or suboptimal RRT algorithm are solved;
practice proves that the method provided by the invention is not only suitable for a simple structured environment, but also suitable for a complex unstructured environment.
Drawings
Fig. 1 is a flowchart of a mobile robot path planning method based on an improved RRT algorithm in the present invention.
Fig. 2 is a flow chart of the pruning algorithm in the present invention.
Fig. 3 is a schematic diagram of a smoothing path using bezier curves in the present invention.
Fig. 4 is a schematic diagram of a convex obstacle environment and a non-convex obstacle environment according to the present invention, wherein fig. (a) shows a schematic diagram of a convex obstacle environment and fig. (b) shows a schematic diagram of a non-convex obstacle environment.
Fig. 5 is a three-time simulation result diagram of the basic RRT algorithm in the present invention in the convex obstacle environment and the non-convex obstacle environment, wherein diagrams (a) to (c) represent simulation result diagrams in the convex obstacle environment, and diagrams (d) to (f) represent simulation result diagrams in the non-convex obstacle environment.
Fig. 6 is a diagram of simulation results of the mobile robot path planning method based on the improved RRT algorithm in different maps, where the diagrams (a) to (d) respectively represent simulation result diagrams of 4 different maps.
Fig. 7 is a comparison diagram of the basic RRT algorithm, and the mobile robot path planning method of the improved RRT algorithm in the present invention in terms of path length.
Fig. 8 is a comparison diagram of the basic RRT algorithm, and the mobile robot path planning method of the improved RRT algorithm in the present invention in terms of planning time.
Detailed Description
The invention will be further described with reference to the accompanying drawings and examples of specific embodiments.
As shown in fig. 1, a mobile robot path planning method based on an improved RRT algorithm includes the steps of:
step 1: acquiring working environment information of a robot through a vision camera, a laser radar sensor, an ultrasonic sensor and an infrared sensor which are arranged on the mobile robot, and then establishing a Map of the working environment of the mobile robot by utilizing information fusion and SLAM (Simultaneous Localization And Mapping SLAM) technology, wherein the vision camera acquires image information of obstacles, the laser radar sensor and the ultrasonic sensor acquire position information of surrounding objects, the infrared sensor acquires distance information between the mobile robot and the obstacles, and in fig. 4-6, a black part is an obstacle area and a white part is a free area;
step 2: defining a current position point of the mobile robot as a starting point Q start The task place of the mobile robot is a target point Q goal ；
Step 3: dividing the Map into a number of m×n grids according to a preset search step S, namely map= { R (i, j) }, R (i, j) representing the grid located in the ith row and jth column of the Map, wherein i=0, 1, …, m, j=0, 1, …, n, and then starting the Map at a point Q start The grid at which this is located is marked as the starting grid R (X start ,Y start ) Will target point Q goal The grid at which is located is marked as target grid R (X goal ,Y goal ) Wherein X is start 、Y start Respectively represents the horizontal and vertical coordinates, X of the initial grid on the Map goal 、Y goal Respectively representing the horizontal and vertical coordinates of the target grid on the Map;
wherein r is max Representing a maximum radius of the mobile robot;
step 4: determining the next grid R to be searched by adopting a neighborhood expansion strategy next The method is specifically expressed as follows:
step 4.1: set L of searched grids past Initializing, denoted as L past ＝{R(X start ,Y start )}；
Step 4.2: calculating a grid set L to be searched in the next step by using a formula (2) to a formula (3) next ；
L next ＝{∪N(x,y)-L past |R(x,y)∈L past } (2)
N(x,y)＝{R(i,j)|R(i,j)∈Map,0<|x-i|≤1,0<|y-j|≤1} (3)
Wherein R (x, y) represents a searched grid, and N (x, y) represents a neighborhood of the searched grid R (x, y);
step 4.3: estimating the set L by using the formula (4) next Each grid of (a)
To the starting point Q start Path cost of (2)
Then selecting the grid corresponding to the minimum path cost as the grid R to be searched in the next step next ；
In the method, in the process of the invention,
representing the grid that may be searched next, C (R (i, j)) represents the grid R (i, j) to the starting point Q start Path cost of->
Representing grid->
Neighborhood of->
Respectively represent grid->
The abscissa in Map.
Step 5: to grid R next Random sampling experiment is carried out to determine grid R next The number num of valid sampling points in (1) is specifically expressed as:
step 5.1: to grid R next Performing N times of random sampling experiments, and calculating a grid R by using a formula (6) next The area ratio P occupied by the free space in (c),
P＝M/N (6)
wherein M represents the number of sample points falling in the free space;
step 5.2: judging grid R according to P next When p=0, represents the grid R next Is an obstacle grid; when p=1, it represents the grid R next Is a free grid; when 0 is<P<1, represents a grid R next Is an obstacle edge grid;
step 5.3: determining grid R using equation (7) next The number num of effective sampling points in the random sampling experimentSample points falling within the free space,
wherein ρ represents the grid R next Broadcast density of the effective sampling points in the (a), k represents a density weighting factor;
step 5.4: representing num valid sample points as a set L sample ＝{Q h |h＝1,2,…,num}，Q h Indicating the h valid sample point.
Step 6: according to the neighborhood optimization principle, searching a father node for each effective sampling point, and adding the effective sampling point of which the father node is found into a random tree T, wherein the specific expression is as follows:
step 6.1: at the effective sampling point Q h At grid R next In the neighborhood of (2), Q is calculated by using the formulas (8) to (9) h Node with minimum path cost and no collision is taken as Q h Parent node Q of (1) parent ，
C(Q h )＝min{C(Q parent )+w L ||Q h -Q parent ||+w s α(Q ancestor ,Q parent ,Q h )},Q parent ∈T∩N(R next ) (8)
Wherein C (Q) h ) Representation node Q h To the starting point Q start Is equal to the path cost of N (R next ) Representing a grid R next Neighborhood of Q parent Is represented as falling within N (R next ) Nodes, C (Q parent ) Representation node Q parent To the starting point Q start Path cost, w L Weights representing path length, w s Weights representing path smoothness, |Q h -Q parent I represents the valid sampling point Q h To node Q parent Is of Europe of (A)Distance of (L), Q ancestor Representation node Q parent Is a parent node of alpha (Q) ancestor ,Q parent ,Q h ) Representing the path at node Q parent Steering angle at v 1 Representing the node Q ancestor Pointing to node Q parent Direction vector v of (v) 2 Representing the node Q parent Pointing to the effective sampling point Q h Is a vector of the direction of (2);
step 6.2: adding the effective sampling point for finding the father node into the random tree T, and if the effective sampling point Q is in the adding process h And Q is equal to parent Is connected with the line of (a)
Collision with an obstacle causes no effective sampling point Q h Adding to a random tree;
step 6.3: and (6) repeating the steps 6.1-6.2, calculating the father node of each effective sampling point and adding the father node into the random tree T.
Step 7: grid R using memory strategy next Status update is performed while updating grid R next The path cost of (a) is specifically expressed as: according to grid R next Whether num effective sampling points in the tree can be successfully added into the random tree T, and the grid R next The status update of (2) is divided into three cases:
1) If num valid sample points can be successfully added to the random tree T, then the grid R is applied next Mark as searched grid and store grid R next Added to L past In (2), then update R using equation (10) next Path cost C (R) next ) Finally, turning to the step 4.1 to continue execution;
C(R next )＝(∑C(Q h ))/num (10)
2) If none of the num valid sample points is successfully added to the random tree T, then the grid R is applied next Marked as unsearched grid, and from L next R is removed from next Finally, turning to the step 4.3 to continue execution;
3) If part of num valid sample points is validThe sampling points are successfully added to the random tree T, and then the grid R is obtained next Marked as unsearched grid, and from L next R is removed from next R is then updated with equation (10) based on valid sample points successfully added to the random tree T next Path cost C (R) next ) Finally, the process goes to step 4.3 to continue execution.
Step 8: repeating the steps 4-7 until the target point Q g o al Successfully adding the target point to the random tree T, and then searching a planning path to be smoothed from the target point to the starting point by using a backtracking method;
step 9: and deleting redundant nodes in the planned path to be smoothed by adopting a pruning algorithm, and smoothing the planned path to be smoothed after deleting the redundant nodes by adopting a Bezier curve to obtain a motion curve which can be used for track tracking and is used as a planned path for tracking the mobile robot.
The pseudocode of the modified RRT algorithm employed in the present invention is shown in table 1, with the programming software being python.
The invention aims to solve the problem that a great number of unnecessary redundant turning points exist in a path generated by an RRT algorithm, and uses a pruning algorithm to carry out smoothing treatment on the planned path obtained in the step 8, as shown in fig. 2, and specifically comprises the following steps:
step 9.1: marking all nodes on the broken line path obtained in the step 8 as P in turn 0 ,P 1 ,…,P s-1 Wherein s is the number of turning points including the start point and the target point on the path, wherein P 0 And P s-1 Respectively representing a starting point and a target point, and making the cache path be path= { P 0 }。
Step 9.2: attempt to connect point P start Initialized to the starting point P 0 Another attempted connection point is denoted as P end The pruning operation is implemented here using a dichotomy: (a) Using low and high to represent the attempted attachment point P end The lower limit and the upper limit of the subscript have initial values of low=start+1 and high=s-1 respectively; (b) End is initialized to be high, and P is judged start And P end Whether the wiring of (a) would cross an obstacle; (c) If crossing the obstacle, let high=end-1, end= (low+high)/2, return to step (b); (d) If the obstacle is not traversed, let low=end, end= (low+high+1)/2, return to step (b); (e) Repeating the steps (b) (c) (d), and when low=end, adding P end Added to path.
Step 9.3: will P start Assigned P end And repeating step 9.2;
step 9.4: repeating step 9.3 until P is reached s-1 And adding the path into the path, and then sequentially connecting nodes in the path to obtain a pruned path.
The path after pruning operation is still a folded segment and does not accord with the kinematic or dynamic constraint of the robot, so that the Bezier curve is continuously used for carrying out smoothing treatment on the path, as shown in fig. 3;
in order to verify the effectiveness and feasibility of the mobile robot path planning method based on the improved RRT algorithm, in a hardware environment with a main frequency of 3.4Ghz and a memory of 8GB of an Intel Core i7-6700 processor, using python to carry out simulation, wherein a map is a 900 multiplied by 900 two-dimensional plane, and a search step length is set to be S=10 in a simulation experiment.
Table 1 improved RRT algorithm pseudocode table
The RRT algorithm is a probabilistic complete search algorithm sensitive to the environment, when the structure of the environment is complex, the planning success rate of the algorithm is low, especially when a narrow channel or a non-convex obstacle exists in the environment, the algorithm often causes planning failure because of sinking into local optimization, the environment is divided into a convex obstacle environment and a non-convex obstacle environment according to the convexity of the obstacle, the convex obstacle environment refers to that all the obstacles in the environment are convex, the non-convex obstacle environment refers to that a concave obstacle exists in the environment, and a sample diagram of the convex obstacle environment and the non-convex obstacle environment is shown in fig. 4.
In order to verify the performances of the basic RRT algorithm in the two environments, 50 simulations are respectively carried out in the two environments, the maximum iteration number is set to 10000, and the experimental result shows that: for the convex obstacle environment, the searching success rate of the algorithm is 100%, the convergence speed is high, and the average iteration number is 1853; when the environment is a non-convex environment, the searching success rate of the algorithm is 38%, and the average iteration number is 7568; three groups of simulation results are randomly displayed in fig. 5, wherein graphs (a) to (c) represent simulation result graphs in a convex obstacle environment, graphs (d) to (f) represent simulation result graphs in a non-convex obstacle environment, gray lines in the graphs represent random trees generated by an improved RRT algorithm, black thin lines represent paths from target points to starting points in the random trees, black thick lines represent smooth paths obtained after pruning operation, and simulation shows that the RRT algorithm is easy to oversubsample in a local minimum area in the non-convex obstacle environment, and paths generated each time often deviate greatly.
In order to verify the capability of the improved RRT algorithm of the invention to adapt to the environment, simulation experiments are carried out in different environments, the expansion treatment is carried out on the obstacle according to the size of the robot, the experimental results are shown in fig. 6-8, and in four simulation maps shown in fig. 6, gray lines represent random trees generated by the improved RRT algorithm, black thin lines represent paths from target points to starting points in the random trees, and black thick lines represent smooth paths obtained after pruning operation; from simulation results, the path searched by the improved RRT algorithm provided by the invention is optimal, and a path can be planned in an extremely complex environment.
In order to verify the environmental adaptability of the improved RRT algorithm of the present invention, multiple experimental simulations are performed on four maps shown in fig. 6 (a) to (d) using the basic RRT algorithm, and the improved RRT algorithm herein, respectively, table 2 counts the success rate of planning of these algorithms in different environments, where the maximum iteration number is set to 40000, and maps 1 to Map4 in table 2 correspond to the four different maps shown in fig. 6 (a) to (d), respectively.
Table 2 basic RRT algorithm, experimental results comparison analysis table of basic RRT algorithm and modified RRT algorithm
It can be seen from table 2 that the improved RRT algorithm of the present invention has very strong adaptability, whether it is a simple convex obstacle environment or a complex non-convex obstacle environment.
In order to compare the basic RRT, and improve the stability and instantaneity of the RRT algorithm, multiple simulation experiments are performed in the second type of map shown in fig. 6 (b), and fig. 7 and 8 respectively count experimental data of the three algorithms in terms of path length and calculation time, and simulation results show that the improved RRT algorithm has stability in multiple simulations, regardless of path length or calculation time.
Claims (1)
1. The mobile robot path planning method based on the improved RRT algorithm is characterized by comprising the following steps of:
step 1: acquiring working environment information of the robot through a vision camera, a laser radar sensor, an ultrasonic sensor and an infrared sensor which are equipped with the mobile robot, and then establishing a Map of the working environment of the mobile robot by utilizing information fusion and SLAM technology;
step 2: defining a current position point of the mobile robot as a starting point Q start The task place of the mobile robot is a target point Q goal ；
Step 3: dividing the Map into a number of m×n grids according to a preset search step S, namely map= { R (i, j) }, R (i, j) representing the grid located in the ith row and jth column of the Map, wherein i=0, 1, …, m, j=0, 1, …, n, and then starting the Map at a point Q start The grid at which this is located is marked as the starting grid R (X start ,Y start ) Will target point Q goal The grid at which is located is marked as target grid R (X goal ,Y goal ) Wherein X is start 、Y start Respectively represents the horizontal and vertical coordinates, X of the initial grid on the Map goal 、Y goal Respectively representing the horizontal and vertical coordinates of the target grid on the Map;
wherein r is max Representing a maximum radius of the mobile robot;
step 4: determining the next grid R to be searched by adopting a neighborhood expansion strategy next ；
Step 5: to grid R next Random sampling experiment is carried out to determine grid R next The number num of valid sampling points in (1);
step 6: searching a parent node for each effective sampling point according to a neighborhood optimal principle, and adding the effective sampling point of the found parent node into a random tree T;
step 7: grid R using memory strategy next Status update is performed while updating grid R next Path cost of (a);
step 8: repeating the steps 4-7 until the target point Q goal Successfully adding the target point to the random tree T, and then searching a planning path to be smoothed from the target point to the starting point by using a backtracking method;
step 9: removing redundant nodes in the path to be smoothed by adopting a pruning algorithm, and then smoothing the path to be smoothed after removing the redundant nodes by adopting a Bezier curve to obtain a motion curve which can be used for track tracking and is used as a planning path for tracking the mobile robot;
the step 4 is specifically expressed as:
step 4.1: set L of searched grids past Initializing, denoted as L past ＝{R(X start ,Y start )}；
Step 4.2: calculating a grid set L to be searched in the next step by using a formula (2) to a formula (3) next ；
L next ＝{∪N(x,y)-L past |R(x,y)∈L past } (2)
N(x,y)＝{R(i,j)|R(i,j)∈Map,0<|x-i|≤1,0<|y-j|≤1} (3)
Wherein R (x, y) represents a searched grid, and N (x, y) represents a neighborhood of the searched grid R (x, y);
step 4.3: estimating the set L by using the formula (4) next Each grid of (a)
To the starting point Q start Path cost of (2)
Then selecting the grid corresponding to the minimum path cost as the grid R to be searched in the next step next ；
In the method, in the process of the invention,
representing the grid that may be searched next, C (R (i, j)) represents the grid R (i, j) to the starting point Q start Path cost of->
Representing grid->
Neighborhood of->
Respectively represent grid->
The abscissa in Map;
the step 5 is specifically expressed as:
step 5.1: to grid R next Performing N times of random sampling experiments, and calculating a grid R by using a formula (6) next The area ratio P occupied by the free space in (c),
P＝M/N (6)
wherein M represents the number of sample points falling in the free space;
step 5.2: judging grid R according to P next When p=0, represents the grid R next Is an obstacle grid; when p=1, it represents the grid R next Is a free grid; when 0 is<P<1, represents a grid R next Is an obstacle edge grid;
step 5.3: determining grid R using equation (7) next The number num of valid sampling points in the random sampling experiment, the valid sampling points are sample points falling in free space,
wherein ρ represents the grid R next Broadcast density of the effective sampling points in the (a), k represents a density weighting factor;
step 5.4: representing num valid sample points as a set L sample ＝{Q h |h＝1,2,…,num}，Q h Representing the h valid sampling point;
the step 6 is specifically expressed as:
step 6.1: at the effective sampling point Q h At grid R next In the neighborhood of (2), Q is calculated by using the formulas (8) to (9) h Node with minimum path cost and no collision is taken as Q h Parent node Q of (1) parent ，
C(Q h )＝min{C(Q parent )+w L ||Q h -Q parent ||+w s α(Q ancestor ,Q parent ,Q h )},Q parent ∈T∩N(R next ) (8)
Wherein C (Q) h ) Representation node Q h To the starting point Q start Is equal to the path cost of N (R next ) Representing a grid R next Neighborhood of Q parent Is represented as falling within N (R next ) Nodes, C (Q parent ) Representation node Q parent To the starting point Q start Path cost, w L Weights representing path length, w s Weights representing path smoothness, |Q h -Q parent I represents the valid sampling point Q h To node Q parent Euclidean distance, Q ancestor Representation node Q parent Is a parent node of alpha (Q) ancestor ,Q parent ,Q h ) Representing the path at node Q parent Steering angle at v 1 Representing the node Q ancestor Pointing to node Q parent Direction vector v of (v) 2 Representing the node Q parent Pointing to the effective sampling point Q h Is a vector of the direction of (2);
step 6.2: adding the effective sampling point for finding the father node into the random tree T, and if the effective sampling point Q is in the adding process h And Q is equal to parent Is connected with the line of (a)
Collision with an obstacle causes no effective sampling point Q h Adding to a random tree;
step 6.3: repeating the steps 6.1-6.2, calculating the father node of each effective sampling point and adding the father node into the random tree T;
the step 7 is specifically expressed as: according to grid R next Whether num effective sampling points in the tree can be successfully added into the random tree T, and the grid R next The status update of (2) is divided into three cases:
1) If num valid sample points can be successfully added to randomIn tree T, grid R next Mark as searched grid and store grid R next Added to L past In (2), then update R using equation (10) next Path cost C (R) next ) Finally, turning to the step 4.1 to continue execution;
C(R next )＝(∑C(Q h ))/num (10)
2) If none of the num valid sample points is successfully added to the random tree T, then the grid R is applied next Marked as unsearched grid, and from L next R is removed from next Finally, turning to the step 4.3 to continue execution;
3) If a part of the num valid sampling points is successfully added to the random tree T, the grid R is used for next Marked as unsearched grid, and from L next R is removed from next R is then updated with equation (10) based on valid sample points successfully added to the random tree T next Path cost C (R) next ) Finally, the process goes to step 4.3 to continue execution.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010466195.4A CN111610786B (en) | 2020-05-28 | 2020-05-28 | Mobile robot path planning method based on improved RRT algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010466195.4A CN111610786B (en) | 2020-05-28 | 2020-05-28 | Mobile robot path planning method based on improved RRT algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111610786A CN111610786A (en) | 2020-09-01 |
| CN111610786B true CN111610786B (en) | 2023-06-23 |
Family
ID=72194817
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010466195.4A Active CN111610786B (en) | 2020-05-28 | 2020-05-28 | Mobile robot path planning method based on improved RRT algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111610786B (en) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112711256B (en) * | 2020-12-23 | 2022-05-20 | 杭州电子科技大学 | Automatic generation method for target search observation points of mobile robot |
| CN113296498B (en) * | 2021-04-12 | 2022-09-27 | 山东科技大学 | Improved RRT path planning method based on adaptive resolution octree map |
| CN113156970B (en) * | 2021-05-08 | 2023-06-09 | 珠海一微半导体股份有限公司 | Path fusion planning method for traffic area, robot and chip |
| CN113934218B (en) * | 2021-11-16 | 2022-03-25 | 杭州云象商用机器有限公司 | Cleaning robot path planning method, device, equipment and storage medium |
| CN114115271B (en) * | 2021-11-25 | 2024-04-26 | 江苏科技大学 | Robot path planning method and system for improving RRT |
| CN114489052B (en) * | 2021-12-31 | 2024-05-28 | 杭州电子科技大学 | Path planning method for improving RRT algorithm reconnection strategy |
| CN115016458B (en) * | 2022-05-05 | 2024-04-16 | 安徽理工大学 | RRT algorithm-based path planning method for hole exploration robot |
| CN115077556B (en) * | 2022-07-26 | 2022-11-18 | 中国电子科技集团公司第二十八研究所 | Unmanned vehicle field operation path planning method based on multi-dimensional map |
| CN116578121B (en) * | 2023-07-10 | 2023-11-03 | 广东电网有限责任公司云浮供电局 | Constraint sampling-based generation method and track planning method for extended random tree |
| CN116922398B (en) * | 2023-09-15 | 2023-12-22 | 华侨大学 | Rope robot and path planning method and device thereof |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108983780A (en) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm |
-
2020
- 2020-05-28 CN CN202010466195.4A patent/CN111610786B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108983780A (en) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm |
Non-Patent Citations (5)
| Title |
|---|
| Jonathan D. Gammell.Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic.2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.2014,全文. * |
| Nan Chao.Grid-based RRT∗ for minimum dose walking path-planning in complex radioactive environments.《Annals of Nuclear Energy》.2018,Pages 73-82. * |
| wei zhang.Improve RRT algorithm for Path Planning in Complex Environments.2020 39th Chinese Control Conference (CCC).2020,全文. * |
| 冯来春;梁华为;杜明博;余彪.基于A~*引导域的RRT智能车辆路径规划算法.计算机***应用.2017,(第08期),第127-133页. * |
| 杨邱滟.基于Kinect的仿人机器人伺服抓取物体研究.中国优秀硕士学位论文全文数据库信息科技辑.2018, I140-666. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111610786A (en) | 2020-09-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111610786B (en) | Mobile robot path planning method based on improved RRT algorithm | |
| CN113219998B (en) | Improved bidirectional-RRT-based vehicle path planning method | |
| CN110887484B (en) | Mobile robot path planning method based on improved genetic algorithm and storage medium | |
| CN113467456B (en) | Path planning method for specific target search under unknown environment | |
| CN110908377B (en) | Robot navigation space reduction method | |
| Pirker et al. | CD SLAM-continuous localization and mapping in a dynamic world | |
| CN110083165A (en) | A kind of robot paths planning method under complicated narrow environment | |
| CN111562785A (en) | Path planning method and system for collaborative coverage of cluster robots | |
| CN116804879B (en) | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm | |
| CN113589809B (en) | Work track planning method and device for obstacle-avoidance excavator working device | |
| CN112486178A (en) | Dynamic path planning method based on directed D (delta) algorithm | |
| CN114877905A (en) | Inform-RRT path planning method for bidirectional dynamic growth | |
| LU102942B1 (en) | Path planning method based on improved a* algorithm in off-road environment | |
| CN113110507A (en) | Path planning method for autonomous obstacle avoidance | |
| Huang et al. | An online multi-lidar dynamic occupancy mapping method | |
| Bai et al. | Design and Simulation of a Collision-free Path Planning Algorithm for Mobile Robots Based on Improved Ant Colony Optimization. | |
| CN108227718B (en) | Self-adaptive switching automatic carrying trolley path planning method | |
| Wang et al. | Target-biased informed trees: Sampling-based method for optimal motion planning in complex environments | |
| Short et al. | Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps | |
| CN115826586B (en) | Path planning method and system integrating global algorithm and local algorithm | |
| CN117109625A (en) | Unmanned path planning method based on improved PRM algorithm | |
| CN117029846A (en) | Generalized laser ranging path planning algorithm for mobile robot in complex environment | |
| Seegmiller et al. | The Maverick planner: An efficient hierarchical planner for autonomous vehicles in unstructured environments | |
| CN115167476A (en) | Unmanned system path planning method based on deep reinforcement learning | |
| Zhang et al. | 2D map building and path planning based on LiDAR |
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 |