Particle Swarm Optimizer for Robot Path Planner in C++

The robotics domain has become interested in using heuristic techniques to solve the path planning problem because of the benefits they offer, such as easy implementation and quick generation of an acceptable solution. The relatively straightforward yet extremely effective heuristic optimization technique known as Particle Swarm Optimization (PSO) has shown to be very successful in solving a variety of challenging optimization issues.

The following article continues my discussion from my previous one, which you can find here (Grey Wolf Optimizer).

Source files (in C++) supporting discussed article you will find on my GitHub.

Particle Swarm Optimizer

The PSO was developed by James Kennedy and Russell Eberhart in 1995. The concept is based on animals' behavior and here swarm intelligence. We can simplify the swarm behavior to the following principles. When a fish or bird searching for food discovers a good path to the food, it immediately notifies the rest of the swarm, who then slowly and gradually drift toward the goal (food).

For algorithm purposes, the bird or fish, or other animal is associated with a particle which is initiated by assigning each particle in the swarm a random position and velocity.
These particles are added to a problem or function’s search space. These particles are used to evaluate the cost function (the cost function is specified below), and the global best of the entire swarm is kept in Gbest while each particle's personal best is stored in Pbest.
These particles are subsequently transferred to new places in the following iteration using update equations (for velocity and position).

Particle update (by author)


V is the velocity of the particle, X is the position, and w is an inertia weight that correlates between the exploration and exploitation characteristics in PSO, factors c1 and c2 give are associated with constant balancing factors for the particle's best parameters and social parameters/ acceleration constant.

By communicating the personal best and global best positions to one another, the particles gradually achieve the global best positions. The cycle is repeated until the maximum number of iterations is reached or until all the particles converge to the same place.

The simplified pseudo-code can be presented as follows,

Particle Swarm Optimizer. (by author)

In this article, I looked for a collision-free path for the 2D robot.

In order to find the cost function I applied the following strategy. The robot position is expressed by the following equations,

The distance between the mobile robot and the obstacle must be kept to the maximum, however, the time constraint, regarding the shortest path has to be also incorporated (the robot has to achieve the target soonest possible).

The cost function optimized by PSO can be depicted as follows,


where the parameter α1 reflects how fast the robot reaches the target Factor α2 controls the distance between the robot and the obstacle. The last one α3 can be considered a smoothing factor to control the robot path smoothness. drObs and drGoal variables are used to calculate the Euclidian distance between the robot and the obstacles and the distance between the robot and the goal, respectively.

Robot position in 2D space. (by author)

Before I started with the robot path planner I checked the performance of PSO by finding the maximum value of the multiplication of two variables in the range (-5, 5). After running 500 iterations the value of the function equals 25, as expected (you find also this program on my GitHub).

Performance of the PSO algorithm. The robot avoids obstacles.

Robot avoids obstacle (by author)

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,

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