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 pluginlib-based architecture where optimization plugins are dynamically loaded at startup. Each plugin inherits from TrajectoryOptimizerPluginBase and is loaded via the ROS 2 pluginlib system.

Plugin Loading and Execution

Plugins are loaded based on the plugin_names parameter, which defines both which plugins to load and their execution order:

plugin_names:
  - "autoware::trajectory_optimizer::plugin::TrajectoryPointFixer"
  - "autoware::trajectory_optimizer::plugin::TrajectoryQPSmoother"
  - "autoware::trajectory_optimizer::plugin::TrajectoryEBSmootherOptimizer"
  - "autoware::trajectory_optimizer::plugin::TrajectorySplineSmoother"
  - "autoware::trajectory_optimizer::plugin::TrajectoryVelocityOptimizer"
  - "autoware::trajectory_optimizer::plugin::TrajectoryExtender"
  - "autoware::trajectory_optimizer::plugin::TrajectoryPointFixer"

Available Plugins

1. TrajectoryPointFixer - Removes invalid/repeated points and fixes trajectory direction
2. TrajectoryQPSmoother - QP-based path smoothing with jerk constraints
3. TrajectoryEBSmootherOptimizer - Elastic Band path smoothing
4. TrajectorySplineSmoother - Akima spline interpolation
5. TrajectoryVelocityOptimizer - Velocity profile optimization with lateral acceleration limits
6. TrajectoryExtender - Extends trajectory backward using past ego states

Each plugin can be enabled/disabled at runtime via activation flags (e.g., use_qp_smoother) 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
plugin_names	array	List of plugin class names to load in execution order. Plugins will be loaded dynamically at startup and executed in the order specified.	['autoware::trajectory_optimizer::plugin::TrajectoryPointFixer', 'autoware::trajectory_optimizer::plugin::TrajectoryQPSmoother', 'autoware::trajectory_optimizer::plugin::TrajectoryEBSmootherOptimizer', 'autoware::trajectory_optimizer::plugin::TrajectorySplineSmoother', 'autoware::trajectory_optimizer::plugin::TrajectoryVelocityOptimizer', 'autoware::trajectory_optimizer::plugin::TrajectoryExtender', 'autoware::trajectory_optimizer::plugin::TrajectoryPointFixer']	N/A
use_akima_spline_interpolation	boolean	Enable Akima spline interpolation for trajectory smoothing	False	N/A
use_eb_smoother	boolean	Enable Elastic Band smoother for trajectory smoothing	False	N/A
use_qp_smoother	boolean	Enable QP-based trajectory smoothing with explicit jerk constraints	True	N/A
fix_invalid_points	boolean	Remove repeated or invalid points, or points that go against the general trajectory direction	True	N/A
extend_trajectory_backward	boolean	Extend trajectory backward using ego pose history	True	N/A
optimize_velocity	boolean	Enable velocity profile optimization with jerk filtering and constraint enforcement	True	N/A
trajectory_point_fixer.orientation_threshold_deg	float	Yaw threshold for removing wrongly oriented points [deg]	5.0	≥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	≥0
trajectory_extender.backward_trajectory_extension_m	float	Length to extend trajectory backward using ego history [m]	5.0	≥0
trajectory_spline_smoother.interpolation_resolution_m	float	Interpolation resolution for Akima spline [m]	0.5	>0
trajectory_spline_smoother.max_distance_discrepancy_m	float	Maximum position deviation allowed for orientation copying [m]	5.0	≥0
trajectory_spline_smoother.preserve_input_trajectory_orientation	boolean	Copy orientations from input trajectory to spline output	True	N/A
trajectory_qp_smoother.weight_smoothness	float	Weight for path curvature minimization (geometric smoothness) [dimensionless]	10.0	≥0
trajectory_qp_smoother.weight_fidelity	float	Weight for path fidelity (staying close to original path) [dimensionless]	1.0	≥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	False	N/A
trajectory_qp_smoother.preserve_input_trajectory_orientation	boolean	Copy orientations from input trajectory to smoothed output	True	N/A
trajectory_qp_smoother.max_distance_for_orientation_m	float	Max distance for nearest neighbor matching when copying orientations [m]	5.0	≥0
trajectory_qp_smoother.use_velocity_based_fidelity	boolean	Enable velocity-dependent fidelity weighting (lower fidelity at low speeds)	True	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.0	≥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	≥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	≥0
trajectory_velocity_optimizer.target_pull_out_speed_mps	float	Target speed during pull-out maneuver [m/s]	1.0	≥0
trajectory_velocity_optimizer.target_pull_out_acc_mps2	float	Target acceleration during pull-out maneuver [m/s²]	1.0	≥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	False	N/A
trajectory_velocity_optimizer.limit_speed	boolean	Enable speed limiting	True	N/A
trajectory_velocity_optimizer.limit_lateral_acceleration	boolean	Enable lateral acceleration limiting	False	N/A
trajectory_velocity_optimizer.smooth_velocities	boolean	Enable jerk-filtered velocity smoothing	False	N/A
jerk_filter_params.jerk_weight	float	Weight for smoothness cost for jerk	10.0	≥0
jerk_filter_params.over_v_weight	float	Weight for over speed limit cost	100000.0	≥0
jerk_filter_params.over_a_weight	float	Weight for over accel limit cost	5000.0	≥0
jerk_filter_params.over_j_weight	float	Weight for over jerk limit cost	2000.0	≥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	≥0
option.enable_warm_start	boolean	Enable warm start for optimization	True	N/A
option.enable_optimization_validation	boolean	Enable optimization validation	False	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	>0
weight.smooth_weight	float	Weight for smoothness constraint	1.0	≥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	≥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.

Parameter Types

1. Plugin Loading (plugin_names) - Array of plugin class names determining load order and execution sequence
2. Activation Flags - Boolean flags for runtime enable/disable (e.g., use_qp_smoother, use_akima_spline_interpolation)
3. Plugin-Specific Parameters - Namespaced parameters for each plugin (e.g., trajectory_qp_smoother.weight_smoothness)

Configuring Plugin Order

To change plugin execution order, modify the plugin_names array in config/trajectory_optimizer.param.yaml:

# Example: Run spline smoother before velocity optimizer
plugin_names:
  - "autoware::trajectory_optimizer::plugin::TrajectoryPointFixer"
  - "autoware::trajectory_optimizer::plugin::TrajectorySplineSmoother"
  - "autoware::trajectory_optimizer::plugin::TrajectoryVelocityOptimizer"
  - "autoware::trajectory_optimizer::plugin::TrajectoryPointFixer"

CRITICAL: QP Smoother Ordering Constraint

The TrajectoryQPSmoother plugin MUST run before any plugins that resample or modify trajectory structure:

• TrajectorySplineSmoother (Akima spline - resamples trajectory)
• TrajectoryEBSmootherOptimizer (Elastic Band - resamples trajectory)
• TrajectoryVelocityOptimizer (velocity smoothing with resampling)
• TrajectoryExtender (adds/modifies points at trajectory start)

The QP solver requires constant time intervals (Δt = 0.1s) between points. These plugins modify the time domain structure or add points, breaking the QP solver assumptions. If you need QP smoothing, it must appear first in the pipeline after TrajectoryPointFixer.

Note: Plugin order changes require node restart. Runtime enable/disable is controlled by activation flags.