Skip to content

Autoware Trajectory Optimizer#

The autoware_trajectory_optimizer package generates smooth and feasible trajectories for autonomous vehicles using a plugin-based optimization pipeline. It takes candidate trajectories as input and applies a sequence of optimization plugins to produce smooth, drivable trajectories with proper velocity and acceleration profiles.

Features#

  • Plugin-based architecture - Modular optimization pipeline where each step is a separate plugin
  • Multiple smoothing methods:
    • Elastic Band (EB) smoother for path optimization
    • Akima spline interpolation for smooth path interpolation
    • QP-based smoother with quadratic programming for path smoothing with jerk constraints
  • Velocity optimization - Jerk-filtered velocity smoothing from autoware_velocity_smoother
  • Trajectory validation - Removes invalid points and fixes trajectory orientation
  • Backward trajectory extension - Extends trajectory using past ego states
  • Dynamic parameter reconfiguration - Runtime parameter updates supported

Architecture#

The package uses a plugin architecture where each optimization step inherits from TrajectoryOptimizerPluginBase. The current plugin execution order is:

  1. TrajectoryPointFixer - Removes invalid/repeated points and fixes trajectory direction
  2. TrajectoryQPSmoother - QP-based path smoothing with jerk constraints (optional)
  3. TrajectoryEBSmootherOptimizer - Elastic Band path smoothing (optional)
  4. TrajectorySplineSmoother - Akima spline interpolation (optional)
  5. TrajectoryVelocityOptimizer - Velocity profile optimization
  6. TrajectoryExtender - Extends trajectory backward using past ego states (optional, disabled by default)
  7. TrajectoryPointFixer - Final cleanup of trajectory points

Each plugin can be enabled/disabled via parameters and manages its own configuration independently.

⚠️ Important: Plugin Ordering Constraints#

The order of plugin execution is critical and must be carefully maintained:

  • QP Smoother must run before EB/Akima smoothers: The QP solver relies on constant time intervals (Δt) between trajectory points (default: 0.1s). Both Elastic Band and Akima spline smoothers resample trajectories without preserving the time domain structure, which breaks the QP solver's assumptions. Therefore, when using multiple smoothers together, the QP smoother must execute first.
  • Trajectory Extender positioning: The trajectory extender has known discontinuity issues when placed early in the pipeline. It negatively affects the QP solver results and introduces artifacts. For this reason, it has been moved to near the end of the pipeline and is disabled by default (extend_trajectory_backward: false). Fixing the extender's discontinuity issues is future work.

QP Smoother#

The QP smoother uses quadratic programming (OSQP solver) to optimize trajectory paths with advanced features:

  • Objective: Minimizes path curvature while maintaining fidelity to the original trajectory
  • Decision variables: Path positions (x, y) for each trajectory point
  • Constraints: Fixed initial position (optionally fixed last position)
  • Velocity-based fidelity: Automatically reduces fidelity weight at low speeds for aggressive smoothing of noise
  • Post-processing: Recalculates velocities, accelerations, and orientations from smoothed positions

For detailed documentation, see docs/qp_smoother.md which covers:

  • Mathematical formulation
  • Velocity-based fidelity weighting (sigmoid function)
  • Parameter tuning guidelines
  • Usage examples
  • Performance characteristics

Dependencies#

  • autoware_motion_utils - Trajectory manipulation utilities
  • autoware_osqp_interface - QP solver interface for QP smoother
  • autoware_path_smoother - Elastic Band smoother
  • autoware_velocity_smoother - Velocity smoothing algorithms
  • autoware_utils - Common utilities (geometry, ROS helpers)
  • autoware_vehicle_info_utils - Vehicle information

Parameters#

