🚚 WIA Delivery Robot Standard Ebook | Chapter 5 of 8

Chapter 5: Phase 3 Navigation Protocol — Planning, Traffic Rules and Emergency Response

Chapters 3 and 4 described what a delivery robot says: the shape of its state, package and route records, and the REST calls that move them around. Phase 3 of WIA ROB-010 describes what the robot does between those messages. It fixes a behaviour contract: how a global route is chosen, how the next velocity command is picked every control cycle, how much space the robot leaves around people, how it crosses a street, which states it can be in, and what happens in the first fraction of a second after something goes wrong. The sensing and localization theory behind those decisions was covered in Chapter 2; this chapter assumes the robot already has a pose estimate and a list of classified obstacles, and focuses on turning them into safe motion.

Throughout the chapter we reuse the sample robot from the specification, robot_id 550e8400-e29b-41d4-a716-446655440000. Numbers that appear in the Phase 3 text are quoted as spec targets; every other number is marked as an example and should be tuned for your platform and jurisdiction.

5.1 Principles and the Priority Order

5.1.1 The five navigation principles

Phase 3 opens (§1.1) with five principles: safety first (pedestrian safety is paramount), rule compliance (obey traffic laws and local regulations), predictability (behave in ways that people nearby can anticipate), adaptability (cope with dynamic environments and edge cases) and efficiency (optimize time, energy and user satisfaction). They read like a mission statement, but two of them have direct engineering consequences.

Predictability rules out planners that jitter between nearly equal options. A robot that swerves left, then right, then left again around a pedestrian may never come close to a collision, yet it looks erratic and causes people to freeze or step into its path. In practice this principle becomes hysteresis in the local planner and a penalty for changing the passing side once committed. Adaptability, on the other hand, is the reason the specification defines failure modes (§6.2) instead of assuming sensors always work: the robot must keep a safe, degraded behaviour available when a sensor, the network or the positioning system disappears.

5.1.2 The priority order as a lexicographic cost

When objectives conflict, §7.2 ranks them: 1. safety (avoid collision), 2. rule compliance, 3. task completion, 4. efficiency, 5. user experience (smooth, predictable motion). The important word is order. A weighted sum such as 10·safety + 5·rules + 1·efficiency always admits a trade: with a large enough efficiency gain, the sum prefers a slightly less safe option. An ordered list forbids that trade. No amount of saved time justifies a reduction in pedestrian clearance.

The cleanest implementation is a lexicographic comparison: each candidate action is scored as a tuple, and tuples are compared element by element. Only when two candidates tie on safety does rule compliance decide, and so on. Python and most languages compare tuples this way natively.

from dataclasses import dataclass

@dataclass(frozen=True)
class Score:
    safety_violation: float   # 0.0 = no margin intruded; larger = worse
    rule_violation: float     # 0.0 = compliant
    task_penalty: float       # e.g. predicted lateness in seconds
    energy_cost: float        # e.g. predicted Wh
    comfort_penalty: float    # jerk, heading changes

    def key(self):
        # Rounding creates small "tie bands" so that noise in a lower
        # priority term cannot be overruled by noise in a higher one.
        return (round(self.safety_violation, 2),
                round(self.rule_violation, 2),
                round(self.task_penalty, 1),
                round(self.energy_cost, 1),
                round(self.comfort_penalty, 2))

def choose(candidates):
    """candidates: list of (action, Score). Lower key is better."""
    return min(candidates, key=lambda c: c[1].key())[0]

Two refinements are common. First, the safety term is often turned into a hard filter: any candidate that intrudes on a safety margin is discarded before scoring, so safety is never compared at all among the survivors. Second, pure lexicographic ordering can be brittle when measurements are noisy, which is why the example rounds each term into a tie band. The DWA and A* sections below show where these tuples come from.

5.2 Global Planning with A*

5.2.1 How the algorithm works

Global planning answers the question “which sequence of sidewalk segments, crossings and doors leads from here to the destination?” Phase 3 §4.1 specifies A* for this layer. A* explores a graph (a grid of cells, or a network of sidewalk segments) by always expanding the node with the lowest f(n) = g(n) + h(n), where g(n) is the cost already paid to reach n and h(n) is a heuristic estimate of the remaining cost to the goal. When the heuristic never overestimates the true remaining cost — it is admissible — the first time A* pops the goal, the path found is optimal for the given cost function.

