[Mobile Robot Motion Planning] 02 —— Planning Algorithm Based on Sampling

foreword

Part of the content of this article refers to the mobile robot motion planning of Deep Blue Academy , and makes relevant notes and arrangements accordingly. Previous articles have also introduced sampling-based algorithms, so this article does not focus on the basic concepts of such algorithms, but mainly supplements the previous articles.

Related code finishing:

1. https://gitee.com/lxyclara/motion-plan-homework/
2. https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master
3. https://gitee.com/aries-wu/Motion-plan/blob/main/
4. Link: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp Extraction code: rhtp

related articles:

Path planning for autonomous driving - path planning algorithm based on probability sampling (PRM) https://blog.csdn.net/sinat_52032317/article/details/127177278
Path planning for autonomous driving - path planning algorithm based on probability sampling (RRT, RRT *) https://blog.csdn.net/sinat_52032317/article/details/127197120

basic concept

Concept of planning completeness

• Complete Planner: always answers a path planning query correctly in bounded time
• Probabilistic Complete Planner: if a solution exists, planner will eventually find it, using random sampling (e.g. Monte Carlo sampling)
• Resolution Complete Planner: same as above but based on a deterministic sampling (e.g sampling on a fixed grid).【采样更确定】

Probabilistic Road Map

The previous blog has already introduced and code examples: Autonomous driving path planning - path planning algorithm based on probability sampling (PRM)

basic process

It can generally be divided into two phases: the preprocessing phase (Learing phase/preprocess phase) and the query phase (query phase).

preprocessing stage

• initialization . Let $G(V,E)$ is an undirected graph, where the vertex set$V$ represents the vertex set without collision, and the connection set$E$ stands for collision-free path. The initial state is empty.
• configuration sampling . Sample a collision-free point a ( i ) a(i) from configuration space$a\left(i\right)$ and added to the vertex setVVin$V.$
• field calculations . Define the distance $\rho$ , for the vertex set VVthat already existsA point in$V$$a\left(i\right)$ has a distance less than$\rho$ , then call it the pointa ( i ) a(i)The neighbor points of$a$$($$i$$)\; .$
• Border connections . Point $a\left(i\right)$ is connected with its domain points to generate a connection line$t$ .
• Collision detection . Detect connection $Whether\; \tau$ collides with the obstacle, if there is no collision, add it to the connection set$E.$ _
• end condition . When all sampling points (meeting the sampling quantity requirements) have completed the above steps, otherwise repeat 2-5.
  

query stage

Use algorithms such as AStar or Dijkstra to search from the start point to the end point.

Advantages and disadvantages (pros & cons)

A more detailed summary of features has been elaborated in the previous blog, and only a few key points are listed here.

advantage:

• Probabilistic completeness
• High efficiency in dealing with high-dimensional space planning
• Not easy to get stuck in local minima

shortcoming:

• Boundary value problems (kinematic constraints) are not considered yet
• The two-stage algorithm is tedious.

some improved algorithms

Lazy collision-checking

Improvement points:
No collision detection processing is performed when collecting points to build a map, and collision detection processing is only performed in the subsequent Search stage. If a collision is detected, delete the colliding points and edges in the path, reconstruct the road map, and search again until a path is found.

The following is a schematic diagram

Rapidly-exploring Random Tree

The previous blog has already introduced and code examples: Autonomous driving path planning - path planning algorithm based on probability sampling (RRT, RRT*)

Algorithm Pseudocode

some improved algorithms

KD-tree

Reference: https://blog.csdn.net/junshen1314/article/details/51121582

Use kd-tree to find the nearest node (find the median each time)

Bidirectional RRT / RRT Connect

The previous blog of Bidirectional RRT / RRT Connect has been introduced: Path planning for autonomous driving - path planning algorithm based on probability sampling (RRT, RRT*)

Optimal sampling-based path planning methods

Rapidly-exploring Random Tree*

This part can also refer to automatic driving path planning - path planning algorithm based on probability sampling (RRT, RRT*)

Algorithm pseudocode:

Kinodynamic-RRT*

Considering the Kinematic Constraints of Robots

Paper: Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints
https://arxiv.org/abs/1205.5088

Anytime-RRT*

During the robot movement, the RRT is always updated*

Anytime Motion Planning using the RRT*https://ieeexplore.ieee.org/document/5980479

Advanced Sampling-based Methods

Informed RRT*

Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic
https://ieeexplore.ieee.org/abstract/document/6942976

process

