Obstacle Cruise Planner

Overview

The obstacle_cruise_planner package has following modules.

• Stop planning
  • stop when there is a static obstacle near the trajectory.
• Cruise planning
  • cruise a dynamic obstacle in front of the ego.
• Slow down planning
  • slow down when there is a static/dynamic obstacle near the trajectory.

Interfaces

Input topics

Output topics

Design

Design for the following functions is defined here.

• Behavior determination against obstacles
• Stop planning
• Cruise planning
• Slow down planning

A data structure for cruise and stop planning is as follows. This planner data is created first, and then sent to the planning algorithm.

struct PlannerData
{
  rclcpp::Time current_time;
  autoware_auto_planning_msgs::msg::Trajectory traj;
  geometry_msgs::msg::Pose current_pose;
  double ego_vel;
  double current_acc;
  std::vector<Obstacle> target_obstacles;
};

struct Obstacle
{
  rclcpp::Time stamp;  // This is not the current stamp, but when the object was observed.
  geometry_msgs::msg::Pose pose;  // interpolated with the current stamp
  bool orientation_reliable;
  Twist twist;
  bool twist_reliable;
  ObjectClassification classification;
  std::string uuid;
  Shape shape;
  std::vector<PredictedPath> predicted_paths;
};

Behavior determination against obstacles

Obstacles for cruising, stopping and slowing down are selected in this order based on their pose and velocity. The obstacles not in front of the ego will be ignored.

Determine cruise vehicles

The obstacles meeting the following condition are determined as obstacles for cruising.

• The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.cruise.max_lat_margin.

• The object type is for cruising according to common.cruise_obstacle_type.*.
• The object is not crossing the ego’s trajectory (*1).
• If the object is inside the trajectory.
  • The object type is for inside cruising according to common.cruise_obstacle_type.inside.*.
  • The object velocity is larger than behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop.
• If the object is outside the trajectory.
  • The object type is for outside cruising according to common.cruise_obstacle_type.outside.*.
  • The object velocity is larger than behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold.
  • The highest confident predicted path collides with the ego’s trajectory.
  • Its collision’s period is larger than behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold.

Determine stop vehicles

Among obstacles which are not for cruising, the obstacles meeting the following condition are determined as obstacles for stopping.

• The object type is for stopping according to common.stop_obstacle_type.*.
• The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.stop.max_lat_margin.
• The object velocity along the ego’s trajectory is smaller than behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise.
• The object
  • does not cross the ego’s trajectory (*1)
  • with the velocity smaller than behavior_determination.crossing_obstacle.obstacle_velocity_threshold
  • and its collision time margin is large enough (*2).

Determine slow down vehicles

Among obstacles which are not for cruising and stopping, the obstacles meeting the following condition are determined as obstacles for slowing down.

• The object type is for slowing down according to common.slow_down_obstacle_type.*.
• The lateral distance from the object to the ego’s trajectory is smaller than behavior_determination.slow_down.max_lat_margin.

NOTE

*1: Crossing obstacles

Crossing obstacle is the object whose orientation’s yaw angle against the ego’s trajectory is smaller than behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold.

*2: Enough collision time margin

We predict the collision area and its time by the ego with a constant velocity motion and the obstacle with its predicted path. Then, we calculate a collision time margin which is the difference of the time when the ego will be inside the collision area and the obstacle will be inside the collision area. When this time margin is smaller than behavior_determination.stop.crossing_obstacle.collision_time_margin, the margin is not enough.

Stop planning

The role of the stop planning is keeping a safe distance with static vehicle objects or dynamic/static non vehicle objects.

The stop planning just inserts the stop point in the trajectory to keep a distance with obstacles. The safe distance is parameterized as common.safe_distance_margin. When it stops at the end of the trajectory, and obstacle is on the same point, the safe distance becomes terminal_safe_distance_margin.

When inserting the stop point, the required acceleration for the ego to stop in front of the stop point is calculated. If the acceleration is less than common.min_strong_accel, the stop planning will be cancelled since this package does not assume a strong sudden brake for emergency.

Cruise planning

The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition. This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle.

The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.

assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle’s deceleration, $v_{ego}$ is the ego’s current velocity, $v_{obstacle}$ is the front obstacle’s current velocity, $a_{ego}$ is the ego’s acceleration, and $a_{obstacle}$ is the obstacle’s acceleration. These values are parameterized as follows. Other common values such as ego’s minimum acceleration is defined in common.param.yaml.

The detailed formulation is as follows.

Slow down planning

