Dijkstra’s algorithm: find the lowest-cost route

A route with fewer steps can cost more. A robot crossing six grid cells might spend more energy than one taking eight easier moves.

Dijkstra’s algorithm finds a route with the smallest total cost on a graph with nonnegative edge costs. You will trace its choices, update its estimates and recover the route it finds.

Turn the planning problem into a graph

A graph contains nodes and edges. A node represents a state, such as a robot at a grid cell. An edge represents an allowed move from one node to another.

Choose a start node S and a goal node G. Give each directed edge u → v a cost w(u, v). The reverse move can have a different cost, or no edge at all.

For the experiment, each free grid cell is a node. Edges join horizontal and vertical neighbors. Blocked cells have no incident edges, and diagonal moves are unavailable.

These choices define the problem the search can solve. A missing edge prevents a move even if the drawing makes it look possible.

Define what a route costs

Add the costs of its edges:

The numbers can represent distance, time or a defined penalty. All edges must use a compatible unit. An energy model also needs an additive cost for each represented move.

The demo charges the destination cell's entry cost. Its starting cell contributes zero. Entering the five middle cells at cost 2, then the goal at cost 1, gives a direct-route cost of 11.

An eight-move detour through cost-1 cells costs 8. The search chooses the smaller sum. Breadth-first search would optimize the number of edges when every edge has the same cost.

Track estimates, parents and the frontier

Keep three records while searching:

• Estimate g(v): the cheapest route to v found so far. Start with g(S) = 0 and every other estimate at infinity.
• Parent of v: the preceding node on that current route. Following parents backward recovers the route.
• Frontier: discovered nodes awaiting removal from the priority queue. Order them by g.

Removing a node with the smallest estimate settles its cost. A settled node has its true minimum cost under the nonnegative-edge assumption.

Before settlement, treat the estimate as a candidate. A later edge check can find a cheaper route on a general weighted graph.

Improve an estimate by relaxing an edge

Suppose the search settles u. Reaching neighbor v through u would cost g(u) + w(u, v). Relaxing the edge compares that candidate with the current estimate:

Use four nodes with these directed edges:

After S, the estimates are A = 7 and B = 2. Settling B improves A to 2 + 1 = 3 and discovers G at 2 + 8 = 10. Settling A then improves G to 3 + 2 = 5.

The parent chain becomes G ← A ← B ← S. Reverse it to get S → B → A → G, whose cost is 2 + 1 + 2 = 5. An equal candidate can leave the existing parent unchanged; several cheapest routes may exist.

Settle the smallest frontier cost

Why can the smallest frontier estimate become final? Imagine a cheaper route to the node u that the queue is about to remove. On that route, find the first node x that has not settled yet.

Its preceding node already settled, so relaxing that edge gave x an estimate no greater than the cheaper route's cost. Nonnegative edges make the route's remaining cost at least zero. Thus x would have a smaller estimate than u, contradicting the queue's choice.

This argument also permits zero-cost edges. MIT’s Dijkstra lecture notes give the formal proof and queue analysis.

A negative edge breaks the argument. If S → G costs 2, S → B costs 5 and B → G costs −4, the queue removes G at 2 before discovering the route through B costing 1. Use an algorithm designed for negative edges, such as Bellman–Ford, when those costs belong in the model.

Stop when the goal leaves the queue

For a single goal, stop when the queue removes and settles G. In the four-node example, G first enters the queue at cost 10. Stopping at that moment would miss the cost-5 route.

If the frontier empties first, the goal is unreachable from S on this finite graph. If S and G are the same node, settling it returns a route with zero edges and zero cost.

For distances to every reachable node, keep going until the frontier empties. Parent links then form a shortest-path tree rooted at S. Early stopping can leave other frontier estimates unfinished.

Follow the search across a weighted grid

Lowest accumulated cost first

Trace Dijkstra’s search one node at a time

Move between neighboring cells up, down, left or right. Entering a cell adds its displayed cost. The starting cell costs zero.

Dijkstra mapCostly shortcutOpen gridWall with a gapDisconnected goalStart equals goal

Search step

4 of 34 queue removals

Next search stepFinish Dijkstra search

Entry cost above, route estimate below

Frontier in removal order

Choose the smallest route estimate g. Equal costs use row first, then column.

Frontier node coordinates and accumulated route costs

Node	Cost g
(1, 1)	2
(1, 2)	2
(1, 3)	2
(0, 4)	2
(1, 0)	3

The next step settles the first node in this table and checks its outgoing moves.

This step updated 1 estimate.

• Solid blue cell: settled cost; dashed border: frontier estimate.
• Double border: current node; amber line: final route.

Search step

4

Settled nodes

4

Frontier nodes

5

Current node

(0, 0)

Current settled cost

2

Goal estimate

Not discovered

Result

Searching

Route cost

Not settled yet

Route moves

Not settled yet

Searching. 4 nodes have settled costs. The goal's current estimate is not discovered.

Every incoming move to a cell uses that cell’s entry cost. This restricted cost model discovers each finite estimate at its final value under Dijkstra’s ordering. General weighted graphs can improve a discovered estimate; the earlier four-node example shows that case.

Reset Dijkstra experiment

Start with Costly shortcut and finish the search. The result costs 8 over eight moves; the straight six-move route costs 11. Coordinates use column first and row second, with rows increasing downward.

The slider counts queue removals, including the start and the final goal. Rewind to inspect the frontier before each choice. Equal costs use increasing row, then column, which selects one repeatable route among ties.