The specification's reference listing is a textbook version. Production implementations usually add three details it omits: a closed set (or a check that skips stale queue entries, since the same node can be pushed several times with different priorities), a tie-breaking counter so the priority queue never has to compare node objects, and a limit on expansions so a missing connection in the map produces a timely failure instead of an exhaustive search.

import heapq, itertools, math

def a_star(graph, start, goal, heuristic, edge_cost, max_expansions=200_000):
    counter = itertools.count()          # tie-breaker for equal f values
    open_heap = [(heuristic(start, goal), next(counter), start)]
    g = {start: 0.0}
    came_from = {}
    closed = set()
    expansions = 0

    while open_heap:
        _, _, current = heapq.heappop(open_heap)
        if current in closed:            # stale entry
            continue
        if current == goal:
            return reconstruct(came_from, current), g[current]
        closed.add(current)
        expansions += 1
        if expansions > max_expansions:
            break

        for nbr in graph.neighbors(current):
            c = edge_cost(current, nbr)
            if c is None:                # edge forbidden by a hard constraint
                continue
            tentative = g[current] + c
            if tentative < g.get(nbr, math.inf):
                came_from[nbr] = current
                g[nbr] = tentative
                heapq.heappush(open_heap,
                               (tentative + heuristic(nbr, goal), next(counter), nbr))
    return None, math.inf                # no path: report, do not guess

def reconstruct(came_from, node):
    path = [node]
    while node in came_from:
        node = came_from[node]
        path.append(node)
    return path[::-1]

5.2.2 Cost terms

§4.1 lists five ingredients of the edge cost. The table below restates them together with an implementation note for each. The terrain multipliers are the values given in the specification; the other factors are left to the implementer.

The last row hides a subtle trap. A bonus implemented as a negative edge cost can make the true remaining cost smaller than the straight-line distance, which breaks the admissibility of a Euclidean heuristic; with enough negative edges, A* is no longer guaranteed to return the best path. Expressing the same preference as a positive penalty on road segments keeps every edge cost at least equal to its length (because the smallest terrain multiplier is 1.0), so the straight-line distance remains a valid lower bound.

5.2.3 Linking the planner to the Route record

The Phase 1 Route schema carries two fields that must influence the planner. route_preference takes one of four values; a practical approach is to map each value to a different weighting of the cost terms. The weights below are examples only; the standard does not define them.

navigation_constraints are different: they are constraints, not preferences. sidewalk_only and avoid_roads remove road edges from the graph (the edge_cost function returns None in the listing above); elevator_allowed = false removes elevator edges from indoor graphs; and max_speed_ms caps the velocity window of the local planner. Keeping the two concepts separate prevents an expensive-but-allowed road segment from being confused with a forbidden one. Whatever mapping you choose, record it in your implementation documentation so that a fleet operator selecting safest knows what they are getting.

Finally, the planner's output becomes the Route's waypoints array. Most planners produce hundreds of cells; publish only the waypoints that matter — the start, turns, crossings, elevator doors, charging stops and the destination — using the waypoint_type values from Phase 1.

5.3 Local Planning with the Dynamic Window Approach

5.3.1 The dynamic window

The global path is computed against a map; the local planner reacts to the world as it is right now. Phase 3 §4.2 uses the Dynamic Window Approach (DWA), originally described by Fox, Burgard and Thrun in 1997. For a differential-drive or similar wheeled robot, a command is a pair (v, ω) of linear and angular velocity. DWA restricts the search to three intersecting sets:

• Possible velocities — everything the drive train can produce, capped by the Route's max_speed_ms and by context (for example the 0.5 m/s limit in crowds, section 5.4).
• Dynamic window — only velocities reachable within the next control interval Δt given the maximum accelerations: v ∈ [v₀ − a·Δt, v₀ + a·Δt], and likewise for ω. This is the “kinematically feasible” condition in the specification.
• Admissible velocities — velocities from which the robot can still stop before the nearest obstacle on the predicted arc: v ≤ √(2·b·d), where b is the braking deceleration and d the free distance along the arc. For a delivery robot, d should be measured to the edge of the safety margin, not to the obstacle itself.

