CN111610786A - Mobile robot path planning method based on improved RRT algorithm - Google Patents

Abstract

The invention provides a mobile robot path planning method based on an improved RRT algorithm in an indoor environment. Firstly, establishing a high-precision grid map of a robot working environment, dividing the map, then determining the next grid to be searched by adopting a neighborhood expansion strategy, determining the type of a searched grid and the number of effective sampling points by using a random sampling experiment, then adding the effective sampling points into a random tree by adopting a neighborhood optimal principle, updating the state of the grid based on a memory strategy, and finally smoothing the obtained planned path by adopting a pruning algorithm and a Bezier curve. The map segmentation and obstacle edge detection ideas are introduced, so that the sampling efficiency of the RRT algorithm in complex maps such as narrow channels is improved; due to the introduction of a memory mechanism and a neighborhood expansion strategy, the success rate of planning of the algorithm in a complex environment is improved; and the father 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

Mobile robot path planning method based on improved RRT algorithm

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

With the development and progress of science and technology, the intelligent robot is more and more widely applied to human life. The path planning is used as a key technology of the intelligent robot, is the basis for executing various high-level tasks and realizing autonomous navigation, and aims to search a collision-free path from a starting point to a target point in an environment with obstacles. The currently commonly used path planning algorithms mainly include a fast-expanding random tree (RRT), a random road mapping 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. The A-star and Dijkstra algorithms are 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 fuzzy logic algorithm is influenced by an environment structure and is easy to fall into a local minimum value, so that the path planning fails; the intelligent bionic algorithms such as particle swarms and ant swarms have the problems of large calculated amount and poor real-time performance, and the setting of related parameters in the algorithms seriously influences the success rate of planning, so that the intelligent bionic algorithms cannot be effectively adapted to various environments of different types; the RRT and the PRM can only ensure completeness on probability, and have the defects of inconsistent multi-time planning results, non-optimal path, low success rate in complex environments and the like.

The fast-expanding 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 be combined with the kinematic or dynamic constraint of a mobile robot to plan a path. However, the planning success rate of the RRT algorithm is affected by the environmental structure or the shape of the obstacle: when no narrow passage exists in the environment and all the obstacles are convex, the algorithm can plan a path with higher success rate; on the contrary, if there are some concave obstacles in the environment, such as "walls" in an indoor scene, the algorithm is easily trapped in a local minimum, resulting in a low success rate of the algorithm planning. Meanwhile, the RRT algorithm has strong randomness, and the planned path is often far away from the optimal solution. Although the RRT algorithm can achieve a gradual optimization of the path by introducing a "rewiring" process, such optimization sacrifices the performance of the RRT algorithm in rapidly converging, and is not suitable for 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 an RRT algorithm and the like of the RRT algorithm, the invention provides a mobile robot path planning method based on an improved RRT algorithm, which overcomes the defects of the prior art, improves the planning success rate and real-time performance of the algorithm in the complex environment, reduces the randomness of the path and enables the planned path to be close to the optimal path as much as possible.

In order to achieve the technical effect, the invention provides a mobile robot path planning method based on an improved RRT algorithm, which comprises the following steps:

step 1: the method comprises the steps that working environment information of the robot is collected through a vision camera, a laser radar sensor, an ultrasonic sensor and an infrared sensor which are arranged on the mobile robot, and then a Map of the working environment of the mobile robot is established by utilizing information fusion and SLAM technology;

step 2: defining the current position point of the mobile robot as a starting point Q_startThe task place of the mobile robot is a target point Q_goal；

Step 3, dividing the Map into m × n grids according to a preset search step S, namely Map is { R (i, j) }, R (i, j) represents the grids positioned in the ith row and the jth column in the Map, wherein i is 0,1, …, m, j is 0,1, …, n, and then setting a starting point Q_startThe grid where is marked as the starting grid R (X)_start,Y_start) Aim point Q_goalThe grid where is marked as target grid R (X)_goal,Y_goal) Wherein X is_start、Y_startRespectively represent the horizontal and vertical coordinates, X, of the starting grid on the Map_goal、Y_goalRespectively representing the horizontal coordinates and the vertical coordinates of the target grid on the Map;

