Blog

Algorithms in Robotics: Pathfinding and Navigation Explained

A mobile robot’s navigation software splits into a few layers: a map and a position estimate (localization or SLAM), a global planner that finds a route on the map (A*, Dijkstra, RRT), and a local planner or controller that follows the route while dodging obstacles it sees right now. Most robots you can read about use grid-based A* globally and a fast sampling or optimization controller locally. This post shows how those pieces work, with Python that was run, and where the usual mistakes are.

The layers of robot navigation

  1. Perception and mapping. Sensors (lidar, cameras, wheel odometry, an IMU) are turned into a map, often an occupancy grid where each cell is free, occupied or unknown.
  2. Localization. Where am I on that map? Methods include particle filters, Kalman filters and scan matching.
  3. Global planning. Given the map, start and goal, find a collision-free path.
  4. Local planning and control. Turn the path into velocity commands each fraction of a second, reacting to sensor data and to the robot’s own limits on speed, acceleration and turning.
  5. Recovery. If the robot is stuck, back up, spin, clear the local map, or replan.

Separating the layers matters. The global planner can be slow and thorough because it runs rarely; the controller must be fast and runs many times per second.

A* on a grid

A* is Dijkstra’s algorithm with a guide. It expands the node with the lowest f = g + h, where g is the cost so far and h is a heuristic estimate of the cost remaining. If h never overestimates (it is admissible), the first time you reach the goal the path is optimal.

The heuristic must match the moves. If the robot can move diagonally, Manhattan distance overestimates: from (0,0) to (5,5) it says 10, but eight diagonal steps cost only 5 × 1.414 = 7.07. Use the octile distance, which accounts for diagonals. The code below also forbids cutting the corner of an obstacle, since a real robot has width:

import heapq, math

MOVES = [(-1,0,1), (1,0,1), (0,-1,1), (0,1,1),
         (-1,-1,math.sqrt(2)), (-1,1,math.sqrt(2)),
         (1,-1,math.sqrt(2)),  (1,1,math.sqrt(2))]

def octile(a, b):
    dx, dy = abs(a[0]-b[0]), abs(a[1]-b[1])
    return (dx + dy) + (math.sqrt(2) - 2) * min(dx, dy)

def free(grid, r, c):
    return 0 <= r < len(grid) and 0 <= c < len(grid[0]) and grid[r][c] == 0

def a_star(grid, start, goal, h=octile):
    open_heap = [(h(start, goal), 0.0, start)]
    g = {start: 0.0}
    parent, closed = {}, set()
    while open_heap:
        f, gc, cur = heapq.heappop(open_heap)
        if cur in closed:
            continue                      # stale heap entry
        if cur == goal:
            path = [cur]
            while cur in parent:
                cur = parent[cur]
                path.append(cur)
            return path[::-1], gc
        closed.add(cur)
        for dr, dc, cost in MOVES:
            nxt = (cur[0] + dr, cur[1] + dc)
            if not free(grid, *nxt):
                continue
            if dr and dc and not (free(grid, cur[0] + dr, cur[1]) and
                                  free(grid, cur[0], cur[1] + dc)):
                continue                  # don't cut corners
            ng = gc + cost
            if ng < g.get(nxt, float("inf")):
                g[nxt] = ng
                parent[nxt] = cur
                heapq.heappush(open_heap, (ng + h(nxt, goal), ng, nxt))
    return None, float("inf")

grid = [
 [0,0,0,0,0,0,0],
 [0,0,0,1,0,0,0],
 [0,0,0,1,0,0,0],
 [0,0,0,1,0,0,0],
 [0,0,0,0,0,0,0],
 [0,0,0,0,0,0,0],
]
path, cost = a_star(grid, (0, 0), (3, 6))
print(round(cost, 3))   # 7.828
# path: (0,0) (0,1) (0,2) (0,3) (0,4) (1,4) (2,5) (3,6)
# *****..
# ...#*..
# ...#.*.
# ...#..*

Details that cause real bugs: use a heap with “lazy deletion” (skip entries already closed) instead of searching the open list, which the common copy-pasted version does in O(n) per step; and store a node’s best g so you only push a node when you find a cheaper route to it.

How much does the heuristic help? On an empty 50×50 grid, going from (0,0) to (49,30), A* with the octile heuristic expanded 233 nodes. The same search with h = 0, which is Dijkstra, expanded 2,376. On cluttered maps the gap is smaller, and with a poor heuristic it can vanish, but the heuristic never makes the answer worse as long as it is admissible.

Dijkstra and BFS

Dijkstra is A* with h = 0. Use it when you need distances to all cells (for example, a cost field that every robot position can follow downhill), or when there is no good heuristic. Breadth-first search is Dijkstra when every step costs the same, and it is the right tool for unweighted problems. If you want practice on exactly these, AlgoCademy has BFS Min Distance to Every Vertex and Dijkstra’s Algorithm; our post on the A* search algorithm covers the algorithm itself in more depth.

Why grid paths are not enough

