Lane Change design

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

Lane Change Requirement

• During 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 does not collide with other dynamic objects (see the figure below)
  • Lane change candidate path is approved by an operator.

Generating Lane Change Candidate Path

The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path.

Preparation phase

The preparation trajectory is the candidate path’s first and the straight portion generated along the ego vehicle’s current lane. The length of the preparation trajectory is computed as follows.

lane_change_prepare_distance = current_speed * lane_change_prepare_duration + 0.5 * deceleration * lane_change_prepare_duration^2

During the preparation phase, the turn signal will be activated when the remaining distance is equal to or less than lane_change_search_distance.

Lane-changing phase

The lane-changing phase consist of the shifted path that moves ego from current lane to the target lane. Total distance of lane-changing phase is as follows. Note that during the lane changing phase, the ego vehicle travels at a constant speed.

lane_change_prepare_velocity = std::max(current_speed + deceleration * lane_change_prepare_duration, minimum_lane_changing_velocity)
lane_changing_distance = lane_change_prepare_velocity * lane_changing_duration

The backward_length_buffer_for_end_of_lane is added to allow some window for any possible delay, such as control or mechanical delay during brake lag.

Multiple candidate path samples (longitudinal acceleration)

Lane change velocity is affected by the ego vehicle’s current velocity. High velocity requires longer preparation and lane changing distance. However we also need to plan lane changing trajectories in case ego vehicle slows down. Computing candidate paths that assumes ego vehicle’s slows down is performed by substituting predetermined deceleration value into prepare_length, prepare_velocity and lane_changing_length equation.

The predetermined longitudinal acceleration values are a set of value that starts from longitudinal_acceleration = maximum_longitudinal_acceleration, and decrease by longitudinal_acceleration_resolution until it reaches longitudinal_acceleration = -maximum_longitudinal_deceleration. Both maximum_longitudinal_acceleration and maximum_longitudinal_deceleration are calculated as: defined in the common.param file as normal.min_acc.

maximum_longitudinal_acceleration = min(common_param.max_acc, lane_change_param.max_acc)
maximum_longitudinal_deceleration = max(common_param.min_acc, lane_change_param.min_acc)

where common_param is vehicle common parameter, which defines vehicle common maximum longitudinal acceleration and deceleration. Whereas, lane_change_param has maximum longitudinal acceleration and deceleration for the lane change module. For example, if a user set and common_param.max_acc=1.0 and lane_change_param.max_acc=0.0, maximum_longitudinal_acceleration becomes 0.0, and the lane change does not accelerate in the lane change phase.

The longitudinal_acceleration_resolution is determine by the following

longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_acceleration) / longitudinal_acceleration_sampling_num

Note that when the current_velocity is lower than minimum_lane_changing_velocity, the vehicle needs to accelerate its velocity to minimum_lane_changing_velocity. Therefore, longitudinal acceleration becomes positive value (not decelerate).

The following figure illustrates when longitudinal_acceleration_sampling_num = 4. Assuming that maximum_deceleration = 1.0 then a0 == 0.0 == no deceleration, a1 == 0.25, a2 == 0.5, a3 == 0.75 and a4 == 1.0 == maximum_deceleration. a0 is the expected lane change trajectories should ego vehicle do not decelerate, and a1’s path is the expected lane change trajectories should ego vehicle decelerate at 0.25 m/s^2.

Which path will be chosen will depend on validity and collision check.

Multiple candidate path samples (lateral acceleration)

In addition to sampling longitudinal acceleration, we also sample lane change paths by adjusting the value of lateral acceleration. Since lateral acceleration influences the duration of a lane change, a lower lateral acceleration value results in a longer lane change path, while a higher lateral acceleration value leads to a shorter lane change path. This allows the lane change module to generate a shorter lane change path by increasing the lateral acceleration when there is limited space for the lane change.

The maximum and minimum lateral accelerations are defined in the lane change parameter file as a map. The range of lateral acceleration is determined for each velocity by linearly interpolating the values in the map. Let’s assume we have the following map

In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.25 and 0.4 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values.

Within this range, we sample the lateral acceleration for the ego vehicle. Similar to the method used for sampling longitudinal acceleration, the resolution of lateral acceleration (lateral_acceleration_resolution) is determined by the following:

lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num

Candidate Path’s validity check

A candidate path is valid if the total lane change distance is less than

1. distance to the end of current lane
2. distance to the next intersection
3. distance from current pose to the goal.
4. distance to the crosswalk.

The goal must also be in the list of the preferred lane.

The following flow chart illustrates the validity check.

@startuml
skinparam monochrome true
skinparam defaultTextAlignment center
skinparam noteTextAlignment left

title Selecting Valid Candidate Paths
start
:**INPUT** std::vector<LaneChangePath> input_paths;

