Skip to content

Commit

Permalink
docs(obstacle_stop_planner): update module documentation
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Aug 29, 2022
1 parent 7fbeb1c commit 3e329f3
Show file tree
Hide file tree
Showing 9 changed files with 66 additions and 542 deletions.
109 changes: 57 additions & 52 deletions planning/obstacle_stop_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,21 +11,6 @@
- Adaptive Cruise Controller (ACC)
- embeds target velocity in trajectory when there is a dynamic point cloud on the trajectory.

In order to stop with a `stop margin` from the obstacle exists, the stop point (`v=0`) is inserted at a distance of `baselink to front` + `stop margin` from the obstacle. The `baselink to front` means the distance between `base_link`(center of rear-wheel axis) and front of the car.

If a stop point has already been inserted by other nodes between the obstacle and a position which is `stop margin` meters away from the obstacle, the stop point is inserted at a distance of `baselink to front` + `min behavior stop margin` from the obstacle.

<div align="center">
<img src="./docs/insert_velocity1.drawio.svg" width=45%>
<img src="./docs/insert_velocity2.drawio.svg" width=45%>
</div>

When the deceleration section is inserted, the start point of the section is inserted in front of the target point cloud by the distance of `baselink to front` + `slow down forward margin`. the end point of the section is inserted behind the target point cloud by the distance of `slow down backward margin` + `baselink to rear`. The `baselink to rear` means the distance between `base_link` and rear of the car. The velocities of points in the deceleration section are modified to the deceleration velocity. `slow down backward margin` and `slow down forward margin` are determined by the parameters described below.

<div align="center">
<img src="./docs/insert_decel_velocity.drawio.svg" width=45%>
</div>

## Input topics

| Name | Type | Description |
Expand All @@ -48,27 +33,41 @@ When the deceleration section is inserted, the start point of the section is ins

### Common Parameter

| Parameter | Type | Description |
| ------------------- | ------ | ---------------------------------------------------------------------------------------- |
| `enable_slow_down` | bool | enable slow down planner [-] |
| `max_velocity` | double | max velocity [m/s] |
| `hunting_threshold` | double | # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
| `lowpass_gain` | double | low pass gain for calculating acceleration [-] |
| Parameter | Type | Description |
| ------------------- | ------ | -------------------------------------------------------------------------------------- |
| `enable_slow_down` | bool | enable slow down planner [-] |
| `max_velocity` | double | max velocity [m/s] |
| `hunting_threshold` | double | even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] |
| `lowpass_gain` | double | low pass gain for calculating acceleration [-] |

### Obstacle Stop Planner

#### Role

`Obstacle Stop Planner` module inserts a stop point in trajectory when there is a static point cloud on the trajectory. This module does not work when `Adaptive Cruise Controller` works.
`obstacle stop planner` module inserts the stop point in the trajectory and modifies that's velocity if there is any obstacle point cloud that has risk to collide with the ego on the trajectory.

This module inserts the stop point before the obstacle with margin. In nominal case, the margin is the sum of `baselink_to_front` and `max_longitudinal_margin`. The `baselink_to_front` means the distance between `base_link`(center of rear-wheel axis) and front of the car.

<figure markdown>
![example](./docs/collision_parameters.svg){width=1000}
<figcaption>parameters for obstacle stop planner</figcaption>
</figure>

If another stop point has already been inserted by other modules within `max_longitudinal_margin`, the margin is the sum of `baselink_to_front` and `min_longitudinal_margin`. This feature exists to avoid stopping unnaturally position. (For example, the ego stops unnaturally far away from stop line of crosswalk that pedestrians cross to without this feature.)

<figure markdown>
![example](./docs/min_longitudinal_margin.svg){width=1000}
<figcaption>minimium longitudinal margin</figcaption>
</figure>

| Parameter | Type | Description |
| ---------------------------------------- | ------ | ----------------------------------------------------------------------------- |
| `stop_planner.stop_margin` | double | stop margin distance from obstacle on the path [m] |
| `stop_planner.min_behavior_stop_margin` | double | stop margin distance when any other stop point is inserted in stop margin [m] |
| `stop_planner.step_length` | double | step length for pointcloud search range [m] |
| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] |
| `stop_planner.expand_stop_range` | double | margin of vehicle footprint [m] |
| `stop_planner.hold_stop_margin_distance` | double | parameter for restart prevention (See following section) [m] |
| Parameter | Type | Description |
| ---------------------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------- |
| `stop_planner.max_longitudinal_margin` | double | stop margin distance from obstacle on the path [m] |
| `stop_planner.min_longitudinal_margin` | double | if any obstacle exists within `max_longitudinal_margin`, this module set margin as the value of _stop margin_ to `min_longitudinal_margin` [m] |
| `stop_planner.lateral_margin` | double | lateral margin from the vehicle footprint for collision obstacle detection area [m] |
| `stop_planner.step_length` | double | step length for pointcloud search range [m] |
| `stop_planner.extend_distance` | double | extend trajectory to consider after goal obstacle in the extend_distance [m] |
| `stop_planner.hold_stop_margin_distance` | double | parameter for restart prevention (See following section) [m] |

#### Flowchart

Expand Down Expand Up @@ -117,7 +116,7 @@ The module searches the obstacle pointcloud within detection area. When the poin

If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away).

