Planning and Control in Autonomous Vehicles

The Planning and Control architecture in autonomous vehicles integrates multiple modules
to ensure safe and reliable operation.

Architecture: Planning and Control in a Broader Sense

• The Map and Localization module processes raw sensor data (e.g., point cloud,
GPS) to determine the vehicle's position.
• The Perception module detects surrounding objects.
• Other modules, including routing, traffic prediction, behavioral decision, motion
planning, and feedback control, contribute to predicting environmental behavior
and determining the vehicle’s movement.

All modules operate within a synchronized clock cycle, where they fetch, compute, and
publish data for downstream consumption. Collaboration between perception and
planning/control is critical, ensuring minimal module scope and problem complexity.

The divide-and-conquer approach structures functional modules according to data flow,

categorizing prediction and routing under planning and control as dependency modules.
Traditional decision-making, planning, and control aspects are discussed in the next chapter.

While conventional techniques dominate current systems, AI-driven end-to-end solutions

are becoming increasingly viable. These solutions, covered in a later chapter, aim to
seamlessly process sensor inputs and map data to execute vehicle maneuvers.
Scope of Each Module in Autonomous Vehicle Planning and Control

The planning and control system in autonomous vehicles consists of multiple

interdependent modules, each solving a specific sub-problem to improve efficiency and
modularity.

Key Modules and Their Functions

1. Routing Module
o Issues top-level navigation commands.
o More complex than traditional map services, relying on HD maps tailored for
autonomous driving.
2. Traffic Prediction Module
o Processes perceived objects (e.g., vehicles, pedestrians) and predicts their
future trajectories.
o Initially implemented as a software library but now a dedicated module for
better real-time performance.
o Inputs are used by the behavioral decision module.
3. Behavioral Decision Module
o Acts as a "co-pilot," determining how the vehicle should respond based on
routing and traffic prediction.
o Generates commands such as lane-following, stopping, or yielding.
o Converts individual behavioral decisions into constraints for motion planning.
4. Motion Planning Module
o Computes an optimized trajectory for the vehicle based on constraints from
the behavioral decision module and goals from the routing module.
o Ensures smooth and executable trajectories while maintaining consistency
across planning cycles.
o Uses redundant mapping and perception data as a backup for failed predictions
or newly detected obstacles.
5. Feedback Control Module
o Converts planned trajectories into drive-by-wire signals for vehicle control
(braking, steering, acceleration).
o Ensures the actual path conforms to the planned trajectory while considering
physical constraints.

Collaboration and Design Philosophy

• Modularization simplifies software development by dividing complex problems into

smaller, manageable tasks.
• The system follows a data stream hierarchy, where downstream modules rely on
upstream computations.
• Conflicts are resolved at the upstream level whenever possible, ensuring consistency
across modules.

This structured approach provides a comprehensive, coherent solution to autonomous

vehicle planning and control, balancing modular design with real-world adaptability.
Traffic Prediction

Objective

Traffic prediction aims to forecast the behavior of detected objects (e.g., vehicles,
pedestrians) in the near future by computing spatial-temporal trajectory points. The
prediction process considers object attributes such as position, velocity, heading, and
acceleration, along with behavioral and environmental factors.

Problem Formulation

Traffic prediction is divided into two sub-problems:

1. Classification Problem: Determines categorical behaviors, such as lane changes or

pedestrian crossing.
2. Regression Problem: Predicts precise trajectory paths, including speed and timing.

Behavior Prediction as Classification

• Vehicle behavior is more predictable than pedestrian or cyclist behavior.

• A classification model predicts whether a vehicle will follow a particular lane
sequence.
• Instead of defining categories based on specific intersections, behavior is classified
based on lane sequences.
• Lane sequence classification enables scalable and structured learning.

Feature Design for Vehicle Behavior Prediction

Three key categories of features:

1. Vehicle History Features: Includes absolute and relative positions of a vehicle over
a historical window.
2. Lane Sequence Features: Captures lane properties such as curvature, heading, and
boundary distance.
3. Surrounding Object Features: Includes distances between a vehicle and adjacent
lane vehicles.