The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see slow_down.labels), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. Each obstacle type has a static and a moving parameter set, so it is possible to customize the slow down response of the ego vehicle according to the obstacle type and if it is moving or not. If an obstacle is determined to be moving, the corresponding moving set of parameters will be used to compute the vehicle slow down, otherwise, the static parameters will be used. The static and moving separation is useful for customizing the ego vehicle slow down behavior to, for example, slow down more significantly when passing stopped vehicles that might cause occlusion or that might suddenly open its doors.

An obstacle is classified as static if its total speed is less than the moving_object_speed_threshold parameter. Furthermore, a hysteresis based approach is used to avoid chattering, it uses the moving_object_hysteresis_range parameter range and the obstacle’s previous state (moving or static) to determine if the obstacle is moving or not. In other words, if an obstacle was previously classified as static, it will not change its classification to moving unless its total speed is greater than moving_object_speed_threshold + moving_object_hysteresis_range. Likewise, an obstacle previously classified as moving, will only change to static if its speed is lower than moving_object_speed_threshold - moving_object_hysteresis_range.

The closest point on the obstacle to the ego’s trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows.

The calculated velocity is inserted in the trajectory where the obstacle is inside the area with behavior_determination.slow_down.max_lat_margin.

Implementation

Flowchart

Successive functions consist of obstacle_cruise_planner as follows.

Various algorithms for stop and cruise planning will be implemented, and one of them is designated depending on the use cases. The core algorithm implementation generateTrajectory depends on the designated algorithm.

@startuml
title onTrajectory
start

group convertToObstacles
  :check obstacle's label;
  :check obstacle is in front of ego;
  :check obstacle's lateral deviation to trajectory;
  :create obstacle instance;
end group

group determineEgoBehaviorAgainstObstacles
  :resampleExtendedTrajectory;
  group for each obstacle
    :createCruiseObstacle;
    :createStopObstacle;
    :createSlowDownObstacle;
  end group
  :update previous obstacles;
end group

:createPlannerData;

:generateStopTrajectory;

:generateCruiseTrajectory;

:generateSlowDownTrajectory;

:publish trajectory;

:publishDebugData;

:publish and print calculation time;

stop
@enduml

Algorithm selection for cruise planner

Currently, only a PID-based planner is supported. Each planner will be explained in the following.

PID-based planner

Stop planning

In the pid_based_planner namespace,

Only one obstacle is targeted for the stop planning. It is the obstacle among obstacle candidates whose velocity is less than obstacle_velocity_threshold_from_cruise_to_stop, and which is the nearest to the ego along the trajectory. A stop point is inserted keepingcommon.safe_distance_margin distance between the ego and obstacle.

Note that, as explained in the stop planning design, a stop planning which requires a strong acceleration (less than common.min_strong_accel) will be canceled.

Cruise planning

In the pid_based_planner namespace,

In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (motion_velocity_smoother by default). The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance.

Optimization-based planner

under construction

Minor functions

Prioritization of behavior module’s stop point

When stopping for a pedestrian walking on the crosswalk, the behavior module inserts the zero velocity in the trajectory in front of the crosswalk. Also obstacle_cruise_planner’s stop planning also works, and the ego may not reach the behavior module’s stop point since the safe distance defined in obstacle_cruise_planner may be longer than the behavior module’s safe distance. To resolve this non-alignment of the stop point between the behavior module and obstacle_cruise_planner, common.min_behavior_stop_margin is defined. In the case of the crosswalk described above, obstacle_cruise_planner inserts the stop point with a distance common.min_behavior_stop_margin at minimum between the ego and obstacle.

A function to keep the closest stop obstacle in target obstacles

In order to keep the closest stop obstacle in the target obstacles, we check whether it is disappeared or not from the target obstacles in the checkConsistency function. If the previous closest stop obstacle is remove from the lists, we keep it in the lists for stop_obstacle_hold_time_threshold seconds. Note that if a new stop obstacle appears and the previous closest obstacle removes from the lists, we do not add it to the target obstacles again.

How To Debug

How to debug can be seen here.

Known Limits

• Common
  • When the obstacle pose or velocity estimation has a delay, the ego sometimes will go close to the front vehicle keeping deceleration.
  • Current implementation only uses predicted objects message for static/dynamic obstacles and does not use pointcloud. Therefore, if object recognition is lost, the ego cannot deal with the lost obstacle.
  • The current predicted paths for obstacle’s lane change does not have enough precision for obstacle_cruise_planner. Therefore, we set rough_detection_area a small value.
• PID-based planner
  • The algorithm strongly depends on the velocity smoothing package (motion_velocity_smoother by default) whether or not the ego realizes the designated target speed. If the velocity smoothing package is updated, please take care of the vehicle’s behavior as much as possible.