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. TrajectoryMPTOptimizer - Model predictive trajectory optimization with adaptive corridor bounds. Uses bicycle kinematics model for trajectory refinement. Disabled by default (experimental). See docs/mpt_optimizer.md for details.
6. TrajectoryVelocityOptimizer - Velocity profile optimization with lateral acceleration limits
7. TrajectoryExtender - Extends trajectory backward using past ego states
8. TrajectoryKinematicFeasibilityEnforcer - Enforces Ackermann steering and yaw rate constraints

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 (use_trajectory_extender: 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

{{ json_to_markdown(“planning/autoware_trajectory_optimizer/schema/trajectory_optimizer.schema.json”) }}

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:

```yaml

File truncated at 100 lines see the full file

Changelog for package autoware_trajectory_optimizer

0.50.0 (2026-02-14)

• Merge remote-tracking branch 'origin/main' into humble

• feat(autoware_trajectory_optimizer): velocity optimizer refactor (#12021)

  • feat: diffusion planner optimizer refactor
  • feat: fix bugs in optimization formulation
  • fix: clean un-used function
  • fix: package.xml sort package
  • feat: merge max velocity settings; clean up merge; clean limit_accel_acceleration
  • fix: restore pull out acc
  • feat: add publish debugging trajectories
  • feat: do not add the final point to the trajectory after spline smoother
  • fix: no lint next line
  • tune the parameter into a smoother one, tested in bus Psim
  • feat: clean include
  • feat: use internal tool for better angle handling
  • fix: clean up debugging publishers
  • feat: update max lateral accel value
  • feat: avoid delta time error
  • revert: restore plugin loops
  • feat: clean up jerk filter
  • further clean parameters
  • feat: expliticly choose if max speed update in place
  • fix: clean up inclid in utils.cpp and trajectory_optimizer.cpp
  • clean up debug publishers
  • feat: documentation for the continuous jerk filter
  • fix doc
  • fix pre-commit in readme

  * update documentation list ---------

• fix(autoware_trajectory_optimizer): prevent yaw spikes when creating splines (#12043)

  • use spline util for first and last point instead of direct insertion of original traj points

  * use last_s to actually get the last value in the loop ---------

• feat(autoware_trajectory_optimizer): add external velocity limit subscription (#12023)

  • add external velocity limit subscription
  • change topic name in code
  • subscribe to the external velocity limiter selector topic and not directly the API's
  • make the default velocity limit the same as the common_param velocity
  • Add /output/current_velocity_limit_mps publisher
  • output right topic name
  • change velocity limit input to API only, add common param to sync default velocity limit to that of Autoware common.param.yaml
  • change launch to use var name for the input external velocity for consistency
  • add mps to var name for consistency

  * update dependencies in package.xml ---------

• Contributors: Ryohsuke Mitsudome, Yuxuan Liu, danielsanchezaran

0.49.0 (2025-12-30)

• Merge remote-tracking branch 'origin/main' into prepare-0.49.0-changelog

• refactor(autoware_trajectory_optimizer): move functions and change parameter names to accommodate standard (#11760)

  • wip refactor point fixer
  • move point fixer functions to fixer utils
  • move extender functions to extender utils
  • move velocity optimizer functions to its utils function
  • move spline to utils code
  • use assignment by reference to prevent a copy
  • remove unnecessary ref
  • review recommendations
  • change ERROR logs to WARN logs

  * refactor remove close proximity points function ---------

• feat(trajectory_optimizer): change max velocity (#11735) change max velocity

• feat(trajectory_optimizer): change error throttle to warn throttle (#11726) change error to warning

File truncated at 100 lines see the full file