Simulated Annealing Optimization for Robot Path Planner in C++

Simulated annealing is a stochastic method that mimics the annealing of material phenomenon found in nature to improve solving optimization problems. The process of progressively heating and cooling a solid is known as annealing.
The discussed algorithm simulates a minor, arbitrary atom move. Random movement affects the changes in the total energy of the system. If the energy change is negative, the new configuration is accepted because its energy state is lower (algorithm approach solution). On the other hand, if the new configuration (configuration of atom in the solid material) has a higher energy state, so the new configuration can be accepted but according to Boltzmann probability factor:

Please note the relation of acceptance probability depends highly on the current temperature T. kb is the Boltzmann constant. When the material cools, the probability decreases.

The source code in C++ for this article you will find on my GitHub.

Simulated Annealing. Intuition applied to robot path planner.

The behavior of the simulated annealing algorithm can be derived and applied to the robot path planner. Comparison is drawn between energy and the objective function of the path planner (the robot has to achieve the goal without colliding with obstacles). The search begins with a high “temperature”. In the beginning, we have to initialize the “starting” positions for the robot path.
For the purpose of this article, I connect start and goal configurations by line and generated a robot position along that line. Now these “starting positions” will be optimized (moved randomly in 2D space) in order to minimize iteratively the objective function.
The path planner function (described below in the following article) requires the function to achieve the global minimum. While running the path planner algorithm the position generated in the previous iteration is subjected to random disturbances. The new configuration of the robot path becomes the current path if the objective function is lower; if the objective is higher, it may still be accepted based on the acceptance probability provided by the Boltzmann factor. If the random number is less than the Boltzmann probability, the configuration is accepted. The random number is chosen at random from a uniform distribution between 0 and 1. The algorithm can avoid local minima as a result of this.
The most important here is to remember the acceptance strategy of the new solution (configuration of the path). The following figure depicts the in a simplified way the applied approach.

Simulated Annealing (by author)

Please remember we apply a path planner to find an obstacle-free path for the robot. In this article, path planning is rather simple since we consider only 2 dimensions in working space. For manipulators (for example 6DOF) the discussed algorithm has to be applied not in 2D but in joint space, instead. Additionally, we have to consider also path smoothing. I discussed the problem in one of my previous articles.

The pseudo-code for the Simulated Annealing algorithm deployed in this article can be presented as follows,

Simulated Annealing. (by author)

Optimization function for path planner

The optimization function (cost function) I applied to search collision free can be described as follows (I took from this article),

The path is defined as a sequence of points x = {p0 , p1 , · · · , pn } where
p0 is the start point and pn is the goal point. Where the first term is the length of the section (I used Eulidencian distance) and the second term is the intersection between the path and the obstacles. Kp > 0 is a constant. Formulas to check intersections with obstacles I check using equations defined here.
Please note, we compute the cost function for all robot positions! (see implementation in C++).
As it was discussed previously, we approach the optimal solution (we are looking for minimum function) by moving randomly (the current) position of the robot. The equation which I applied can be expressed as follows (K∆ is a constant that defines the range for algorithm search).

We compute the total energy (cost function) and compare it with Bolzman acceptance criteria which decrease while we iterate.

Discussed optimization algorithm requires several adjustments (tunning process). We have to consider the choice of initial conditions of temperatures, starting position of the robot, number of iterations, number of initialization positions, choice of random boundary for position to be updated K∆, etc.
Below I displayed the algorithm performance.

Performance 1.
Performance 2.
Performance 3.
Performance 4.

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.h

Thank you for reading.

--

--

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store