1. Introduction
- Identify different types of path planning algorithms
- Understand the inner workings of a set of algorithms
- Evaluate the suitability of an algorithm for a specific application
- Implement the search algorithm
2. Example of path planning
the term
Completeness - An algorithm is complete if it can find a path between the starting point and the goal.
Optimality - An algorithm is optimal if it can find the best solution.
bug algorithm
The following problem demonstrates an example where there is a solution, but the bug algorithm cannot find it:
The robot will endlessly traverse the perimeter of the outer wall of the obstacle, but some variants of the bug algorithm can make up for this error. Most path planning algorithms rely on other principles that will be introduced in this article. When studying new algorithms, we will reconsider the concepts of completeness and optimality when analyzing the applicability of an algorithm to a task.
3. Path Planning Methods
Path Planning Methods
In this article, three different path planning methods will be studied. The first, called discrete (or combinatorial) path planning, is the most straightforward of the three methods. Two other approaches, called sample-based path planning and probabilistic path planning, build on discrete programming to develop more broadly applicable path planning solutions.
Discrete programming
Discrete programming is to explicitly discretize the robot's workspace into a connected graph, and apply the graph-search (graph search) algorithm to calculate the optimal path. This process is very precise (in fact, the precision can be tuned explicitly by changing how finely the space is discretized), and very thorough, since it discretizes the entire workspace. As a result, discrete programming can be very computationally expensive—which can be prohibitive for large path planning problems.
The figure below shows one possible implementation of discrete path planning applied to a 2D workspace.
Discrete path planning is elegant in its accuracy, but is best suited for low-dimensional problems. For high-dimensional problems, sample-based path planning is a more appropriate approach.
Sample-Based Planning
Sample-based path planning probes the workspace to incrementally construct a graph. Unlike each part of the discrete workspace, sample-based planning takes many samples and uses them to build a discrete representation of the workspace. The resulting graphs are not as accurate as those created using discrete programming, but are much faster to construct because of the relatively small number of samples used.
Paths generated using sample-based planning may not be optimal, but in some applications - generating feasible paths quickly is better than waiting hours or even days to generate optimal paths.
The figure below shows a graphical representation of a 2D workspace created using sample-based planning.
Probabilistic Path Planning
The final type of path planning studied in this module is probabilistic path planning. Whereas the first two approaches consider the path planning problem generically—without knowing who or what might be performing the action—probabilistic path planning takes into account the uncertainty of the robot's motion.
While this may not provide a significant benefit in some environments, it is especially useful in highly restricted environments or those with sensitive or high-risk areas.
The figure below shows probabilistic path planning applied to an environment containing hazards (the lake in the upper right):
Multi-Lesson map
In this article, learn several discrete path planning algorithms and sample-based and probability planning, and then apply it to path planning experiments, and use C++ to write search algorithms.
4. Continuity Representation
In order to account for the geometry of the robot and simplify the task of path planning, the obstacles in the workspace can be inflated to create a new space called configuration space (or C-space). The edge of the obstacle expands with the radius of the robot, and the robot body can be regarded as a point, making it easier for the algorithm to search for a path. C space is the collection of all robot poses, which can be decomposed into C_Free and C_Obs.
5. Minkowski sum
Minkowski sum
Minkowski sum is a mathematical property that can be used to compute the configuration space for a given obstacle and robot geometry.
The intuition of Minkowski's sum calculation method can be understood by imagining painting the exterior of an obstacle with a brush shaped like a robot, with the robot's origin being the tip of the brush. The painted area is C_obs. The figure below shows this.
To create the configuration space, Minkowski sums are computed this way for each obstacle in the workspace. The image below shows three configuration spaces created from one workspace by three robots of different sizes. As you can see, if the robot is just a point, the obstacles in the workspace are only inflated by a small amount to create the C space. As the size of the robot increases, the obstacles will also expand larger and larger.
For convex polygons, computing the convolution is trivial and can be done in linear time - however for non-convex polygons (i.e. polygons with gaps or holes), it is much more expensive to compute.
If you are interested in understanding Minkowski sums in more detail, see the following resources:
Quiz: Minkowski Summation
As shown below:
Which of the following images represents the configuration space for the robot (purple) and obstacles (white) shown above? Answer: B
6. C++ implementation of Minkowski summation
Now that you have learned Minkowski summation , please try to write code in C++!
The diagram is as follows:
In this example, two triangles can be seen - one blue and one red. Suppose the robot is represented by a blue triangle and the obstacle is represented by a red triangle. The task is to calculate the configuration space C of robot A and obstacle B in C++.
- Robot: blue triangle, denoted by A
- Obstacles: red triangles, denoted by B
Here are the steps to write the Minkowski sum in C++:
Non-shifted (unconverted) legend
The Minkowski summation code was written above in C++ and the configuration space was generated. Notice that the red obstacle is not fully inflated, the blue robot can still hit the obstacle. This is because the configuration space still needs to be transferred to the obstacles.
First convert the robot into an obstacle, calculate the configuration space, and then convert it into a robot and an obstacle.
Converted legend
The image above is the final image after transformation, where both the blue robot and the green configuration space have been transformed. You can see the yellow fill, which represents the transformed configuration space surrounded by red obstacles. The blue robot will no longer hit the red obstacle because it inflates well.
The complete code is as follows: (see - https://github.com/udacity/RoboND-MinkowskiSum for details )
#include <iostream>
#include <vector>
#include <algorithm>
#include "../lib/matplotlibcpp.h"
#include <math.h>
using namespace std;
namespace plt = matplotlibcpp;
// Print 2D vectors coordinate values
void print2DVector(vector<vector<double> > vec)
{
for (int i = 0; i < vec.size(); ++i) {
cout << "(";
for (int j = 0; j < vec[0].size(); ++j) {
if (vec[i][j] >= 0)
cout << vec[i][j] << " ";
else
cout << "\b" << vec[i][j] << " ";
}
cout << "\b\b)" << endl;
}
}
// Check for duplicate coordinates inside a 2D vector and delete them
vector<vector<double> > delete_duplicate(vector<vector<double> > C)
{
// Sort the C vector
sort(C.begin(), C.end());
// Initialize a non duplicated vector
vector<vector<double> > Cn;
for (int i = 0; i < C.size() - 1; i++) {
// Check if it's a duplicate coordinate
if (C[i] != C[i + 1]) {
Cn.push_back(C[i]);
}
}
Cn.push_back(C[C.size() - 1]);
return Cn;
}
// Compute the minkowski sum of two vectors
vector<vector<double> > minkowski_sum(vector<vector<double> > A, vector<vector<double> > B)
{
vector<vector<double> > C;
for (int i = 0; i < A.size(); i++) {
for (int j = 0; j < B.size(); j++) {
// Compute the current sum
vector<double> Ci = { A[i][0] + B[j][0], A[i][1] + B[j][1] };
// Push it to the C vector
C.push_back(Ci);
}
}
C = delete_duplicate(C);
return C;
}
// Compute the centroid of a polygon
vector<double> compute_centroid(vector<vector<double> > vec)
{
vector<double> centroid(2);
double centroid_x=0, centroid_y=0;
for (int i = 0; i < vec.size(); i++) {
centroid_x += vec[i][0];
centroid_y += vec[i][1];
}
centroid[0] = centroid_x / vec.size();
centroid[1] = centroid_y / vec.size();
return centroid;
}
// Compute the angle of each point with respect to the centroid and append in a new column
// Resulting vector[xi,yi,anglei]
vector<vector<double> > compute_angle(vector<vector<double> > vec)
{
vector<double> centroid = compute_centroid(vec);
double prec = 0.0001;
for (int i = 0; i < vec.size(); i++) {
double dy = vec[i][1] - centroid[1];
double dx = vec[i][0] - centroid[0];
// If the point is the centroid then delete it from the vector
if (abs(dx) < prec && abs(dy) < prec) {
vec.erase(vec.begin() + i);
}
else {
// compute the centroid-point angle
double theta = (atan2(dy, dx) * 180) / M_PI;
// append it to the vector in a 3rd column
vec[i].push_back(theta);
}
}
return vec;
}
// Sort the vector in increasing angle (clockwise) for plotting
vector<vector<double> > sort_vector(vector<vector<double> > vec)
{
vector<vector<double> > sorted_vec = compute_angle(vec);
// Change the 0 angle to 90 degrees
for (int i = 0; i < sorted_vec.size(); i++) {
if (sorted_vec[i][2] == 0)
sorted_vec[i][2] = 90.0;
}
// Sort with respect to the 3rd column(angles)
sort(sorted_vec.begin(),
sorted_vec.end(),
[](const vector<double>& a, const vector<double>& b) {
return a[2] < b[2];
});
return sorted_vec;
}
// Process the shapes and plot them
void plot(vector<vector<double> > vec, string color)
{
// Sort the vector coordinates in clockwise
vector<vector<double> > sorted_vec;
sorted_vec = sort_vector(vec);
// Add the first element to the end of the vector
sorted_vec.push_back(sorted_vec[0]);
// Loop through vector original size
for (int i = 0; i < sorted_vec.size() - 1; i++) {
// Connect coordinate point and plot the lines (x1,x2)(y1,y2)
plt::plot({ sorted_vec[i][0], sorted_vec[i + 1][0] }, { sorted_vec[i][1], sorted_vec[i + 1][1] }, color);
}
}
// Translate the configuration space toward the obstacle
vector<vector<double> > shift_space(vector<vector<double> > B, vector<vector<double> > C)
{
// Compute the obstacle and space centroids
vector<double> centroid_obstacle = compute_centroid(B);
vector<double> centroid_space = compute_centroid(C);
// Compute the translations deltas
double dx = centroid_space[0] - centroid_obstacle[0];
double dy = centroid_space[1] - centroid_obstacle[1];
// Translate the space
for (int i = 0; i < C.size(); i++) {
C[i][0] = C[i][0] - dx;
C[i][1] = C[i][1] - dy;
}
return C;
}
// Draw A, B and C shapes
void draw_shapes(vector<vector<double> > A, vector<vector<double> > B, vector<vector<double> > C)
{
//Graph Format
plt::title("Minkowski Sum");
plt::xlim(-5, 5);
plt::ylim(-5, 5);
plt::grid(true);
// Draw triangle A
plot(A, "b-");
// Draw triangle B
plot(B, "r-");
// Draw configuration space C
// Trasnlate the C space
vector<vector<double> > shifted_C = shift_space(B, C);
plot(shifted_C, "y-");
// Plot the original C shape
plot(C, "g-");
//Save the image and close the plot
plt::save("../images/Minkowski_Sum.png");
plt::clf();
}
int main()
{
// Define the coordinates of triangle A and B in 2D vectors
vector<vector<double> > A(3, vector<double>(2));
// Robot A
A = {
{ 0, -1 }, { 0, 1 }, { 1, 0 },
};
vector<vector<double> > B(3, vector<double>(2));
// Obstacle B
B = {
{ 0, 0 }, { 1, 1 }, { 1, -1 },
};
// Translating Robot toward the obstacle
A=shift_space(B,A);
// Compute the Minkowski Sum of triangle A and B
vector<vector<double> > C;
C = minkowski_sum(A, B);
// Print the resulting vector
print2DVector(C);
// Draw all the shapes
draw_shapes(A, B, C);
return 0;
}In short, the following steps must be followed to generate an arbitrary polygon:
- Calculate the centroid of each polygon
- Calculate the angle of each point's centroid relative to the x-axis
- Sort points by angle in ascending order (clockwise)
- Draw a line between each consecutive point
7. Three-dimensional configuration space
The robot's configuration space depends on its rotation, and allowing the robot to rotate adds a degree of freedom—thus, it also complicates the configuration space. The dimensionality of the configuration space is equal to the number of degrees of freedom the robot has. While a two-dimensional configuration space is capable of representing the robot's x and y translations, a third dimension is required to represent the robot's rotation. Let's look at a robot and its corresponding configuration space for two different rotations. The first is 0° and the second is 18°.
A three-dimensional configuration space can be generated by stacking two-dimensional configuration spaces into layers - as shown in the following figure:
If we were to compute the configuration space of infinitesimal rotations of the robot, and stack them together - we would get a graph like this: 
The figure above shows the configuration space of a triangular robot that can translate in two dimensions and rotate around the z-axis. While this image looks complicated, there are a few tricks you can use to generate 3D configuration spaces and move around them. The following video is from the Freie Universität Berlin and is a wonderful visualization of a configuration space in 3D. This video will show different types of motion and describe how certain robot motions are mapped to a 3D configuration space: Configuration Space Visualization .