Behavior Path Planner

Purpose / Use cases

The behavior_path_planner module is responsible to generate

1. path based on the traffic situation,
2. drivable area that the vehicle can move (defined in the path msg),
3. turn signal command to be sent to the vehicle interface.

Depending on the situation, a suitable module is selected and executed on the behavior tree system.

The following modules are currently supported:

• Lane Following: Generate lane centerline from map.
• Lane Change: Performs a lane change. This module is performed when it is necessary and a collision check with other vehicles is cleared.
• Obstacle Avoidance: Perform an obstacle avoidance. This module is for avoidance of a vehicle parked on the edge of the lane or overtaking a low-speed obstacle.
• Pull Over: Performs a pull over. This module is performed when goal is in the shoulder lane. Ego-vehicle will stop at the goal.
• Pull Out: Performs a pull out. This module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road.
• Side Shift: (For remote control) Shift the path to left or right according to an external instruction.

[WIP]

• Free Space: xxx.

Design

Inputs / Outputs / API

output

• path [autoware_auto_planning_msgs/PathWithLaneId] : The path generated by modules.
• path_candidate [autoware_auto_planning_msgs/Path] : The path the module is about to take. To be executed as soon as external approval is obtained.
• turn_indicators_cmd [autoware_auto_vehicle_msgs/TurnIndicatorsCommand] : Turn indicators command.
• hazard_lights_cmd [autoware_auto_vehicle_msgs/HazardLightsCommand] : Hazard lights command.
• force_available [tier4_planning_msgs/PathChangeModuleArray] : (For remote control) modules that are force-executable.
• ready_module [tier4_planning_msgs/PathChangeModule] : (For remote control) modules that are ready to be executed.
• running_modules [tier4_planning_msgs/PathChangeModuleArray] : (For remote control) Current running module.

input