Each surviving pair is simulated forward for a short horizon (typically one to three seconds) and scored:

score(v, ω) = α · heading(v, ω) + β · clearance(v, ω) + γ · velocity(v, ω)

heading    alignment of the predicted end pose with the direction to the next waypoint
clearance  distance to the nearest obstacle along the predicted arc
velocity   forward speed (prefer faster when safe)
all three terms normalized to [0, 1] over the current candidate set

Normalization matters: without it, a term measured in metres dominates one measured in radians regardless of the weights.

5.3.2 A worked example

The following numbers are illustrative. The robot moves at v₀ = 1.0 m/s along a sidewalk, with a maximum acceleration of 1.0 m/s² and a control interval of 0.25 s, so its dynamic window for linear speed is 0.75–1.25 m/s. A pedestrian is walking toward it, slightly to its right. The weights are α = 0.4, β = 0.4, γ = 0.2, and the pedestrian safety margin is taken as 1.5 m, the lower end of the Phase 3 range. For this example, clearance is normalized so that the edge of the margin (1.5 m) scores 0 and 2.5 m or more scores 1; velocity is divided by the fastest candidate, 1.2 m/s; heading is 1 for a candidate aimed straight at the next waypoint.

Candidate D is removed before scoring, which is how the priority order of section 5.1 is enforced: it would have been attractive on speed, but safety is not traded. Among the survivors, B wins: it gives up a little progress to pass with comfortable clearance. Candidate A technically respects the margin, but only just; a slight error in the pedestrian's predicted trajectory would push it inside. That is why many teams add a clearance term that rises steeply near the margin instead of linearly.

To honour the predictability principle, the next cycle should prefer candidates that continue passing on the same side. A simple way is a small penalty for reversing the sign of ω within a short time window, applied in the comfort position of the lexicographic tuple.

5.3.3 Why two planners

DWA alone is short-sighted: it can steer into a dead end that only a global map would reveal. A* alone is too slow and too static to react to a child stepping off a bench. Phase 3 therefore uses the global path as a guide (the heading term points at the next global waypoint several metres ahead) and lets DWA handle everything within a few metres. When DWA finds no admissible candidate for several cycles, the robot stops and transitions to WAITING (section 5.6); if the blockage persists, the global planner is asked for an alternative before the stuck timer of section 5.7 fires.

5.4 Safety Margins and Sidewalk Etiquette

5.4.1 Margins by obstacle class

Phase 3 §3.2 defines minimum distances by obstacle class. They are the most directly testable numbers in the navigation protocol.

A convenient way to implement the margins is an inflated cost map: each detected obstacle is expanded by its class-specific margin, and the local planner treats the inflated region as occupied. This is the same idea as the inflation layer of the ROS 2 Nav2 costmap, extended to be class-aware. The inflation should grow with relative speed: a margin that is comfortable when passing a person standing still is too small when the two of you approach each other at a combined speed of 2.5 m/s.

The obstacle object shown in §3.2 of the specification uses class values such as pedestrian; the specification does not publish a complete class enumeration. If you want the 2.0 m wheelchair/stroller rule to be auditable, you need distinct classes for them (for example wheelchair and stroller). Document these values as your own extension until the standard defines them.

5.4.2 Sidewalk etiquette

§5.2 lists six etiquette rules: always yield to pedestrians; keep at least 1.5 m distance; pass on the left when possible; slow to 0.5 m/s in crowded areas; stop completely if the path is blocked; and never block wheelchair ramps or doorways. Five of these are universal and can be implemented as fixed behaviour. The passing side is not. Which side people keep to on a footpath depends on local custom, and it can differ between neighbouring cities or even between a train station and the street outside. Implement it as a parameter of a site or jurisdiction profile rather than hard-coding “left”.

The following profile is an illustrative configuration format, not part of the standard. It shows the kind of values that should be externalized so that the same software can be deployed in different places.

{
  "profile_id": "example-site-profile",
  "passing_side": "right",
  "crowd_speed_limit_ms": 0.5,
  "pedestrian_margin_m": 1.8,
  "vehicle_margin_m": 2.5,
  "static_margin_m": 0.4,
  "mobility_aid_margin_m": 2.0,
  "keep_clear_zones": ["ramp", "doorway", "crosswalk_curb_cut", "bus_stop_boarding"],
  "crowd_density_threshold_per_m2": 0.5
}

