Path optimizer

Purpose

This package generates a trajectory that is kinematically-feasible to drive and collision-free based on the input path, drivable area. Only position and orientation of trajectory are updated in this module, and velocity is just taken over from the one in the input path.

Feature

This package is able to

• make the trajectory inside the drivable area as much as possible
  • NOTE: Static obstacles to avoid can be removed from the drivable area.
• insert stop point before the planned footprint will be outside the drivable area

Note that the velocity is just taken over from the input path.

Inputs / Outputs

input

output

Flowchart

Flowchart of functions is explained here.

@startuml
title pathCallback
start

:isDataReady;

:createPlannerData;

group generateOptimizedTrajectory
  group optimizeTrajectory
    :check replan;
    if (replanning required?) then (yes)
      :getEBTrajectory;
      :getModelPredictiveTrajectory;
      if (optimization failed?) then (no)
      else (yes)
        :send previous\n trajectory;
      endif
    else (no)
      :send previous\n trajectory;
    endif
  end group

  :applyInputVelocity;
  :insertZeroVelocityOutsideDrivableArea;
  :publishDebugMarkerOfOptimization;
end group


:extendTrajectory;

:setZeroVelocityAfterStopPoint;

:publishDebugData;

stop
@enduml

createPlannerData

The following data for planning is created.

struct PlannerData
{
  // input
  Header header;
  std::vector<TrajectoryPoint> traj_points; // converted from the input path
  std::vector<geometry_msgs::msg::Point> left_bound;
  std::vector<geometry_msgs::msg::Point> right_bound;

  // ego
  geometry_msgs::msg::Pose ego_pose;
  double ego_vel;
};

check replan

When one of the following conditions are met, trajectory optimization will be executed. Otherwise, previously optimized trajectory is used with updating the velocity from the latest input path.

max_path_shape_around_ego_lat_dist

• Ego moves longer than replan.max_ego_moving_dist in one cycle. (default: 3.0 [m])
  • This is for when the ego pose is set again in the simulation.
• Trajectory’s end, which is considered as the goal pose, moves longer than replan.max_goal_moving_dist in one cycle. (default: 15.0 [ms])
  • When the goal pose is set again, the planning should be reset.
• Time passes. (default: 1.0 [s])
  • The optimization is skipped for a while sine the optimization is sometimes heavy.
• The input path changes laterally longer than replan.max_path_shape_around_ego_lat_dist in one cycle. (default: 2.0)

getModelPredictiveTrajectory

This module makes the trajectory kinematically-feasible and collision-free. We define vehicle pose in the frenet coordinate, and minimize tracking errors by optimization. This optimization considers vehicle kinematics and collision checking with road boundary and obstacles. To decrease the computation cost, the optimization is applied to the shorter trajectory (default: 50 [m]) than the whole trajectory, and concatenate the remained trajectory with the optimized one at last.

The trajectory just in front of the ego must not be changed a lot so that the steering wheel will be stable. Therefore, we use the previously generated trajectory in front of the ego.

Optimization center on the vehicle, that tries to locate just on the trajectory, can be tuned along side the vehicle vertical axis. This parameter mpt.kinematics.optimization center offset is defined as the signed length from the back-wheel center to the optimization center. Some examples are shown in the following figure, and it is shown that the trajectory of vehicle shape differs according to the optimization center even if the reference trajectory (green one) is the same.

More details can be seen here.

applyInputVelocity

Velocity is assigned in the optimized trajectory from the velocity in the behavior path. The shapes of the optimized trajectory and the path are different, therefore the each nearest trajectory point to the path is searched and the velocity is interpolated with zero-order hold.

insertZeroVelocityOutsideDrivableArea

Optimized trajectory is too short for velocity planning, therefore extend the trajectory by concatenating the optimized trajectory and the behavior path considering drivability. Generated trajectory is checked if it is inside the drivable area or not, and if outside drivable area, output a trajectory inside drivable area with the behavior path or the previously generated trajectory.

As described above, the behavior path is separated into two paths: one is for optimization and the other is the remain. The first path becomes optimized trajectory, and the second path just is transformed to a trajectory. Then a trajectory inside the drivable area is calculated as follows.

• If optimized trajectory is inside the drivable area, and the remained trajectory is inside/outside the drivable area,
  • the output trajectory will be just concatenation of those two trajectories.
  • In this case, we do not care if the remained trajectory is inside or outside the drivable area since generally it is outside the drivable area (especially in a narrow road), but we want to pass a trajectory as long as possible to the latter module.
