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

Name	Type	Description
~/input/trajectory	autoware_auto_planning_msgs::Trajectory	input trajectory
~/input/objects	autoware_auto_perception_msgs::PredictedObjects	dynamic objects
~/input/odometry	nav_msgs::msg::Odometry	ego odometry

Output topics

Name	Type	Description
~/output/trajectory	autoware_auto_planning_msgs::Trajectory	output trajectory
~/output/velocity_limit	tier4_planning_msgs::VelocityLimit	velocity limit for cruising
~/output/clear_velocity_limit	tier4_planning_msgs::VelocityLimitClearCommand	clear command for velocity limit
~/output/stop_reasons	tier4_planning_msgs::StopReasonArray	reasons that make the vehicle to stop

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.

Parameter	Type	Description
common.cruise_obstacle_type.inside.unknown	bool	flag to consider unknown objects for cruising
common.cruise_obstacle_type.inside.car	bool	flag to consider unknown objects for cruising
common.cruise_obstacle_type.inside.truck	bool	flag to consider unknown objects for cruising
...	bool	...
common.cruise_obstacle_type.outside.unknown	bool	flag to consider unknown objects for cruising
common.cruise_obstacle_type.outside.car	bool	flag to consider unknown objects for cruising
common.cruise_obstacle_type.outside.truck	bool	flag to consider unknown objects for cruising
...	bool	...
behavior_determination.cruise.max_lat_margin	double	maximum lateral margin for cruise obstacles
behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop	double	maximum obstacle velocity for cruise obstacle inside the trajectory
behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold	double	maximum obstacle velocity for cruise obstacle outside the trajectory
behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold	double	maximum overlap time of the collision between the ego and obstacle

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).

Parameter	Type	Description
common.stop_obstacle_type.unknown	bool	flag to consider unknown objects for stopping
common.stop_obstacle_type.car	bool	flag to consider unknown objects for stopping
common.stop_obstacle_type.truck	bool	flag to consider unknown objects for stopping
...	bool	...
behavior_determination.stop.max_lat_margin	double	maximum lateral margin for stop obstacles
behavior_determination.crossing_obstacle.obstacle_velocity_threshold	double	maximum crossing obstacle velocity to ignore
behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise	double	maximum obstacle velocity for stop

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.

Parameter	Type	Description
common.slow_down_obstacle_type.unknown	bool	flag to consider unknown objects for slowing down
common.slow_down_obstacle_type.car	bool	flag to consider unknown objects for slowing down
common.slow_down_obstacle_type.truck	bool	flag to consider unknown objects for slowing down
...	bool	...
behavior_determination.slow_down.max_lat_margin	double	maximum lateral margin for slow down obstacles

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.

Parameter	Type	Description
behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold	double	maximum angle against the ego's trajectory to judge the obstacle is crossing the trajectory [rad]

*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.

Parameter	Type	Description
behavior_determination.stop.crossing_obstacle.collision_time_margin	double	maximum collision time margin of the ego and obstacle

Stop planning

Parameter	Type	Description
common.min_strong_accel	double	ego's minimum acceleration to stop [m/ss]
common.safe_distance_margin	double	distance with obstacles for stop [m]
common.terminal_safe_distance_margin	double	terminal_distance with obstacles for stop, which cannot be exceed safe distance margin [m]

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

Parameter	Type	Description
common.safe_distance_margin	double	minimum distance with obstacles for cruise [m]

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.

\[ d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, \]

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.

Parameter	Type	Description
common.idling_time	double	idling time for the ego to detect the front vehicle starting deceleration [s]
common.min_ego_accel_for_rss	double	ego's acceleration for RSS [m/ss]
common.min_object_accel_for_rss	double	front obstacle's acceleration for RSS [m/ss]

The detailed formulation is as follows.

\[ \begin{align} d_{error} & = d - d_{rss} \\ d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\ d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ v_{pid} & = pid(d_{quad, normalized}) \\ v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise}) \end{align} \]

Variable	Description
d	actual distance to obstacle
d_{rss}	ideal distance to obstacle based on RSS
v_{min, cruise}	min_cruise_target_vel
w_{acc}	output_ratio_during_accel
lpf(val)	apply low-pass filter to val
pid(val)	apply pid to val

Slow down planning

Parameter	Type	Description
slow_down.min_lat_velocity	double	minimum velocity to linearly calculate slow down velocity [m]
slow_down.max_lat_velocity	double	maximum velocity to linearly calculate slow down velocity [m]
slow_down.min_lat_margin	double	minimum lateral margin to linearly calculate slow down velocity [m]
slow_down.max_lat_margin	double	maximum lateral margin to linearly calculate slow down velocity [m]

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 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.

Variable	Description
v_{out}	calculated velocity for slow down
v_{min}	slow_down.min_lat_velocity
v_{max}	slow_down.max_lat_velocity
l_{min}	slow_down.min_lat_margin
l_{max}	slow_down.max_lat_margin
l'_{max}	behavior_determination.slow_down.max_lat_margin

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.

Algorithm selection for cruise planner

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

Parameter	Type	Description
common.planning_method	string	cruise and stop planning algorithm, selected from "pid_base"

PID-based planner

Stop planning

In the pid_based_planner namespace,

Parameter	Type	Description
obstacle_velocity_threshold_from_cruise_to_stop	double	obstacle velocity threshold to be stopped from cruised [m/s]

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,

Parameter	Type	Description
kp	double	p gain for pid control [-]
ki	double	i gain for pid control [-]
kd	double	d gain for pid control [-]
output_ratio_during_accel	double	The output velocity will be multiplied by the ratio during acceleration to follow the front vehicle. [-]
vel_to_acc_weight	double	target acceleration is target velocity * vel_to_acc_weight [-]
min_cruise_target_vel	double	minimum target velocity during cruise [m/s]

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.

Parameter	Type	Description
common.min_behavior_stop_margin	double	minimum stop margin when stopping with the behavior module enabled [m]

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.

Parameter	Type	Description
behavior_determination.stop_obstacle_hold_time_threshold	double	maximum time for holding closest stop obstacle [s]

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.