Two items need an operational definition. “Crowded” is not defined in the specification; a density threshold (people per square metre within the sensing radius) or a count of tracked pedestrians within a few metres are both workable, but the choice must be written down so that testers can reproduce it. “Never block ramps or doorways” implies that stopping is constrained, not just driving: when the robot has to wait, it must choose a waiting spot outside keep-clear zones, even if that means reversing a short distance. Mark those zones in the map so the local planner can check them.

5.5 Crosswalk Protocol

Crossing a road is where a sidewalk robot meets vehicles, and §5.1 gives it a dedicated five-step protocol:

1. Detect the crosswalk using both vision and map data.
2. Check the traffic signal, if present: walk/green — proceed with caution; don't walk/red — stop and wait; flashing — evaluate the time remaining.
3. Scan for approaching vehicles with LiDAR and camera.
4. If clear or the signal allows: signal intent with lights or sound, cross at a steady pace of 1–2 m/s, and keep monitoring for vehicles even on green.
5. If a vehicle approaches: yield even if the robot has the right of way, and resume when safe.

The “flashing” case is the one that needs arithmetic. The robot should start only if it can finish before the signal ends, with a margin for acceleration and for having to yield mid-crossing. The sketch below makes that decision explicit; the crossing speed and margin are example values within the spec's 1–2 m/s band.

def may_start_crossing(signal, remaining_s, crossing_length_m,
                       vehicles_clear, cross_speed_ms=1.2, margin_s=4.0):
    if not vehicles_clear:
        return False                       # step 3 always applies
    if signal in ("walk", "green"):
        return True
    if signal in ("dont_walk", "red"):
        return False
    if signal == "flashing":
        if remaining_s is None:            # no countdown visible or received
            return False                   # conservative default
        needed = crossing_length_m / cross_speed_ms + margin_s
        return remaining_s >= needed
    if signal == "none":                   # unsignalized crossing
        return True                        # but only after a full scan
    return False                           # unknown state: do not go

# Example: a 12 m crossing at 1.2 m/s needs 10 s + 4 s margin = 14 s.
# With 9 s left on the countdown, the robot waits for the next cycle.

Several design decisions follow from the protocol. First, the signal state should come from more than one source where possible — a camera classifier and, where the city offers it, a connected signal feed — and disagreement between them should resolve to “wait”. Second, once the robot is in the roadway, stopping is not always the safest action; the protocol's “yield” means letting the vehicle pass while remaining predictable, and in many cases continuing at a steady pace to the kerb is safer than freezing mid-lane. Encode this as a separate in-crossing behaviour rather than reusing the sidewalk stop rule. Third, a completed crossing should be logged with the signal state, the time remaining and the minimum vehicle distance observed, because those are the facts an incident investigator will ask for.

Whether a delivery robot may use a crosswalk at all, and under what conditions, is a matter for local law and site authorization. The protocol describes behaviour when crossing is allowed; it does not grant permission.

5.6 The Behaviour State Machine

5.6.1 States and transitions

§7.1 defines six behaviour states and seven transitions. Drawn as a diagram:

                  task assigned
        +--------+ ------------> +------------+  obstacle / signal  +---------+
        |  IDLE  |               | NAVIGATING | ------------------> | WAITING |
        +--------+ <--+          +------------+ <------------------ +---------+
            ^         |                |              path clear
            |         |                | arrived at destination
            |         |                v
            |         |          +------------+
            |         +----------| DELIVERING |   package delivered
            |                    +------------+
            |  manually cleared
        +--------+
        | ERROR  | <=====  ANY state on critical failure
        +--------+

        +----------+   (entry and exit transitions not defined in §7.1)
        | CHARGING |
        +----------+

The machine is deliberately small, and it leaves some behaviour undefined. An implementation needs answers to at least the following, and the answers should be documented as part of the implementation's conformance statement:

