You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
feat(map_based_prediction): use obstacle acceleration for map prediction (#6072)
* add acc filtering and decaying acc to model
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* fixed compilation problem, acc is used to predict search_dist
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* Use an equivalent velocity to calculate paths
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* change decaying factor to T/4
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
* coment out cerr for evaluation
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
* simplify code
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* comments
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* add missing constant to decaying acc model
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* Use an equivalent velocity to calculate paths
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* add missing constant to decaying acc model
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* Implement lanelet speed limit for acc consideration
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* add option to activate on and off acceleration feature
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* add params
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* add params
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* delete unused class
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
* update docs
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
* delete comments
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* fix comments
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* update comments, refactor code
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
* remove unused line
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---------
Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Copy file name to clipboardexpand all lines: perception/map_based_prediction/README.md
+42-6
Original file line number
Diff line number
Diff line change
@@ -124,7 +124,7 @@ See paper [2] for more details.
124
124
125
125
`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.)
126
126
127
-
#### Pruning predicted paths with lateral acceleration constraint
It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated.
130
130
@@ -136,11 +136,47 @@ Currently we provide three parameters to tune the lateral acceleration constrain
136
136
137
137
You can change these parameters in rosparam in the table below.
## Using Vehicle Acceleration for Path Prediction (for Vehicle Obstacles)
146
+
147
+
By default, the `map_based_prediction` module uses the current obstacle's velocity to compute its predicted path length. However, it is possible to use the obstacle's current acceleration to calculate its predicted path's length.
148
+
149
+
### Decaying Acceleration Model
150
+
151
+
Since this module tries to predict the vehicle's path several seconds into the future, it is not practical to consider the current vehicle's acceleration as constant (it is not assumed the vehicle will be accelerating for `prediction_time_horizon` seconds after detection). Instead, a decaying acceleration model is used. With the decaying acceleration model, a vehicle's acceleration is modeled as:
152
+
153
+
$\ a(t) = a\_{t0} \cdot e^{-\lambda \cdot t} $
154
+
155
+
where $\ a\_{t0} $ is the vehicle acceleration at the time of detection $\ t0 $, and $\ \lambda $ is the decay constant $\ \lambda = \ln(2) / hl $ and $\ hl $ is the exponential's half life.
156
+
157
+
Furthermore, the integration of $\ a(t) $ over time gives us equations for velocity, $\ v(t) $ and distance $\ x(t) $ as:
With this model, the influence of the vehicle's detected instantaneous acceleration on the predicted path's length is diminished but still considered. This feature also considers that the obstacle might not accelerate past its road's speed limit (multiplied by a tunable factor).
166
+
167
+
Currently, we provide three parameters to tune the use of obstacle acceleration for path prediction:
168
+
169
+
-`use_vehicle_acceleration`: to enable the feature.
170
+
-`acceleration_exponential_half_life`: The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds.
171
+
-`speed_limit_multiplier`: Set the vehicle type obstacle's maximum predicted speed as the legal speed limit in that lanelet times this value. This value should be at least equal or greater than 1.0.
172
+
173
+
You can change these parameters in `rosparam` in the table below.
Copy file name to clipboardexpand all lines: perception/map_based_prediction/config/map_based_prediction.param.yaml
+3
Original file line number
Diff line number
Diff line change
@@ -16,6 +16,9 @@
16
16
check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints
17
17
max_lateral_accel: 2.0# [m/ss] max acceptable lateral acceleration for predicted vehicle paths
18
18
min_acceleration_before_curve: -2.0# [m/ss] min acceleration a vehicle might take to decelerate before a curve
19
+
use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
20
+
speed_limit_multiplier: 1.5# When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
21
+
acceleration_exponential_half_life: 2.5# [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
0 commit comments