Grid A* gives paths that follow 8 directions, which look jagged and ignore the robot’s shape. Four common fixes:

  • Inflate obstacles by the robot’s radius in the cost map, so the robot can be treated as a point.
  • Add a cost gradient near obstacles so paths keep a safety margin instead of hugging walls.
  • Smooth the path, or use any-angle planners such as Theta* that check line of sight between cells.
  • Plan in the robot’s state space (x, y, heading) for car-like robots that cannot turn on the spot. Hybrid-A* does this using motion primitives.

Sampling-based planning: RRT

Grids get too large when the robot has many degrees of freedom. A robot arm with 6 joints lives in a 6-dimensional space; a grid with 100 cells per axis would have 1012 cells. Sampling-based planners such as RRT (Rapidly-exploring Random Tree) avoid building that grid. They grow a tree from the start: sample a random point, find the nearest tree node, take a small step toward the sample, and keep the new node only if the segment is collision-free. Sampling the goal itself occasionally (a goal bias) speeds up convergence.

import math, random

circles = [(0.5, 0.5, 0.2), (0.25, 0.75, 0.1)]     # obstacles: (x, y, radius)

def collides(p, q):
    for cx, cy, r in circles:
        for i in range(11):                         # check points along the segment
            t = i / 10
            x = p[0] + (q[0] - p[0]) * t
            y = p[1] + (q[1] - p[1]) * t
            if (x - cx) ** 2 + (y - cy) ** 2 <= r * r:
                return True
    return False

def rrt(start, goal, step=0.05, iters=5000, goal_bias=0.1):
    nodes, parent = [start], {start: None}
    for i in range(iters):
        s = goal if random.random() < goal_bias else (random.random(), random.random())
        near = min(nodes, key=lambda n: (n[0]-s[0])**2 + (n[1]-s[1])**2)
        d = math.dist(near, s)
        if d == 0:
            continue
        k = min(step, d) / d
        new = (near[0] + (s[0]-near[0]) * k, near[1] + (s[1]-near[1]) * k)
        if collides(near, new):
            continue
        nodes.append(new)
        parent[new] = near
        if math.dist(new, goal) < step:
            path, n = [goal], new
            while n is not None:
                path.append(n)
                n = parent[n]
            return path[::-1]
    return None

random.seed(3)
path = rrt((0.05, 0.05), (0.95, 0.95))
print(path is not None, len(path))   # True 38

With this seed it found a route in 129 iterations using a 38-node path. The length was 1.8 against a straight-line distance of 1.27, which shows the main property of plain RRT: it finds a path quickly, but the path is wobbly and not optimal. RRT* rewires the tree to improve paths as sampling continues, and a smoothing pass usually follows either one. Sampling-based planners are also only probabilistically complete: if no path exists, they never say so, so always cap the iterations.

Local planning and obstacle avoidance

The global path is only a guide. Once the robot moves, the controller samples candidate velocity commands, simulates each one forward for a short time, scores them (progress along the path, distance to obstacles, smoothness, speed), and picks the best. The Dynamic Window Approach (DWA) is the classic form; model-predictive controllers such as MPPI (model predictive path integral control) are a newer, optimization-based alternative. Potential fields are simpler still, pulling the robot toward the goal and pushing it away from obstacles, but they can trap it in local minima such as a U-shaped wall.

SLAM: map and position at the same time

If the robot has no map, it has to build one while localizing in it, which is the SLAM problem (simultaneous localization and mapping). The difficulty is that errors compound: a small wrong turn estimate makes every later map update wrong. Practical systems use filters or graph optimization, and rely on loop closure: recognizing a place already visited and correcting the accumulated drift. Writing a real SLAM system is a big project. Start by learning the occupancy grid and the idea of updating it from sensor readings, then use an existing implementation.

How this appears in real stacks

You rarely write these from scratch on a real robot. The ROS 2 navigation stack, Nav2, is built from plugins in exactly the layers above. As of October 2026 its documentation lists planners including NavFn (a grid planner that uses A* or Dijkstra expansion and assumes a point-sized, holonomic robot), the Smac planner family (2D grid A*, Hybrid-A* for car-like robots, and a state lattice planner) and Theta*, plus controllers including DWB (a modified Dynamic Window Approach) and MPPI. See the Nav2 plugin selection guide for the current list and recommendations, which change between releases.

Common mistakes

  • Using a heuristic that overestimates the true cost, which silently returns non-optimal paths.
  • Planning for a point robot, then driving a robot with width into narrow gaps. Inflate obstacles.
  • Ignoring the robot’s dynamics: a path made of sharp corners may be impossible to follow.
  • Replanning from scratch every cycle on a large map. Plan globally at a low rate and track locally at a high rate.
  • Trusting a stale map in dynamic environments. Keep a separate, short-lived local cost map.

Where to go from here

Implement grid A* and BFS with different move sets and check the path costs by hand, as above. Then add weighted cells (terrain costs), then try RRT with your own obstacles. If you want guided practice with graph search before moving to robotics code, start with the graph lessons on AlgoCademy, then look at ROS 2 and Nav2 tutorials to see the same ideas inside a real framework.

A lesson in AlgoCademy: the step to write, the line typed in the editor and checked, Step 3 passed; then a typo, the error pointing at its line, the fix, and Step 4 passed.

Before you go

Start thinking like a software engineer.

Learn to code and solve interview problems, one small, checked step at a time.

Try Dijkstra's Algorithm free

No account needed. You write real code in your browser.