Skip to content

Commit

Permalink
feat(behavior_velocity): add jerk limit for occlusion spot decelerati…
Browse files Browse the repository at this point in the history
…on (autowarefoundation#344)

* fix(behavior_velocity): use general naming

Signed-off-by: taikitanaka3 <taiki.tanaka@tier4.jp>

* feat(behavior_velocity): safe velocity consider jerk limit

Signed-off-by: taikitanaka <ttatcoder@outlook.jp>

* feat(behavior_velocity): apply safe velocity and offset

Signed-off-by: taikitanaka <ttatcoder@outlook.jp>

* feat(behavior_velocity): consider delay response

Signed-off-by: taikitanaka <ttatcoder@outlook.jp>

* feat(behavior_velocity): update gtest of caliculation of velocity

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add margin for debug viz

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): fix stop dist

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): consider safety margin

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc(behavior_velocity): update doc

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): revert lateral distance threshold param

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): use same velocity after passing safe margin point

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc(behavior_velocity): update doc

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): rename collision point to collision point with margin

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): add collision marker

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* ci(pre-commit): autofix

* chore(behavior_velocity): better expression of safe motion

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): fix better naming and change ego min slow down method

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc(behavior_velocity): update doc

* doc(behavior_velocity): update figure

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
3 people authored and satoshi-ota committed May 20, 2022
1 parent 7a81b5a commit 8a44072
Show file tree
Hide file tree
Showing 15 changed files with 271 additions and 190 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -2,20 +2,16 @@
ros__parameters:
occlusion_spot:
pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity
safety_time_buffer: 0.1 # [s] delay for the system response
threshold:
detection_area_length: 100.0 # [m] the length of path to consider perception range
stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop
lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision
# road type parameters
public_road:
min_velocity: 1.5 # [m/s] minimum velocity to ignore occlusion spot
ebs_decel: -2.5 # [m/s^2] maximum deceleration to assume for emergency stops.
pbs_decel: -1.0 # [m/s^2] deceleration to assume for predictive braking stops
private_road:
min_velocity: 1.5 # [m/s] minimum velocity to ignore occlusion spot
ebs_decel: -2.0 # [m/s^2] maximum deceleration to assume for emergency stops.
pbs_decel: -1.0 # [m/s^2] deceleration to assume for predictive braking stops.
motion:
safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety
max_slow_down_acceleration: -1.5 # [m/s^2] minimum deceleration for safe deceleration.
min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed
delay_time: 0.1 # [s] safety time buffer for delay system response
safe_margin: 1.0 # [m] maximum safety distance for any error
sidewalk:
min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
slice_size: 1.5 # [m] size of sidewalk slices in both length and distance relative to the ego trajectory.
Expand Down
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
Expand Up @@ -71,24 +71,22 @@ struct Sidewalk
double slice_size; // [m] size of each slice
double min_occlusion_spot_size; // [m] minumum size to care about the occlusion spot
};

struct VehicleInfo
struct Velocity
{
double vehicle_width; // [m] vehicle_width from parameter server
double baselink_to_front; // [m] wheel_base + front_overhang
};

struct EgoVelocity
{
double ebs_decel; // [m/s^2] emergency braking system deceleration
double pbs_decel; // [m/s^2] predictive braking system deceleration
double min_velocity; // [m/s] minimum allowed velocity not to stop
double safety_ratio; // [-] safety margin for planning error
double max_stop_jerk; // [m/s^3] emergency braking system jerk
double max_stop_accel; // [m/s^2] emergency braking system deceleration
double max_slow_down_accel; // [m/s^2] maximum allowed deceleration
double min_allowed_velocity; // [m/s] minimum allowed velocity not to stop
double a_ego; // [m/s^2] current ego acceleration
double v_ego; // [m/s] current ego velocity
double delay_time; // [s] safety time buffer for delay response
double safe_margin; // [m] maximum safety distance for any error
};

