# Optimal Robot Path Planning Using PSO in MATLAB

URL: https://yarpiz.com/path-planning-ypap115
Author: Mostapha Kalami Heris
Published: 2015-09-22
Updated: 2026-10-30
Topics: Applications

> Plan a 2D robot path in MATLAB with PSO, spline handle points, circular obstacles, sampled path lengths, and obstacle-violation penalties.

This package plans a path for a mobile robot in MATLAB using Particle Swarm Optimization (PSO). The robot moves from a source point to a target point in a plane and must avoid circular obstacles.

Path planning is a basic task in robotics: find a route from a start point to a goal that is short, smooth and free of collisions. This code describes the path with a few handle points and uses PSO to search for a short path with no detected obstacle violation at the sampled points.

## Problem definition

The robot starts at the source $(x_s, y_s)$ and must reach the target $(x_t, y_t)$. The area contains circular obstacles, each with a center $(x_k^{o}, y_k^{o})$ and a radius $r_k$.

The decision variables are the coordinates of $n$ handle points $(x_1, y_1), \dots, (x_n, y_n)$, which must lie inside the area bounds. The path is a smooth curve through the source, the handle points and the target, in this order. `ParseSolution.m` builds this curve with MATLAB's `spline` function (a cubic spline) and samples it at 100 points.

The path length $L$ is the sum of the distances between the sampled points. For each obstacle, the violation at a sampled point is $\max(0, \; 1 - d/r_k)$, where $d$ is the distance of the point from the center of the obstacle. This is 0 outside the obstacle and grows to 1 at its center. The total violation $V$ is the sum, over all obstacles, of the mean violation along the path.

## What is in the package

- `CreateModel.m`: defines the source, the target, the obstacles (centers and radii) and the bounds of the area.
- `CreateRandomSolution.m`: creates a random path, given by intermediate points between the source and the target.
- `ParseSolution.m`: turns the points into a smooth path and measures its length and its violation of the obstacles.
- `MyCost.m`: the cost is the path length multiplied by a penalty factor that grows with the obstacle violation.
- `pso.m`: the PSO algorithm that minimizes this cost.
- `PlotSolution.m`: plots the obstacles and the path.

The cost in `MyCost.m` is

$$
z = L \, (1 + \beta V), \qquad \beta = 100
$$

The code marks a path as feasible when $V = 0$ at the sampled points. Its cost then equals the sampled length, but that check does not establish clearance along the whole continuous spline.

## The example model

In `CreateModel.m`, the source is at $(0, 0)$ and the target is at $(4, 6)$. There are three obstacles, given by the vectors `xobs`, `yobs` and `robs`:

| Obstacle | Center | Radius |
| --- | --- | ---: |
| 1 | (1.5, 4.5) | 1.5 |
| 2 | (4.0, 3.0) | 1.0 |
| 3 | (1.2, 1.5) | 0.8 |

The handle points may lie anywhere in the square from $-10$ to $10$ in both directions (`xmin`, `xmax`, `ymin` and `ymax`). To change the obstacles, edit these three vectors. Each position in the vectors is one obstacle, so you can add or remove obstacles by changing their length.

## PSO settings

PSO was introduced by Kennedy and Eberhart in 1995. In this code, the position of a particle has two parts, `x` and `y`, and both are updated with the usual velocity rule. The first particle starts as the straight line from the source to the target. The others start at random.

| Parameter | Meaning | Value |
| --- | --- | ---: |
| `model.n` | Number of handle points | 3 |
| `MaxIt` | Number of iterations | 500 |
| `nPop` | Swarm size | 150 |
| `w`, `wdamp` | Inertia weight and its damping ratio | 1, 0.98 |
| `c1`, `c2` | Personal and global learning coefficients | 1.5, 1.5 |
| `alpha` | Maximum velocity as a fraction of the range | 0.1 |

The number of handle points is set in `CreateModel.m` and again in `pso.m` (`model.n=3`), so change it in `pso.m`. More handle points allow more complex paths around many obstacles, but they also make the search space larger.

## How to run

Run `pso.m`.

In each iteration, the program prints the best cost. It adds a star when the best path is feasible, and otherwise it prints the violation. It also draws the obstacles, the best path and its handle points, so you can watch the path improve. At the end, it plots the best cost against the iteration.

## Check the result

Inspect the continuous spline between samples, not only the 100 sampled points: a curve can cross an obstacle between two checked points. Increase or adapt collision-check resolution and include the robot footprint and required clearance before using a path beyond this point-robot example. Also check the whole curve against workspace bounds because bounding the handle points does not bound every spline segment. A lower penalized score does not certify collision-free motion or a globally shortest path.

## Applications

The same approach can be used for mobile robots and automated guided vehicles in factories and warehouses, for planning the routes of drones, and for similar problems in games and simulations. The cost function can be extended with other terms, such as the smoothness of the path or a safety margin around the obstacles.

## Related posts

- [Particle Swarm Optimization in MATLAB](https://yarpiz.com/particle-swarm-optimization-ypea102)

## How to cite

Mostapha Kalami Heris, Optimal Robot Path Planning Using PSO in MATLAB (YPAP115), Yarpiz, 2015. https://yarpiz.com/path-planning-ypap115