Skip to content

Commit 04ed652

Browse files
takayuki5168takayuki5168
authored
feat: introduce motion_velocity_obstacle_<stop/slow_down/cruise>_module (#9807)
* implement obstacle stop, slow_down, and cruise Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * fix clang-tidy Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * revert obstacle_cruise_planner Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 17927bf commit 04ed652

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

52 files changed

+8541
-0
lines changed

‎planning/.pages

+3
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,9 @@ nav:
6363
- 'Motion Velocity Planner':
6464
- 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/
6565
- 'Available Modules':
66+
- 'Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/
67+
- 'Obstacle Slow Down': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/
68+
- 'Obstacle Cruise': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/
6669
- 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/
6770
- 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/
6871
- 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/CMakeLists.txt

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_motion_velocity_obstacle_cruise_module)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml)
7+
8+
ament_auto_add_library(${PROJECT_NAME} SHARED
9+
DIRECTORY src
10+
)
11+
12+
ament_auto_package(INSTALL_TO_SHARE config)

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md

+118
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
# Obstacle Cruise
2+
3+
## Role
4+
5+
The `obstacle_cruise` module does the cruise planning against a dynamic obstacle in front of the ego.
6+
7+
## Activation
8+
9+
This module is activated if the launch parameter `launch_obstacle_cruise_module` is set to true.
10+
11+
## Inner-workings / Algorithms
12+
13+
### Obstacle Filtering
14+
15+
The obstacles meeting the following condition are determined as obstacles for cruising.
16+
17+
- The lateral distance from the object to the ego's trajectory is smaller than `obstacle_filtering.max_lat_margin`.
18+
19+
- The object type is for cruising according to `obstacle_filtering.object_type.*`.
20+
- The object is not crossing the ego's trajectory (\*1).
21+
- If the object is inside the trajectory.
22+
- The object type is for inside cruising according to `obstacle_filtering.object_type.inside.*`.
23+
- The object velocity is larger than `obstacle_filtering.obstacle_velocity_threshold_from_cruise`.
24+
- If the object is outside the trajectory.
25+
- The object type is for outside cruising according to `obstacle_filtering.object_type.outside.*`.
26+
- The object velocity is larger than `obstacle_filtering.outside_obstacle.obstacle_velocity_threshold`.
27+
- The highest confident predicted path collides with the ego's trajectory.
28+
- Its collision's period is larger than `obstacle_filtering.outside_obstacle.ego_obstacle_overlap_time_threshold`.
29+
30+
#### NOTE
31+
32+
##### \*1: Crossing obstacles
33+
34+
Crossing obstacle is the object whose orientation's yaw angle against the ego's trajectory is smaller than `obstacle_filtering.crossing_obstacle.obstacle_traj_angle_threshold`.
35+
36+
##### Yield for vehicles that might cut in into the ego's lane
37+
38+
It is also possible to yield (cruise) behind vehicles in neighbor lanes if said vehicles might cut in the ego vehicle's current lane.
39+
40+
The obstacles meeting the following condition are determined as obstacles for yielding (cruising).
41+
42+
- The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`.
43+
- The object is not crossing the ego's trajectory (\*1).
44+
- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle.
45+
- The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles`
46+
- Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
47+
48+
If the above conditions are met, the ego vehicle will cruise behind the moving obstacle, yielding to it so it can cut in into the ego's lane to avoid the stopped obstacle.
49+
50+
### Cruise Planning
51+
52+
The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition.
53+
This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle.
54+
55+
The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.
56+
57+
$$
58+
d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
59+
$$
60+
61+
assuming that $d_{rss}$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration.
62+
These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`.
63+
64+
| Parameter | Type | Description |
65+
| ------------------------------------------ | ------ | ----------------------------------------------------------------------------- |
66+
| `cruise_planning.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] |
67+
| `cruise_planning.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] |
68+
| `cruise_planning.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] |
69+
70+
The detailed formulation is as follows.
71+
72+
$$
73+
\begin{align}
74+
d_{error} & = d - d_{rss} \\
75+
d_{normalized} & = lpf(d_{error} / d_{obstacle}) \\
76+
d_{quad, normalized} & = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\
77+
v_{pid} & = pid(d_{quad, normalized}) \\
78+
v_{add} & = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\
79+
v_{target} & = max(v_{ego} + v_{add}, v_{min, cruise})
80+
\end{align}
81+
$$
82+
83+
| Variable | Description |
84+
| ----------------- | --------------------------------------- |
85+
| `d` | actual distance to obstacle |
86+
| `d_{rss}` | ideal distance to obstacle based on RSS |
87+
| `v_{min, cruise}` | `min_cruise_target_vel` |
88+
| `w_{acc}` | `output_ratio_during_accel` |
89+
| `lpf(val)` | apply low-pass filter to `val` |
90+
| `pid(val)` | apply pid to `val` |
91+
92+
#### Algorithm selection for cruise planner
93+
94+
Currently, only a PID-based planner is supported.
95+
Each planner will be explained in the following.
96+
97+
| Parameter | Type | Description |
98+
| --------------------------- | ------ | ------------------------------------------------------------ |
99+
| `option.planning_algorithm` | string | cruise and stop planning algorithm, selected from "pid_base" |
100+
101+
#### PID-based planner
102+
103+
In order to keep the safe distance, the target velocity and acceleration is calculated and sent as an external velocity limit to the velocity smoothing package (`velocity_smoother` by default).
104+
The target velocity and acceleration is respectively calculated with the PID controller according to the error between the reference safe distance and the actual distance.
105+
106+
#### Optimization-based planner
107+
108+
under construction
109+
110+
## Debugging
111+
112+
### Obstacle for cruise
113+
114+
Orange sphere which is an obstacle for cruise is visualized by `obstacles_to_cruise` in the `~/debug/marker` topic.
115+
116+
Orange wall which means a safe distance to cruise if the ego's front meets the wall is visualized in the `~/debug/cruise/virtual_wall` topic.
117+
118+
![cruise_visualization](./docs/cruise_visualization.png)

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml

