Skip to content

Commit

Permalink
feat(avoidance): consider acceleration during avoidance maneuver (#4186)
Browse files Browse the repository at this point in the history 
* feat(avoidance): use improved path shifting logic

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(avoidance): remove function to keep current speed during avoidance maneuver

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

---------

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota authored Jul 12, 2023
1 parent 97b9d56 commit b12feef
Show file tree
Hide file tree
Showing 6 changed files with 43 additions and 117 deletions.
11 changes: 5 additions & 6 deletions planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,18 +165,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
Expand Down
28 changes: 14 additions & 14 deletions planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand Down
8 changes: 0 additions & 8 deletions ...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -475,12 +475,6 @@ class AvoidanceModule : public SceneModuleInterface
// than two lanes since which way to avoid is not obvious
void generateExtendedDrivableArea(BehaviorModuleOutput & output) const;

/**
* @brief insert slow down point to prevent acceleration in avoidance maneuver.
* @param avoidance path.
*/
void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path);

/**
* @brief fill debug markers.
*/
Expand Down Expand Up @@ -621,8 +615,6 @@ class AvoidanceModule : public SceneModuleInterface

std::shared_ptr<AvoidanceParameters> parameters_;

std::shared_ptr<double> ego_velocity_starting_avoidance_ptr_;

helper::avoidance::AvoidanceHelper helper_;

AvoidancePlanningData avoidance_data_;
Expand Down
14 changes: 6 additions & 8 deletions ...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,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;

Expand Down Expand Up @@ -248,19 +251,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
Expand Down
93 changes: 15 additions & 78 deletions planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2374,79 +2374,6 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output
}
}

void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & path)
{
const auto ego_idx = avoidance_data_.ego_closest_path_index;
const auto N = path.shift_length.size();

if (!ego_velocity_starting_avoidance_ptr_) {
ego_velocity_starting_avoidance_ptr_ = std::make_shared<double>(getEgoSpeed());
}

// find first shift-change point from ego
constexpr auto SHIFT_DIFF_THR = 0.1;
size_t target_idx = N;
const auto current_shift = path.shift_length.at(ego_idx);
for (size_t i = ego_idx + 1; i < N; ++i) {
if (std::abs(path.shift_length.at(i) - current_shift) > SHIFT_DIFF_THR) {
// this index do not have to be accurate, so it can be i or i + 1.
// but if the ego point is already on the shift-change point, ego index should be a target_idx
// so that the distance for acceleration will be 0 and the ego speed is directly applied
// to the path velocity (no acceleration while avoidance)
target_idx = i - 1;
break;
}
}
if (target_idx == N) {
DEBUG_PRINT("shift length has no changes. No velocity limit is applied.");
return;
}

constexpr auto NO_ACCEL_TIME_THR = 3.0;

// update ego velocity if the shift point is far
const auto s_from_ego = avoidance_data_.arclength_from_ego.at(target_idx) -
avoidance_data_.arclength_from_ego.at(ego_idx);
const auto t_from_ego = s_from_ego / std::max(getEgoSpeed(), 1.0);
if (t_from_ego > NO_ACCEL_TIME_THR) {
*ego_velocity_starting_avoidance_ptr_ = getEgoSpeed();
}

// update ego velocity if the ego is faster than saved velocity.
if (*ego_velocity_starting_avoidance_ptr_ < getEgoSpeed()) {
*ego_velocity_starting_avoidance_ptr_ = getEgoSpeed();
}

// calc index and velocity to NO_ACCEL_TIME_THR
const auto v0 = *ego_velocity_starting_avoidance_ptr_;
auto vmax = 0.0;
size_t insert_idx = ego_idx;
for (size_t i = ego_idx; i <= target_idx; ++i) {
const auto s =
avoidance_data_.arclength_from_ego.at(target_idx) - avoidance_data_.arclength_from_ego.at(i);
const auto t = s / std::max(v0, 1.0);
if (t < NO_ACCEL_TIME_THR) {
insert_idx = i;
vmax = std::max(
parameters_->min_avoidance_speed_for_acc_prevention,
std::sqrt(v0 * v0 + 2.0 * s * parameters_->max_avoidance_acceleration));
break;
}
}

// apply velocity limit
constexpr size_t V_LIM_APPLY_IDX_MARGIN = 0;
for (size_t i = insert_idx + V_LIM_APPLY_IDX_MARGIN; i < std::min(path.path.points.size(), N);
++i) {
path.path.points.at(i).point.longitudinal_velocity_mps =
std::min(path.path.points.at(i).point.longitudinal_velocity_mps, static_cast<float>(vmax));
}

DEBUG_PRINT(
"s: %f, t: %f, v0: %f, a: %f, vmax: %f, ego_i: %lu, target_i: %lu", s_from_ego, t_from_ego, v0,
parameters_->max_avoidance_acceleration, vmax, ego_idx, target_idx);
}

PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const
{
// special for avoidance: take behind distance upt ot shift-start-point if it exist.
Expand Down Expand Up @@ -2535,9 +2462,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
auto avoidance_path = generateAvoidancePath(path_shifter_);
debug_data_.output_shift = avoidance_path.shift_length;

// modify max speed to prevent acceleration in avoidance maneuver.
modifyPathVelocityToPreventAccelerationOnAvoidance(avoidance_path);

// post processing
{
postProcess(); // remove old shift points
Expand Down Expand Up @@ -2720,8 +2644,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);

Expand Down Expand Up @@ -2759,7 +2684,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
Expand Down
6 changes: 3 additions & 3 deletions planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,16 +196,15 @@ AvoidanceModuleManager::AvoidanceModuleManager(
p.nominal_jerk = get_parameter<double>(node, ns + "nominal_jerk");
p.max_deceleration = get_parameter<double>(node, ns + "max_deceleration");
p.max_jerk = get_parameter<double>(node, ns + "max_jerk");
p.min_avoidance_speed_for_acc_prevention =
get_parameter<double>(node, ns + "min_avoidance_speed_for_acc_prevention");
p.max_avoidance_acceleration = get_parameter<double>(node, ns + "max_avoidance_acceleration");
p.max_acceleration = get_parameter<double>(node, ns + "max_acceleration");
}

// constraints (lateral)
{
std::string ns = "avoidance.constraints.lateral.";
p.nominal_lateral_jerk = get_parameter<double>(node, ns + "nominal_lateral_jerk");
p.max_lateral_jerk = get_parameter<double>(node, ns + "max_lateral_jerk");
p.max_lateral_acceleration = get_parameter<double>(node, ns + "max_lateral_acceleration");
}

// velocity matrix
Expand Down Expand Up @@ -308,6 +307,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
const std::string ns = "avoidance.constrains.lateral.";
updateParam<double>(parameters, ns + "nominal_lateral_jerk", p->nominal_lateral_jerk);
updateParam<double>(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk);
updateParam<double>(parameters, ns + "max_lateral_acceleration", p->max_lateral_acceleration);
}

{
Expand Down

0 comments on commit b12feef

Please sign in to comment.