Skip to content

Commit

Permalink
Update documentation
Browse files Browse the repository at this point in the history
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 committed Feb 15, 2024
1 parent 97b76cc commit dcde099
Showing 1 changed file with 24 additions and 0 deletions.
24 changes: 24 additions & 0 deletions planning/behavior_path_lane_change_module/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,29 @@ detach
@enduml
```

To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions.

```plantuml
@startuml
skinparam defaultTextAlignment center
skinparam backgroundColor #WHITE
title Abort Lane Change
if (Perform collision check?) then (<color:green><b>SAFE</b></color>)
:Reset unsafe_hysteresis_count_;
else (<color:red><b>UNSAFE</b></color>)
:Increase unsafe_hysteresis_count_;
if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (<color:green><b>SAFE</b></color>)
else (<color:red><b>UNSAFE</b></color>)
#LightPink:Check abort condition;
stop
endif
endif
:Continue lane changing;
@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.
Expand Down Expand Up @@ -430,6 +453,7 @@ The following parameters are configurable in `lane_change.param.yaml`.
| `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 |
| `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 |
| `cancel.overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 |
| `unsafe_hysteresis_threshold` | [-] | int | threshold that helps prevent frequent switching between safe and unsafe decisions | 10 |

### Debug

Expand Down

0 comments on commit dcde099

Please sign in to comment.