After the path is generated, the length of the path from the red point to the green point (the red part) is twice the semi-major axis (2a), and the red point and the green point are used as the focus to generate an ellipse. Sampling and planning are performed within the scope of the ellipse, and the above steps are repeated after regenerating the path. informed RRT* improves the planning speed, reduces CPU operation time, and makes the path smoother.

Cross-entropy motion planning

Cross-entropy motion planninghttps://ieeexplore.ieee.org/document/6301069

First get a path
and then use each point in the path as the center of a Gaussian model to sample in a multi-Gaussian model to get multiple paths.
Then average the multiple paths and rebuild the multi-Gaussian model.

other variants

• Lower Bound Tree RRT (LBTRRT)[a]
• Sparse Stable RRT[b]
• Transition-based RRT (T-RRT)[c]
• Vector Field RRT[d]
• Parallel RRT (pRRT)[e]
• Etc.[f]

[1] An Overview of the Class of Rapidly-Exploring Random Trees
[2] http://msl.cs.uiuc.edu/rrt/
[a] https://arxiv.org/pdf/1308.0189.pdf
[b] http://pracsyslab.org/sst_software
[c] http://homepages.laas.fr/jcortes/Papers/jaillet_aaaiWS08.pdf
[d] https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6606360
[e] https://robotics.cs.unc.edu/publications/Ichnowski2012_IROS.pdf
[f] https://github.com/zychaoqun

practice

[1] https://ompl.kavrakilab.org/
[2] https://moveit.ros.org/
[3] https://industrial-training-master.readthedocs.io/en/melodic/_source/session4/Motion-Planning-CPP.html

code link

Link: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp Extraction code: rhtp

homework ideas

[1] Chapter 3 homework ideas explanation 1
[2] Chapter 3 homework ideas explanation 2

MATLAB

RRT

RRT*

Goal-bias RRT*

Roughly calculate the average planning time (s) of RRT, RRTStar, Goal-bias RRTStar's 15 plans, the cost of the average planning path, the average number of nodes visited, and the node tree of the tree. MAP1, MAP2, and MAP3 are three kinds of maps. MAP1 is the map provided by the job, MAP2 is the narrow space test map, and MAP3 is the stricter narrow space test map. RRT, RRTStar, and Goal-bias RRTStar three algorithms use a step size of 20, of which the selection range of the choose parent part of RRTStar and Goal-bias RRTStar is 50, and the bias probability of Goal-bias RRTStar to the target point is 0.3.

Since RRT expands the tree structure by randomly sampling points, it can find the corresponding feasible path to the target point on the tree structure. Theoretically, the RRT algorithm is probabilistically complete. As the number of sampling points increases, the probability of finding a path approaches 1. Therefore, the distribution of sampling points largely determines the speed of the RRT algorithm to find a path.

The problem with RRT is that the algorithm is not asymptotically optimal, and RRTStar makes the algorithm achieve the characteristics of asymptotically optimal by reselecting parent nodes and rewiring, so it can make the path better and the cost value smaller, which is obtained from the average of the three maps The cost value of path planning can be seen. At the same time, the average number of nodes of RRTstar is much less than that of RRT tree, and the structure of the tree is more concise.

Goal-bias RRTStar simply adopts the method of heuristic target point bias: try to connect the tree and the target point at a certain probability, so that the tree grows more towards the target point, compare the left and right pictures above, and add heuristic target points Bias, RRT scales with fewer points and finds solutions faster. From the data in the table, it can be seen that the average planning time of Goal-bias RRTStar is much smaller than that of RRT and RRTStar, even in the case of narrow space, its planning speed is faster.

ROS

OMPL

OMPL, an open motion planning library, consists of many advanced sampling-based motion planning algorithms.
OMPL itself does not contain any code related to collision checking or visualization. This is an intentional design choice so that OMPL is not tied to a specific collision checker or visual front end. The library is designed so that it can be easily integrated into systems that provide other required components.

OMPL official website https://ompl.kavrakilab.org
Related documents https://ompl.kavrakilab.org/geometricPlanningSE3.html

OMPL call process

1. Build state space RealVectorStateSpace(3);
2. set state space boundaries realVectorBounds(3);
3. Build status information SpaceInformation;
4. Build problem instances ProblemDefinition;
5. Construct start and end points and set values ScopedState;
6. Set the optimization goal, here the path length is used;
7. Build planners Plannersuch as RRTstar;
8. planner solution Planner->solve();
9. If the solution is successful, call getSolutionPath()to get the path point

result