in the formula, r_maxRepresents a maximum radius of the mobile robot;

and 4, step 4: determining the next grid R to be searched by adopting a neighborhood expansion strategy_next；

And 5: to the grid R_nextPerforming random sampling experiment to determine grid R_nextThe number num of middle effective sampling points;

step 6: according to a neighborhood optimization principle, searching a father node for each effective sampling point, and adding the effective sampling points of the found father node into the random tree T;

and 7: using memory strategy to mesh R_nextPerforming state updates while updating grid R_nextThe path cost of (2);

and 8: repeating the steps 4 to 7 until the target point Q_goalSuccessfully adding the path to the random tree T, and then searching a path to be smoothly planned from a target point to a starting point by using a backtracking method;

and step 9: and deleting redundant nodes in the path to be smoothly planned by adopting a pruning algorithm, and then smoothing the path to be smoothly planned after the redundant nodes are deleted by adopting a Bezier curve to obtain a motion curve which can be used for track tracking and is used as the planned path of the tracking mobile robot.

The step 4 is specifically expressed as follows:

step 4.1: for the set L of searched grids_pastIs initialized and is marked as L_past＝{R(X_start,Y_start)}；

Step 4.2: calculating a grid set L needing to be searched next step by using the formulas (2) to (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 the searched grid, N (x, y) represents the neighborhood of the searched grid R (x, y);

step 4.3: predicting the set L using equation (4)_nextEach grid in

To the starting point Q_startPath cost of

Then, the grid corresponding to the minimum path cost is selected as the grid R to be searched next step_next；

In the formula (I), the compound is shown in the specification,

representing the grid that may be searched next, and C (R (i, j)) representing the grid R (i, j) to the starting point Q_startThe cost of the path of (a) is,

representation grid

The neighborhood of (a) is determined,

respectively representing grids

Abscissa and ordinate in Map.

The step 5 is specifically expressed as follows:

step 5.1: to the grid R_nextPerforming N random sampling experiments, and calculating the grid R by using a formula (6)_nextThe area ratio P occupied by the medium free space,

P＝M/N (6)

in the formula, M represents the number of sample points falling in free space;

step 5.2: judging grid R according to P_nextWhen P is 0, denotes a grid R_nextIs a barrier grid; when P is 1, it represents a grid R_nextIs a free grid; when 0 is present<P<At 1, it represents a grid R_nextAs barrier edge grids；

Step 5.3: determining the grid R using equation (7)_nextIs the number num of valid sampling points that fall within free space in a random sampling experiment,

where ρ represents a grid R_nextThe broadcast density of the middle effective sampling point, and 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_hRepresenting the h-th valid sample point.

The step 6 is specifically expressed as:

step 6.1: at the effective sampling point Q_hGrid of positions R_nextIn the neighborhood of (2), Q is calculated by the following equations (8) to (9)_hThe node with the minimum path cost and no collision is taken as Q_hParent node Q of_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)

In the formula, C (Q)_h) Representing a node Q_hTo the starting point Q_startPath cost of N (R)_next) Represents a grid R_nextNeighborhood of (2), Q_parentDenotes a number falling within N (R)_next) Node, C (Q), within and having been added to the random tree T_parent) Representing a node Q_parentTo the starting point Q_startPath cost of, w_LWeight, w, representing path length_sWeight representing smoothness of the path, | Q_h-Q_parentI represents the effective sampling point Q_hTo node Q_parentEuclidean distance of, Q_ancestorRepresenting a node Q_parentParent node of α (Q)_ancestor,Q_parent,Q_h) Representing a path at node Q_parentSteering angle of (v)₁Representation is represented by node Q_ancestorPoint of reference Q_parentDirection vector of (v)₂Representation is represented by node Q_parentPointing to valid sampling point Q_hThe direction vector of (a);

step 6.2: adding the effective sampling points of the found father nodes into the random tree T, and if the effective sampling points Q are added in the adding process_hAnd Q_parentOf (2) a connection line

When colliding with the obstacle, the effective sampling point Q cannot be obtained_hAdding the random tree into a random tree;