Name Type Description Default Range
use_akima_spline_interpolation boolean Enable Akima spline interpolation for trajectory smoothing 0 N/A
use_eb_smoother boolean Enable Elastic Band smoother for trajectory smoothing 0 N/A
use_qp_smoother boolean Enable QP-based trajectory smoothing with explicit jerk constraints 1 N/A
fix_invalid_points boolean Remove repeated or invalid points, or points that go against the general trajectory direction 1 N/A
extend_trajectory_backward boolean Extend trajectory backward using ego pose history 1 N/A
optimize_velocity boolean Enable velocity profile optimization with jerk filtering and constraint enforcement 1 N/A
trajectory_point_fixer.orientation_threshold_deg float Yaw threshold for removing wrongly oriented points [deg] 5 ≥0
trajectory_extender.nearest_dist_threshold_m float Distance threshold for trajectory matching [m] 1.5 ≥0
trajectory_extender.nearest_yaw_threshold_deg float Yaw threshold for trajectory matching [deg] 60 ≥0
trajectory_extender.backward_trajectory_extension_m float Length to extend trajectory backward using ego history [m] 5 ≥0
trajectory_spline_smoother.interpolation_resolution_m float Interpolation resolution for Akima spline [m] 0.5 >0
trajectory_spline_smoother.max_yaw_discrepancy_deg float Maximum yaw deviation allowed for spline outlier detection [deg] 2.5 ≥0
trajectory_spline_smoother.max_distance_discrepancy_m float Maximum position deviation allowed for spline outlier detection [m] 5 ≥0
trajectory_spline_smoother.copy_original_orientation boolean Copy orientation from original trajectory to spline-interpolated trajectory 1 N/A
trajectory_qp_smoother.weight_smoothness float Weight for path curvature minimization (geometric smoothness) [dimensionless] 10 ≥0
trajectory_qp_smoother.weight_fidelity float Weight for path fidelity (staying close to original path) [dimensionless] 1 ≥0
trajectory_qp_smoother.time_step_s float Fixed time step for velocity/acceleration calculations [s] 0.1 >0
trajectory_qp_smoother.osqp_eps_abs float OSQP absolute convergence tolerance 0.0001 >0
trajectory_qp_smoother.osqp_eps_rel float OSQP relative convergence tolerance 0.0001 >0
trajectory_qp_smoother.osqp_max_iter integer OSQP maximum solver iterations 100 ≥1
trajectory_qp_smoother.osqp_verbose boolean Enable OSQP verbose solver output 0 N/A
trajectory_qp_smoother.fix_orientation boolean Enable orientation correction for QP solver output 1 N/A
trajectory_qp_smoother.orientation_correction_threshold_deg float Yaw threshold for correcting QP solver output orientation [deg] 5 ≥0
trajectory_qp_smoother.use_velocity_based_fidelity boolean Enable velocity-dependent fidelity weighting (lower fidelity at low speeds) 1 N/A
trajectory_qp_smoother.velocity_threshold_mps float Velocity threshold at sigmoid midpoint for velocity-based fidelity [m/s] 0.3 ≥0
trajectory_qp_smoother.sigmoid_sharpness float Sigmoid steepness for velocity-based fidelity transition (higher = sharper) 50 ≥1
trajectory_qp_smoother.min_fidelity_weight float Minimum fidelity weight at very low speeds [dimensionless] 0.01 ≥0
trajectory_qp_smoother.max_fidelity_weight float Maximum fidelity weight at high speeds [dimensionless] 1 ≥0
trajectory_qp_smoother.num_constrained_points_start integer Number of points from the start of the trajectory to constrain as hard constraints (preserves initial state) 3 ≥0
trajectory_qp_smoother.num_constrained_points_end integer Number of points from the end of the trajectory to constrain as hard constraints 3 ≥0
trajectory_velocity_optimizer.nearest_dist_threshold_m float Distance threshold for trajectory matching [m] 1.5 ≥0
trajectory_velocity_optimizer.nearest_yaw_threshold_deg float Yaw threshold for trajectory matching [deg] 60 ≥0
trajectory_velocity_optimizer.target_pull_out_speed_mps float Target speed during pull-out maneuver [m/s] 1 ≥0
trajectory_velocity_optimizer.target_pull_out_acc_mps2 float Target acceleration during pull-out maneuver [m/s²] 1 ≥0
trajectory_velocity_optimizer.max_speed_mps float Maximum allowed speed [m/s] 8.33 ≥0
trajectory_velocity_optimizer.max_lateral_accel_mps2 float Maximum lateral acceleration [m/s²] 1.5 ≥0
trajectory_velocity_optimizer.set_engage_speed boolean Set minimum speed during engage 0 N/A
trajectory_velocity_optimizer.limit_speed boolean Enable speed limiting 1 N/A
trajectory_velocity_optimizer.limit_lateral_acceleration boolean Enable lateral acceleration limiting 0 N/A
trajectory_velocity_optimizer.smooth_velocities boolean Enable jerk-filtered velocity smoothing 0 N/A
jerk_filter_params.jerk_weight float Weight for smoothness cost for jerk 10 ≥0
jerk_filter_params.over_v_weight float Weight for over speed limit cost 100000 ≥0
jerk_filter_params.over_a_weight float Weight for over accel limit cost 5000 ≥0
jerk_filter_params.over_j_weight float Weight for over jerk limit cost 2000 ≥0
jerk_filter_params.jerk_filter_ds float Resampling ds for jerk filter [m] 0.1 >0
common.output_delta_arc_length float Delta arc length for output trajectory [m] 0.5 >0
common.output_backward_traj_length float Backward length for backward trajectory from base_link [m] 5 ≥0
option.enable_warm_start boolean Enable warm start for optimization 1 N/A
option.enable_optimization_validation boolean Enable optimization validation 0 N/A
common.num_points integer Number of points for optimization 100 ≥1
common.delta_arc_length float Delta arc length for optimization [m] 1 >0
weight.smooth_weight float Weight for smoothness constraint 1 ≥0
weight.lat_error_weight float Weight for lateral error constraint 0.001 ≥0
elastic_band_params.ego_nearest_dist_threshold float Distance threshold for ego nearest search [m] 3 ≥0
elastic_band_params.ego_nearest_yaw_threshold float Yaw threshold for ego nearest search [rad] 1.046 ≥0

Parameters can be set via YAML configuration files in the config/ directory.

Main node parameters control plugin activation (e.g., use_qp_smoother, use_akima_spline_interpolation), while plugin-specific parameters use namespaced names (e.g., trajectory_qp_smoother.weight_smoothness).