partition selectValidPaths {
:**INITIALIZE** std::vector<LaneChangePath> valid_paths;

:idx = 0;

while (idx < input_paths.size()?)

:path = input_paths.at(idx);

partition hasEnoughDistance {

if(lane_change_total_distance < distance to end of current lanes
&&
lane_change_total_distance < distance to the next intersection
&&
lane_change_total_distance < distance from current pose to the goal
&&
lane_change_total_distance < distance to crosswalk
&&
goal is in route
) then (true)
:path_validity  = true;
else (\n false)
:path_validity  = false;
endif
}

if(path_validity == true)then (true)

:valid_paths.push_back(path);

else (\nfalse)
endif
:++idx;
endwhile (false)

:**RETURN** valid_paths;

}
stop
@enduml

Candidate Path’s Safety check

See safety check utils explanation

Objects selection and classification

First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are check_objects_on_current_lanes and check_objects_on_other_lanes.

Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in documentation for avoidance module.

Collision check in prepare phase

The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag enable_collision_check_at_prepare_phase can be enabled to gain this behavior. The following image illustrates the differences between the false and true cases.

The parameter prepare_phase_ignore_target_speed_thresh can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects.

If the lane is blocked and multiple lane changes

When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can’t change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance can be computed from shift length and minimum lane changing velocity.

lane_changing_time = f(shift_length, lat_acceleration, lat_jerk)
minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + lane_change_finish_judge_buffer

The following figure illustrates when the lane is blocked in multiple lane changes cases.

Stopping position when an object exists ahead

When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case.

When the ego vehicle is near the end of the lane change

Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead.

When the ego vehicle is not near the end of the lane change

If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead.

If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead.

When the target lane is far away

When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead.

Lane Change When Stuck

The ego vehicle is considered stuck if it is stopped and meets any of the following conditions:

• There is an obstacle in front of the current lane
• The ego vehicle is at the end of the current lane

In this case, the safety check for lane change is relaxed compared to normal times. Please refer to the ‘stuck’ section under the ‘Collision checks during lane change’ for more details. The function to stop by keeping a margin against forward obstacle in the previous section is being performed to achieve this feature.

Lane change regulations

If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. To regulate lane change on crosswalks or intersections, change regulation.crosswalk or regulation.intersection to true. If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection. If the ego vehicle stops more than stuck_detection.stop_time seconds, it is regarded as a stuck. If the ego vehicle velocity is smaller than stuck_detection.velocity, it is regarded as stopping.

Aborting lane change

The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise.

The following depicts the flow of the abort lane change check.

@startuml
skinparam monochrome true
skinparam defaultTextAlignment center

title Abort Lane Change

while(Lane Following)
  if (Lane Change required) then (**YES**)
    if (Safe to change lane) then (**SAFE**)
      while(Lane Changing)
        if (Lane Change Completed) then (**YES**)
        break
        else (**NO**)
          if (Is Abort Condition Satisfied) then (**NO**)
          else (**YES**)
            if (Is Enough margin to retry lane change) then (**YES**)
              if (Ego is on lane change prepare phase) then (**YES**)
              :Cancel lane change;
              break
              else (**NO**)
              if (Will the overhang to target lane be less than threshold?) then (**YES**)
              :Perform abort maneuver;
              break
              else (NO)
              :Stop or Cruise depending on the situation;
              endif
            endif
          else (**NO**)
          endif
        endif
        endif
      :Stop and wait;
      endwhile
    else (**UNSAFE**)
    endif
  else (**NO**)
  endif
endwhile
-[hidden]->
detach
@enduml

Cancel

Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver.

The function can be enabled by setting enable_on_prepare_phase to true.

The following image illustrates the cancel process.

Abort

Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN’T GUARANTEE a safe abort process, as it didn’t check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both enable_on_prepare_phase and enable_on_lane_changing_phase to true. The parameter max_lateral_jerk need to be set to a high value in order for it to work.

Stop/Cruise

The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN’T GUARANTEE safe maneuver due to the unexpected behavior that might’ve occurred during these critical scenarios. The following images illustrate the situation.

Parameters

Essential lane change parameters

The following parameters are configurable in lane_change.param.yaml.

Lane change regulations

Ego vehicle stuck detection

Collision checks during lane change

The following parameters are configurable in behavior_path_planner.param.yaml and lane_change.param.yaml.

execution

cancel

stuck

(*1) the value must be negative.

Abort lane change

The following parameters are configurable in lane_change.param.yaml.

Debug

The following parameters are configurable in lane_change.param.yaml.

Debug Marker & Visualization

To enable the debug marker, execute (no restart is needed)

ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner lane_change.publish_debug_marker true

or simply set the publish_debug_marker to true in the lane_change.param.yaml for permanent effect (restart is needed).

Then add the marker

/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left

in rviz2.

Available information

1. Ego to object relation, plus safety check information
2. Ego vehicle interpolated pose up to the latest safety check position.
3. Object is safe or not, shown by the color of the polygon (Green = Safe, Red = unsafe)
4. Valid candidate paths.
5. Position when lane changing start and end.