• Entering and leaving CHARGING. The battery rule in §6.2 (return to charge below 15 %) implies a transition from NAVIGATING toward a charging station and from CHARGING back to IDLE; neither is listed. Chapter 6 describes the docking sequence.
• Failed delivery. DELIVERING → IDLE covers only success. If the recipient does not appear, the package must move to the Phase 1 status failed or returned, and the robot needs a transition back to NAVIGATING with a new destination.
• Stuck versus error. “Stuck for 30 s” leads to a request for remote assistance, not necessarily to ERROR. Treat it as a sub-state of WAITING unless the assistance request itself fails.
• Remote operation. Phase 1 has a manual mode, but Phase 3 has no corresponding state. Teleoperation must be entered and left explicitly, and the safety layer (margins, emergency stop) should remain active while a human drives.

5.6.2 Mapping behaviour states to the Phase 1 mode field

RobotState's mode enumeration (autonomous, manual, idle, charging, error) is coarser than the Phase 3 state set. Three behaviour states collapse into one mode, which means a fleet dashboard reading only mode cannot tell a robot that is driving from one that is waiting at a red light or handing over a parcel.

The Phase 1 RobotState schema does not set additionalProperties: false, so a namespaced extension field can carry the detailed state without breaking validation. The field name below is an implementation choice, not a standard field:

{
  "robot_id": "550e8400-e29b-41d4-a716-446655440000",
  "timestamp": "2025-12-26T09:14:05Z",
  "version": "1.0.0",
  "position": {"latitude": 37.5665, "longitude": 126.9780, "source": "fused"},
  "battery_level": 64,
  "speed": 0.0,
  "mode": "autonomous",
  "x_behaviour": {
    "state": "WAITING",
    "reason": "signal_dont_walk",
    "since": "2025-12-26T09:13:51Z"
  }
}

Consumers that do not understand x_behaviour ignore it and still see a valid RobotState. Consumers that do can show operators why a robot is stationary, which is the most frequent question in fleet operations.

5.7 Emergency Stop and Failure Modes

5.7.1 Emergency-stop triggers

§6.1 lists five conditions that must trigger an emergency stop: the physical emergency button is pressed; a remote emergency command is received (the synchronous endpoint described in Chapter 4); a critical sensor failure is detected; an obstacle comes within 0.3 m; or the robot loses control, for example by tipping or skidding. The last two require measurements that must be available even when the navigation stack is not: a dedicated short-range sensor or bumper for the 0.3 m zone, and IMU-based tilt and wheel-slip detection.

5.7.2 The response sequence

The response is an ordered sequence of six actions, and only the first one carries a timing requirement:

The 100 ms cutoff is only credible if it does not depend on the operating system scheduler, the middleware or the network. Industrial practice describes this distinction in IEC 60204-1 as stop categories: category 0 removes power from the actuators immediately, while category 1 performs a controlled stop and then removes power. A delivery robot on a slope may need category 1 behaviour (brake, then cut power) so that it does not roll; either way, the stop path belongs in the safety-rated hardware and firmware, with software informed rather than in charge. Steps 4 and 5 are also important for later analysis: the snapshot should contain the last few seconds of RobotState, the obstacle list and the commands issued, so that the trigger can be reconstructed.

“Await manual clearance” means the robot must not restart itself when the trigger disappears. If a pedestrian steps within 0.3 m and then steps away, the robot remains stopped until an operator — remotely or on site — clears it. Operators often want to relax this for the proximity trigger; if you do, document the exception, because it is a deviation from the specification.

5.7.3 The failure-mode table

§6.2 defines six degraded modes. The table repeats them and adds the resulting behaviour state and what the robot should report, so that each row can be tested.

Several rows need interpretation before they can be implemented:

• Camera failure has no timer. Choose one (for example, a number of consecutive missed frames) and document it. Also consider that the crosswalk protocol depends on vision for signal state; without a camera, a conservative implementation stops using signalized crossings.
• “Voltage drop” is a poor battery indicator. Voltage sags under load and recovers at rest. Use the state-of-charge estimate from the battery management system, which is also what battery_level reports, and apply the 15 % threshold to it. Chapter 6 covers proactive charging at 20–30 %, which should normally prevent this row from firing.
• Communication loss and remote stop. Continuing the task without the cloud is sensible, but it means the remote emergency command cannot arrive. Every other safety function must therefore run locally, and the robot should not start a new task or a road crossing that it would not start without remote supervision if your safety case assumes supervision.
• Combined failures. The table treats failures one at a time. When two occur together — for example GPS loss inside a tunnel and a LiDAR fault — the speed reduction and positioning fallbacks may no longer be sufficient. A simple rule is that two simultaneous perception or positioning failures escalate to a stop and an assistance request.

