SMART-Merge Planner: Safe Real-Time Merging

Updated 20 July 2025

The SMART-Merge Planner is a lattice-based motion planning approach that safely handles forced highway on-ramp merges by optimally selecting gaps and adapting to dynamic traffic conditions.
It integrates cost shaping, motion prediction, and dynamic speed heuristics to minimize merge times while ensuring comfort and collision avoidance.
Empirical studies show its real-time performance on single-core CPUs, achieving smoother and faster merges compared to baseline methods in complex traffic scenarios.

The Safe Merging and Real-Time Merge (SMART-Merge) Planner is a class of motion planning algorithms for autonomous vehicles, with a particular focus on the highway on-ramp merging problem. Highway merging represents a prototypical interaction-rich scenario, combining dynamic spatial constraints, unpredictable neighboring agents, stringent safety requirements, and the need for time-efficient, comfortable maneuvers. The SMART-Merge Planner, most recently formalized in a high-fidelity, real-time lattice-based implementation (Mohammadnejad et al., 15 Jul 2025), integrates advances in cost shaping, gap assessment, motion prediction, and safety logic to achieve reliable autonomous merging in complex, forced-merge scenarios.

1. Problem Setting and Objectives

The SMART-Merge Planner addresses forced highway on-ramp merging, characterized by the necessity for the ego vehicle to (i) identify a safe gap among nearby targets and followers, (ii) adjust its speed and position, and (iii) execute the merging maneuver within tight spatiotemporal boundaries so as to minimize merge time, maximize comfort, and guarantee collision avoidance (Mohammadnejad et al., 15 Jul 2025). This requires real-time, robust planning in scenarios with uncertain perception, variable traffic flow, and non-cooperative participants.

Core objectives of the planner include:

Safe gap selection, with explicit avoidance of collisions.
Minimization of merge duration and travel time before lane incorporation.
Comfort optimization, reducing high accelerations and jerks.
Adaptivity to real-world constraints, such as short ramps and dense mainline traffic.
Online feasibility, permitting deployment on resource-constrained automotive hardware.

2. Algorithmic Foundations and Architecture

The latest SMART-Merge Planner adopts a conformal spatiotemporal lattice-based motion planning approach (Mohammadnejad et al., 15 Jul 2025). The planner samples candidate trajectories in a spatial state space, typically x, y, θ, κ, with each lattice point representing a kinematically feasible vehicle state with respect to the current road geometry.

Candidate motions are constructed by connecting sampled states using curvature polynomial splines, which are kinematically consistent with the vehicle's bicycle dynamics:

$\frac{dx}{ds} = \cos\theta(s), \qquad \frac{dy}{ds} = \sin\theta(s), \qquad \frac{d\theta}{ds} = \kappa(s),$

with

$\kappa(s) = a_0 + a_1 s + a_2 s^2 + a_3 s^3.$

Each generated trajectory is then augmented with time, acceleration, and velocity profiles, resulting in a high-dimensional trajectory candidate set to be assessed for feasibility and cost.

A key innovation is the explicit adaptation of cost terms tailored to forced merging (Mohammadnejad et al., 15 Jul 2025). The planner employs:

Merge cost (C_center): Aggressively penalizes lateral deviation from the mainline lane during the merge, incentivizing early alignment and discouraging “late” merges that may be hazardous.
Dynamic obstacle collision cost (J_obs): Adapts according to whether the ego is in a lane-keeping or merging state, combining time-to-collision (TTC) and distance-based penalties to account for relative speeds, reaction/braking distances, and proximity to critical traffic agents.
Desired speed heuristic: Instead of defaulting to speed limits, the planner computes a safety-constrained speed profile by predicting safe longitudinal distances to the preceding merge-lane leader, the main-lane leader, and the main-lane follower, using simple physics-based filtering:

$\widetilde{d}_{\text{lead}} = d_{\text{lead}} - \frac{(v - v_{\text{lead}})^2}{2 a_{\text{max, dec}}}, \qquad d_{\text{safe}} = v_{\text{ego}} t_r + \frac{v_{\text{ego}}^2}{2 a_{\text{max, dec}}}$

The desired speed is selected at each state as the minimum among those allowed by safety (distance to collision, braking distance, safety factor), facilitating proactive deceleration or acceleration in anticipation of the evolving gap (Mohammadnejad et al., 15 Jul 2025).

3. Real-Time Safety Guarantees and Dynamic Adaptation

Explicit safety logic underpins the SMART-Merge Planner's operation. Each trajectory candidate is checked against dynamic constraints (maximum vehicle acceleration, deceleration), kinematic limits, and, crucially, temporal and spatial safe distances to surrounding vehicles. By:

Treating adverse TTC events (low relative gap, insufficient reaction time) as high-cost or infeasible,
Enforcing hard bounds on lateral/longitudinal excursions,
Incorporating rolling predictions of other agents’ positions via constant speed or, when available, more advanced models,

