Highway Lane-Change Planner with Collision Checking

The problem

Automated lane changes on highways must decide when and how to move between lanes while other vehicles accelerate, brake and cut in. A planner that ignores prediction uncertainty or comfort limits either refuses useful manoeuvres or takes risky ones. This project builds a sampling-based planner with explicit collision checking and measures its safety and comfort.

Objectives

• Generate candidate lateral and longitudinal trajectories in a Frenet frame.
• Predict surrounding vehicle motion and check candidates for collision with safety margins.
• Define a cost function covering safety, comfort, progress and lane preference.
• Evaluate success rate, minimum time gap and jerk across a scenario suite.

What you need

Key concepts

Frenet frame

Coordinates along the road (s) and across it (d). Lane changes become a change in d over time, which makes candidate generation and lane logic simple on curved roads.

Quintic polynomial trajectories

Fifth-order polynomials in time that join start and end states with matched position, velocity and acceleration, giving smooth, low-jerk manoeuvres.

Capsule collision checking

Vehicles approximated as capsules (a line segment with a radius) so collision checks between trajectories reduce to fast distance tests.

Time gap

The gap to a following or leading vehicle expressed in seconds of travel; a common safety measure for lane changes.

Step-by-step guide

1. 1

   Define the ODD and requirements

   Scope the planner to dual or triple-lane highways with speeds from 60 to 120 km/h, dry conditions and lane markings present. Set requirements: no collision in the scenario suite, a minimum time gap of, say, 1 s to vehicles in the target lane at all times, lateral acceleration and jerk below comfort limits, and completion within a set time when a gap exists. Define what triggers a lane change: a slower lead vehicle, a lane ending or a route request.

   Checkpoint: A requirements table with safety, comfort and progress targets and lane-change triggers.

2. 2

   Build the scenario suite

   Use drivingScenario to create a three-lane highway with a gentle curve. Script traffic cases: a slow truck ahead with a clear left lane, a gap that closes as a fast car approaches from behind, a vehicle cutting in during the manoeuvre, a lead vehicle braking hard, dense traffic with no safe gap, and a lane drop. Give each actor a speed profile. Store ground-truth actor states for prediction and scoring.

   Checkpoint: At least eight scenarios that replay correctly with all actor states logged.

3. 3

   Set up the Frenet reference

   Create a referencePathFrenet from the centreline waypoints of the ego lane and compute lane centre offsets in d for each lane. Write helper functions that convert ego and actor Cartesian states to Frenet and back using frenet2global and global2frenet. Verify by converting actors along a curved section and back, checking the round-trip error is small. Also check that lane centre offsets stay constant along the curve, because errors here make the planner think adjacent-lane vehicles are drifting towards the ego lane.

   Checkpoint: Round-trip Frenet conversion error below a few centimetres on the curved section.

4. 4

   Generate candidate trajectories

   Configure trajectoryOptimalFrenet, or write your own sampler, with terminal states covering the current and adjacent lane centres, several manoeuvre durations (for example 3 to 7 s) and target speeds around the desired speed. Generate quintic lateral and quartic longitudinal profiles for each combination. Reject candidates that exceed lateral acceleration, curvature or jerk limits before collision checking. Log how many candidates each limit rejects; if almost all are rejected, widen the duration range rather than relaxing comfort limits.

   Checkpoint: A visualisation of all feasible candidates fanning across lanes from the ego position.

5. 5

   Predict other vehicles

   Start with constant-velocity prediction along each actor's lane in Frenet coordinates over the planning horizon. Add a constant-acceleration variant and inflate the predicted footprint with time to represent growing uncertainty. For cut-in cases, add a second hypothesis where the actor moves into the ego's target lane. Store predictions as time-indexed poses for collision checking. Keep prediction in its own function with a fixed interface so a learned predictor can replace it later without touching the planner.

   Checkpoint: Predicted actor paths drawn on the bird's-eye plot over the horizon.

6. 6

   Check collisions

   Represent the ego and each actor as capsules in a dynamicCapsuleList, or implement a capsule distance check yourself, and test each candidate against every prediction hypothesis at every time step. Add a safety margin that grows with relative speed. Mark candidates as colliding, safe or marginal, and compute the minimum time gap each safe candidate achieves. Measure the time spent in collision checking per cycle, since it usually dominates planning time as the number of candidates grows.

   Checkpoint: Colliding candidates are correctly discarded in the closing-gap scenario.

7. 7

   Design the cost and selection

   Score each safe candidate with a weighted sum of jerk, lateral acceleration, deviation from desired speed, time to complete, lane preference and a penalty that rises as minimum time gap approaches the limit. Select the lowest-cost candidate and replan every 0.1 to 0.2 s with the current state as the new start. Add hysteresis so the planner does not switch between keeping lane and changing lane each cycle.

   Checkpoint: The planner commits to a lane change in the slow-truck scenario and aborts when the gap closes.

8. 8

   Close the loop and evaluate

   Track the chosen trajectory with a simple controller or by moving the ego along it directly. Run all scenarios with several variations of actor speeds and random seeds. Record collisions, minimum time gap, peak lateral acceleration and jerk, manoeuvre duration and whether the change completed. Count aborted manoeuvres and check that each abort was justified by the scenario. Replay any failed run with the candidate visualisation turned on to see which check let it through.

   Checkpoint: A table with zero collisions or a clear explanation of each failure.

9. 9

   Sensitivity and report

   Vary cost weights, safety margin and prediction model, and plot safety against progress, for example minimum time gap against average speed. Discuss how constant-velocity prediction fails during cut-ins and how the second hypothesis helps. Write up the method, scenario suite, results and limitations, noting that all tests were in simulation. Include bird's-eye snapshots of the key moment in each scenario and a table of the final cost weights, horizon and margins so the study can be reproduced.

   Checkpoint: A report with the safety-progress trade-off and per-scenario results.

Testing & validation

• Collision count and minimum time gap per scenario.
• Peak lateral acceleration and jerk against comfort limits.
• Manoeuvre completion rate when a safe gap exists, and correct aborts when it does not.
• Sensitivity of results to prediction model and safety margin.

Expected results

A Frenet-based lane-change planner with collision checking against predicted traffic, a scenario suite that exercises it, and quantified safety, comfort and progress results.

Take it further

• Replace constant-velocity prediction with a learned predictor.
• Model the planner's interaction with a cooperative or aggressive driver in the target lane.
• Describe the scenarios in ASAM OpenSCENARIO and run them in another simulator.

Viva and review questions

Learn it at DIYguru

These DIYguru courses and programmes teach what this project needs.