diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index f73520e8d7c40..509f6f1054e27 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -164,18 +164,17 @@ # lateral constraints lateral: - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] + nominal_lateral_jerk: 0.2 # [m/sss] + max_lateral_jerk: 1.0 # [m/sss] + max_lateral_acceleration: 0.5 # [m/ss] # longitudinal constraints longitudinal: nominal_deceleration: -1.0 # [m/ss] nominal_jerk: 0.5 # [m/sss] max_deceleration: -2.0 # [m/ss] - max_jerk: 1.0 - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] + max_jerk: 1.0 # [m/sss] + max_acceleration: 1.0 # [m/ss] target_velocity_matrix: col_size: 2 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 33ff0db7cc150..8c61e78a382af 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -717,23 +717,23 @@ namespace: `avoidance.constraints.` namespace: `avoidance.constraints.lateral.` -| a Name | Unit | Type | Description | Default value | -| :------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------- | :------------ | -| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | -| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | -| nominal_lateral_jerk | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego. | 0.2 | -| max_lateral_jerk | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego. | 1.0 | +| a Name | Unit | Type | Description | Default value | +| :----------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------- | :------------ | +| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | +| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | +| nominal_lateral_jerk | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego. | 0.2 | +| max_lateral_jerk | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego. | 1.0 | +| max_lateral_acceleration | [m/s3] | double | Avoidance path gets sharp up to this accel limit when there is not enough distance from ego. | 0.5 | namespace: `avoidance.constraints.longitudinal.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :------ | :----- | :-------------------------------------------------------------------------- | :------------ | -| nominal_deceleration | [m/ss] | double | Nominal deceleration limit. | -1.0 | -| nominal_jerk | [m/sss] | double | Nominal jerk limit. | 0.5 | -| max_deceleration | [m/ss] | double | Max decelerate limit. | -2.0 | -| max_jerk | [m/sss] | double | Max jerk limit. | 1.0 | -| min_avoidance_speed_for_acc_prevention | [m] | double | Minimum speed limit to be applied to prevent acceleration during avoidance. | 3.0 | -| max_avoidance_acceleration | [m/ss] | double | Maximum acceleration during avoidance. | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------- | :------ | :----- | :------------------------------------- | :------------ | +| nominal_deceleration | [m/ss] | double | Nominal deceleration limit. | -1.0 | +| nominal_jerk | [m/sss] | double | Nominal jerk limit. | 0.5 | +| max_deceleration | [m/ss] | double | Max decelerate limit. | -2.0 | +| max_jerk | [m/sss] | double | Max jerk limit. | 1.0 | +| max_acceleration | [m/ss] | double | Maximum acceleration during avoidance. | 1.0 | (\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 63166b0cbf7df..c9a00a7f57fc3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -123,6 +123,9 @@ struct AvoidanceParameters // comfortable jerk double nominal_jerk; + // To prevent large acceleration while avoidance. + double max_acceleration; + // upper distance for envelope polygon expansion. double upper_distance_for_polygon_expansion; @@ -245,19 +248,14 @@ struct AvoidanceParameters // if the avoidance path exceeds this lateral jerk, it will be not used anymore. double max_lateral_jerk; + // To prevent large acceleration while avoidance. + double max_lateral_acceleration; + // For the compensation of the detection lost. Once an object is observed, it is registered and // will be used for planning from the next time. If the object is not observed, it counts up the // lost_count and the registered object will be removed when the count exceeds this max count. double object_last_seen_threshold; - // For velocity planning to avoid acceleration during avoidance. - // Speeds smaller than this are not inserted. - double min_avoidance_speed_for_acc_prevention; - - // To prevent large acceleration while avoidance. The max velocity is limited with this - // acceleration. - double max_avoidance_acceleration; - // The avoidance path generation is performed when the shift distance of the // avoidance points is greater than this threshold. // In multiple targets case: if there are multiple vehicles in a row to be avoided, no new diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 58ef8f5fa5b3e..b8e8379312c5f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2717,8 +2717,9 @@ void AvoidanceModule::addNewShiftLines( } const auto current_shift_lines = path_shifter.getShiftLines(); - const auto new_shift_length = new_shift_lines.front().end_shift_length; - const auto new_shift_end_idx = new_shift_lines.front().end_idx; + const auto front_new_shift_line = new_shift_lines.front(); + const auto new_shift_length = front_new_shift_line.end_shift_length; + const auto new_shift_end_idx = front_new_shift_line.end_idx; DEBUG_PRINT("min_start_idx = %lu", min_start_idx); @@ -2756,7 +2757,19 @@ void AvoidanceModule::addNewShiftLines( future.push_back(sl); } + const double road_velocity = + avoidance_data_.reference_path.points.at(front_new_shift_line.start_idx) + .point.longitudinal_velocity_mps; + const double shift_time = PathShifter::calcShiftTimeFromJerk( + front_new_shift_line.getRelativeLength(), parameters_->max_lateral_jerk, + parameters_->max_lateral_acceleration); + const double longitudinal_acc = + std::clamp(road_velocity / shift_time, 0.0, parameters_->max_acceleration); + path_shifter.setShiftLines(future); + path_shifter.setVelocity(getEgoSpeed()); + path_shifter.setLongitudinalAcceleration(longitudinal_acc); + path_shifter.setLateralAccelerationLimit(parameters_->max_lateral_acceleration); } AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidates) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 9b7a8b68b969d..61b59ba75c8e6 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -195,9 +195,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.nominal_jerk = get_parameter(node, ns + "nominal_jerk"); p.max_deceleration = get_parameter(node, ns + "max_deceleration"); p.max_jerk = get_parameter(node, ns + "max_jerk"); - p.min_avoidance_speed_for_acc_prevention = - get_parameter(node, ns + "min_avoidance_speed_for_acc_prevention"); - p.max_avoidance_acceleration = get_parameter(node, ns + "max_avoidance_acceleration"); + p.max_acceleration = get_parameter(node, ns + "max_acceleration"); } // constraints (lateral) @@ -205,6 +203,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.constraints.lateral."; p.nominal_lateral_jerk = get_parameter(node, ns + "nominal_lateral_jerk"); p.max_lateral_jerk = get_parameter(node, ns + "max_lateral_jerk"); + p.max_lateral_acceleration = get_parameter(node, ns + "max_lateral_acceleration"); } // velocity matrix @@ -307,6 +306,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "nominal_lateral_jerk", p->nominal_lateral_jerk); updateParam(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk); + updateParam(parameters, ns + "max_lateral_acceleration", p->max_lateral_acceleration); } {