Skip to content

Motion Velocity Smoother#

Purpose#

motion_velocity_smoother outputs a desired velocity profile on a reference trajectory. This module plans a velocity profile within the limitations of the velocity, the acceleration and the jerk to realize both the maximization of velocity and the ride quality. We call this module motion_velocity_smoother because the limitations of the acceleration and the jerk means the smoothness of the velocity profile.

Inner-workings / Algorithms#

Flow chart#

motion_velocity_smoother_flow

Extract trajectory#

For the point on the reference trajectory closest to the center of the rear wheel axle of the vehicle, it extracts the reference path between extract_behind_dist behind and extract_ahead_dist ahead.

Apply external velocity limit#

It applies the velocity limit input from the external of motion_velocity_smoother. Remark that the external velocity limit is different from the velocity limit already set on the map and the reference trajectory. The external velocity is applied at the position that it is able to reach the velocity limit with the deceleration and the jerk constraints set as the parameter.

Apply stop approaching velocity#

It applies the velocity limit near the stopping point. This function is used to approach near the obstacle or improve the accuracy of stopping.

Apply lateral acceleration limit#

It applies the velocity limit to decelerate at the curve. It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration max_lateral_accel. The velocity limit is set as not to fall under min_curve_velocity.

Resample trajectory#

It resamples the points on the reference trajectory with designated time interval. Note that the range of the length of the trajectory is set between min_trajectory_length and max_trajectory_length, and the distance between two points is longer than min_trajectory_interval_distance. It samples densely up to the distance traveled between resample_time with the current velocity, then samples sparsely after that. By sampling according to the velocity, both calculation load and accuracy are achieved since it samples finely at low velocity and coarsely at high velocity.

Calculate initial state#

Calculate initial values for velocity planning. The initial values are calculated according to the situation as shown in the following table.

Situation Initial velocity Initial acceleration
First calculation Current velocity 0.0
Engaging engage_velocity engage_acceleration
Deviate between the planned velocity and the current velocity Current velocity Previous planned value
Normal Previous planned value Previous planned value

Smooth velocity#

It plans the velocity. The algorithm of velocity planning is chosen from JerkFiltered, L2 and Linf, and it is set in the launch file. In these algorithms, they use OSQP[1] as the solver of the optimization.

JerkFiltered#

It minimizes the sum of the minus of the square of the velocity and the square of the violation of the velocity limit, the acceleration limit and the jerk limit.

L2#

It minimizes the sum of the minus of the square of the velocity, the square of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit.

Linf#

It minimizes the sum of the minus of the square of the velocity, the maximum absolute value of the the pseudo-jerk[2] and the square of the violation of the velocity limit and the acceleration limit.

Post process#

It performs the post-process of the planned velocity.

  • Set zero velocity ahead of the stopping point
  • Set maximum velocity given in the config named max_velocity
  • Set velocity behind the current pose
  • Resample trajectory (post resampling)
  • Output debug data

After the optimization, a resampling called post resampling is performed before passing the optimized trajectory to the next node. Since the required path interval from optimization may be different from the one for the next module, post resampling helps to fill this gap. Therefore, in post resampling, it is necessary to check the path specification of the following module to determine the parameters. Note that if the computational load of the optimization algorithm is high and the path interval is sparser than the path specification of the following module in the first resampling, post resampling would resample the trajectory densely. On the other hand, if the computational load of the optimization algorithm is small and the path interval is denser than the path specification of the following module in the first resampling, the path is sparsely resampled according to the specification of the following module.

Inputs / Outputs#

Input#

Name Type Description
~/input/trajectory autoware_planning_msgs/Trajectory Reference trajectory
/planning/scenario_planning/max_velocity std_msgs/Float32 External velocity limit [m/s]
/localization/twist geometry_msgs/TwistStamped Current twist
/tf tf2_msgs/TFMessage TF
/tf_static tf2_msgs/TFMessage TF static

Output#

Name Type Description
~/output/trajectory autoware_planning_msgs/Trajectory Modified trajectory
/planning/scenario_planning/current_max_velocity std_msgs/Float32 Current external velocity limit [m/s]
~/closest_velocity std_msgs/Float32 Planned velocity closest to ego base_link (for debug)
~/closest_acceleration std_msgs/Float32 Planned acceleration closest to ego base_link (for debug)
~/closest_jerk std_msgs/Float32 Planned jerk closest to ego base_link (for debug)
~/debug/trajectory_raw autoware_planning_msgs/Trajectory Extracted trajectory (for debug)
~/debug/trajectory_external_velocity_limited autoware_planning_msgs/Trajectory External velocity limited trajectory (for debug)
~/debug/trajectory_lateral_acc_filtered autoware_planning_msgs/Trajectory Lateral acceleration limit filtered trajectory (for debug)
~/debug/trajectory_time_resampled autoware_planning_msgs/Trajectory Time resampled trajectory (for debug)
~/distance_to_stopline std_msgs/Float32 Distance to stop line from current ego pose (max 50 m) (for debug)
~/stop_speed_exceeded std_msgs/Bool It publishes true if planned velocity on the point which the maximum velocity is zero is over threshold

