VehicleMapping HD AV Spatial Data

Weighting Road Graphs for AV Route Planning

A lane-level graph with correct topology still routes badly if its edges are costed by length alone: it will take six lane changes to save two metres, cut through a conflicted left turn to avoid fifty metres of straight road, and prefer a service road to a carriageway because the service road is shorter.

Fixing that is a modelling problem, not a tuning problem. This task assigns costs for road network graph construction in a single unit — time — so every term is a quantity somebody can measure and disagree with on evidence.

The four cost terms, and where each one comes from:

The Four Cost Terms and Their Sources Four stacked rows naming a cost term, its formula, its typical magnitude and whether it is measured or derived. termformula typicalsource traversal the base unit length / speed limit 1–30 s measured manoeuvre lane change, turn flat penalty per manoeuvre 1.5 s fleet behaviour conflict exposure unprotected turns 2 s × crossing movements 0–12 s junction model comfort lateral acceleration excess over 1.5 m/s², scaled 0–4 s stored curvature

Prerequisites #

  • Python 3.10+, networkx 3.x, NumPy 1.24+.
  • Input: the expanded graph, with edge lengths, speed limits, curvature and conflict annotations already attached.
  • Upstream stage: junction expansion, as in building routing graphs from OpenDRIVE junctions.
  • Output: a cost attribute in seconds on every edge, plus an admissible heuristic.

Step-by-Step #

1. Cost traversal in seconds #

python
def traversal_cost(edge) -> float:
    """Seconds to traverse the edge at its posted speed."""
    v = max(edge["speed_limit_m_s"], 1.0)
    return edge["length_m"] / v

Clamping the speed keeps a zero-speed attribute — common on a lane that is mapped but closed — from producing an infinite cost that silently removes the lane from every route. If a lane really is closed, remove the edge rather than pricing it out.

2. Price manoeuvres in the same unit #

python
MANOEUVRE_S = {"lane_change": 1.5, "junction_entry": 0.8, "successor": 0.0}

def manoeuvre_cost(edge) -> float:
    return MANOEUVRE_S.get(edge["kind"], 0.0)

Because the penalty is in seconds, a reader can check it against a claim: 1.5 s at 25 m/s is about 38 m of equivalent detour, which is roughly what a driver will accept to avoid a routine lane change. In distance units the same number would be unarguable.

3. Price conflict exposure from the junction model #

python
CONFLICT_S = 2.0

def conflict_cost(node_data) -> float:
    """Seconds charged for crossing other movements inside a junction."""
    return CONFLICT_S * len(node_data.get("conflicts", ()))

This is what makes a planner prefer a protected right turn over an unprotected left across two streams of traffic, without anybody encoding "prefer right turns" — the preference falls out of the junction geometry the map already carries.

4. Keep the heuristic admissible #

python
def heuristic(a_xy, b_xy, v_max_m_s: float) -> float:
    """Straight-line time at the network's maximum speed — never an over-estimate."""
    return float(np.hypot(*(np.asarray(b_xy) - np.asarray(a_xy)))) / v_max_m_s

v_max_m_s must be the maximum over the whole network, not the local limit: using a local speed makes the heuristic optimistic in slow areas and pessimistic in fast ones, and the pessimistic half is what breaks optimality.

What each term changes about the route chosen, on the same origin and destination:

How Each Cost Term Changes the Chosen Route Four route variants between one origin and destination with their manoeuvre counts, conflict exposure and total distance under successive cost models. cost modellane changes unprotected turnsdistance distance only 6 1 2 940 m + manoeuvre 2 1 2 985 m + conflict exposure 2 0 3 120 m + comfort 2 0 3 150 m the route gets 7% longer and stops doing three things a passenger would object to

Verification & Acceptance Criteria #

python
import networkx as nx

def assert_weighting(G, v_max, samples) -> None:
    for u, v, d in G.edges(data=True):
        assert d["cost"] > 0, f"edge {u}->{v} has non-positive cost"
        assert np.isfinite(d["cost"]), f"edge {u}->{v} has infinite cost"

    for a, b in samples:                      # admissibility on sampled pairs
        h = heuristic(G.nodes[a]["xy"], G.nodes[b]["xy"], v_max)
        true = nx.shortest_path_length(G, a, b, weight="cost")
        assert h <= true + 1e-9, f"heuristic over-estimates {a}->{b}"

    plain = nx.shortest_path(G, *samples[0], weight="length_m")
    costed = nx.shortest_path(G, *samples[0], weight="cost")
    assert plain != costed, "costs have no effect — check they were written to the graph"

Acceptance gate: every edge cost positive and finite; the heuristic never over-estimating on a sampled set of pairs; and at least one sampled route that differs from the distance-only route, which is the check that catches costs computed but never attached.

What each cost term is worth in metres, which is how a reviewer checks whether it is plausible:

Each Cost Term Expressed as Equivalent Distance Four rows converting each edge-cost term into the equivalent detour distance a driver would trade for it. each term at 25 m/s, expressed as the detour it buys lane change · 1.5 s a routine motorway change ≈ 38 m of detour plausible junction entry · 0.8 s slowing to enter ≈ 20 m of detour plausible conflict · 2 s each per movement crossed ≈ 50 m per crossing plausible comfort · up to 4 s high lateral acceleration ≈ up to 100 m of detour check against your fleet a term nobody can convert into a distance a driver would accept is a term nobody can review

Common Errors & Fixes #

The planner takes six lane changes to save two metres. Manoeuvre costs are zero or missing. Confirm the kind attribute survived junction expansion.

Routes avoid a whole area. A speed limit of zero produced an infinite traversal cost. Clamp the speed and remove genuinely closed lanes from the graph.

A-star returns worse routes than Dijkstra on the same graph. The heuristic is inadmissible, almost always because it uses a local speed rather than the network maximum.

Conflict costs are zero everywhere. The annotations live on interior nodes and the cost function is reading edges. Charge the node's conflicts onto its outgoing edges.

Costs change between runs on an unchanged map. A term is being derived from a live traffic feed rather than from the map. Keep dynamic terms in a separate layer so the map's own costs stay reproducible.

FAQ #

Why cost in time rather than distance? #

Because every other term a route needs to weigh is naturally expressed in seconds. A lane change is worth some number of seconds of detour; a conflicted left turn is worth some number of seconds of waiting. In distance those become arbitrary constants with no unit, so nobody can say whether 25 is too much. In time they are estimates that can be measured from fleet data and argued about on the evidence.

How large should the lane-change penalty be? #

Large enough that the planner does not change lane to save a car length, small enough that it will change lane to make an exit. Measured against fleet behaviour that lands around one to two seconds of equivalent time for a routine change on a motorway, and considerably more in dense traffic. Deriving it from observed driver behaviour rather than choosing it means the number can be re-derived when the fleet or the region changes.

What breaks if the A-star heuristic is not admissible? #

The search stops being optimal, quietly. It still returns a route, and the route still looks reasonable, so the failure surfaces as occasional inexplicably poor routes rather than as an error. Keeping the heuristic to straight-line distance divided by the network's maximum speed guarantees it never over-estimates, which is the only property the optimality proof needs.

Up one level: Road Network Graph Construction — the stage this costing completes.