Skip to content

About

Real time trajectory planning and control stack for an autonomous vehicle in an arbitrary environment. A Hybrid A* planner with cost based behaviour shaping plans the trajectory overtaking slower vehicles safely within a given time budget.

Resources

Stars

0 stars

Watchers

0 watching

Forks

Latest commit

 

History

21 Commits

Folders and files

Repository files navigation

Autonomous Overtaking — Local Trajectory Planning Stack

A real-time motion planning and control stack for an autonomous vehicle that drives on a two-lane rural road, recognizes a slower vehicle ahead, overtakes it through the oncoming lane when safe, and then returns to its original lane and settles at the lane center — all within a strict per-tick compute budget.

The ego vehicle is modeled after the Renault Twizy (2.34 m long, 1.69 m wheelbase), cruising at up to ~60 km/h on a road with a 60 km/h speed limit while the blocked lane traffic moves substantially slower.


System Architecture

System Architecture

The system runs as a closed loop:

  1. Perception (simulated) provides the current world state: ego pose, lane geometry, and surrounding vehicles.
  2. Prediction extrapolates each surrounding vehicle's future motion over the planning horizon (constant-velocity model).
  3. Planner searches for the best feasible trajectory over the next few seconds.
  4. Controller tracks the planned trajectory and produces steering-rate and acceleration commands.
  5. The loop repeats every simulation tick, re-anchoring the goal ahead of the vehicle — there is no fixed destination; the planner continuously "drives toward the horizon".

Two runtime modes are supported: a deterministic sequential mode (plan → execute → replan) and a threaded mode where the controller keeps tracking in real time while the planner runs concurrently in its own process.


How the Planner Works

The core planner is a Hybrid A* search over discretized vehicle poses:

  • Motion primitives generated by a kinematic bicycle model expand the search tree with dynamically feasible steering arcs rather than grid moves, so every candidate path is drivable by construction.
  • A Dubins-based heuristic guides the search toward the goal region.
  • Candidate trajectories are validated against predicted occupancy of other vehicles using oriented bounding boxes with safety margins.
  • The search must return a trajectory within a hard time budget (~2 s) for a 6 s planning horizon, which makes the cost function — not exhaustive search — the main tool for shaping behavior.

Because the planner explores a combinatorial space under time pressure, what the vehicle does is largely decided by what the cost function penalizes. The next section describes how the costs produce the three key behaviors: overtake, return, and stay centered.

The Hybrid A* search core, motion primitives, and base cost terms were originally developed by Daniel as part of a university group project; the overtaking and return-to-lane behavior shaping, performance work, and surrounding architecture are extensions built on top of that foundation.


Shaping Behavior with Costs

Every candidate trajectory accumulates cost along its path. Four families of terms work together to make the vehicle overtake, come back, and then hold the lane center.

1. Overtaking the slow vehicle

While a slower vehicle blocks the lane ahead, the planner must prefer leaving the original lane over trailing behind:

  • An object force field surrounds predicted vehicle footprints; cost grows sharply as the ego's inflated bounding box approaches another vehicle, effectively pushing the search around blockers instead of into them.
  • The target-speed cost penalizes driving below the desired cruise speed, so a trajectory that stays stuck behind a 30 km/h lead vehicle accumulates speed-deficit cost every second.
  • The base opposite-lane penalty is intentionally mild (0.5× multiplier): while a pass is in progress, occupying the oncoming lane is acceptable as long as it is collision-free against predicted oncoming traffic.

Together these make "pull out and pass" the cheapest survivable option once the lead vehicle is slow enough.

2. Returning to the original lane

A pure penalty design fails here: if staying left is only mildly expensive, the vehicle tends to linger beside or ahead of the passed car instead of coming back. The stack therefore adds a return-mode gate on top of the static costs:

  • Before each plan, the gate scans a forward corridor (~150 m along the travel direction, ±5.5 m wide). If no slower blocker is present ahead, the opposite-lane penalty is scaled up ×5.
  • The result: during an active pass (blocker present) the oncoming lane is cheap; the moment the pass completes (blocker clear) it becomes expensive, and the cheapest trajectory bends back into the original lane.
  • A rear hysteresis margin (6 m) keeps recently passed vehicles classified as blockers until they are safely behind, preventing the penalty from flipping on mid-pass and cutting the maneuver short.
  • The gate evaluates the current scene only — flickering predictions cannot toggle the mode halfway through a pass.

3. Staying centered and comfortable

After re-entry, fine-grained terms take over:

  • Lane-center cost (the heaviest single transition weight) grows with lateral offset from the current lane's centerline, pulling the vehicle back to y-center and keeping it there instead of drifting near lane edges.
  • Lane-yaw cost aligns heading with the lane direction, eliminating small oscillations around the center.
  • Comfort costs on longitudinal/lateral acceleration and jerk keep the executed maneuvers smooth — the overtake is a flowing S-curve, not a swerve.
  • A lane-exit cost steeply penalizes any trajectory that leaves the drivable road surface entirely.

The balance is deliberate: strong enough center-pull that the vehicle converges to ~±0.5 m of the lane center after returning, weak enough that it never fights the overtake while a blocker is present.


Scenarios

Scenario Traffic Purpose
overtake_scenario 2 vehicles Baseline: single slow lead + far oncomer; the canonical overtake-return-center run
dense_overtake 7 vehicles Stress test: platoon of slow vehicles in the ego lane plus staggered oncoming traffic
simple_scenario 1 vehicle Minimal traffic for controlled experiments
no_objects_scenario none Free-road sanity check

In the baseline scenario the ego maintains full target speed throughout, passes the slow lead vehicle with a comfortable clearance, re-enters its original lane, and converges to the lane center without collisions across repeated runs.


Future Work

  • Alternative planners — implement RRT/RRT* behind the same planner interface and benchmark solution quality vs. Hybrid A* under identical cost functions and time budgets.
  • Richer scenarios — multi-lane highways, stop-and-go traffic, cut-in maneuvers, partially occluded oncoming vehicles, and varying speed limits.
  • Controller upgrades — extend the MPC with obstacle-aware constraints and richer vehicle dynamics; evaluate a PID baseline on the same trajectories to quantify what predictive control buys.
  • Prediction improvements — replace constant-velocity prediction with learned or interaction-aware models and measure planner robustness to prediction error.

Acknowledgments

This project originated as a university group project.

  • Daniel built the original Hybrid A* planner core — search algorithm, motion primitives, Dubins heuristic, and base cost terms. The overtaking / return-to-lane behavior shaping, performance work, and surrounding architecture are extensions built on top of that foundation.
  • Diana implemented the original visualization module; all changes to it since are my own.

Tech Stack

Python · Hydra/OmegaConf configuration · NumPy/SciPy · Shapely (geometry/collision) · CasADi (MPC optimization) · Matplotlib (visualization)

About

Real time trajectory planning and control stack for an autonomous vehicle in an arbitrary environment. A Hybrid A* planner with cost based behaviour shaping plans the trajectory overtaking slower vehicles safely within a given time budget.

Resources

Stars

0 stars

Watchers

0 watching

Forks

Contributors

Languages