struct PlannerParam
{
// parameters in yaml
double safety_time_buffer; // [s]
double detection_area_length; // [m]
double stuck_vehicle_vel; // [m/s]
double lateral_distance_thr; // [m] lateral distance threshold to consider
Expand All @@ -98,17 +96,27 @@ struct PlannerParam
double angle_thr; // [rad]
bool show_debug_grid; // [-]

VehicleInfo vehicle_info;
EgoVelocity private_road;
EgoVelocity public_road;
// vehicle info
double half_vehicle_width; // [m] half vehicle_width from vehicle info
double baselink_to_front; // [m] wheel_base + front_overhang

Velocity v;
Sidewalk sidewalk;
grid_utils::GridParam grid;
};

struct SafeMotion
{
double stop_dist;
double safe_velocity;
};

struct ObstacleInfo
{
SafeMotion safe_motion; // safe motion of velocity and stop point
geometry_msgs::msg::Point position;
double max_velocity; // [m/s] Maximum velocity of the possible obstacle
double ttc; // [s] time to collision with ego
};

/**
Expand All @@ -123,27 +131,24 @@ struct ObstacleInfo
*/
struct PossibleCollisionInfo
{
ObstacleInfo obstacle_info; // For hidden obstacle
autoware_auto_planning_msgs::msg::PathPoint
collision_path_point; // For baselink at collision point
geometry_msgs::msg::Pose intersection_pose; // For egp path and hidden obstacle
ObstacleInfo obstacle_info; // For hidden obstacle
PathPoint collision_with_margin; // For baselink at collision point
Pose collision_pose; // only use this for debugging
Pose intersection_pose; // For egp path and hidden obstacle
lanelet::ArcCoordinates arc_lane_dist_at_collision; // For ego distance to obstacle in s-d
PossibleCollisionInfo() = default;
PossibleCollisionInfo(
const ObstacleInfo & obstacle_info,
const autoware_auto_planning_msgs::msg::PathPoint & collision_path_point,
const geometry_msgs::msg::Pose & intersection_pose,
const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion)
const ObstacleInfo & obstacle_info, const PathPoint & collision_with_margin,
const Pose & intersection_pose, const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion)
: obstacle_info(obstacle_info),
collision_path_point(collision_path_point),
collision_with_margin(collision_with_margin),
intersection_pose(intersection_pose),
arc_lane_dist_at_collision(arc_lane_dist_to_occlusion)
{
}
};

lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);