Model Selection

1. Memory-less Models (SVM, DNN): Require historical data encoding; suitable for
simpler environments.
2. Memory Models (LSTM, RNN): Retain past information; better for complex traffic
scenarios.

Performance Metrics

• Precision: Measures how many predicted trajectories match actual vehicle

movements.
• Recall: Measures how many actual behaviors were successfully predicted.
Vehicle Trajectory Generation in Traffic Prediction Systems

The section discusses vehicle trajectory generation within traffic prediction systems. After
determining a vehicle’s behavior, the system must generate a spatial-temporal trajectory
following the predicted lane sequence. A Kalman-Filter is used to track the vehicle’s position
along the lane’s centerline in (s, l) coordinates, where s represents longitudinal distance and l
represents lateral deviation. The motion transformation matrix governs trajectory prediction,
with the speed of approximation to the centerline controlled by the parameter βt.

Machine learning-based approaches, such as regression models, could also be used for
trajectory generation by leveraging historical trajectory data. However, the authors argue that
behavior prediction is more critical than trajectory modeling.

The prediction problem is divided into two phases:

1. Behavior Prediction – Modeled as a binary classification problem for each lane

sequence.
2. Trajectory Generation – Uses motion planning techniques or rule-based Kalman
filtering.

Interactions among vehicles are not explicitly modeled but may be implicitly considered with
high-frequency predictions. The extracted features for prediction include:

• Vehicle History Features: Position, speed, heading, acceleration, lane boundary

distance, and vehicle dimensions.
• Lane Features: Lane position, curvature, heading, boundary distances, and lane type.
• Surrounding Obstacle Features: Positions, speeds, and headings of the closest front
and rear vehicles.

These features help classify whether a vehicle will follow a specific lane sequence,
improving traffic prediction accuracy.
Lane-Level Routing Summary

The Lane-Level Routing module determines the optimal lane sequence for an autonomous
vehicle to follow from a source to a destination. Unlike traditional navigational maps like
Google Maps, which focus on roads, this module operates at the lane level using HD maps.
These lanes may not align with natural lane divisions and include virtual turning lanes or
segmented lanes.

Key Concepts:

1. Routing Segments:
o Defined as (lanei, start_positioni, end_positioni), where the start and end
positions are measured along the lane’s central reference line.
o The routing module aims to compute an optimal sequence of these segments.
2. Weighted Directed Graph for Routing:
o The routing problem is transformed into a shortest path search on a
weighted directed graph.
o Lane points (sampled positions on lanes) form the graph’s nodes.
o Directed edges connect lane points based on valid transitions (e.g., forward
movement, lane switches, and turns).
o Edge weights represent motion difficulty, penalizing lane switches and
complex maneuvers.
3. Cost Assignment for Lane Transitions (Figure 5.6):
o Moving forward within the same lane: Cost = 1
o Moving within a right-turn lane: Cost = 2, left-turn lane: Cost = 3
o Right turn transition: Cost = 5, left turn: Cost = 8
o Parallel lane switch: Cost = 10
4. Optimal Route Selection (Figure 5.7):
o Two routes are compared between point A and point B.
o Route 1 follows a straightforward path with a right turn.
o Route 2 involves multiple lane switches and a left turn.
o Even though Route 2 is shorter in distance, Route 1 is preferred due to lower
total cost (23 vs. 45).

Conclusion:

The Lane-Level Routing module ensures safe and efficient autonomous navigation by
computing lane-based paths that align with motion planning constraints. It minimizes
difficult maneuvers while maintaining an optimal route using a cost-based graph approach.
Typical Routing Algorithms for Autonomous Vehicles

The Routing problem in autonomous driving can be solved using shortest path algorithms.
Two commonly used algorithms are Dijkstra’s Algorithm and the A Algorithm*

1. Dijkstra’s Algorithm for Autonomous Vehicle Routing

Dijkstra’s algorithm finds the shortest path between a source node and a destination node
on a weighted graph. It is applied to lane-point-based routing as follows:

Steps in Dijkstra’s Algorithm:

1. Graph Construction:
o Extract lane graph data from the HD map within a given radius.
o Define lane points as graph nodes.
o Set the source lane point (closest to the vehicle’s location) and destination lane
point.
o Initialize all nodes' costs to infinity, except the source node (cost = 0).
2. Processing Nodes:
o Set the current node to the source.
o Maintain an unvisited set of lane points and a prev_map to track
predecessors.
3. Updating Distances:
o For each adjacent lane point, compute a tentative distance based on edge
weights.
o If the tentative distance is smaller than the current distance, update it.
4. Iteration:
o Mark the current lane point as visited and remove it from the unvisited set.
o Continue until the destination is reached or no valid path exists.
5. Path Reconstruction:
o If a path is found, reconstruct it using the prev_map and return a sequence of
lane segments {(lane, start_position, end_position)}.

Complexity Analysis:

• Using a priority queue, the time complexity is O(|E| + V log V), where V = number
of lane points and E = number of edges.

2. A Algorithm for Autonomous Vehicle Routing*

The A algorithm* is an optimized search method that extends Dijkstra’s algorithm by
incorporating a heuristic function (h(v)). It balances known path costs (g(v)) with an
estimated cost (h(v)) to reach the destination.

Formula for Node Cost:

f(v) = g(v) + h(v)

• g(v): Cost from the source to node v.

• h(v): Estimated cost from node v to the destination.

Steps in A Algorithm:*

1. Initialize:
o Define openSet (nodes to explore).
o Set source lane point cost = 0 and all others to infinity.
2. Node Expansion:
o Extract the node with the lowest f(v) value (best estimated path).
o Update distances for adjacent lane points and adjust their g(v) and h(v) values.
3. Heuristic Function (h(v)):
o In autonomous vehicle routing, h(v) can be defined as the Mercator distance
between two lane points.
o If h(v) is admissible (never overestimates the true cost), A* guarantees the
shortest path.
4. Path Reconstruction:
o Similar to Dijkstra’s Algorithm, the shortest path is reconstructed and
returned as a sequence of lane segments.

Comparison with Dijkstra’s Algorithm:

• A is more efficient* because it uses heuristics to guide the search.

• If h(v) = 0, A* reduces to Dijkstra’s Algorithm (i.e., pure shortest path search).

Conclusion:

• Dijkstra’s Algorithm guarantees the shortest path but explores all possible paths.
• A Algorithm* optimizes the search by using heuristics, making it more efficient when
a good heuristic is available.
• Both algorithms are crucial for autonomous vehicle lane-level routing, ensuring safe
and optimal navigation in complex road networks.

Routing Graph Cost: Weak vs. Strong Routing

In autonomous vehicle routing, the configuration of costs between lane points is more
critical than the choice of algorithm. Costs can be dynamically adjusted based on traffic
conditions, road closures, or lane preferences.

Dynamic Cost Adjustments:

• High cost for congested or restricted roads to avoid them.

• Infinite cost for blocked roads to prevent routing through them.
• Ad-hoc graph construction: If a route is unavailable within a small radius, the
system can reload a larger radius and recompute the route.

Types of Routing Requests:

1. Passenger-Initiated Routing: When the vehicle starts its journey, a route is

computed from the source to the destination.
2. System-Initiated Routing: Modules like Behavior Decision or Motion Planning
may request re-routing based on real-time driving conditions.

Strong vs. Weak Routing

1. Strong Routing

• The vehicle strictly follows the computed route lane-by-lane.

• If deviations are required (e.g., due to obstacles), a re-routing request is triggered.

Example: If a slow-moving vehicle is ahead, the autonomous car will reduce speed and
follow it instead of switching lanes.

2. Weak Routing

• The vehicle can deviate from the planned route when necessary.
• Allows lane changes to overtake slow vehicles, similar to human driving behavior.

Example: Instead of slowing down behind a slow vehicle, the car may switch lanes,
overtake, and merge back.

Safety Considerations

