Behavior Planning Primer: How FSMs, Behavior Trees, and POMDPs Choose What an Automated Vehicle Does Next

“Stop at red, follow a slower lead vehicle, and change lanes when the adjacent gap is safe” sounds like a short program. A real road presents all three problems with uncertainty: the signal is partly occluded, the lead vehicle may turn or park, and an adjacent driver appears to yield but has not yet decelerated. An automated vehicle must select a defensible mode of behavior from incomplete observations, then keep revising it as the scene reacts.

Behavior planning is the decision layer between perception/prediction and continuous motion control. Its inputs include ego state, road topology, signals, tracked objects, predicted futures, route intent, and the operational design domain (ODD). Its outputs are discrete intentions—follow, stop, yield, merge, change lane, or enter a minimal-risk condition—plus constraints on lane, speed, time, and clearance. It should not directly invent a steering angle. It tells downstream path, trajectory, and control layers what must be achieved and what must never be violated.

Subaru WRX S4 equipped with the stereo-camera EyeSight driver-assistance system

Example of a production vehicle with driver assistance

Image: Subaru WRX S4 2.0GT-S EyeSight (Tokumeigakarinoaoshima, CC BY-SA 4.0), Wikimedia Commons. This exterior photograph does not reveal the internal state machine or implementation of any product discussed here.

Practical conclusion

Behavior planning selects a maneuver such as stop, follow, merge, lane change, or fallback; path and trajectory planning determine the continuous motion that realizes it.
A finite-state machine is traceable and effective for bounded functions, but exceptions can cause a state-transition explosion. A behavior tree expresses priorities and reusable subtrees, but shared state and concurrent actions can hide complexity.
A POMDP represents occlusion and latent intent as probability distributions, at the cost of expensive belief updates and future search. Production systems commonly combine rules, state machines, optimization, and learned components rather than choosing one universal method.
A perception class label must not become a command. Behavior planning needs timestamps, covariance, existence probability, track history, and multiple prediction hypotheses.
Path planning solves geometric reachability; MPC solves continuous constrained control. Behavior planning supplies their objectives, constraints, priorities, abort criteria, and deadlines—and must react when they report infeasibility.
Safety evidence needs traceable requirements, scenario coverage, boundary sweeps, fault injection, counterfactual analysis, closed-course reproduction, and an independent safety monitor, not only accumulated incident-free miles.

1. A behavioral state machine makes decisions inspectable

The following finite-state machine (FSM) simplifies highway assistance. The system cruises, follows a slower lead vehicle, prepares a lane change after checking route need and a safe gap, executes it, and returns to cruise. Sensor faults, ODD exit, or planning infeasibility can lead from any active state to degradation or a minimal-risk maneuver.

Figure 1 — Duskcoil conceptual behavior state machine. Transitions are deliberately simplified for teaching; the diagram does not represent the implementation or safety specification of Subaru EyeSight or any other named product.

An FSM has a state set $S$ , event or input set $E$ , and transition function

s_{t+1}=\delta(s_t,e_t).

Its main advantage is traceability. A log can say “prepare lane change was aborted because the rear-gap TTC crossed its guard.” Transition tables map naturally to requirements and tests. This makes FSMs valuable for bounded functions such as adaptive cruise control, lane support, engagement, and degradation modes.

The weakness is combinatorics. Right and left lane changes, work zones, emergency vehicles, toll gates, sensor degradation, and driver intervention create a product of states and transitions. Hierarchical FSMs, explicit state ownership, prioritized transitions, side-effect-free guards, and timeouts prevent some of that explosion. A lane-change guard might be

g=ODD\land RouteNeed\land GapSafe\land PerceptionFresh\land TrajectoryFeasible.

This Boolean is useful for control flow, but engineers should retain the relative distance, velocity, covariance, prediction weights, and freshness behind GapSafe; otherwise a changed decision cannot be explained.

2. Behavior trees express priority and reuse

A behavior tree (BT) evaluates a tree of control and leaf nodes. A Sequence succeeds only after its children succeed in order. A Fallback/Selector tries children until one succeeds or remains running. Condition and Action leaves check a gap, request a trajectory, or command a fallback.

The tree structure makes “emergency response before intersection handling before normal cruise” visible and lets teams reuse subtrees for safe stop, protected turn, or parking. It is often easier to extend than an all-to-all transition graph.

Yet a tree is not automatically safe. A reactive tree ticked from the root can alternate behaviors when a noisy condition crosses its threshold. Hysteresis, minimum dwell time, and explicit completion criteria are necessary. A shared blackboard can become hidden global state. If a running action is halted, the tree must cancel its downstream trajectory and verify that stale commands cannot survive. Parallel nodes also need a defined resource policy: two actions cannot independently own longitudinal control.

3. POMDPs represent hidden intent

A merging driver’s intention—yield or enter first—is not directly observable. Neither is a pedestrian behind an occluding truck. A partially observable Markov decision process uses state $s$ , action $a$ , observation $o$ , transition model $T(s'|s,a)$ , observation model $O(o|s')$ , and reward $R(s,a)$ .