step 6.3: and 6.1-6.2 are repeated, and the father node of each effective sampling point is calculated and added into the random tree T.

The step 7 is specifically expressed as: according to a grid R_nextWhether the inner num effective sampling points can be successfully added into the random tree T or not, and the grid R is added_nextThe state update of (2) is divided into three cases:

1) if num valid sample points can be successfully added to the random tree T, the grid R is added_nextMarking as searched grid and marking grid R_nextIs added to L_pastThen, R is updated using the formula (10)_nextPath cost of C (R)_next) Finally, go to 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, the trellis R is added_nextMarking as unsearched grid and from L_nextIn which R is removed_nextFinally, go to step 4.3 to continue execution;

3) if some of the num valid samples are successfully added to the random tree T, the trellis R is set_nextMarking as unsearched grid and from L_nextIn which R is removed_nextR is then updated using equation (10) based on the valid sample points successfully added to the random tree T_nextPath cost of C (R)_next) Finally, go to step 4.3 to continue execution.

The invention has the beneficial effects that:

the invention provides a mobile robot path planning method based on an improved RRT algorithm, which is improved from the following three aspects aiming at the problems of the basic RRT algorithm and the RRT algorithm:

1) the map segmentation and obstacle edge detection ideas are introduced into an RRT basic frame, so that the sampling efficiency of the algorithm in complex maps such as narrow channels is improved;

2) a memory mechanism and a neighborhood expansion strategy are introduced into an RRT basic framework, so that the problem of excessive search 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 the optimal mode of the neighborhood, so that the problems of strong randomness, non-optimal or suboptimal path and the like of an RRT algorithm are solved;

practice proves that the method provided by the invention is not only suitable for simple structured environment, but also suitable for complex unstructured environment.

Drawings

Fig. 1 is a flow chart 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 sample diagram of a convex obstacle environment and a non-convex obstacle environment in the present invention, in which (a) shows a sample diagram of a convex obstacle environment and (b) shows a sample diagram of a non-convex obstacle environment.

Fig. 5 is a graph of three-time simulation results of the basic RRT algorithm in the convex obstacle environment and the non-convex obstacle environment, where (a) to (c) show the simulation results in the convex obstacle environment, and (d) to (f) show the simulation results in the non-convex obstacle environment.

Fig. 6 is a simulation result diagram of the mobile robot path planning method based on the improved RRT algorithm in different maps, wherein 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 graph 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 is further described with reference to the following figures and specific examples.

As shown in fig. 1, a method for planning a path of a mobile robot based on an improved RRT algorithm includes the following steps:

step 1: the method comprises the steps that working environment information of a robot is collected through a vision camera, a laser radar sensor, an ultrasonic sensor And an infrared sensor which are arranged on the mobile robot, then a Map of the working environment of the mobile robot is established by utilizing information fusion And SLAM (Simultaneous localization And Mapping for short SLAM) technology, wherein the vision camera collects image information of obstacles, the laser radar sensor And the ultrasonic sensor collect position information of surrounding objects, the infrared sensor collects distance information between the mobile robot And the obstacles, And in the graphs of 4-6, a black part is an obstacle area And a white part is a free area;

step 2: defining the current position point of the mobile robot as a starting point Q_startThe task place of the mobile robot is a target point Q_goal；

Step 3, dividing the Map into m × n grids according to a preset search step S, namely Map is { R (i, j) }, R (i, j) represents grids positioned at the ith row and the jth column in the Map, wherein i is 0,1, …, m, j is 0,1, …, n, and then setting a starting point Q_startGrid mark of placeIs a starting grid R (X)_start,Y_start) Aim point Q_goalThe grid where is marked as target grid R (X)_goal,Y_goal) Wherein X is_start、Y_startRespectively represent the horizontal and vertical coordinates, X, of the starting grid on the Map_goal、Y_goalRespectively representing the horizontal coordinates and the vertical coordinates of the target grid on the Map;

in the formula, r_maxRepresents a maximum radius of the mobile robot;

and 4, step 4: determining the next grid R to be searched by adopting a neighborhood expansion strategy_nextThe concrete expression is as follows:

step 4.1: for the set L of searched grids_pastIs initialized and is marked as L_past＝{R(X_start,Y_start)}；

Step 4.2: calculating a grid set L needing to be searched next step by using the formulas (2) to (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 the searched grid, N (x, y) represents the neighborhood of the searched grid R (x, y);

step 4.3: predicting the set L using equation (4)_nextEach grid in

To the starting point Q_startPath cost of

Then, the grid corresponding to the minimum path cost is selected as the grid R to be searched next step_next；

In the formula (I), the compound is shown in the specification,

representing the grid that may be searched next, and C (R (i, j)) representing the grid R (i, j) to the starting point Q_startThe cost of the path of (a) is,

representation grid

The neighborhood of (a) is determined,

respectively representing grids

Abscissa and ordinate in Map.

And 5: to the grid R_nextPerforming random sampling experiment to determine grid R_nextThe number num of the middle effective sampling points is specifically expressed as follows:

step 5.1: to the grid R_nextPerforming N random sampling experiments, and calculating the grid R by using a formula (6)_nextThe area ratio P occupied by the medium free space,

P＝M/N (6)

in the formula, M represents the number of sample points falling in free space;

step 5.2: judging grid R according to P_nextWhen P is 0, denotes a grid R_nextIs a barrier grid; when P is 1, it represents a grid R_nextIs a free grid; when 0 is present<P<At 1, it represents a grid R_nextIs a barrier edge grid;

step 5.3: determining the grid R using equation (7)_nextNumber of valid sampling points num in (1), saidThe effective sampling points are sample points falling in free space in a random sampling experiment,

where ρ represents a grid R_nextThe broadcast density of the middle effective sampling point, and 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_hRepresenting the h-th valid sample point.

Step 6: according to the neighborhood optimization principle, a father node is searched for each effective sampling point, the effective sampling points of the found father nodes are added into a random tree T, and the method is specifically expressed as follows:

step 6.1: at the effective sampling point Q_hGrid of positions R_nextIn the neighborhood of (2), Q is calculated by the following equations (8) to (9)_hThe node with the minimum path cost and no collision is taken as Q_hParent node Q of_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)

In the formula, C (Q)_h) Representing a node Q_hTo the starting point Q_startPath cost of N (R)_next) Represents a grid R_nextNeighborhood of (2), Q_parentDenotes a number falling within N (R)_next) Node, C (Q), within and having been added to the random tree T_parent) Representing a node Q_parentTo the starting point Q_startPath cost of, w_LWeight, w, representing path length_sWeight representing smoothness of the path, | Q_h-Q_parentI represents the effective sampling point Q_hTo node Q_parentEuclidean distance ofFrom, Q_ancestorRepresenting a node Q_parentParent node of α (Q)_ancestor,Q_parent,Q_h) Representing a path at node Q_parentSteering angle of (v)₁Representation is represented by node Q_ancestorPoint of reference Q_parentDirection vector of (v)₂Representation is represented by node Q_parentPointing to valid sampling point Q_hThe direction vector of (a);

step 6.2: adding the effective sampling points of the found father nodes into the random tree T, and if the effective sampling points Q are added in the adding process_hAnd Q_parentOf (2) a connection line

When colliding with the obstacle, the effective sampling point Q cannot be obtained_hAdding the random tree into a random tree;

step 6.3: and 6.1-6.2 are repeated, and the father node of each effective sampling point is calculated and added into the random tree T.

And 7: using memory strategy to mesh R_nextPerforming state updates while updating grid R_nextThe path cost is specifically expressed as: according to a grid R_nextWhether the inner num effective sampling points can be successfully added into the random tree T or not, and the grid R is added_nextThe state update of (2) is divided into three cases:

1) if num valid sample points can be successfully added to the random tree T, the grid R is added_nextMarking as searched grid and marking grid R_nextIs added to L_pastThen, R is updated using the formula (10)_nextPath cost of C (R)_next) Finally, go to 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, the trellis R is added_nextMarking as unsearched grid and from L_nextIn which R is removed_nextFinally, go to step 4.3 to continue execution;

