Preliminaries
If you are familiar with pathfinding fundamentals you may skip this section.
Pathfinding is the computational problem of finding a route between two points. Imagine you need to navigate from your home to a coffee shop across town. You could take many different routes, but you probably want one that minimizes travel time, distance, or fuel consumption. Similarly, robots need algorithms to find optimal paths through their environments while avoiding obstacles and respecting their physical constraints.
At its core, pathfinding algorithms answer the question: "What is the best way to get from point A to point B?" But to answer this question, we first need to define what "best" means through a cost function.
Cost Functions and Heuristics
A cost function assigns a numerical value to represent the "expense" of taking a particular action or following a specific path. This cost could represent:
- Distance: How far the robot travels
- Time: How long the journey takes
- Energy: How much battery power is consumed
- Risk: How dangerous the path is (proximity to obstacles)
For example, if a robot is navigating on a grid where each cell represents 1 meter, the cost of moving horizontally or vertically might be 1, while moving diagonally might cost √2 ≈ 1.414 (the actual geometric distance).
| Movement Type | Cost | Reasoning |
|---|---|---|
| Horizontal/Vertical | 1.0 | Direct grid movement |
| Diagonal | 1.414 | Euclidean distance: √(1² + 1²) |
| Through obstacle | ∞ | Impossible/forbidden |
A heuristic is an estimate of the cost from any given point to the goal. While the actual cost requires exploring the entire path, a heuristic provides a "guess" that helps guide the search toward the goal more efficiently.
Shortest Cost Path
The shortest cost path is the route from start to goal that minimizes the total accumulated cost. This is not necessarily the path with the fewest steps, but rather the one that optimizes our chosen cost function.
Consider a robot navigating through terrain with different movement costs:
| Terrain Type | Movement Cost | Description |
|---|---|---|
| Road | 1.0 | Easy, fast movement |
| Grass | 2.0 | Slower, more energy |
| Sand | 3.0 | Difficult terrain |
| Water | 10.0 | Very slow/risky |
The shortest cost path might take a longer route through roads rather than a direct path through difficult terrain.
Path 1: 5 cells × 3.0 cost = 15.0 total cost
Path 2: 8 cells × 1.0 cost = 8.0 total cost
Even though Path 2 visits more cells, its lower cost per cell makes it the optimal choice. This demonstrates why "shortest" in pathfinding refers to cost, not necessarily distance or number of steps.
Introduction to Dijkstra's Algorithm
Dijkstra's algorithm is a fundamental pathfinding algorithm that finds the shortest cost path from a starting node to all other nodes in a graph. It works by systematically exploring nodes in order of their distance from the start, ensuring that when a node is visited, the shortest path to it has been found.
The algorithm maintains three key data structures:
- Distance array: Stores the shortest known distance from start to each node
- Open Set: Orders discovered nodes by their current shortest distance
- Closed set: Tracks which nodes have been fully processed
How Dijkstra's Works
The algorithm follows these steps:
- Initialize: Set distance to start node as 0, all others as infinity
- Select: Choose the unvisited node with minimum distance
- Update: For each neighbor, calculate new potential distance through current node
- Compare: If the new distance is shorter, save it as the best known distance to that neighbor
- Repeat: Continue until all nodes are visited or goal is reached
Checking for a Better Path
The most important step in Dijkstra's algorithm is deceptively simple: each time we visit a node, we look at all of its direct neighbors and ask "Does going through this node give us a shorter route to the neighbor than the best route we've found so far?"
Think of it like updating your GPS mid-journey. You start with no knowledge of the best route, so every node's distance begins at infinity — a placeholder meaning "we haven't found a way there yet." Each time we discover a new connection, we calculate the total cost of reaching the neighbor through the current node and compare it against whatever we already have recorded:
- If the new route is shorter → we update the neighbor's recorded distance to the new, better value.
- If the new route is the same or longer → we ignore it and move on.
This is how the algorithm progressively narrows in on the true shortest path — it keeps replacing rough estimates with better ones until no further improvements can be made.
\(\text{then } \text{distance[neighbor]} = \text{distance[current]} + \text{edge_cost}\)
Current node distance: 10
Edge cost: 3
New potential distance: 10 + 3 = 13
Current neighbor distance: 15
Since 13 < 15, going through the current node is a shorter route to the neighbor. We update its recorded distance from 15 to 13.
Why Dijkstra's is Important
Dijkstra's algorithm is optimal and complete:
- Optimal: It guarantees finding the shortest cost path
- Complete: It will always find a solution if one exists
However, Dijkstra's explores nodes uniformly in all directions, which can be inefficient when we know the goal location. This limitation motivates more advanced algorithms like A*, which uses heuristics to guide the search more intelligently toward the goal.
Understanding these fundamentals provides the foundation for more advanced pathfinding algorithms. Dijkstra's guarantees optimality but can be slow, while algorithms like A* add heuristics to improve efficiency, and RRT handles complex, high-dimensional spaces where traditional grid-based approaches become impractical.
These concepts form the building blocks for understanding how robots make intelligent navigation decisions, balancing optimality, efficiency, and practical constraints in real-world environments.