Probabilistic Roadmaps for Robot Path Planning in C++
In the following article, I will describe the Probabilistic Roadmaps (PRM) algorithm which is often used in robotics (path planning for manipulators).
The PRM is a robotics motion planning method that addresses the challenge of choosing a path between a robot’s initial configuration and its desired configuration while avoiding collisions. PRM algorithm is particularly interesting for manipulators with many degrees of freedom (DOF).
I implemented PRM in C++ so the source code is available on my Github. Plotting requires incorporating the header file which has to be in the same folder as your cpp (a file you can clone from my repository).
Your program can be compiled as follows.
//compile
g++ my_prog.cpp -o my_prog -I/usr/include/python3.8 -lpython3.8//
//run
./my_prog
//folder tree
├── my_prog
├── my_prog.cpp
├── matplotlibcpp.hPRM algorithm was formulated by Lydia E. Kavraki in the paper Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces.
We can simplify the presented discussion and display the general approach which I used while deploying in C++.
The PRM consists of two main stages. The learning phase and the query phase. During the learning phase, a PRM is built and stored as a graph, with nodes denoting collision-free configurations. Roads between free collision nodes are connected using the k-nearest neighbor's algorithm (details later).
In the second stage, the query stage PRM algorithm use A* algorithm to connect (using in the previous stage built graph) any given robot start configuration (node in the graph) and goal configuration (node in the graph). The details of A* algorithm you will find in one of my articles here.
It is possible to apply the Dijaktra algorithm instead of A*, however, the performance as shown in simulations (see below links) is lower. I discussed the Dijaktra algorithm also here.
Lydia E. Kavraki pioneered the PRM algorithm for high-dimensional manipulators. In this case, the node in the graph is represented in high dimensional space. If we consider a 6DOF robot, the node is defined in 6-dimensional space (joint space). High-dimensional space is rather hard to visualize. In this article, I do not transfer node to joint space therefore the robot and node are considered 2D (XY) space.
I recommend checking the excellent YouTube channel for more detailed information. You can find there also the link where you can play the algorithm online.
Study the below figure of how PRM works and how I deployed this algorithm in C++.
Deployed PRM algorithm is parameterized by the number of random nodes generated, and the maximum distance the nodes are connected (KNN algorithm). This influence the performance which can be seen in the below figures.
In the discussed PRM implementation I used two obstacles. You can change their location. The start and the goal are also adjustable.
Thank you for reading.
