An Introduction to Path Planning Algorithms

In the field of mobile robotics, the ability of a robot to autonomously navigate from one point to another is fundamental. Behind every movement and obstacle avoidance lies a complex architecture of perception, localization, and, crucially, path planning algorithms. The task of a global path planner is to determine an optimal, collision-free route from a starting position to a final goal, using a map of the environment that is known a priori or built in real-time.

The global path planner establishes the macro-trajectory the robot should follow, leaving it to local planners (and controllers) to handle the fine details and react to dynamic obstacles along the way. The complexity and efficiency of these algorithms vary, depending on the representation of the environment, the kinematic constraints of the robot and the need for optimality or completeness.

The Grid-Based Approach: Discretizing the Configuration Space

Most global path planning algorithms for mobile robots begin with a discretized representation of the environment. The most common method is the grid map, where the robot’s operating space is divided into a mesh of uniform cells. Each cell can be labeled as “free,” “occupied” or “unknown.”

Formally, we can define the environment as a graph $G=(V,E)$, where $V$ is the set of vertices (the cells of the grid) and $E$ is the set of edges (the allowed transitions between adjacent cells). The objective is to find a path $P=(v_0,v_1,\cdots,v_n)$ such that $v_0$ is the starting position, $v_n$ is the goal, and each edge $(vi,vi+1) \in E$ represents a valid, collision-free transition.

For mobile robots, it’s important to consider the robot’s physical size. This is usually incorporated into the grid map through a process called obstacle inflation. Each obstacle is inflated by a safety margin equal to the robot’s radius (or an approximation of its footprint), ensuring that the center of the robot can follow the planned path without collision.

The A* Algorithm

The A* algorithm is an extension of Dijkstra’s algorithm that incorporates a heuristic function to guide the search of the more efficiently path toward the goal. It is optimal (finds the lowest-cost path) and complete (finds a path if one exists) under certain conditions.

A* evaluates each cell $n$ in the grid using a total cost function $f(n)$ defined by:

\[f(n) = g(n) + h(n)\]

Where $g(n)$ is the cost of the cheapest known path from the starting cell to cell $n$ and $h(n)$ is the estimated heuristic cost from cell $n$ to the goal cell. This function must be admissible (never overestimates the true cost) and ideally consistent (satisfies the triangle inequality) to guarantee optimality.

The algorithm works by maintaining two lists of cells: an open list that contains cells that have been discovered but not yet fully explored (their neighbors have not yet been evaluated), and a closed list, which contains cells that have already been explored.

Steps of the A* Planner

Initialization:
- Add the start cell to the Open List.
- Set $g(start)=0$ and $f(start)=h(start)$. For all other cells $n$, set $g(n)=\infty$.
Main Loop: While the Open List is not empty:

Select the cell $n$ in the Open List with the lowest $f(n)$ value.
Remove $n$ from the Open List and add it to the Closed List.
If $n$ is the goal cell, the path has been found. Reconstruct it from the predecessors.
For each neighbor $v$ of $n$:
- If v is in the Closed List, ignore it.
- Calculate the tentative path cost $g_{temp}(v)=g(n)+cost$( from n to v ).
- If $g_{temp}(v) < g(v)$ (or if $v$ is not in the Open List):
  - Set $g(v)=g_{temp}(v)$.
  - Set the predecessor of $v$ to be $n$.
  - Calculate $f(v)=g(v)+h(v)$.
  - If $v$ is not in the Open List, add it.

Choosing the heuristic $h(n)$

The choice of the heuristic is crucial for A*’s performance. Common heuristics for 2D grid maps include:

The Manhattan Distance

\[h(n)=∣x_n−x_{goal}∣+∣y_n−y_{goal}∣\]

Ideal for orthogonal movements (4 directions).

The Euclidian Distance

\[h(n)=\sqrt{(x_n−x_{goal})^2+(y_n−y_{goal})^2}\]

Best for movements in 8 directions (diagonals allowed).

Introducing Kinematic Constraints

We established the A* algorithm as a powerful tool for finding the shortest path on a grid. However, it operates on a crucial simplification: it assumes the robot as point and can move freely between adjacent cells. This is a valid model for holonomic robots, which can turn in place and move in any direction.

But when we are dealing with non-holonomic robots, with kinematic constraints, they cannot move sideways and are limited by a minimum turning radius. A path generated by a standard A* algorithm, with its sharp 90-degree turns, is often impossible for such a robot to follow.

To solve this, we need a planner that thinks not just in (x,y) space, but in the robot’s full state space, (x,y,θ), where θ represents the robot’s orientation.

Dealing with Kinematic Constraints Using the Hybrid A* Algorithm

Hybrid A* combines a continuous state-space search with a discrete grid for collision checking and heuristics. It finds a path from a starting state $(x_s,y_s,\theta_s)$ to a goal state $(x_g,y_g,\theta_g)$ that is guaranteed to be executable by the robot. Instead of moving to adjacent grid cells, Hybrid A* simulates discrete motion primitives based on the robot’s kinematic model.

The Kinematic Model (Simple Bicycle Model):

The planner uses a set of differential equations to predict the robot’s motion. A common choice is the bicycle model, which describes the robot’s state change over time based on its velocity $v$ and steering angle $\delta$:

\[\dot{x} = v \cdot \cos(\theta) \\ \dot{y} = v \cdot \sin(\theta) \\ \dot{\theta} = \frac{v}{L} \cdot \tan(\delta)\]

Where $L$ is the wheelbase of the robot (the distance between the front and rear axles). By applying a set of discrete steering angles and integrating these equations over a short time step, the algorithm generates a set of kinematically feasible successor states.

The Cost Function Revisited

Like its predecessor, Hybrid A* uses the familiar cost function $f(n)=g(n)+h(n)$, but the components are significantly more nuanced.

$g(n)$ - Represents the actual path length traveled along the continuous motion primitives. It can also be designed to penalize undesirable actions, such as reversing or frequent changes in steering direction, to produce smoother paths.
$h(n)$ - A simple Euclidean distance is no longer an admissible heuristic because it completely ignores the robot’s orientation and turning constraints. A robot at (x,y) facing away from the goal is much “further” away in terms of control effort than a robot at the same position already facing the goal.

To solve this, Hybrid A* uses a combination of two heuristics, taking their maximum to get a highly informed, yet still admissible, estimate:

\[h(n)=\max(h_1(n),h_2(n))\]

$h_1(n)$: Holonomic-with-Obstacles Heuristic. This heuristic accounts for obstacles but ignores kinematics. It’s calculated by running a standard 2D A* once backwards from the goal cell on the grid. The resulting grid contains, for each cell, the length of the shortest path to the goal, providing an excellent obstacle-aware cost estimate.
$h_2(n)$: Non-Holonomic-without-Obstacles Heuristic. This heuristic respects kinematics but ignores obstacles. It calculates the length of the shortest possible kinematically feasible path from the current state to the goal state in an obstacle-free world. These analytical solutions are known as Dubins Paths (for forward-only motion) or Reeds-Shepp Paths (which allow reversals). They consist of a sequence of maximum-curvature turns and straight lines.

By taking the maximum of these two, the heuristic remains admissible while being powerfully informed about both obstacle layout and kinematic limitations.

The Hybrid A* algorithm represents a significant leap forward from simple grid searches. By operating in the continuous state space and using kinematically valid motion primitives, it generates smooth, drivable paths that non-holonomic robots can execute. While computationally more expensive than a standard A*, its ability to respect the kinematics constraints of the robot makes it an indispensable tool for autonomous navigation. In ROS 2 Nav2, this powerful planner is implemented in the Smac Planner package.

From Grids to Graphs: Path Planning with the Rapidly-exploring Random Tree (RRT)

Search-based algorithms like A* and Hybrid A*, while powerful, face a significant challenge when dealing with large environments. For these high-dimensional or open-space problems, we need a different approach—one that doesn’t try to check every possibility but instead intelligently samples the space.

The RRT Philosophy: Exploring Space with Random Samples

RRT is a sampling-based algorithm that, instead of using a pre-defined grid, incrementally builds a search tree of collision-free paths. Its core idea is to bias the growth of this tree towards unexplored regions of the configuration space (C-space), allowing it to connect the start and goal configurations quickly and efficiently.

It is probabilistically complete, meaning that if a path exists, the probability of finding it approaches 1 as the number of samples increases. However, it comes with a critical trade-off: the first path it finds is not guaranteed to be optimal.

A Step-by-Step Guide

The algorithm iteratively grows a tree, $T$, rooted at the starting configuration, $q_{start}$.

Initialization: Create a tree $T$ containing only the starting node, $q_start$.
Iteration Loop: For a predefined number of iterations, $N$, or until the goal is found:

a) Sample ($q_{rand}$): Generate a random configuration, qrand, within the valid C-space. Occasionally (with a small probability), this sample is set to be the goal configuration, $q_{goal}$, to bias the tree’s growth in the right direction.
b) Find Nearest ($q_{near}$): Search the existing tree $T$ to find the node, $q_{near}$, that is closest to $q_{rand}$ according to a distance metric (e.g., Euclidean distance).
c) Steer/Extend: From $q_{near}$, generate a new configuration, $q_{new}$, by taking a small step of size $\varepsilon $ in the direction of $q_{rand}$. This prevents the tree from taking huge, often-colliding leaps across the space. The new node is defined as:

\[q_{new} = q_{near} + \frac{q_{rand} − q_{near}}{\left\| q_{rand} − q_{near} \right\| } \varepsilon\]

d) Collision Check: Check if the path segment (e.g., a straight line) between $q_{near}$ and $q_{new}$ is free of collisions.
e) Add to Tree: If the path is collision-free, add qnew as a new node to the tree $T$, with an edge connecting it back to qnear.

Goal Connection: Once a node $q_{new}$ is added that is within a certain threshold of the goal, the algorithm attempts to connect directly to $q_{goal}$. If successful, the path is found by tracing the parent pointers from $q_{goal}$ back to $q_{start}$.

RRT and its variants represent a fundamental shift from deterministic searches to probabilistic exploration. They excel in solving high-dimensional path planning problems, such as those in robot arm manipulation and multi-robot systems, where grid-based methods are simply not feasible. While less common for standard 2D mobile robot navigation where planners like Hybrid A* offer a better balance of performance and path quality, RRT remains a great tool in the modern roboticist’s path planning arsenal.