+120
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
/**:
2+
ros__parameters:
3+
obstacle_cruise:
4+
option:
5+
planning_algorithm: "pid_base" # currently supported algorithm is "pid_base"
6+
7+
cruise_planning:
8+
idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
9+
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
10+
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
11+
safe_distance_margin : 5.0 # This is also used as a stop margin [m]
12+
13+
pid_based_planner:
14+
use_velocity_limit_based_planner: true
15+
error_function_type: quadratic # choose from linear, quadratic
16+
17+
velocity_limit_based_planner:
18+
# PID gains to keep safe distance with the front vehicle
19+
kp: 10.0
20+
ki: 0.0
21+
kd: 2.0
22+
23+
output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
24+
vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-]
25+
26+
enable_jerk_limit_to_output_acc: false
27+
28+
disable_target_acceleration: true
29+
30+
velocity_insertion_based_planner:
31+
kp_acc: 6.0
32+
ki_acc: 0.0
33+
kd_acc: 2.0
34+
35+
kp_jerk: 20.0
36+
ki_jerk: 0.0
37+
kd_jerk: 0.0
38+
39+
output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
40+
output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-]
41+
42+
enable_jerk_limit_to_output_acc: true
43+
44+
min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss]
45+
min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s]
46+
time_to_evaluate_rss: 0.0
47+
48+
lpf_normalized_error_cruise_dist_gain: 0.2
49+
50+
optimization_based_planner:
51+
dense_resampling_time_interval: 0.2
52+
sparse_resampling_time_interval: 2.0
53+
dense_time_horizon: 5.0
54+
max_time_horizon: 25.0
55+
velocity_margin: 0.2 #[m/s]
56+
57+
# Parameters for safe distance
58+
t_dangerous: 0.5
59+
60+
# For initial Motion
61+
replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s]
62+
engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed)
63+
engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement)
64+
engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity.
65+
stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m]
66+
67+
# Weights for optimization
68+
max_s_weight: 100.0
69+
max_v_weight: 1.0
70+
over_s_safety_weight: 1000000.0
71+
over_s_ideal_weight: 50.0
72+
over_v_weight: 500000.0
73+
over_a_weight: 5000.0
74+
over_j_weight: 10000.0
75+
76+
obstacle_filtering:
77+
object_type:
78+
inside:
79+
unknown: true
80+
car: true
81+
truck: true
82+
bus: true
83+
trailer: true
84+
motorcycle: true
85+
bicycle: true
86+
pedestrian: false
87+
88+
outside:
89+
unknown: false
90+
car: true
91+
truck: true
92+
bus: true
93+
trailer: true
94+
motorcycle: true
95+
bicycle: false
96+
pedestrian: false
97+
98+
max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
99+
100+
# if crossing vehicle is determined as target obstacles or not
101+
crossing_obstacle:
102+
obstacle_velocity_threshold : 1.0 # velocity threshold for crossing obstacle for cruise or stop [m/s]
103+
obstacle_traj_angle_threshold : 0.523599 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
104+
105+
outside_obstacle:
106+
obstacle_velocity_threshold : 1.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s]
107+
ego_obstacle_overlap_time_threshold : 0.2 # time threshold to decide cut-in obstacle for cruise or stop [s]
108+
max_prediction_time_for_collision_check : 10.0 # prediction time to check collision between obstacle and ego
109+
max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s]
110+
num_of_predicted_paths: 1 # number of the highest confidence predicted path to check collision between obstacle and ego
111+
yield:
112+
enable_yield: true
113+
lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding
114+
max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it
115+
max_obstacles_collision_time: 10.0 # how far the blocking obstacle
116+
stopped_obstacle_velocity_threshold: 0.5
117+
118+
# hysteresis for cruise and stop
119+
obstacle_velocity_threshold_from_cruise : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
120+
obstacle_velocity_threshold_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/docs/cruise_visualization.png

75.9 KB
Loading

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/package.xml

+41
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_motion_velocity_obstacle_cruise_module</name>
5+
<version>0.40.0</version>
6+
<description>obstacle cruise feature in motion_velocity_planner</description>
7+
8+
<maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
9+
<maintainer email="yuki.takagi@tier4.jp">Yuki Takagi</maintainer>
10+
<maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
11+
12+
<license>Apache License 2.0</license>
13+
14+
<buildtool_depend>ament_cmake_auto</buildtool_depend>
15+
<buildtool_depend>autoware_cmake</buildtool_depend>
16+
<depend>autoware_motion_utils</depend>
17+
<depend>autoware_motion_velocity_planner_common_universe</depend>
18+
<depend>autoware_osqp_interface</depend>
19+
<depend>autoware_perception_msgs</depend>
20+
<depend>autoware_planning_msgs</depend>
21+
<depend>autoware_route_handler</depend>
22+
<depend>autoware_signal_processing</depend>
23+
<depend>autoware_universe_utils</depend>
24+
<depend>autoware_vehicle_info_utils</depend>
25+
<depend>geometry_msgs</depend>
26+
<depend>libboost-dev</depend>
27+
<depend>pluginlib</depend>
28+
<depend>rclcpp</depend>
29+
<depend>tf2</depend>
30+
<depend>tier4_metric_msgs</depend>
31+
<depend>tier4_planning_msgs</depend>
32+
<depend>visualization_msgs</depend>
33+
34+
<test_depend>ament_cmake_ros</test_depend>
35+
<test_depend>ament_lint_auto</test_depend>
36+
<test_depend>autoware_lint_common</test_depend>
37+
38+
<export>
39+
<build_type>ament_cmake</build_type>
40+
</export>
41+
</package>

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/plugins.xml

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
<library path="autoware_motion_velocity_obstacle_cruise_module">
2+
<class type="autoware::motion_velocity_planner::ObstacleCruiseModule" base_class_type="autoware::motion_velocity_planner::PluginModuleInterface"/>
3+
</library>

0 commit comments

Comments
 (0)