Regardless of strong or weak routing, in emergency situations, the vehicle prioritizes safety
first and may request urgent re-routing to handle critical scenarios.

Behavioral Decisions in Autonomous Vehicles

The behavior decision module acts as the "co-driver," processing multiple data sources,
including:

1. Routing output – Lane sequences and positions.

2. Vehicle attributes – Location, speed, heading, and target lane.
3. Historical data – Previous decisions (follow, stop, turn, switch lanes).
4. Obstacle information – Objects’ locations, speeds, and predicted paths.
5. Traffic & map data – Lane relationships, traffic signals, and legal lane switches.
6. Local traffic rules – Speed limits, right-turn laws, etc.

Given the complexity of integrating these data sources, rule-based systems are widely used
for behavioral decision-making in autonomous vehicles. Many successful systems, like
Stanford's "Junior" and CMU's "Boss," use Finite-State Machines (FSMs) with cost
functions. However, Bayesian models, including Markov Decision Processes (MDP) and
Partially Observable MDPs (POMDPs), are increasingly applied in academic research.

Markov Decision Process (MDP) Approach

An MDP is defined by:

1. State Space (S): Vehicle’s position, lanes, and road environment.

2. Action Space (A): Possible decisions (e.g., Follow, Switch Lane, Turn, Yield, Stop).
3. Transition Probability (Pa): Probability of moving from one state to another given
an action.
4. Reward Function (Ra): Evaluates actions based on safety, comfort, and goal
achievement.
5. Discount Factor (γ): Prioritizes short-term rewards over long-term ones.

The goal is to find an optimal policy (π) that maps states to actions to maximize the
accumulated reward using Dynamic Programming or Bellman’s Value Iteration.

Key Considerations for a Good Reward Function:

1. Reaching the destination – Incentivize actions that follow the routing plan.
2. Safety & collision avoidance – Penalize high-risk moves; reward safer paths.
3. Comfort & smoothness – Prefer gradual speed changes over abrupt maneuvers.

Balancing these factors is crucial to designing an effective MDP-based decision system for
autonomous driving.

Scenario-Based Divide and Conquer Approach in Behavioral Decision-

Making for Autonomous Vehicles

The Scenario-Based Divide and Conquer approach decomposes the vehicle's surroundings
into multiple independent scenarios, each with its own rules to determine Individual
Decisions for objects. These individual decisions are then consolidated to form a Synthetic
Decision that dictates the vehicle’s behavior.

Key Concepts:

1. Synthetic Decision: The final, top-level decision for the autonomous vehicle,
considering all road objects and scenarios.
o Examples: Follow, Turn, Change Lane, Stop.
o Parameters include lane position, speed limits, and distances.
2. Individual Decision: Decisions assigned to individual objects based on scenario-
specific rules.
o Examples: Follow, Stop, Overtake, Yield, Attention.
o Parameters include the object’s ID, distances, and time gaps.
3. Scenario Construction: The surrounding world is divided into layered scenarios,
where:
o Objects typically belong to one scenario.
o Lower layers provide information to upper layers.
o Routing intentions and previous computations are shared.
4. Decision Merging & Conflict Resolution:
o Some objects may appear in multiple scenarios with conflicting decisions.
o A merging layer ensures safety and coherence.
o The final synthetic decision is then computed.
5. System Framework (Figure 6.4):
o Layer 0: Constructs scenarios (e.g., Master Vehicle, Lanes, Traffic Lights).
o Layer 1: Computes individual decisions within each scenario.
o Layer 2: Merges decisions for consistency.
o Final Step: Generates the synthetic decision, which is passed to the motion
planning module.

By using this structured approach, autonomous vehicles make logical and safety-compliant
driving decisions while optimizing motion planning.

Motion Planning in Autonomous Vehicles

Overview
The motion planning module is responsible for generating a trajectory for an autonomous
vehicle and sending it to the feedback control system for execution. It defines a spatial-
temporal trajectory as a sequence of points containing attributes like position, time, speed,
and curvature.

Motion Planning as an Optimization Problem