Parameters#

Constraint parameters#

Name Type Description Default value
max_velocity double Max velocity limit [m/s] 20.0
max_accel double Max acceleration limit [m/ss] 1.0
min_decel double Min deceleration limit [m/ss] -0.5
stop_decel double Stop deceleration value at a stop point [m/ss] 0.0
max_jerk double Max jerk limit [m/sss] 1.0
min_jerk double Min jerk limit [m/sss] -0.5

External velocity limit parameter#

Name Type Description Default value
margin_to_insert_external_velocity_limit double margin distance to insert external velocity limit [m] 0.3

Curve parameters#

Name Type Description Default value
max_lateral_accel double Max lateral acceleration limit [m/ss] 0.5
min_curve_velocity double Min velocity at lateral acceleration limit [m/ss] 2.74
decel_distance_before_curve double Distance to slowdown before a curve for lateral acceleration limit [m] 3.5
decel_distance_after_curve double Distance to slowdown after a curve for lateral acceleration limit [m] 2.0

Engage & replan parameters#

Name Type Description Default value
replan_vel_deviation double Velocity deviation to replan initial velocity [m/s] 5.53
engage_velocity double Engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) 0.25
engage_acceleration double Engage acceleration [m/ss] (use this acceleration when engagement) 0.1
engage_exit_ratio double Exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. 0.5
stop_dist_to_prohibit_engage double If the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] 0.5

Stopping velocity parameters#

Name Type Description Default value
stopping_velocity double change target velocity to this value before v=0 point [m/s] 2.778
stopping_distance double distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. 0.0

Extraction parameters#

Name Type Description Default value
extract_ahead_dist double Forward trajectory distance used for planning [m] 200.0
extract_behind_dist double backward trajectory distance used for planning [m] 5.0
delta_yaw_threshold double Allowed delta yaw between ego pose and trajectory pose [radian] 1.0472

Resampling parameters#

Name Type Description Default value
max_trajectory_length double Max trajectory length for resampling [m] 200.0
min_trajectory_length double Min trajectory length for resampling [m] 30.0
resample_time double Resample total time [s] 10.0
dense_dt double resample time interval for dense sampling [s] 0.1
dense_min_interval_distance double minimum points-interval length for dense sampling [m] 0.1
sparse_dt double resample time interval for sparse sampling [s] 0.5
sparse_min_interval_distance double minimum points-interval length for sparse sampling [m] 4.0

Resampling parameters for post process#

Name Type Description Default value
post_max_trajectory_length double max trajectory length for resampling [m] 300.0
post_min_trajectory_length double min trajectory length for resampling [m] 30.0
post_resample_time double resample total time for dense sampling [s] 10.0
post_dense_dt double resample time interval for dense sampling [s] 0.1
post_dense_min_interval_distance double minimum points-interval length for dense sampling [m] 0.1
post_sparse_dt double resample time interval for sparse sampling [s] 0.1
post_sparse_min_interval_distance double minimum points-interval length for sparse sampling [m] 1.0

Weights for optimization#

JerkFiltered#

Name Type Description Default value
jerk_weight double Weight for "smoothness" cost for jerk 10.0
over_v_weight double Weight for "over speed limit" cost 100000.0
over_a_weight double Weight for "over accel limit" cost 5000.0
over_j_weight double Weight for "over jerk limit" cost 1000.0

L2#

Name Type Description Default value
pseudo_jerk_weight double Weight for "smoothness" cost 100.0
over_v_weight double Weight for "over speed limit" cost 100000.0
over_a_weight double Weight for "over accel limit" cost 1000.0

Linf#

Name Type Description Default value
pseudo_jerk_weight double Weight for "smoothness" cost 100.0
over_v_weight double Weight for "over speed limit" cost 100000.0
over_a_weight double Weight for "over accel limit" cost 1000.0

Others#

Name Type Description Default value
over_stop_velocity_warn_thr double Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] 1.389

Assumptions / Known limits#

  • Assume that the velocity limit or the stopping point is properly set at the point on the reference trajectory
  • If the velocity limit set in the reference path cannot be achieved by the designated constraints of the deceleration and the jerk, decelerate while suppressing the velocity, the acceleration and the jerk deviation as much as possible
  • The importance of the deviations is set in the config file

(Optional) Error detection and handling#

(Optional) Performance characterization#

[1] B. Stellato, et al., "OSQP: an operator splitting solver for quadratic programs", Mathematical Programming Computation, 2020, 10.1007/s12532-020-00179-2.

[2] Y. Zhang, et al., "Toward a More Complete, Flexible, and Safer Speed Planning for Autonomous Driving via Convex Optimization", Sensors, vol. 18, no. 7, p. 2185, 2018, 10.3390/s18072185

(Optional) Future extensions / Unimplemented parts#