• If optimized trajectory is outside the drivable area, and the remained trajectory is inside/outside the drivable area,
  • and if the previously generated trajectory is memorized,
    • the output trajectory will be the previously generated trajectory, where zero velocity is inserted to the point firstly going outside the drivable area.
  • and if the previously generated trajectory is not memorized,
    • the output trajectory will be a part of trajectory just transformed from the behavior path, where zero velocity is inserted to the point firstly going outside the drivable area.

Optimization failure is dealt with the same as if the optimized trajectory is outside the drivable area. The output trajectory is memorized as a previously generated trajectory for the next cycle.

Rationale In the current design, since there are some modelling errors, the constraints are considered to be soft constraints. Therefore, we have to make sure that the optimized trajectory is inside the drivable area or not after optimization.

Limitation

• Computation cost is sometimes high.
• Because of the approximation such as linearization, some narrow roads cannot be run by the planner.
• Roles of planning for behavior_path_planner and path_optimizer are not decided clearly. Both can avoid obstacles.

Comparison to other methods

Trajectory planning problem that satisfies kinematically-feasibility and collision-free has two main characteristics that makes hard to be solved: one is non-convex and the other is high dimension. Based on the characteristics, we investigate pros/cons of the typical planning methods: optimization-based, sampling-based, and learning-based method.

Optimization-based method

• pros: comparatively fast against high dimension by leveraging the gradient descent
• cons: often converge to the local minima in the non-convex problem

Sampling-based method

• pros: realize global optimization
• cons: high computation cost especially in the complex case

Learning-based method

• under research yet

Based on these pros/cons, we chose the optimization-based planner first. Although it has a cons to converge to the local minima, it can get a good solution by the preprocessing to approximate the problem to convex that almost equals to the original non-convex problem.

How to Tune Parameters

Drivability in narrow roads

• modify mpt.clearance.soft_clearance_from_road
  • This parameter describes how much margin to make between the trajectory and road boundaries.
  • Due to the model error for optimization, the constraint such as collision-free is not fully met.
    • By making this parameter larger, the is for narrow-road driving may be resolved. 12180