• /planning/mission_planning/route [autoware_auto_planning_msgs/HADMapRoute] : Current route from start to goal.
• /map/vector_map [autoware_auto_mapping_msgs/HADMapBin] : Map information.
• /perception/object_recognition/objects [autoware_auto_perception_msgs/PredictedObjects] : dynamic objects from perception module.
• /tf [tf2_msgs/TFMessage] : For ego-pose.
• /localization/kinematic_state [`nav_msgs/Odometry] : For ego-velocity.
• path_change_approval [std_msgs::Bool] : (For remote control)
• path_change_force [tier4_planning_msgs::PathChangeModule] : (For remote control)

Inner-workings / Algorithms

Drivable Area Generation

Drivable lanes are quantized and drawn on an image as a drivable area, whose resolution is drivable_area_resolution. To prevent the quantization from causing instability to the planning modules, drivable area's pose follows the rules below.

• Drivable area is generated in the map coordinate.
• Its position is quantized with drivable_area_resolution.
• Its orientation is 0.

The size of the drivable area changes dynamically to realize both decreasing the computation cost and covering enough lanes to follow. For the second purpose, the drivable area covers a certain length forward and backward lanes with some margins defined by parameters.

Parameters for drivable area generation

Name	Unit	Type	Description	Default value
drivable_area_resolution	[m]	double	resolution of the image of the drivable area	0.1
drivable_lane_forward_length	[m]	double	length of the forward lane from the ego covered by the drivable area	50.0
drivable_lane_backward_length	[m]	double	length of the backward lane from the ego covered by the drivable area	5.0
drivable_lane_margin	[m]	double	forward and backward lane margin from the ego covered by the drivable area	3.0
drivable_area_margin	[m]	double	margin of width and height of the drivable area	6.0

Behavior Tree

In the behavior path planner, the behavior tree mechanism is used to manage which modules are activated in which situations. In general, this "behavior manager" like function is expected to become bigger as more and more modules are added in the future. To improve maintainability, we adopted the behavior tree. The behavior tree has the following advantages: easy visualization, easy configuration management (behaviors can be changed by replacing configuration files), and high scalability compared to the state machine.

The current behavior tree structure is shown below. Each modules (LaneChange, Avoidance, etc) have Request, Ready, and Plan nodes as a common function.

• Request: Check if there is a request from the module (e.g. LaneChange has a request when there are multi-lanes and the vehicle is not on the preferred lane),
• Ready: Check if it is safe to execute the plan (e.g. LaneChange is ready when the lane_change path does not have any conflicts with other dynamic objects on S-T space).
• Plan: Calculates path and set it to the output of the BehaviorTree. Until the internal status returns SUCCESS, it will be in running state and will not transit to another module.
• ForceApproval: A lane change-specific node that overrides the result of Ready when a forced lane change command is given externally.

Lane Following

Generate path from center line of the route.

special case

In the case of a route that requires a lane change, the path is generated with a specific distance margin (default: 12.0 m) from the end of the lane to ensure the minimum distance for lane change. (This function works not only for lane following but also for all modules.)

Lane Change

The Lane Change module is activated when lane change is needed and can be safely executed.

start lane change condition (need to meet all of the conditions below)

• lane change request condition
  • The ego-vehicle isn’t on a preferred_lane.
  • There is neither intersection nor crosswalk on the path of the lane change
• lane change ready condition
  • Path of the lane change doesn’t collide with other objects (see the figure below)
  • Lane change is allowed by an operator

finish lane change condition (need to meet any of the conditions below)

• Certain distance (default: 3.0 m) have passed after the vehicle move to the target lane.
• Before the base_link exceeds white dotted line, a collision with the object was predicted (only if enable_abort_lane_change is true.)
  • However, when current velocity is lower than 10km/h and the ego-vehicle is near the lane end, the lane change isn’t aborted and the ego-vehicle plans to stop. Then, after no collision is predicted, the ego-vehicle resume the lane change.

Collision prediction with obstacles

1. Predict each position of the ego-vehicle and other vehicle on the target lane of the lane change at t1, t2,...tn
2. If a distance between the ego-vehicle and other one is lower than the threshold (ego_velocity * stop_time (2s)) at each time, that is judged as a collision

Path Generation

Path to complete the lane change in n + m seconds under an assumption that a velocity of the ego-vehicle is constant. Once the lane change is executed, the path won’t be updated until the "finish-lane-change-condition" is satisfied.

Avoidance

The Avoidance module is activated when dynamic objects to be avoided exist and can be safely avoided.

Target objects

Dynamic objects that satisfy the following conditions are considered to be avoidance targets.

• Semantics type is CAR, TRUCK, or BUS
• low speed (default: < 1.0 m/s)
• Not being around center line (default: deviation from center > 0.5 m)
• Any footprint of the object in on the detection area (driving lane + 1 m margin for lateral direction).

How to generate avoidance path

To prevent sudden changes in the vicinity of the ego-position, an avoidance path is generated after a certain distance of straight lane driving. The process of generating the avoidance path is as follows:

1. detect the target object and calculate the lateral shift distance (default: 2.0 m from closest footprint point)
2. calculate the avoidance distance within the constraint of lateral jerk. (default: 0.3 ~ 2.0 m/s3)
   1. If the maximum jerk constraint is exceeded to keep the straight margin, the avoidance path generation is aborted.
3. generates the smooth path with given avoiding distance and lateral shift length.
4. generate "return to center" path if there is no next target within a certain distance (default: 50 m) after the current target.

single objects case

multiple objects case

If there are multiple avoidance targets and the lateral distances of these are close (default: < 0.5m), those objects are considered as a single avoidance target and avoidance is performed simultaneously with a single steering operation. If the lateral distances of the avoidance targets differ greatly than threshold, multiple steering operations are used to avoid them.

Smooth path generation

The path generation is computed in Frenet coordinates. The shift length profile for avoidance is generated by four segmental constant jerk polynomials, and added to the original path. Since the lateral jerk can be approximately seen as a steering maneuver, this calculation yields a result similar to a Clothoid curve.

Unimplemented parts / limitations for avoidance

• collision check is not implemented
• shift distance should be variable depending on the situation (left/right is free or occupied). Now it is a fixed value.
• collaboration with "avoidance-by-lane-change".
• specific rules for traffic condition (need to back to the center line before entering an intersection).

Pull Over

The Pull Over module is activated when goal is in the shoulder lane. Ego-vehicle will stop at the goal.

start pull over condition (need to meet all of the conditions below)

• Pull over request condition

  • The goal is in shoulder lane
  • The distance from ego-vehicle to the destination is long enough for pull over
    • The distance required for a pull over is determined by the sum of the straight distance before pull over, the straight distance after pull over, and the minimum distance required for pull over

• Pull over ready condition

  • The end of generated path is located before the end of the road
  • Distance required for pull over of generated path is shorter than distance from ego-pose to goal-pose
  • Generated Path of the pull over doesn’t collide with other objects
  • Pull over is allowed by an operator
    • If pull over path is not allowed by an operator, leave distance required for pull over and stop. This case is defined as UC ID: UC-F-11-00004 at ODD Use Case Slide Pullover draft

finish pull over condition (need to meet any of the conditions below)

• The distance to the goal from your vehicle is lower than threshold (default: < 1m)
• The speed of the vehicle is 0.

Collision prediction with obstacles

1. Predict each position of the ego-vehicle and other vehicle on the target lane of the pull over at t1, t2,...tn
2. If a distance between the ego-vehicle and other one is lower than the threshold (ego_velocity * stop_time (2s)) at each time, that is judged as a collision

Path Generation

The path is generated with a certain margin (default: 0.5 m) from left boundary of shoulder lane. Pull over distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one satisfies ready conditions described above is output.

1. Apply uniform offset to centerline of shoulder lane for ensuring margin
2. In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials)
3. Combine this path with center line of road lane

Parameters for path generation

Name	Unit	Type	Description	Default value
straight_distance	[m]	double	straight distance after pull over.	5.0
minimum_lateral_jerk	[m/s3]	double	minimum lateral jerk to calculate shifting distance.	0.5
maximum_lateral_jerk	[m/s3]	double	maximum lateral jerk to calculate shifting distance.	2.0
pull_over_velocity	[m/s]	double	Accelerate/decelerate to this speed before the start of the pullover.	3.0
margin	[m]	double	distance from ego-vehicle's footprint to the edge of the shoulder lane. This value determines shift length.	0.5

Unimplemented parts / limitations for pull over

• When parking on the shoulder of the road, 3.5m space on the right side should be secured

Pull Out

The Pull Out module is activated when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road.

start pull out condition (need to meet all of the conditions below)

• Pull out request condition

  • The speed of the vehicle is 0.
  • The footprint of ego-vehicle is included in shoulder lane
  • The distance from ego-vehicle to the destination is long enough for pull out (same with pull over)
    • The distance required for a pull out is determined by the sum of the straight distance before pull out, the straight distance after pull out, and the minimum distance required for pull out

• Pull out ready condition

  • The end of generated path is located before the end of the road
  • Distance required for pull out of generated path is shorter than distance from ego-pose to goal-pose
  • Generated Path is safe (This condition is temporarily invalid)
    • It is possible to enable or disable this condition. (default: disabled)
    • If safe path cannot be generated from the current position, safe path from a receding position is searched.
  • Pull out is allowed by an operator

finish pull out condition (need to meet any of the conditions below)

• The distance to the goal of your vehicle is lower than threshold (default: < 1m)
• The speed of the vehicle is 0.

Safe check with obstacles in shoulder lane

1. Calculate ego-vehicle's footprint on pull out path between from current position to merge end point. (Illustrated by blue frame)
2. Calculate object's polygon which is located in shoulder lane
3. If a distance between the footprint and the polygon is lower than the threshold (default: 1.5 m), that is judged as a unsafe path

Path Generation

The path is generated with a certain margin from the left edge of the shoulder lane.

Unimplemented parts / limitations for pull put

• Note that the current module's start judgment condition may incorrectly judge that the vehicle is parked on the shoulder, such as when stopping in front of an intersection.

Side Shift

The role of the Side Shift module is to shift the reference path laterally in response to external instructions (such as remote operation).

Parameters for path generation

In the figure, straight margin distance is to avoid sudden shifting, that is calculated by max(min_distance_to_start_shifting, ego_speed * time_to_start_shifting) . The shifting distance is calculated by jerk, with minimum speed and minimum distance parameter, described below. The minimum speed is used to prevent sharp shift when ego vehicle is stopped.

Name	Unit	Type	Description	Default value
min_distance_to_start_shifting	[m]	double	minimum straight distance before shift start.	5.0
time_to_start_shifting	[s]	double	time of minimum straight distance before shift start.	1.0
shifting_lateral_jerk	[m/s3]	double	lateral jerk to calculate shifting distance.	0.2
min_shifting_distance	[m]	double	the shifting distance is longer than this length.	5.0
min_shifting_speed	[m/s]	double	lateral jerk is calculated with the greater of current_speed or this speed.	5.56

Smooth goal connection

If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of refine_goal_search_radius_range from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future.

Turn signal

Turn on signal when the planned path crosses lanes or when a right or left turn is required. The turn signal information includes the direction of the turn signal and the distance to the point where the turn signal is needed.

From planned path

• turn signal direction
  Calculate the lateral movement distance from the shift start and end point and judges whether the line is crossed or not. If the vehicle straddles the line during lateral movement, the system turns on the blinker.
  
  The timing to start lighting is when the time required to reach the shift start point is 3 seconds or less, or when the distance becomes smaller than threshold (default: 10.0 m).
  The Japanese Road Traffic Law requires turn signal to be turned on 3.0 sec before the vehicle starts moving sideways, but it also gives a condition based on distance because the turn signal may be turned off by slowing down before the shift start point.
• distance
  The distance from the top of ego vehicle to the shift end point.

From map information

• turn signal direction
  Determine right or left turns based on the "turn_direction" information embedded in the lanelet.

• distance
  The distance to the lane where you start to turn.

Conciliation

When multiple turn signal conditions are met, the turn signal with the smaller distance is selected.

References / External links

This module depends on the external BehaviorTreeCpp library.

Future extensions / Unimplemented parts

-

Related issues

-