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#
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#
(Optional) References/External links#
[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