The timers translate naturally into a watchdog that runs alongside the navigation stack:

import time

TIMERS = {                      # seconds, from Phase 3 §6.2
    "gnss_fix": 30.0,
    "lidar_data": 5.0,
    "cloud_contact": 60.0,
    "progress": 30.0,
}
CAMERA_TIMEOUT_S = 1.0          # implementation choice: spec gives no timer

class Watchdog:
    def __init__(self, now=time.monotonic):
        self.now = now
        self.last = {k: now() for k in list(TIMERS) + ["camera_frame"]}

    def seen(self, signal):     # call whenever the signal is observed
        self.last[signal] = self.now()

    def active_failures(self, battery_soc):
        t = self.now()
        failed = [k for k, limit in TIMERS.items() if t - self.last[k] > limit]
        if t - self.last["camera_frame"] > CAMERA_TIMEOUT_S:
            failed.append("camera_frame")
        if battery_soc < 15.0:
            failed.append("battery_low")
        return failed

def respond(failures, nav):
    perception = {"lidar_data", "camera_frame", "gnss_fix"} & set(failures)
    if len(perception) >= 2:
        nav.stop_and_request_assistance("multiple_perception_failures")
        return
    if "lidar_data" in failures:
        nav.scale_speed_limit(0.5)
    if "gnss_fix" in failures:
        nav.use_localization_source("slam")
    if "battery_low" in failures:
        nav.return_to_charger()
    if "progress" in failures:
        nav.request_assistance("stuck")
    if "cloud_contact" in failures:
        nav.buffer_telemetry_locally()

Use a monotonic clock for these timers, not wall-clock time: a clock step from a time synchronization update must not make a sensor look dead or alive.

5.8 Performance Metrics

§8 lists the metrics an implementation should monitor. The first five come with numeric spec targets; energy efficiency is stated as a comparison without a target. Targets only mean something if the measurement is defined, so the table adds a measurement method for each.

Three measurement pitfalls are worth stating. First, a detection rate of 99.9 % cannot be demonstrated with a few hundred trials: with zero misses, roughly 3,000 independent encounters are needed to support it at 95 % confidence, and more if any miss occurs (Chapter 7, section 7.6.3, gives the arithmetic). Plan your test campaign with this in mind, and combine simulation with physical trials.

Second, reaction time depends on synchronized clocks when the detection and the command are produced on different processors. The Annex E note on time synchronization (NTPv4 for most deadlines, PTP where deterministic interlocks are needed) applies directly: a clock offset of a few tens of milliseconds between two computers makes a 100 ms target impossible to verify. Measuring both timestamps on the same clock avoids the problem.

Third, route deviation compares the driven distance with the “optimal” route, but the optimum depends on the cost function of section 5.2. For a safest route, the reference must be the safest planned route, not the shortest one; otherwise the metric penalizes exactly the behaviour you asked for. Chapter 7 describes how these measurements feed the conformance evidence package.

5.9 Chapter Summary

• Phase 3 ranks objectives strictly — safety, rules, task, efficiency, user experience — and the cleanest implementation is a hard safety filter followed by a lexicographic comparison.
• A* handles global planning; keep edge costs at least equal to edge length so the straight-line heuristic stays admissible, and treat navigation_constraints as forbidden edges rather than penalties.
• DWA handles the next second of motion; bound the velocity window by acceleration, braking distance to the safety margin, and context speed limits.
• Safety margins and etiquette are mostly universal, but the passing side and the definition of “crowded” must be parameters.
• The six-state machine maps loosely onto the five-value mode field; report the detailed state in an extension field and define the transitions the specification leaves out.
• The emergency stop's 100 ms target belongs in safety hardware, and every failure-mode row needs a defined timer, a resulting state and a report before it can be tested.

← Chapter 4: Phase 2 API Interface — Auth, Robots, Packages, Routes and Fleet | Back to Index | Chapter 6: Phase 4 Fleet Integration — Telemetry, Dispatch, Charging and OTA →