• modify mpt.kinematics.optimization_center_offset

  • The point on the vehicle, offset forward with this parameter from the base link` tries to follow the reference path.
• change or tune the method to approximate footprints with a set of circles.
  • See here
  • Tuning means changing the ratio of circle’s radius.

Computation time

• under construction

Robustness

• Check if the trajectory before or after MPT is not robust
  • if the trajectory before MPT is not robust
  • if the trajectory after MPT is not robust
    • make mpt.weight.steer_input_weight or mpt.weight.steer_rate_weight larger, which are stability of steering wheel along the trajectory.

Other options

• option.enable_skip_optimization skips MPT optimization.
• option.enable_calculation_time_info enables showing each calculation time for functions and total calculation time on the terminal.
• option.enable_outside_drivable_area_stop enables stopping just before the generated trajectory point will be outside the drivable area.

How To Debug

How to debug can be seen here.

Changelog for package autoware_path_optimizer

0.43.0 (2025-03-21)

• Merge remote-tracking branch 'origin/main' into chore/bump-version-0.43
• chore: rename from [autoware.universe]{.title-ref} to [autoware_universe]{.title-ref} (#10306)
• fix(autoware_path_optimizer): hotfix for wrong logic triggering MRM on start in 3 seconds (#10305) fix
• feat(path_optimizer): additional failure logging and failure mode handling (#10276) MRM when MPT fails
• feat: apply splitting of autoware_utils_geometry (#10270)
  • fix build error

  * merge namespace ---------

• feat: adaption to ROS nodes guidelines about directory structure (#10268)
• fix(path_optimizer): remove unnecesary optional (#10181)
• fix(planning): add missing exec_depend (#10134)
  • fix(planning): add missing exec_depend
  • fix find-pkg-share

  * fix find-pkg-share ---------

• Contributors: Arjun Jagdish Ram, Hayato Mizushima, NorahXiong, Takagi, Isamu, Takayuki Murooka, Yutaka Kondo

0.42.0 (2025-03-03)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_utils): replace autoware_universe_utils with autoware_utils (#10191)
• feat: remove dependency on autoware_universe_utils from autoware_interpolation and autoware_motion_utils (#10092) Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]\@users.noreply.github.com>
• feat!: replace tier4_planning_msgs/PathWithLaneId with autoware_internal_planning_msgs/PathWithLaneId (#10023)
• feat(planning_test_manager): abstract message-specific functions (#9882)
  • abstract message-specific functions
  • include necessary header
  • adapt velocity_smoother to new test manager
  • adapt behavior_velocity_planner to new test manager
  • adapt path_optimizer to new test manager
  • fix output subscription
  • adapt behavior_path_planner to new test manager
  • adapt scenario_selector to new test manager
  • adapt freespace_planner to new test manager
  • adapt planning_validator to new test manager
  • adapt obstacle_stop_planner to new test manager
  • adapt obstacle_cruise_planner to new test manager
  • disable test for freespace_planner
  • adapt behavior_velocity_crosswalk_module to new test manager
  • adapt behavior_path_lane_change_module to new test manager
  • adapt behavior_path_avoidance_by_lane_change_module to new test manager
  • adapt behavior_path_dynamic_obstacle_avoidance_module to new test manager
  • adapt behavior_path_external_request_lane_change_module to new test manager
  • adapt behavior_path_side_shift_module to new test manager
  • adapt behavior_path_static_obstacle_avoidance_module to new test manager
  • adapt path_smoother to new test manager
  • adapt behavior_velocity_blind_spot_module to new test manager
  • adapt behavior_velocity_detection_area_module to new test manager
  • adapt behavior_velocity_intersection_module to new test manager
  • adapt behavior_velocity_no_stopping_area_module to new test manager
  • adapt behavior_velocity_run_out_module to new test manager
  • adapt behavior_velocity_stop_line_module to new test manager
  • adapt behavior_velocity_traffic_light_module to new test manager
  • adapt behavior_velocity_virtual_traffic_light_module to new test manager
  • adapt behavior_velocity_walkway_module to new test manager
  • adapt motion_velocity_planner_node_universe to new test manager
  • include necessary headers

  * Odometries -> Odometry ---------Co-authored-by: Takayuki Murooka <<takayuki5168@gmail.com>>

• fix(path_optimizer): fix possible zero division (#10022)
• Contributors: Fumiya Watanabe, Mamoru Sobue, Mitsuhiro Sakamoto, Ryohsuke Mitsudome, 心刚

0.41.2 (2025-02-19)

• chore: bump version to 0.41.1 (#10088)
• Contributors: Ryohsuke Mitsudome

0.41.1 (2025-02-10)

0.41.0 (2025-01-29)

• Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base
• feat(autoware_path_optimizer)!: tier4_debug_msgs changed to autoware-internal_debug_msgs in autoware_path_optimizer (#9907)
  • feat: tier4_debug_msgs changed to autoware-internal_debug_msgs in files planning/autoware_path_optimizer

  * Update planning/autoware_path_optimizer/package.xml ---------Co-authored-by: Ryohsuke Mitsudome <<43976834+mitsudome-r@users.noreply.github.com>>

• feat(motion_planning): use StringStamped in autoware_internal_debug_msgs (#9742) feat(motion planning): use StringStamped in autoware_internal_debug_msgs
• Contributors: Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan

0.40.0 (2024-12-12)

• Merge branch 'main' into release-0.40.0
• Revert "chore(package.xml): bump version to 0.39.0 (#9587)" This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5.
• fix: fix ticket links in CHANGELOG.rst (#9588)
• chore(package.xml): bump version to 0.39.0 (#9587)
  • chore(package.xml): bump version to 0.39.0
  • fix: fix ticket links in CHANGELOG.rst

  * fix: remove unnecessary diff ---------Co-authored-by: Yutaka Kondo <<yutaka.kondo@youtalk.jp>>

• fix: fix ticket links in CHANGELOG.rst (#9588)
• fix(cpplint): include what you use - planning (#9570)
• refactor(global_parameter_loader): prefix package and namespace with autoware (#9303)
• docs: update the list styles (#9555)
• fix(path_optimizer): solve issue with time keeper (#9483) solve issue with time keeper
• 0.39.0
• update changelog
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• refactor(fake_test_node): prefix package and namespace with autoware (#9249)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo, danielsanchezaran

0.39.0 (2024-11-25)

• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• fix: fix ticket links to point to https://github.com/autowarefoundation/autoware_universe (#9304)
• chore(package.xml): bump version to 0.38.0 (#9266) (#9284)
  • unify package.xml version to 0.37.0
  • remove system_monitor/CHANGELOG.rst
  • add changelog

  * 0.38.0

• Contributors: Esteve Fernandez, Yutaka Kondo

0.38.0 (2024-11-08)

• unify package.xml version to 0.37.0
• refactor(osqp_interface): added autoware prefix to osqp_interface (#8958)
• chore(path_optimizer): add warn msg for exceptional behavior (#9033)
• refactor(autoware_interpolation): prefix package and namespace with autoware (#8088) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
• fix(motion_planning): align the parameters with launcher (#8792) parameters in motion_planning aligned
• fix(autoware_path_optimizer): fix unusedFunction (#8644) fix:unusedFunction
• fix(autoware_path_optimizer): fix unreadVariable (#8361)
  • fix:unreadVariable

  * fix:unreadVariable ---------

• fix(autoware_path_optimizer): fix passedByValue (#8190) fix:passedByValue
• fix(path_optimizer): revert the feature of publishing processing time (#8160)
• feat(autoware_universe_utils): add TimeKeeper to track function's processing time (#7754)
• fix(autoware_path_optimizer): fix redundantContinue warnings (#7577)
• refactor(universe_utils/motion_utils)!: add autoware namespace (#7594)
• refactor(motion_utils)!: add autoware prefix and include dir (#7539) refactor(motion_utils): add autoware prefix and include dir
• feat(autoware_universe_utils)!: rename from tier4_autoware_utils (#7538) Co-authored-by: kosuke55 <<kosuke.tnp@gmail.com>>
• feat(path_optimizer): rename to include/autoware/{package_name} (#7529)
• refactor(test_utils): move to common folder (#7158)
  • Move autoware planning test manager to autoware namespace
  • fix package share directory for behavior path planner
  • renaming files and directory
  • rename variables that has planning_test_utils in its name.
  • use autoware namespace for test utils
  • move folder to common
  • update .pages file
  • fix test error
  • removed obstacle velocity limiter test artifact

  * remove namespace from planning validator, it has using keyword ---------

• refactor(vehicle_info_utils)!: prefix package and namespace with autoware (#7353)
  • chore(autoware_vehicle_info_utils): rename header
  • chore(bpp-common): vehicle info
  • chore(path_optimizer): vehicle info
  • chore(velocity_smoother): vehicle info
  • chore(bvp-common): vehicle info
  • chore(static_centerline_generator): vehicle info
  • chore(obstacle_cruise_planner): vehicle info
  • chore(obstacle_velocity_limiter): vehicle info
  • chore(mission_planner): vehicle info
  • chore(obstacle_stop_planner): vehicle info
  • chore(planning_validator): vehicle info
  • chore(surround_obstacle_checker): vehicle info
  • chore(goal_planner): vehicle info
  • chore(start_planner): vehicle info
  • chore(control_performance_analysis): vehicle info
  • chore(lane_departure_checker): vehicle info
  • chore(predicted_path_checker): vehicle info
  • chore(vehicle_cmd_gate): vehicle info
  • chore(obstacle_collision_checker): vehicle info
  • chore(operation_mode_transition_manager): vehicle info
  • chore(mpc): vehicle info
  • chore(control): vehicle info
  • chore(common): vehicle info
  • chore(perception): vehicle info
  • chore(evaluator): vehicle info
  • chore(freespace): vehicle info
  • chore(planning): vehicle info
  • chore(vehicle): vehicle info
  • chore(simulator): vehicle info
  • chore(launch): vehicle info
  • chore(system): vehicle info
  • chore(sensing): vehicle info

  * fix(autoware_joy_controller): remove unused deps ---------

• refactor(path_optimizer, velocity_smoother)!: prefix package and namespace with autoware (#7354)
  • chore(autoware_velocity_smoother): update namespace

  * chore(autoware_path_optimizer): update namespace ---------

• feat!: replace autoware_auto_msgs with autoware_msgs for planning modules (#7246) Co-authored-by: Cynthia Liu <<cynthia.liu@autocore.ai>> Co-authored-by: NorahXiong <<norah.xiong@autocore.ai>> Co-authored-by: beginningfan <<beginning.fan@autocore.ai>>
• chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (#7202)
  • chore(autoware_path_optimizer): rename package and namespace
  • chore(autoware_static_centerline_generator): rename package and namespace
  • chore: update module name
  • chore(autoware_velocity_smoother): rename package and namespace
  • chore(tier4_planning_launch): update module name
  • chore: update module name
  • fix: test
  • fix: test

  * fix: test ---------

• Contributors: Esteve Fernandez, Kosuke Takeuchi, Ryohsuke Mitsudome, Ryuta Kambe, Satoshi OTA, Takayuki Murooka, Yuki TAKAGI, Yutaka Kondo, Zhe Shen, Zulfaqar Azmi, kobayu858

0.26.0 (2024-04-03)