Trapezoidal Prism Corridor in Motion Planning

• Trapezoidal Prism Corridor is a geometric construct that defines safe, bounded zones for autonomous motion planning in complex urban and highway environments.
• It integrates into modular pipelines like FRENETIX, enabling independent module design for preprocessing, trajectory sampling, and feasibility checks.
• Empirical evidence shows that such corridors improve real-time trajectory selection, optimize multi-objective cost functions, and enhance safety in dynamic scenarios.

FRENETIX is a modular, high-performance motion planning framework developed by the Technical University of Munich (TUM) for autonomous vehicle trajectory generation in complex urban and highway environments. The FRENETIX framework is architected around a sampling-based trajectory planning algorithm optimized for integration, extensibility, and computational efficiency. The system is designed to enable real-time multi-objective motion planning, explicitly supporting the separation of planning modules, fast feasibility checks, and compatibility with occlusion-aware safety assessment modules such as FRENETIX-Occlusion. The implementation is open source and accessible at https://github.com/TUM-AVS/Frenetix-Motion-Planner (Trauth et al., 2024).

1. Modular Pipeline Architecture

FRENETIX employs a strictly modular pipeline architecture, with each stage designed to be independently swappable or augmentable. The main stages are:

• Preprocessing: Ingests structured driving scenarios (notably CommonRoad format), constructs lanelet-based semantic maps, identifies static and dynamic obstacles, and computes a global route and reference path Γ via graph-based search (e.g., Dijkstra or A*). Optional modules can handle explicit vehicle motion prediction, risk evaluation, occlusion handling, and high-level behavior planning.
• Motion-Planning Cycle: Operates at each control timestep and is composed of the following:
  1. Vehicle-state update from perception or simulation sources.
  2. Trajectory sampling in the Frenet frame, decoupling lateral (d) and longitudinal (s) motion relative to Γ.
  3. Feasibility checking under vehicle kinematic and dynamic constraints.
  4. Cost evaluation aggregating comfort, safety, and path precision objectives.
  5. Multi-objective optimization using weighted-sum cost aggregation, followed by sorting of trajectories.
  6. Continuous collision and road-boundary check via oriented bounding-boxes and lanelet boundaries.
  7. Output selection of the lowest-cost feasible trajectory, a risk-minimizing fallback, or an emergency plan if none are feasible.

Table 1 summarizes principal modular components:

Stage	Function	Swappable Modules
Preprocessing	Map, route, and scenario ingest	Lanelet extractors, predictors
Trajectory Sampling	Sample trajectories in Frenet	RRT*, polynomial samplers
Feasibility/Cost Checking	Kinematics, costs, collision	Cost functions, coll. checkers
Output	Trajectory selection	Controller interface, fallback

The modularity supports research and deployment flexibility; e.g., the quintic-polynomial sampler can be replaced with a sampling-based planner (RRT*), cost terms and their weights ($\omega_i$) can be tuned or learned, and independent collision checkers or risk modules can be inserted (Trauth et al., 2024).

2. Trajectory Generation and Mathematical Formalism

FRENETIX utilizes a Frenet-frame parameterization, with each candidate trajectory defined as $(s(t), d(t))$, where $s$ is the longitudinal arc length along the reference path $\Gamma$, and $d$ is the lateral offset.

• Trajectory Sampling: Final states are sampled over discrete sets: time horizons $\tau \in \{t_1, ..., t_{max}\}$, lateral offsets $d_\tau \in \{d_{min}, ..., d_{max}\}$, and final velocities $v_\tau \in \{v_{min}, ..., v_{max}\}$ (or final $s_\tau$ positions).

• Polynomial Parameterization:

  • Lateral motion $\zeta(t) = d(t)$ is fit using a quintic polynomial:

  $(s(t), d(t))$0

  with boundary conditions: $(s(t), d(t))$1, $(s(t), d(t))$2, $(s(t), d(t))$3, $(s(t), d(t))$4, $(s(t), d(t))$5, $(s(t), d(t))$6. - Longitudinal motion is fit via quartic or quintic polynomials, enforcing zero jerk at terminal points if $(s(t), d(t))$7-sampling is employed.

• Kinematic Feasibility Constraints: Must be satisfied $(s(t), d(t))$8:

  • Acceleration: $(s(t), d(t))$9, with $s$0 velocity-dependent.
  • Curvature: $s$1.
  • Curvature-rate and yaw-rate bounds.
• Collision and Road-Boundary Checking: Continuous collision-free guarantee via oriented bounding-box (OBB) checks at each time step (DrivabilityChecker); ensures trajectories remain within lanelet bounds.

The cross product of $s$2 longitudinal and $s$3 lateral samples yields up to $s$4 candidate trajectories per planning cycle.

3. Cost Functions, Optimization, and Output Selection

Each feasible trajectory $s$5 is evaluated using a multi-objective cost function:

$s$6

with modular partial terms including:

• Comfort: $s$7 (acceleration squared integral), $s$8 (longitudinal jerk), $s$9 (lateral jerk).
• Efficiency/Precision: $\Gamma$0 (speed-tracking error), $\Gamma$1 (distance to reference path).
• Safety: $\Gamma$2 (distance to obstacles), $\Gamma$3 (collision probability), $\Gamma$4 (time-varying Mahalanobis distance to dynamic agents).