3) if part of num valid samples are effectively sampledIf the point is successfully added to the random tree T, the grid R is added_nextMarking as unsearched grid and from L_nextIn which R is removed_nextR is then updated using equation (10) based on the valid sample points successfully added to the random tree T_nextPath cost of C (R)_next) Finally, go to step 4.3 to continue execution.

And 8: repeating the steps 4 to 7 until the target point Q_go_alSuccessfully adding the path to the random tree T, and then searching a path to be smoothly planned from a target point to a starting point by using a backtracking method;

and step 9: and deleting redundant nodes in the path to be smoothly planned by adopting a pruning algorithm, and then smoothing the path to be smoothly planned after the redundant nodes are deleted by adopting a Bezier curve to obtain a motion curve which can be used for track tracking and is used as the planned path of the tracking mobile robot.

The pseudo code for the improved RRT algorithm used in the present invention is shown in table 1, with the programming software used being python.

In order to solve the problem that a path generated by an RRT algorithm has a large number of unnecessary redundant turning points, the invention uses a pruning algorithm to carry out smooth processing on the planned path obtained in the step 8, and as shown in FIG. 2, the invention specifically comprises the following steps:

step 9.1: marking all nodes on the broken line path obtained in the step 8 as P in sequence₀,P₁,…,P_s-1Where s is the number of turning points on the path including the starting point and the target point, where P₀And P_s-1Respectively representing the start point and the target point, and setting the cache path as path { P ═ P₀}。

Step 9.2: trying to connect to point P_startInitialized to a starting point P₀Another attempted connection point is denoted as P_endHere, the pruning operation is implemented using the dichotomy: (a) the attempted connection point P is indicated by low and high_endThe lower limit and the upper limit of the subscript respectively have the initial values of low ═ start +1 and high ═ s-1; (b) initializing end to high, judging P_startAnd P_endWhether the connection line of (a) will cross the barrier; (c) if the obstacle is crossed, let high ═end-1, end ═ low + high)/2, return to step (b); (d) if the obstacle is not crossed, making low equal to end and end equal to (low + high +1)/2, and returning to the step (b); (e) repeating the steps (b), (c) and (d), and when low is end, adding P_endAdded to the path.

Step 9.3: will P_startAssigned a value of P_endAnd repeating step 9.2;

step 9.4: repeat step 9.3 until P is reached_s-1Adding the path into the path, and then sequentially connecting the junction points in the path to obtain the pruned path.

The path after pruning operation is still a broken line segment, which does not conform to the kinematics or dynamics constraint of the robot, so the path is continuously smoothed by using a Bezier curve, 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, simulation is carried out by using python under the hardware environment of an Intel Core i7-6700 processor with the main frequency of 3.4Ghz and the internal memory of 8GB, a map is a two-dimensional plane of 900 x 900, and the search step length is set to be S-10 in a simulation experiment.

TABLE 1 pseudo code table for improved RRT algorithm

The RRT algorithm is a complete search algorithm with probability sensitive to the environment, when the structure of the environment is complex, the success rate of planning of the algorithm is low, particularly when a narrow channel or a non-convex obstacle exists in the environment, the algorithm often fails to plan due to the fact that the narrow channel or the non-convex obstacle exists in the environment, the environment is divided into a convex obstacle environment and a non-convex obstacle environment according to the concave-convex performance of the obstacle, the convex obstacle environment refers to the situation that all obstacles in the environment are convex, the non-convex obstacle environment refers to the situation that a concave obstacle exists in the environment, and a sample graph of the convex obstacle environment and the non-convex obstacle environment is shown in FIG. 4.

In order to verify the performance of the basic RRT algorithm in the two environments, 50 simulations are performed in the two environments respectively, and the maximum iteration number is set to 10000 times, and the experimental results show that: for the convex obstacle environment, the search success rate of the algorithm is 100%, the convergence rate is high, and the average iteration number is 1853; when the environment is a non-convex environment, the search success rate of the algorithm is 38%, and the average iteration number is 7568; three groups of simulation results are randomly shown 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 the 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, simulation shows that the RRT algorithm is easy to generate oversampling in a local minimum area in the non-convex obstacle environment, and the paths generated each time are often very deviated.