This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors.
This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors.

<figure markdown>
![example](./docs/restart_prevention.svg){width=1000}
Expand All @@ -138,15 +137,33 @@ This module has parameter `hold_stop_margin_distance` in order to prevent from t

#### Role

`Slow Down Planner` module inserts a deceleration point in trajectory when there is a point cloud near the trajectory.
`slow down planner` module inserts the deceleration point in trajectory when there is a point cloud near the trajectory.

This module inserts the slow down section before the obstacle with forward margin and backward margin. The forward margin is the sum of `baselink_to_front` and `longitudinal_forward_margin`, and the backward margin is the sum of `baselink_to_front` and `longitudinal_backward_margin`. The ego keeps slow down velocity in slow down section. The velocity is calculated the following equation.

$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{margin}} (v_{max} - v_{min} )$

- $v_{target}$ : slow down target velocity [m/s]
- $v_{min}$ : `slow_down_planner.min_slow_down_velocity` [m/s]
- $v_{max}$ : `slow_down_planner.max_slow_down_velocity` [m/s]
- $l_{ld}$ : lateral deviation between the obstacle and the ego footprint [m]
- $l_{margin}$ : `slow_down_planner.lateral_margin` [m]
- $l_{vw}$ : width of the ego footprint [m]

The above equation means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the slow down section.

<figure markdown>
![example](./docs/slow_down_parameters.svg){width=1000}
<figcaption>parameters for slow down planner</figcaption>
</figure>

| Parameter | Type | Description |
| --------------------------------------------- | ------ | ---------------------------------------------------------------------------------------------- |
| `slow_down_planner.slow_down_forward_margin` | double | margin distance from slow down point to vehicle front [m] |
| `slow_down_planner.slow_down_backward_margin` | double | margin distance from slow down point to vehicle rear [m] |
| `slow_down_planner.expand_slow_down_range` | double | offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] |
| `slow_down_planner.max_slow_down_vel` | double | max slow down velocity [m/s] |
| `slow_down_planner.min_slow_down_vel` | double | min slow down velocity [m/s] |
| Parameter | Type | Description |
| ------------------------------------------------ | ------ | ----------------------------------------------------------------------------------- |
| `slow_down_planner.longitudinal_forward_margin` | double | margin distance from slow down point to vehicle front [m] |
| `slow_down_planner.longitudinal_backward_margin` | double | margin distance from slow down point to vehicle rear [m] |
| `slow_down_planner.lateral_margin` | double | lateral margin from the vehicle footprint for slow down obstacle detection area [m] |
| `slow_down_planner.max_slow_down_velocity` | double | max slow down velocity [m/s] |
| `slow_down_planner.min_slow_down_velocity` | double | min slow down velocity [m/s] |

#### Flowchart

Expand Down Expand Up @@ -178,26 +195,14 @@ stop

First, this module cut off the trajectory behind the car and decimates the points of trajectory for reducing computational costs. ( This is the same process as that of `Obstacle Stop planner` module. )

Then, a detection area is generated by the decimated trajectory as following figure. The detection area in this module is the extended area of the detection area used in `Obstacle Stop Planner` module. The distance to be extended depends on the above parameter `expand_slow_down_range`.
Then, a detection area is generated by the decimated trajectory as following figure. The detection area in this module is the extended area of the detection area used in `Obstacle Stop Planner` module. The distance to be extended depends on the above parameter `lateral_margin`.

![vehicle_shape_decel](./docs/vehicle_shape_decel.drawio.svg)

The module searches the obstacle pointcloud within detection area. When the pointcloud is found, the deceleration point is inserted to the trajectory.

![pointcloud_decel](./docs/point_cloud_decel.drawio.svg)

The deceleration point means the point with low velocity; the value of the velocity $v_{target}$ is determined as follows.

$v_{target} = v_{min} + \frac{l_{ld} - l_{vw}/2}{l_{er}} (v_{max} - v_{min} )$

- $v_{min}$ is minimum target value of `Slow Down Planner` module. The value of $v_{min}$ depends on the parameter `min_slow_down_vel`.
- $v_{max}$ is maximum target value of `Slow Down Planner` module. The value of $v_{max}$ depends on the parameter `max_slow_down_vel`.
- $l_{ld}$ is the lateral deviation of the target pointcloud.
- $l_{vw}$ is the vehicle width.
- $l_{er}$ is the expand range of detection area. The value of $l_{er}$ depends on the parameter `expand_slow_down_range`

The above method means that the smaller the lateral deviation of the pointcloud, the lower the velocity of the deceleration point.

<!-- Moreover, to avoid sudden deceleration, If the current velocity of vehicle is higher than $v_{target}$, the deceleration is planned based on the current velocity. This indicates that there are cases where it is not possible to decelerate according to $v_{target}$ in such a situation that a point cloud in the detection area is suddenly appeared near the vehicle. -->

<!-- ![velocity_limitation](./docs/velocity_limitation.drawio.svg) -->
Expand Down
3 changes: 3 additions & 0 deletions planning/obstacle_stop_planner/docs/collision_paramters.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
4 changes: 0 additions & 4 deletions planning/obstacle_stop_planner/docs/insert_decel_velocity.drawio.svg
View file

This file was deleted.

Loading

0 comments on commit 3e329f3

Please sign in to comment.