Motion planning in autonomous vehicles is simpler than general robotics due to predefined
road networks and constraints on vehicle movement (throttle, brake, steering). The
optimization problem consists of:

1. Optimization Object: Finding the trajectory with minimal cost while ensuring
collision avoidance and adherence to road shape.
2. Constraints: Ensuring smooth movement within feasible curvature and acceleration
limits.

Approaches to Motion Planning

Two major approaches exist:

1. Path and Speed Planning (Divide and Conquer):

o Path planning determines a trajectory shape on the 2D plane.
o Speed planning determines how the vehicle should traverse the path.
o Works well for urban low-speed driving.
2. SL-Coordinate System-Based Planning:
o Considers longitudinal (s-direction) and lateral (l-direction) movements
separately.
o Naturally integrates speed into trajectory shape.
o More suitable for high-speed highway driving.

Vehicle and Road Modeling

A vehicle’s pose is represented as:

xˉ=(x,y,θ,κ,v)\bar{x} = (x, y, θ, κ, v)

where:

• (x,y)(x, y) is the position,

• θθ is the direction,
• κκ is the curvature,
• vv is the speed.

The SL-coordinate system defines the road as a set of longitudinal (s) and lateral (l)
positions using a high-definition map (HD-map) and lane reference lines.

Key Equations

The pose variables satisfy:

x˙=vcos⁡θ,y˙=vsin⁡θ,θ˙=vκ\dot{x} = v \cosθ, \quad \dot{y} = v \sinθ, \quad \dot{θ} = vκ

In the SL-coordinate system, the pose transformation follows:

xr(s,l)=rx(s)+lcos⁡(rθ(s)+π/2)x_r(s, l) = r_x(s) + l \cos(r_θ(s) + π/2)

yr(s,l)=ry(s)+lsin⁡(rθ(s)+π/2)y_r(s, l) = r_y(s) + l \sin(r_θ(s) + π/2)

Conclusion

Motion planning involves trajectory optimization while considering upstream decisions (e.g.,
collision avoidance) and downstream execution constraints. While path-speed separation
works well in urban areas, SL-based planning is more effective for high-speed scenarios.

Motion Planning with Path and Speed Planning

Motion planning for autonomous vehicles is achieved through path planning and speed
planning in a road-based SL-coordinate system.
1. Path Planning

• Defines a vehicle’s path as a continuous mapping from the range [0,1][0,1] to a set of
vehicle poses.
• The goal is to find an optimal path from an initial pose to a desired end pose while
minimizing cost and satisfying constraints.
• Uses piece-wise polynomial splines (cubic or quintic spirals) to connect sampled
trajectory points.
• Candidate paths are generated by segmenting the lane into grids, with 256 possible
paths (in a 4×4 grid).
• Dynamic Programming is applied to find the minimum cost path by treating the
problem as a search on a directed weighted graph.
• Cost Function Considerations:
o Road Alignment: Paths close to the lane’s central reference line have lower
costs.
o Obstacle Avoidance: Collision-prone grid points have high costs.
o Smoothness: Ensures minimal curvature changes for driving comfort and
control feasibility.

2. Speed Planning with ST-Graph

• Determines the speed at which a vehicle moves along the planned path while
respecting physical and safety constraints.
• Uses an ST-Graph (Speed-Time Graph), where:
o S-axis represents the longitudinal distance traveled.
o T-axis represents time.
• Obstacles (e.g., other vehicles) are mapped onto the ST-Graph as time-dependent
occupied regions.
• Three speed planning strategies:
1. Speed Plan 1: Yielding—staying behind all vehicles.
2. Speed Plan 2: Overtaking one vehicle while yielding to another.
3. Speed Plan 3: Overtaking all vehicles.
• Behavioral decision-making influences cost assignment to different regions of the
ST-Graph, guiding the search algorithm to favor safer, feasible paths.
• Cost Considerations for Speed Planning:
o Collision avoidance: High costs assigned to risky grid areas.
o Acceleration constraints: High costs for rapid acceleration changes to ensure
control stability.