In order to verify the ability of the improved RRT algorithm to adapt to the environment, simulation experiments are performed in different environments, the obstacle has been subjected to expansion processing according to the size of the robot, the experimental results are shown in fig. 6 to 8, and in four simulation maps given in fig. 6, wherein a gray line represents a random tree generated by the improved RRT algorithm, a black thin line represents a path from a target point to a starting point in the random tree, and a black thick line represents a smooth path obtained after pruning; as can be seen 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 environment adaptability of the improved RRT algorithm of the present invention, a plurality of experimental simulations are performed on four maps shown in fig. 6 (a) to (d) below using the basic RRT algorithm, and the improved RRT algorithm herein, respectively, table 2 shows the success rate of planning the algorithms in different environments, where the maximum number of iterations is set to 40000, and maps 1 to Map4 in table 2 correspond to four different maps shown in fig. 6 (a) to (d), respectively.

TABLE 2 comparative analysis table of experimental results of basic RRT algorithm, basic RRT algorithm and improved RRT algorithm

As can be seen from Table 2, the improved RRT algorithm of the present invention has strong adaptability to both simple convex obstacle environments and complex non-convex obstacle environments.

In order to compare the basic RRT, and improve the stability and real-time performance of the RRT algorithm, a plurality of simulation experiments are performed in a second type of map shown in fig. 6 as a map (b), fig. 7 and 8 respectively count the experimental data of the three algorithms in terms of both the path length and the calculation time, and the simulation results show that the improved RRT algorithm has stability in a plurality of simulations regardless of the path length or the calculation time.

Claims (5)

1. A mobile robot path planning method based on an improved RRT algorithm is characterized by comprising the following steps:

step 1: the method comprises the steps that working environment information of the robot is collected through a vision camera, a laser radar sensor, an ultrasonic sensor and an infrared sensor which are arranged on the mobile robot, and then a Map of the working environment of the mobile robot is established by utilizing information fusion and SLAM technology;

step 2: defining the current position point of the mobile robot as a starting point Q_startThe task place of the mobile robot is a target point Q_goal；

Step 3, dividing the Map into m × n grids according to a preset search step S, namely Map is { R (i, j) }, R (i, j) represents grids positioned at the ith row and the jth column in the Map, wherein i is 0,1, …, m, j is 0,1, …, n, and then setting a starting point Q_startThe grid where is marked as the starting grid R (X)_start,Y_start) Aim point Q_goalThe grid where is marked as target grid R (X)_goal,Y_goal) Wherein X is_start、Y_startRespectively represent the horizontal and vertical coordinates, X, of the starting grid on the Map_goal、Y_goalRespectively representing the horizontal coordinates and the vertical coordinates of the target grid on the Map;

in the formula, r_maxRepresents a maximum radius of the mobile robot;

and 4, step 4: determining the next grid R to be searched by adopting a neighborhood expansion strategy_next；

And 5: to the grid R_nextPerforming random sampling experiment to determine grid R_nextThe number num of middle effective sampling points;

step 6: according to a neighborhood optimization principle, searching a father node for each effective sampling point, and adding the effective sampling points of the found father node into the random tree T;

and 7: using memory strategy to mesh R_nextPerforming state updates while updating grid R_nextThe path cost of (2);

and 8: repeating the steps 4 to 7 until the target point Q_goalSuccessfully adding the path to the random tree T, and then searching a path to be smoothly planned from a target point to a starting point by using a backtracking method;

and step 9: and deleting redundant nodes in the path to be smoothly planned by adopting a pruning algorithm, and then smoothing the path to be smoothly planned after the redundant nodes are deleted by adopting a Bezier curve to obtain a motion curve which can be used for track tracking and is used as the planned path of the tracking mobile robot.

2. The method for planning the path of the mobile robot based on the improved RRT algorithm of claim 1, wherein the step 4 is specifically expressed as:

step 4.1: for the set L of searched grids_pastIs initialized and is marked as L_past＝{R(X_start,Y_start)}；