// Note : consider offset_from_start_to_ego and safety margin for collision here
inline void handleCollisionOffset(
std::vector<PossibleCollisionInfo> & possible_collisions, double offset, double margin)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,51 +26,69 @@ namespace behavior_velocity_planner
{
namespace occlusion_spot_utils
{
void applySafeVelocityConsideringPossibleCollison(
autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path,
std::vector<PossibleCollisionInfo> & possible_collisions, const double current_vel,
const EgoVelocity & ego, const PlannerParam & param);
void applySafeVelocityConsideringPossibleCollision(
PathWithLaneId * inout_path, std::vector<PossibleCollisionInfo> & possible_collisions,
const PlannerParam & param);

int insertSafeVelocityToPath(
const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param,
autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);
PathWithLaneId * inout_path);

// @brief calculates the maximum velocity allowing to decelerate within the given distance
inline double calculatePredictiveBrakingVelocity(
const double ego_vel, const double dist2col, const double pbs_decel)
inline double calculateMinSlowDownVelocity(
const double v0, const double len, const double a_max, const double safe_vel)
{
return std::sqrt(std::max(std::pow(ego_vel, 2.0) - 2.0 * std::abs(pbs_decel) * dist2col, 0.0));
// if target velocity is inserted backward return current velocity as limit
if (len < 0) return safe_vel;
return std::sqrt(std::max(std::pow(v0, 2.0) - 2.0 * std::abs(a_max) * len, 0.0));
}

/**
* @param: safety_time: safety time buffer for reaction
* @param: dist_to_obj: distance to virtual darting object
* @param: v_obs: relative velocity for virtual darting object
* @param: ebs_decel: emergency brake
* @return safe velocity considering rpb
* @param: sv: ego velocity config
* @param: ttc: time to collision
* @return safe motion
**/
inline double calculateSafeRPBVelocity(
const double safety_time, const double dist_to_obj, const double v_obs, const double ebs_decel)
inline SafeMotion calculateSafeMotion(const Velocity & v, const double ttc)
{
const double t_vir = dist_to_obj / v_obs;
// min safety time buffer is at least more than 0
const double ttc_virtual = std::max(t_vir - safety_time, 0.0);
// safe velocity consider emergency brake
const double v_safe = std::abs(ebs_decel) * ttc_virtual;
return v_safe;
SafeMotion sm;
const double j_max = v.safety_ratio * v.max_stop_jerk;
const double a_max = v.safety_ratio * v.max_stop_accel;
const double t1 = v.delay_time;
double t2 = a_max / j_max;
double & v_safe = sm.safe_velocity;
double & stop_dist = sm.stop_dist;
if (ttc <= t1) {
// delay
v_safe = 0;
stop_dist = 0;
} else if (ttc <= t2 + t1) {
// delay + const jerk
t2 = ttc - t1;
v_safe = -0.5 * j_max * t2 * t2;
stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6;
} else {
const double t3 = ttc - t2 - t1;
// delay + const jerk + const accel
const double v2 = -0.5 * j_max * t2 * t2;
v_safe = v2 - a_max * t3;
stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3;
}
stop_dist += v.safe_margin;
return sm;
}

inline double getPBSLimitedRPBVelocity(
const double pbs_vel, const double rpb_vel, const double min_vel, const double original_vel)
inline double calculateInsertVelocity(
const double min_allowed_vel, const double safe_vel, const double min_vel,
const double original_vel)
{
const double max_vel_noise = 0.05;
// ensure safe velocity doesn't exceed maximum allowed pbs deceleration
double rpb_pbs_limited_vel = std::max(pbs_vel + max_vel_noise, rpb_vel);
double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel);
// ensure safe path velocity is also above ego min velocity
rpb_pbs_limited_vel = std::max(rpb_pbs_limited_vel, min_vel);
cmp_safe_vel = std::max(cmp_safe_vel, min_vel);
// ensure we only lower the original velocity (and do not increase it)
rpb_pbs_limited_vel = std::min(rpb_pbs_limited_vel, original_vel);
return rpb_pbs_limited_vel;
cmp_safe_vel = std::min(cmp_safe_vel, original_vel);
return cmp_safe_vel;
}

} // namespace occlusion_spot_utils
Expand Down
62 changes: 48 additions & 14 deletions planning/behavior_velocity_planner/occlusion-spot-design.md
Original file line number Diff line number Diff line change
Expand Up @@ -53,28 +53,50 @@ Occlusion spot computation: searching occlusion spots for all cells in the occup

Note that the accuracy and performance of this search method is limited due to the approximation.

#### Occlusion Spot Common

##### The Concept of Safe Velocity

Safe velocity is calculated from below parameters of ego emergency braking system and time to collision.

- jerk limit[m/s^3]
- deceleration limit[m/s2]
- delay response time[s]
- time to collision of pedestrian[s]
with these parameters we can briefly define safe motion before occlusion spot for ideal environment.
![occupancy_grid](./docs/occlusion_spot/safe_motion.svg)

##### Safe Behavior After Passing Safe Margin Point

This module defines safe margin consider ego distance to stop and collision path point geometrically.
while ego is cruising from safe margin to collision path point ego keeps same velocity as occlusion spot safe velocity.

![brief](./docs/occlusion_spot/behavior_after_safe_margin.svg)

#### Module Parameters

| Parameter | Type | Description |
| -------------------- | ------ | ------------------------------------------------------------------------- |
| `pedestrian_vel` | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point |
| `safety_time_buffer` | double | [m/s] time buffer for the system delay |
| Parameter | Type | Description |
| ---------------- | ------ | ------------------------------------------------------------------------- |
| `pedestrian_vel` | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point |