the planner strives to guarantee collision-free execution, even in transiently high-density traffic or during forced merges where the available window is brief.

The safety logic is modular and can be extended—for example, incorporating chance-constrained CBFs for stochastic safety (Lyu et al., 2021), interactive game-theoretic models for complex multi-agent anticipation (Liu et al., 2021), or RSS-based right-of-way assignment (Lin et al., 20 Mar 2024) for mixed autonomy settings.

4. Computational Efficiency and Hardware Feasibility

Performance is critically dependent on the ability to run at control loop frequencies typical of automotive ECUs. The SMART-Merge lattice-based planner is designed for real-time execution on a single-core CPU, achieving 10 Hz planning rates in an IPG CarMaker/ROS environment (Mohammadnejad et al., 15 Jul 2025). Analytical closed-form or quickly solved QP formulations are preferred for trajectory generation, and cost evaluation leverages precomputed or efficiently updatable lookup structures.

Reported simulation times for merge decision-making and full trajectory recomputation are in the millisecond range, well below the 100 ms upper bound typical for automotive real-time requirements (Mohammadnejad et al., 15 Jul 2025, Müller et al., 2019).

5. Empirical Performance and Comparative Results

Extensive simulation studies were conducted using realistic traffic models and high-fidelity road geometry representations (e.g., HOMER dataset scenarios and variable-density synthetic traffic) (Mohammadnejad et al., 15 Jul 2025). Key outcomes include:

Success Rate: 100% across 160 heterogeneous highway merging scenarios—meaning the ego vehicle was always able to complete the merge before the on-ramp ended, regardless of competing traffic behaviors.
Merge Time: The SMART-Merge Planner significantly reduced mean merge times (e.g., 13.24 s versus 14.71 s and 15.96 s for baselines), directly attributable to the merge cost and desired speed heuristic.
Comfort: Reduced maximum longitudinal/lateral accelerations and jerks, confirming that the merging behavior was consistently smooth and within comfort thresholds.
Comparative Advantage: Planners omitting either the desired speed heuristic (using the speed limit naively) or the merge cost term (treating merge lane and main lane equally) exhibited higher failure rates (missed merges) or longer merge durations.

Simulation result videos visually corroborate these quantitative metrics and are available online (Mohammadnejad et al., 15 Jul 2025).

6. Relationship to Broader Merging and Safe Planning Literature

The SMART-Merge Planner's underlying principles align with and expand upon several key lines of research:

Meta-planning and modular safety-web approaches (e.g., FaSTrack framework (Fridovich-Keil et al., 2017)), which allow dynamic switching among planners with explicit safety bounds.
Joint risk-comfort optimization, blending jerk minimization and probabilistic safety in closed-form trajectory synthesis (Müller et al., 2019).
Trajectory layer decoupling and post-optimization smoothing, as in the Dual-Layer AMP and Descent-Based TO (Jiang et al., 2021).
Interactive, real-time adaptation to observed or inferred behaviors of other vehicles, including POMDP belief updating and game-theoretic prediction (Kruse et al., 2022, Liu et al., 2021, Kollarčík et al., 10 Dec 2024).
Rule-compliance and responsibility-sensitive safety for right-of-way and legal adherence (Lin et al., 20 Mar 2024).

The consistent emphasis is on combining computational tractability (lattice/graph methods, quadratic programs) with robust, context-sensitive safety and comfort-aware decision making.

7. Implementation Considerations and Deployment Challenges

While the SMART-Merge Planner demonstrates strong practical feasibility, several limitations remain from the simulation-based evaluations (Mohammadnejad et al., 15 Jul 2025):

Prediction models for other vehicles are typically based on constant speed or basic car-following assumptions, which may misrepresent rare or adversarial behaviors. Incorporation of learning-based or intent-aware models may further enhance safety in genuinely unpredictable traffic.
The planner is implemented as a local motion layer; full deployment in a production autonomous system requires careful integration with upstream route planning, scene understanding, and downstream controller stack.
Systematic testing under rare edge cases, diverse weather/sensor limitations, and mixed (human-autonomous) traffic is necessary to achieve regulatory and safety-case acceptance.
While real-time performance is achieved on a single-core CPU, further optimization or parallelization may be necessary as scenario complexity or agent count increases.

8. Supplementary Resources

Simulation result videos and additional technical documentation are provided at https://sites.google.com/view/smart-merge-planner/home (Mohammadnejad et al., 15 Jul 2025). These resources substantiate the planner's performance across a range of highway merge scenes and support further benchmarking by the academic and industry community.

The SMART-Merge Planner provides a structured, performance-validated solution for autonomous highway on-ramp merging, distilling advances across classic trajectory planning, risk-aware cost optimization, and real-time safety logic. Through explicit cost shaping, dynamic desired speed estimation, and scalable lattice-based computation, it achieves reliable, comfortable forced merging and sets a reference point for future developments in mixed-traffic autonomous planning systems.