The planner maintains a belief $b_t(s)$ over hidden state. After action $a_t$ and observation $o_{t+1}$ , the conceptual update is

b_{t+1}(s')=\eta O(o_{t+1}|s')\sum_s T(s'|s,a_t)b_t(s),

where $\eta$ normalizes probability. It seeks a policy such as

\pi^*=\arg\max_\pi E\left[\sum_{k=0}^{H}\gamma^kR(s_{t+k},a_{t+k})\right].

Collision can receive a very large penalty, with smaller penalties for rule violation, discomfort, and delay. The attraction is that uncertainty is explicit; an “edge forward to see around the obstruction” action may improve information. The cost is computation and modeling. Continuous traffic state, many actors, and a long horizon make exact solutions impractical. Poor rewards can produce aggressive behavior or a vehicle that never moves. Practical systems restrict candidates with rules and use approximations such as online search, Monte Carlo tree search, learned value functions, or compact intent models.

4. Hybrid designs divide responsibilities

Method	Strength	Weakness	Appropriate role
FSM / hierarchical FSM	deterministic and traceable	transition explosion	modes, ODD, degradation, bounded ADAS
Behavior tree	priority, reuse, recovery	blackboard and tick semantics	task composition and exception handling
Rules / rulebooks	explicit legal and safety priority	cannot enumerate the world	hard constraints and candidate ranking
MDP / POMDP	future interaction under uncertainty	model and compute cost	merging, intersections, negotiation
Imitation / reinforcement learning	approximates complex interaction	distribution shift and assurance	prediction or candidate generation in a bounded ODD

A plausible hybrid uses an FSM for system and fault modes, a BT for behavioral priorities, a POMDP or learned model to score merging options, and a verifiable safety envelope to reject unsafe output. A learned component proposes; a separately reviewed constraint layer authorizes. The exact boundary depends on the ODD and safety case.

5. Connect perception and prediction with distributions

“Car at $(x,y)$ ” is not a sufficient interface. Behavior planning needs measurement time, frame, track ID, dimensions, velocity, acceleration, existence probability, class probabilities, covariance, occlusion, and sensor health. Motion from Feature Tracking or Optical Flow is not automatically 3D object velocity.

Instead of committing to one future, prediction can retain modes

P(\tau_i|z_{1:t})=\sum_m w_mP(\tau_i|m,z_{1:t}),\qquad \sum_mw_m=1,

for straight, turn, lane change, or stop. The planner must consider low-probability but high-severity conflicts, not merely the most likely path. It should expand margin when covariance grows and slow when data become stale. A track-ID swap must not transfer one driver’s inferred intent to another vehicle.

6. Connect behavior, path, trajectory, and MPC bidirectionally

“Change to the left lane” does not prove that a collision-free, dynamically feasible trajectory exists. Path Planning searches geometric free space. Trajectory Generation adds time, velocity, and acceleration. Model Predictive Control optimizes continuous control under vehicle and actuator constraints.

A representative MPC objective is

J=\sum_{k=0}^{N-1}\left(\|x_k-x_k^{ref}\|_Q^2+\|u_k\|_R^2+\|\Delta u_k\|_S^2\right)+\|x_N-x_N^{ref}\|_P^2.

Behavior planning provides the target lane, speed envelope, stop line, forbidden space, comfort target, and completion deadline—not just $x^{ref}$ . If trajectory optimization reports infeasible, behavior planning must select another option rather than forcing a command. The complete loop is:

Perception and prediction update a probabilistic world model.
Behavior planning generates and prioritizes maneuver candidates.
Path and trajectory planning test geometric and dynamic feasibility.
MPC computes steering, propulsion, and braking while returning tracking limits.
Independent safety supervision checks deadlines, freshness, clearance, and deviations.

A 10 Hz behavior loop is not fresh if its object tracks are already 300 ms old. Carry acquisition timestamps and expiry through the entire pipeline. Time-mismatched transforms, responses from a previous request, and an uncancelled old trajectory are common integration failures.

7. What current ADAS and automated-driving examples actually show

Subaru describes EyeSight as driver-assistance technology centered on stereo-camera perception, supporting functions such as collision avoidance assistance and following. Public feature descriptions do not reveal whether an internal implementation uses an FSM, a BT, or learned decision components. The owner’s manual—not a product nickname—defines weather, visibility, speed, road, and driver obligations.

SAE Level 2 highway assistance may control steering and speed simultaneously while the driver continuously monitors the road. NHTSA explicitly distinguishes Level 2 ADAS, which assists an engaged human driver, from ADS at Levels 3–5. In a limited Level 3 function such as Mercedes-Benz DRIVE PILOT, the system performs the driving task while engaged and can request takeover at its boundary. Mercedes-Benz announced German approval up to 95 km/h under specified conditions for its 2025 update; that claim must not be generalized across roads, weather, vehicles, or jurisdictions.

A geographically limited driverless service such as Waymo combines behavior planning with redundant sensing, maps, fleet operations, remote assistance, ODD management, and a safety case. Waymo publishes crash-rate comparisons and downloadable data, but those results are not proof of universal performance. Readers must inspect the operating cities, road exposure, reporting criteria, benchmark construction, mileage, and confidence intervals.

8. Safety means constraining intelligence

Even a high-utility action must be rejected when it violates a safety constraint:

a^*=\arg\max_{a\in A_{safe}}U(a),\qquad A_{safe}=\{a\in A\mid C_j(a)\le0,\ \forall j\}.

If $A_{safe}$ is empty, the system must expose planning failure and enter a minimal-risk strategy. Constraints can cover collision margin, guaranteed stopping space, road departure, vehicle stability, signals, right of way, and actuator limits. Rulebooks help define priority when progress, courtesy, law, and safety cannot all be optimized as a single opaque scalar.

ISO 26262 addresses hazards caused by E/E malfunction. ISO 21448, SOTIF, addresses unreasonable risk from functional or specification insufficiency even without a component fault—for example, a perception limitation in fog or a misunderstood construction worker gesture. Cybersecurity is a further dimension: a forged map or misleading V2X input can drive a fault-free planner toward danger. As the V2X Primer explains, a valid signature authenticates origin and integrity, not the physical truth of a message.

An independent safety supervisor should avoid sharing every assumption and failure mode with the main planner. It can monitor time to collision, drivable space, speed, tracking error, computation deadline, and sensor health, then override normal behavior. Fallback and emergency paths require revalidation whenever the normal planner changes.

9. Evaluation needs more than a success rate

Safety: collisions, minimum TTC, time to react, stopping margin, severe evasions.
Rules and social interaction: signals, stop lines, priority, yielding, aggressiveness, and excessive caution.
Comfort: acceleration, lateral acceleration, yaw rate, and jerk $j=da/dt$ .
Progress: arrival rate, travel time, deadlock, unnecessary stops, and replans.
Robustness: degradation under rain, glare, occlusion, map change, latency, and sensor loss.
Traceability: reproducible selected action, rejected alternatives, input age, rule/model version, and safety margin.

Zero failures in a finite test does not imply a zero failure probability. Public-road miles give realistic exposure but sample rare hazardous combinations inefficiently. Combine simulation, log replay, parameter sweeps, closed courses, and staged public operation. Separate average performance from distribution tails and define the desired confidence before collecting evidence.

10. An experimental workflow

Define ODD and responsibility. Record road class, speed, weather, lighting, mapping, connectivity, and driver role. Allocate DDT, object-and-event detection and response, and fallback using the [SAE Automation Levels Primer] étoiles?
Define the behavior vocabulary. Give stop, follow, yield, merge, lane change, and pull over explicit entry, completion, abort, and timeout conditions.
Freeze interfaces. Version frames, timestamps, covariance, prediction modes, trajectory IDs, cancellation acknowledgements, and computation deadlines.
Inspect the decision structure. Find unreachable states, cycles, priority inversions, simultaneously true guards, and paths that cannot reach a minimal-risk condition.
Test invariants. Property-test claims such as “do not cross a reachable red stop line” and “do not accelerate from expired perception.”
Replay logs. Hold perception output constant and compare planner versions. Human driving is useful evidence but not the unique ground truth.
Sweep scenario boundaries. Vary relative speed, occlusion, friction, illumination, and latency on both sides of transition thresholds; use metamorphic tests to expose irrational flips.
Inject faults. Drop cameras, swap track IDs, jump GNSS, offset maps, forge V2X data, time out MPC, and limit actuators; verify degradation.
Stage deployment. Move from simulation to hardware-in-the-loop, closed course, safety-driver operation, and bounded service with explicit stop criteria.
Compare counterfactual and physical replay. When simulation estimates what would happen without an intervention, expose model error and reproduce representative cases on a closed course.
Trace changes. A perception-model update changes the behavior distribution. Link requirement, design, code, test, and safety claim, then choose regression scope from the actual dependency graph.

The final log should say more than “lane change selected.” Connect the timestamped objects, belief or prediction modes, candidate actions, costs, safety margins, rejection reasons, selected behavior, downstream feasibility, actual command, and tracking error under one trace ID. Otherwise a one-off field decision cannot be reconstructed.

A useful paradox: safe driving must sometimes make progress

A naive planner can avoid collision in a screenshot by waiting forever at an intersection. In traffic, excessive hesitation blocks others, violates expectations, and can provoke unsafe overtaking. The opposite error is treating another vehicle’s slight deceleration as a binding promise to yield.

Behavior planning is difficult because other road users predict and react to the ego vehicle. Edging forward changes their prediction; their response changes the ego belief. This feedback explains the research progression from if-statements to POMDPs, game theory, and learned policies. A production system must still translate that sophistication back into testable statements: why did it proceed, what observation would make it stop, and what margin remained?

Primary and related sources

#behavior planning #automated driving #ADAS #FSM #behavior tree #POMDP #path planning #MPC #ODD