Try Wall with a gap. Reaching the opening takes extra moves, giving cost 10. Disconnected goal empties the frontier without reaching G, and Start equals goal finishes at cost zero.

This grid has a special restriction: every incoming edge to a cell has the same entry cost. Dijkstra first reaches that cell from a predecessor with the smallest possible settled cost, so its first finite estimate already equals its final value. The earlier four-node graph shows the estimate revisions that general edge costs allow.

Connect a graph route to robot motion

Graph search trusts the graph it receives. A cost-optimal route only answers which represented sequence has the smallest sum.

For a physical robot, use configuration space to represent its shape and pose. Validate each edge's complete motion with collision checking. Clear endpoints alone leave the motion between them untested.

Probabilistic roadmaps build a different graph by sampling valid configurations and checking their connections. The same cost-ordered search can then answer several start-to-goal queries.

A finer grid changes the available routes and the amount of search work. A coarse grid can miss a narrow passage or impose extra distance through its limited directions. The graph's lowest-cost route can differ from the shortest continuous path through the room.

Time-dependent costs, moving obstacles or motion constraints need a state and edge model that includes those effects. After choosing a geometric path, trajectory time scaling addresses speed and acceleration along it.

Choose a queue and count its work

A priority queue lets the search repeatedly remove the smallest estimate. With an adjacency list and a binary heap that supports decreasing a key, Dijkstra runs in O((V + E) log V) time and uses O(V + E) space, including the graph.

Here V counts nodes and E counts directed edges. LaValle’s graph-search discussion connects this cost ordering to planning problems and priority queues.

The widget sorts its small frontier and stores snapshots for the slider. That teaching implementation has extra sorting and storage costs. Its node counts show search behavior; they are not runtime benchmarks.

The Python example uses a heap with replacement entries. It leaves older entries in the heap and skips them when their cost no longer matches the current estimate. In a general graph, that version can store O(E) heap entries and takes O((V + E) log(V + E)) time.

A* search adds a lower bound on the remaining cost to guide the queue. Setting that bound to zero recovers Dijkstra’s cost ordering.

Reproduce the search in Python

This standard-library example reproduces the four-node graph. It prints each successful relaxation, including the two revisions to already discovered nodes.

from heapq import heappop, heappush
from math import inf, isfinite

graph = {
    "S": [("A", 7), ("B", 2)],
    "A": [("G", 2)],
    "B": [("A", 1), ("G", 8)],
    "G": [],
}

def dijkstra(graph, start, goal):
    for edges in graph.values():
        for neighbor, weight in edges:
            if neighbor not in graph or not isfinite(weight) or weight < 0:
                raise ValueError("Use known nodes and finite nonnegative costs")
    if start not in graph or goal not in graph:
        raise ValueError("Start and goal must be graph nodes")
    costs = {node: inf for node in graph}
    costs[start] = 0
    parents = {}
    frontier = [(0, start)]
    while frontier:
        cost, node = heappop(frontier)
        if cost != costs[node]:
            continue  # Skip a superseded heap entry.
        print(f"settle {node}: {cost}")
        if node == goal:
            path = [goal]
            while path[-1] != start:
                path.append(parents[path[-1]])
            return list(reversed(path)), cost
        for neighbor, weight in graph[node]:
            candidate = cost + weight
            if candidate < costs[neighbor]:
                costs[neighbor] = candidate
                parents[neighbor] = node
                heappush(frontier, (candidate, neighbor))
                print(f"update {neighbor}: {candidate} via {node}")
    return [], inf

path, cost = dijkstra(graph, "S", "G")
print("path:", " -> ".join(path))
print("cost:", cost)

Expected output:

settle S: 0
update A: 7 via S
update B: 2 via S
settle B: 2
update A: 3 via B
update G: 10 via B
settle A: 3
update G: 5 via A
settle G: 5
path: S -> B -> A -> G
cost: 5

The node names are strings, so Python can compare them when heap costs tie. For arbitrary node objects, include a unique counter in each heap entry to supply a comparable tie-breaker.

Try it yourself

1. Make the shortcut worthwhile. Keep the experiment's direct route through five middle cells and the cost-1 goal. Give each middle cell the same entry cost c. Compare that route with the eight-move cost-1 detour: when is the direct route cheaper, and when do they tie?

Show the route-cost solution

The direct route costs 5c + 1. The detour costs 8, so the direct route is cheaper when c < 7/5 = 1.4. They tie at c = 1.4.

The widget offers integer entry costs 1 and 2. Those choices fall on opposite sides of the threshold: the direct route costs 6 at c = 1 and 11 at c = 2. Intermediate values belong to this paper exercise.

2. Revisit the goal estimate. Change B → G in the four-node example from cost 8 to cost 2. List the settlement order, the final parent of G and the total route cost. Explain why the estimate for G first becomes finite before the search can stop.

Show the changed-graph solution

The settlement order remains S, B, A, G, with costs 0, 2, 3, 4. Settling B gives G an estimate of 4 and parent B. The later candidate through A costs 3 + 2 = 5, so G keeps its parent.

The route is S → B → G at cost 4. When B discovers G, node A still has the smaller queue cost 3 and must settle next. Removing G at cost 4 supplies the stopping condition.

Sources and further study

• E. W. Dijkstra, A Note on Two Problems in Connexion with Graphs, 1959. Problem 2 presents the shortest-route procedure in its original form.
• MIT 6.006, Lecture 13: Dijkstra’s Algorithm. Nonnegative edge weights, relaxation, the settlement proof and priority-queue costs.
• Steven LaValle, Planning Algorithms: Dijkstra’s algorithm. Cost-ordered graph search for discrete planning.