| Parameter /threshold | Type | Description |
| ----------------------- | ------ | --------------------------------------------------------- |
| `detection_area_length` | double | [m] the length of path to consider occlusion spot |
| `stuck_vehicle_vel` | double | [m/s] velocity below this value is assumed to stop |
| `lateral_distance` | double | [m] maximum lateral distance to consider hidden collision |

| Parameter /(public or private)\_road | Type | Description |
| ------------------------------------ | ------ | -------------------------------------------------------------------- |
| `min_velocity` | double | [m/s] minimum velocity to ignore occlusion spot |
| `ebs_decel` | double | [m/s^2] maximum deceleration to assume for emergency braking system. |
| `pbs_decel` | double | [m/s^2] deceleration to assume for predictive braking system |
| Parameter /motion | Type | Description |
| --------------------- | ------ | ------------------------------------------------------------ |
| `safety_ratio` | double | [-] safety ratio for jerk and acceleration |
| `max_slow_down_accel` | double | [m/s^2] deceleration to assume for predictive braking system |
| `v_min` | double | [m/s] minimum velocity not to stop |
| `delay_time` | double | [m/s] time buffer for the system delay |
| `safe_margin` | double | [m] maximum error to stop with emergency braking system. |

| Parameter /sidewalk | Type | Description |
| ------------------------- | ------ | --------------------------------------------------------------- |
| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot |
| `slice_size` | double | [m] the distance of divided detection area |
| `focus_range` | double | [m] buffer around the ego path used to build the sidewalk area. |

| Parameter /grid | Type | Description |
Expand Down Expand Up @@ -121,8 +143,12 @@ note right
- occlusion spot is calculated by longitudinally closest point of unknown cells.
- intersection point is where ego front bumper and darting object will crash.
- collision path point is calculated by arc coordinate consider ego vehicle's geometry.
- safe velocity and safe margin is calculated from performance of ego emergency braking system.
end note
:calculate safe velocity and safe margin for possible collision;
note right
- safe velocity and safe margin is calculated from performance of ego emergency braking system.
end note
}
partition process_possible_collision {
:filter possible collision by road type;
Expand All @@ -137,10 +163,10 @@ end note
note right
consider offset from path start to ego vehicle for possible collision
end note
:apply safe velocity consider possible collision;
:apply safe velocity comparing with allowed velocity;
note right
calculated by
- ebs deceleration [m/s] emergency braking system consider lateral distance to the occlusion spot.
- safe velocity calculated from emergency brake performance.
- maximum allowed deceleration [m/s^2]
- min velocity [m/s] the velocity that is allowed on the road.
- original_velocity [m/s]
Expand Down Expand Up @@ -186,12 +212,16 @@ note right
- intersection point is where ego front bumper and darting object will crash.
- collision path point is calculated by arc coordinate consider ego vehicle's geometry.
end note
:calculate safe velocity and safe margin for possible collision;
note right
- safe velocity and safe margin is calculated from performance of ego emergency braking system.
end note
}
partition process_possible_collision {
:filter collision by road type;
:calculate slow down points for possible collision;
:handle collision offset;
:calculate safe velocity consider lateral distance and safe velocity;
:apply safe velocity comparing with allowed velocity;
:insert safe velocity to path;
}
stop
Expand Down Expand Up @@ -236,12 +266,16 @@ note right
- consider occlusion which is nearer than `lateral_distance_threshold`.
end note
:calculate collision path point and intersection point;
:calculate safe velocity and safe margin for possible collision;
note right
- safe velocity and safe margin is calculated from performance of ego emergency braking system.
end note
}
partition handle_possible_collision {
:filter collision by road type;
:calculate slow down points for possible collision;
:handle collision offset;
:calculate safe velocity consider lateral distance and safe velocity;
:apply safe velocity comparing with allowed velocity;
:insert safe velocity to path;
}
stop
Expand Down
Loading

0 comments on commit 8a44072

Please sign in to comment.