This two-step approach—path planning followed by speed planning—enables safe, efficient,

and optimal motion planning for autonomous vehicles.

Motion Planning with Longitudinal and Lateral Planning

Instead of separating path planning and speed planning, motion planning can be approached
using longitudinal and lateral planning within the SL-coordinate system.
1. Lateral Planning

• Focuses on movement in the l-dimension (sideways movement).

• Uses a quintic polynomial to ensure smooth motion by minimizing jerk.
• Start state P0=[d0,d˙0,d¨0]P_0 = [d_0, \dot{d}_0, \ddot{d}_0] is determined by the
previous frame’s end state.
• Candidate lateral end states d1d_1 are evaluated with zero velocity and acceleration,
ensuring the vehicle moves parallel to the lane.
• Cost Function Considerations:
o Penalizes slow convergence.
o Prefers minimal lateral offset from the center of the lane.
o Ensures consistency with behavioral decisions and collision-free paths.
• Low-Speed Adjustments:
o At low speeds, lateral planning must depend on longitudinal motion due to
vehicle constraints.
o The trajectory is then optimized as a function of longitudinal position ss
instead of time tt.

2. Longitudinal Planning

• Focuses on movement in the s-dimension (forward motion).

• The target trajectory starget(t)s_{\text{target}}(t) depends on the driving goal:
o Following a vehicle: Maintains a safe distance and time gap from the lead
vehicle.
o Lane changing (yielding/overtaking): Adjusts trajectory to avoid obstacles
or overtake another vehicle.
o Stopping: Brings the vehicle to a halt at a fixed position with zero speed and
acceleration.
• Uses a quartic or quintic polynomial depending on whether speed or position is the
priority.

3. Selecting the Optimal Trajectory

• Combines longitudinal and lateral candidate trajectories into all possible motion
plans.
• Filters out infeasible trajectories based on:
o Behavioral decision constraints.
o Collision risks.
o Control feasibility.
• The final trajectory is chosen based on minimum weighted cost across both
dimensions.

This method provides smooth, efficient motion planning while maintaining flexibility for
different driving scenarios.

Feedback Control in Autonomous Driving

6.3 Feedback Control

Feedback control in autonomous driving is similar to general mechanical control, where the
system continuously tracks the difference between the actual and desired trajectory. Existing
research has incorporated obstacle avoidance and route optimization into feedback control.
Since traditional vehicle pose feedback control is well-established, this section focuses on
two key concepts: the Bicycle Model and PID Feedback Control.

6.3.1 Bicycle Model

The Bicycle Model is a simplified vehicle model used in feedback control. It assumes:

• The vehicle is a rigid body with a front and rear axle.

• The front wheels can rotate, while the rear wheels remain parallel to the body.
• Vehicles cannot move laterally without forward movement (non-holonomic
constraint).
• Inertial and tire slip effects are neglected at low speeds but become significant at high
speeds.

The model represents the vehicle’s pose using:

• Position: (x,y)(x, y)
• Heading angle: θ\theta
• Steering wheel angle: δ\delta

Equations describe the relationship between the front and rear wheel velocities, constrained
by the vehicle’s geometry. A Unicycle Model is often used as a further simplification,
reducing control to steering angle change rate ω\omega and throttle/brake percentage.

6.3.2 PID Control

PID (Proportional-Integral-Derivative) Control is widely used for trajectory tracking. The

error function e(t)e(t) represents the difference between the actual and desired vehicle pose.
The PID system consists of:

• P (Proportional) Control: Direct response to error (KpK_p)

• I (Integral) Control: Corrects accumulated past errors (KIK_I)
• D (Derivative) Control: Predicts future errors (KDK_D)

Two PID controllers manage:

1. Steering Wheel Angle (δ\delta) – Adjusts based on heading error (θe\theta_e) and
lateral error (lel_e).
2. Forward Speed (VsV_s) – Adjusts throttle/brake based on speed error.

These controllers help autonomous vehicles closely follow a planned trajectory. More
advanced control systems are needed for smoother motion and better passenger experience.