Step 4.2: calculating a grid set L needing to be searched next step by using the formulas (2) to (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 the searched grid, N (x, y) represents the neighborhood of the searched grid R (x, y);

step 4.3: predicting the set L using equation (4)_nextEach grid in

To the starting point Q_startPath cost of

Then, the grid corresponding to the minimum path cost is selected as the grid R to be searched next step_next；

In the formula (I), the compound is shown in the specification,

representing the grid that may be searched next, and C (R (i, j)) representing the grid R (i, j) to the starting point Q_startThe cost of the path of (a) is,

representation grid

The neighborhood of (a) is determined,

respectively representing grids

Sit-ups and sit-downs in MapAnd (4) marking.

3. The method for planning the path of the mobile robot based on the improved RRT algorithm as claimed in claim 1, wherein the step 5 is specifically expressed as:

step 5.1: to the grid R_nextPerforming N random sampling experiments, and calculating the grid R by using a formula (6)_nextThe area ratio P occupied by the medium free space,

P＝M/N (6)

in the formula, M represents the number of sample points falling in free space;

step 5.2: judging grid R according to P_nextWhen P is 0, denotes a grid R_nextIs a barrier grid; when P is 1, it represents a grid R_nextIs a free grid; when 0 is present<P<At 1, it represents a grid R_nextIs a barrier edge grid;

step 5.3: determining the grid R using equation (7)_nextIs the number num of valid sampling points that fall within free space in a random sampling experiment,

where ρ represents a grid R_nextThe broadcast density of the middle effective sampling point, and 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_hRepresenting the h-th valid sample point.

4. The method for planning the path of the mobile robot based on the improved RRT algorithm of claim 1, wherein the step 6 is specifically expressed as:

step 6.1: at the effective sampling point Q_hGrid of positions R_nextIn the neighborhood of (2), Q is calculated by the following equations (8) to (9)_hThe node with the minimum path cost and no collision is used as the nodeQ_hParent node Q of_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)

In the formula, C (Q)_h) Representing a node Q_hTo the starting point Q_startPath cost of N (R)_next) Represents a grid R_nextNeighborhood of (2), Q_parentDenotes a number falling within N (R)_next) Node, C (Q), within and having been added to the random tree T_parent) Representing a node Q_parentTo the starting point Q_startPath cost of, w_LWeight, w, representing path length_sWeight representing smoothness of the path, | Q_h-Q_parentI represents the effective sampling point Q_hTo node Q_parentEuclidean distance of, Q_ancestorRepresenting a node Q_parentParent node of α (Q)_ancestor,Q_parent,Q_h) Representing a path at node Q_parentSteering angle of (v)₁Representation is represented by node Q_ancestorPoint of reference Q_parentDirection vector of (v)₂Representation is represented by node Q_parentPointing to valid sampling point Q_hThe direction vector of (a);

step 6.2: adding the effective sampling points of the found father nodes into the random tree T, and if the effective sampling points Q are added in the adding process_hAnd Q_parentOf (2) a connection line

When colliding with the obstacle, the effective sampling point Q cannot be obtained_hAdding the random tree into a random tree;

step 6.3: and 6.1-6.2 are repeated, and the father node of each effective sampling point is calculated and added into the random tree T.

5. The method for planning the path of the mobile robot based on the improved RRT algorithm of claim 1, wherein the step 7 is specifically expressed as: according to a grid R_nextWhether the inner num effective sampling points can be successfully added into the random tree T or not, and the grid R is added_nextThe state update of (2) is divided into three cases:

1) if num valid sample points can be successfully added to the random tree T, the grid R is added_nextMarking as searched grid and marking grid R_nextIs added to L_pastThen, R is updated using the formula (10)_nextPath cost of C (R)_next) Finally, go to 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, the trellis R is added_nextMarking as unsearched grid and from L_nextIn which R is removed_nextFinally, go to step 4.3 to continue execution;

3) if some of the num valid samples are successfully added to the random tree T, the trellis R is set_nextMarking as unsearched grid and from L_nextIn which R is removed_nextR is then updated using equation (10) based on the valid sample points successfully added to the random tree T_nextPath cost of C (R)_next) Finally, go to step 4.3 to continue execution.

Images