map_based_prediction

Role

map_based_prediction is a module to predict the future paths (and their probabilities) of other vehicles and pedestrians according to the shape of the map and the surrounding environment.

Assumptions

• The following information about the target obstacle is needed
  • Label (type of person, car, etc.)
  • The object position in the current time and predicted position in the future time.
• The following information about the surrounding environment is needed
  • Road network information with Lanelet2 format

Inner-workings / Algorithms

Flow chart

Path prediction for road users

Remove old object history

Store time-series data of objects to determine the vehicle’s route and to detect lane change for several duration. Object Data contains the object’s position, speed, and time information.

Get current lanelet and update Object history

Search one or more lanelets satisfying the following conditions for each target object and store them in the ObjectData.

• The CoG of the object must be inside the lanelet.
• The centerline of the lanelet must have two or more points.
• The angle difference between the lanelet and the direction of the object must be within the threshold given by the parameters.
  • The angle flip is allowed, the condition is diff_yaw < threshold or diff_yaw > pi - threshold.
• The lanelet must be reachable from the lanelet recorded in the past history.

Get predicted reference path

• Get reference path:
  • Create a reference path for the object from the associated lanelet.
• Predict object maneuver:
  • Generate predicted paths for the object.
  • Assign probability to each maneuver of Lane Follow, Left Lane Change, and Right Lane Change based on the object history and the reference path obtained in the first step.
  • Lane change decision is based on two domains:
    • Geometric domain: the lateral distance between the center of gravity of the object and left/right boundaries of the lane.
    • Time domain: estimated time margin for the object to reach the left/right bound.

The conditions for left lane change detection are:

• Check if the distance to the left lane boundary is less than the distance to the right lane boundary.
• Check if the distance to the left lane boundary is less than a dist_threshold_to_bound_.
• Check if the lateral velocity direction is towards the left lane boundary.
• Check if the time to reach the left lane boundary is less than time_threshold_to_bound_.

Lane change logics is illustrated in the figure below.An example of how to tune the parameters is described later.

• Calculate object probability:
  • The path probability obtained above is calculated based on the current position and angle of the object.
• Refine predicted paths for smooth movement:
  • The generated predicted paths are recomputed to take the vehicle dynamics into account.
  • The path is calculated with minimum jerk trajectory implemented by 4th/5th order spline for lateral/longitudinal motion.

Tuning lane change detection logic

Currently we provide two parameters to tune lane change detection:

• dist_threshold_to_bound_: maximum distance from lane boundary allowed for lane changing vehicle
• time_threshold_to_bound_: maximum time allowed for lane change vehicle to reach the boundary
• cutoff_freq_of_velocity_lpf_: cutoff frequency of low pass filter for lateral velocity

You can change these parameters in rosparam in the table below.

Tuning threshold parameters

Increasing these two parameters will slow down and stabilize the lane change estimation.

Normally, we recommend tuning only time_threshold_for_lane_change_detection because it is the more important factor for lane change decision.

Tuning lateral velocity calculation

Lateral velocity calculation is also a very important factor for lane change decision because it is used in the time domain decision.

The predicted time to reach the lane boundary is calculated by

where $d_{lat}$ and $v_{lat}$ represent the lateral distance to the lane boundary and the lateral velocity, respectively.

File truncated at 100 lines see the full file