Trajectories are sorted by $\Gamma$5, and the lowest-cost collision-free trajectory is selected. If no feasible trajectory exists, the system produces either an emergency or risk-minimizing fallback maneuver as defined by Eq. (22) (Trauth et al., 2024).

4. Occlusion-Aware Motion Planning: FRENETIX-Occlusion

FRENETIX-Occlusion is an open-source Python module (https://github.com/TUM-AVS/Frenetix-Occlusion) that addresses trajectory safety in the presence of blind spots and occlusions (Moller et al., 2024). It operates as follows:

• Occlusion Analysis: Calculates the visible ($\Gamma$6) and occluded ($\Gamma$7) areas using map topology, vehicle pose, and sensor model, producing occluded regions caused by static boundaries and dynamic obstacles:

$\Gamma$8

• Phantom Agent (PA) Generation: At each occlusion boundary, the system spawns phantom agents (pedestrian, bicycle, vehicle) at candidate spawn points, simulating their potential movements via constant-velocity kinematic models (along lanelets or across the road for pedestrians). All feasible successor routes ($\Gamma$9) are enumerated.
• Criticality Metrics: For each candidate trajectory $d$0 and each spawned PA, the following metrics are evaluated in real-time:
  • Distance-to-Closest-Encounter (DCE)
  • Time-to-Collision (TTC)
  • Brake-Threat-Number (BTN)
  • Risk & Harm (HR)
  • Collision Probability (CP)

A trajectory is marked as valid if all metrics remain below configured thresholds:

$d$1

FRENETIX-Occlusion is integrated as an additional occlusion-safety filtering stage after standard feasibility and collision checks but before trajectory selection. Through this, the planner can either discard unsafe trajectories or re-rank candidates based on aggregated safety scores.

5. Empirical Performance and Scalability

FRENETIX has been evaluated on 1,750 CommonRoad benchmark scenarios (urban and highway, including highly dynamic interactions). Key empirical findings are:

• Success Rate: 88.0% of cases (1,539 out of 1,750) reached the goal safely; collisions occurred in 8.2%, while 2.2% saw no feasible solution, and 1.6% exceeded the time limit.
• Real-time Computation: On an AMD 7950X (C++ multiprocess), 800 trajectories are generated and evaluated in ≈8 ms. For 3,500 and even 13,000 trajectories, real-time capability is retained using multiprocessing.
• Implementation Scalability: The pipeline is embarrassingly parallel, supporting multi-threading, GPU-based evaluation, or distributed node execution, and is suitable for integration in both simulation and real-vehicle platforms.

A summary of computation times for 800 trajectories:

Implementation	Single-core (ms)	Multiprocessing (ms)
C++	29.37	7.87
Python	457.87	214.72

Benchmarks also demonstrate that stricter occlusion risk thresholds in FRENETIX-Occlusion lead to preemptive deceleration and collision avoidance in scenarios with hidden road users.

6. Extensibility and Open-Source Ecosystem

The FRENETIX architecture exposes clear interfaces for customizing or replacing modules, including:

• Vehicle Model: Kinematic bounds and polynomial trajectory boundary conditions can be adapted for different platforms.
• Scenario-Specific Preprocessing: Map and obstacle extraction pipelines can be replaced for varying sensor suites or map providers.
• Cost Function Tuning: Weights $d$2 may be statically adjusted or learned online, e.g., via inverse reinforcement learning, to reflect different driving styles.
• Integration with Third-Party Modules: The funnel structure supports occlusion handling, risk assessment, and high-level behavior planning as pluggable modules.

Both FRENETIX and FRENETIX-Occlusion provide comprehensive open-source packages. FRENETIX features a C++ core (with CMake scripts), Python wrapper, experiment scripts, and built-in support for CommonRoad scenario sets. FRENETIX-Occlusion provides Python modules for visibility analysis, spawn-point generation, trajectories, and metric evaluation. The systems are designed for reproducibility and extensibility, as outlined in their respective repositories (Trauth et al., 2024, Moller et al., 2024).

7. Limitations and Future Directions

Current limitations include:

• Occlusion Handling: The constant-velocity prediction for phantom agents omits agent-agent interaction and braking, and phantom agents lack temporal continuity. Future work may implement set-based reachability, POMDP-based forecasts, or multi-hypothesis tracking.
• Static Risk Thresholds: Risk, harm, and probability thresholds are fixed, but scene-adaptive parameters (conditioned on environment or traffic state) could optimize conservatism and efficiency.
• Vehicle-PA Prediction Scalability: Computation scales linearly with the number of spawn points and routes; C++ re-implementation and hardware acceleration are potential avenues for further runtime reduction.
• Real-world Validation: To date, FRENETIX has primarily been evaluated in simulation. Real-world experiments under sensor noise, actuation delay, and environment uncertainty are pending.

FRENETIX and FRENETIX-Occlusion present an integrated suite for extendable, real-time, and safety-critical motion planning under complex urban driving conditions, with particular emphasis on modularity and reproducibility (Trauth et al., 2024, Moller et al., 2024).