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
- Priority queue: Orders nodes by their current shortest distance
- Visited 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
- Relax: If new distance is shorter, update the neighbor's distance
- Repeat: Continue until all nodes are visited or goal is reached
The Relaxation Process
The relaxation step is crucial to Dijkstra's algorithm. When examining a node's neighbors, we ask: "Is going through the current node a shorter path to the neighbor?"
\(\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, we update the neighbor's distance to 13. This "relaxation" found a shorter path to the neighbor through the current node.
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.