Skip to content

Commit 4b0da5d

Browse files
danielsanchezarankarishma1911
authored andcommitted
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>
1 parent 4aa5352 commit 4b0da5d

File tree

6 files changed

+224
-29
lines changed

6 files changed

+224
-29
lines changed

perception/map_based_prediction/README.md

+42-6
Original file line numberDiff line numberDiff line change
@@ -124,7 +124,7 @@ See paper [2] for more details.
124124

125125
`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.)
126126

127-
#### Pruning predicted paths with lateral acceleration constraint
127+
#### Pruning predicted paths with lateral acceleration constraint (for vehicle obstacles)
128128

129129
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.
130130

@@ -136,11 +136,47 @@ Currently we provide three parameters to tune the lateral acceleration constrain
136136

137137
You can change these parameters in rosparam in the table below.
138138

139-
| param name | default value |
140-
| ----------------------------------------- | -------------- |
141-
| `check_lateral_acceleration_constraints_` | `false` [bool] |
142-
| `max_lateral_accel` | `2.0` [m/s^2] |
143-
| `min_acceleration_before_curve` | `-2.0` [m/s^2] |
139+
| param name | default value |
140+
| ---------------------------------------- | -------------- |
141+
| `check_lateral_acceleration_constraints` | `false` [bool] |
142+
| `max_lateral_accel` | `2.0` [m/s^2] |
143+
| `min_acceleration_before_curve` | `-2.0` [m/s^2] |
144+
145+
## 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:
158+
159+
$\ v(t) = v*{t0} + a*{t0} \* (1/\lambda) \cdot (1 - e^{-\lambda \cdot t}) $
160+
161+
and
162+
163+
$\ x(t) = x*{t0} + (v*{t0} + a*{t0} \* (1/\lambda)) \cdot t + a*{t0}(1/λ^2)(e^{-\lambda \cdot t} - 1) $
164+
165+
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.
174+
175+
| Param Name | Default Value |
176+
| ------------------------------------ | -------------- |
177+
| `use_vehicle_acceleration` | `false` [bool] |
178+
| `acceleration_exponential_half_life` | `2.5` [s] |
179+
| `speed_limit_multiplier` | `1.5` [] |
144180

145181
### Path prediction for crosswalk users
146182

perception/map_based_prediction/config/map_based_prediction.param.yaml

+3
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,9 @@
1616
check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints
1717
max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths
1818
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
1922
# parameter for shoulder lane prediction
2023
prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
2124

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

+8-1
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222

2323
#include <rclcpp/rclcpp.hpp>
2424
#include <tier4_autoware_utils/ros/transform_listener.hpp>
25+
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
2526
#include <tier4_autoware_utils/system/stop_watch.hpp>
2627

2728
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
@@ -89,6 +90,7 @@ struct LaneletData
8990
struct PredictedRefPath
9091
{
9192
float probability;
93+
double speed_limit;
9294
PosePath path;
9395
Maneuver maneuver;
9496
};
@@ -175,6 +177,10 @@ class MapBasedPredictionNode : public rclcpp::Node
175177
double max_lateral_accel_;
176178
double min_acceleration_before_curve_;
177179

180+
bool use_vehicle_acceleration_;
181+
double speed_limit_multiplier_;
182+
double acceleration_exponential_half_life_;
183+
178184
// Stop watch
179185
StopWatch<std::chrono::milliseconds> stop_watch_;
180186

@@ -231,7 +237,8 @@ class MapBasedPredictionNode : public rclcpp::Node
231237
void addReferencePaths(
232238
const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths,
233239
const float path_probability, const ManeuverProbability & maneuver_probability,
234-
const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths);
240+
const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
241+
const double speed_limit = 0.0);
235242
std::vector<PosePath> convertPathType(const lanelet::routing::LaneletPaths & paths);
236243

237244
void updateFuturePossibleLanelets(

perception/map_based_prediction/include/map_based_prediction/path_generator.hpp

+17-3
Original file line numberDiff line numberDiff line change
@@ -91,25 +91,38 @@ class PathGenerator
9191
PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object);
9292

9393
PredictedPath generatePathForOnLaneVehicle(
94-
const TrackedObject & object, const PosePath & ref_paths);
94+
const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0);
9595

9696
PredictedPath generatePathForCrosswalkUser(
9797
const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const;
9898

9999
PredictedPath generatePathToTargetPoint(
100100
const TrackedObject & object, const Eigen::Vector2d & point) const;
101101

102+
void setUseVehicleAcceleration(const bool use_vehicle_acceleration)
103+
{
104+
use_vehicle_acceleration_ = use_vehicle_acceleration;
105+
}
106+
107+
void setAccelerationHalfLife(const double acceleration_exponential_half_life)
108+
{
109+
acceleration_exponential_half_life_ = acceleration_exponential_half_life;
110+
}
111+
102112
private:
103113
// Parameters
104114
double time_horizon_;
105115
double lateral_time_horizon_;
106116
double sampling_time_interval_;
107117
double min_crosswalk_user_velocity_;
118+
bool use_vehicle_acceleration_;
119+
double acceleration_exponential_half_life_;
108120

109121
// Member functions
110122
PredictedPath generateStraightPath(const TrackedObject & object) const;
111123

112-
PredictedPath generatePolynomialPath(const TrackedObject & object, const PosePath & ref_path);
124+
PredictedPath generatePolynomialPath(
125+
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
113126

114127
FrenetPath generateFrenetPath(
115128
const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length);
@@ -125,7 +138,8 @@ class PathGenerator
125138
const TrackedObject & object, const FrenetPath & frenet_predicted_path,
126139
const PosePath & ref_path);
127140

128-
FrenetPoint getFrenetPoint(const TrackedObject & object, const PosePath & ref_path);
141+
FrenetPoint getFrenetPoint(
142+
const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0);
129143
};
130144
} // namespace map_based_prediction
131145

perception/map_based_prediction/src/map_based_prediction_node.cpp

+74-12
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,6 @@
2424
#include <tier4_autoware_utils/math/constants.hpp>
2525
#include <tier4_autoware_utils/math/normalization.hpp>
2626
#include <tier4_autoware_utils/math/unit_conversion.hpp>
27-
#include <tier4_autoware_utils/ros/uuid_helper.hpp>
2827

2928
#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
3029

@@ -385,11 +384,7 @@ bool withinRoadLanelet(
385384
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
386385
const bool use_yaw_information = false)
387386
{
388-
using Point = boost::geometry::model::d2::point_xy<double>;
389-
390387
const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
391-
const Point p_object{obj_pos.x, obj_pos.y};
392-
393388
lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
394389
// nearest lanelet
395390
constexpr double search_radius = 10.0; // [m]
@@ -788,10 +783,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
788783
prediction_time_horizon_rate_for_validate_lane_length_ =
789784
declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");
790785

786+
use_vehicle_acceleration_ = declare_parameter<bool>("use_vehicle_acceleration");
787+
speed_limit_multiplier_ = declare_parameter<double>("speed_limit_multiplier");
788+
acceleration_exponential_half_life_ =
789+
declare_parameter<double>("acceleration_exponential_half_life");
790+
791791
path_generator_ = std::make_shared<PathGenerator>(
792792
prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
793793
min_crosswalk_user_velocity_);
794794

795+
path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
796+
path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
797+
795798
sub_objects_ = this->create_subscription<TrackedObjects>(
796799
"~/input/objects", 1,
797800
std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1));
@@ -817,6 +820,13 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
817820
updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_);
818821
updateParam(
819822
parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_);
823+
updateParam(parameters, "use_vehicle_acceleration", use_vehicle_acceleration_);
824+
updateParam(parameters, "speed_limit_multiplier", speed_limit_multiplier_);
825+
updateParam(
826+
parameters, "acceleration_exponential_half_life", acceleration_exponential_half_life_);
827+
828+
path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
829+
path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
820830

821831
rcl_interfaces::msg::SetParametersResult result;
822832
result.successful = true;
@@ -1010,7 +1020,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
10101020

10111021
for (const auto & ref_path : ref_paths) {
10121022
PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
1013-
yaw_fixed_transformed_object, ref_path.path);
1023+
yaw_fixed_transformed_object, ref_path.path, ref_path.speed_limit);
10141024
if (predicted_path.path.empty()) continue;
10151025

10161026
if (!check_lateral_acceleration_constraints_) {
@@ -1555,14 +1565,63 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
15551565
object.kinematics.twist_with_covariance.twist.linear.x,
15561566
object.kinematics.twist_with_covariance.twist.linear.y);
15571567

1568+
// Using a decaying acceleration model. Consult the README for more information about the model.
1569+
const double obj_acc = (use_vehicle_acceleration_)
1570+
? std::hypot(
1571+
object.kinematics.acceleration_with_covariance.accel.linear.x,
1572+
object.kinematics.acceleration_with_covariance.accel.linear.y)
1573+
: 0.0;
1574+
const double t_h = prediction_time_horizon_;
1575+
const double λ = std::log(2) / acceleration_exponential_half_life_;
1576+
1577+
auto get_search_distance_with_decaying_acc = [&]() -> double {
1578+
const double acceleration_distance =
1579+
obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
1580+
double search_dist = acceleration_distance + obj_vel * t_h;
1581+
return search_dist;
1582+
};
1583+
1584+
auto get_search_distance_with_partial_acc = [&](const double final_speed) -> double {
1585+
constexpr double epsilon = 1E-5;
1586+
if (std::abs(obj_acc) < epsilon) {
1587+
// Assume constant speed
1588+
return obj_vel * t_h;
1589+
}
1590+
// Time to reach final speed
1591+
const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
1592+
// It is assumed the vehicle accelerates until final_speed is reached and
1593+
// then continues at constant speed for the rest of the time horizon
1594+
const double search_dist =
1595+
// Distance covered while accelerating
1596+
obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
1597+
obj_vel * t_f +
1598+
// Distance covered at constant speed
1599+
final_speed * (t_h - t_f);
1600+
return search_dist;
1601+
};
1602+
15581603
std::vector<PredictedRefPath> all_ref_paths;
1604+
15591605
for (const auto & current_lanelet_data : current_lanelets_data) {
1560-
// parameter for lanelet::routing::PossiblePathsParams
1561-
const double search_dist = prediction_time_horizon_ * obj_vel +
1562-
lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet);
1606+
const lanelet::traffic_rules::SpeedLimitInformation limit =
1607+
traffic_rules_ptr_->speedLimit(current_lanelet_data.lanelet);
1608+
const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
1609+
1610+
double final_speed_after_acceleration =
1611+
obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
1612+
1613+
const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
1614+
const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;
1615+
const bool object_has_surpassed_limit_already = obj_vel > final_speed_limit;
1616+
1617+
double search_dist = (final_speed_surpasses_limit && !object_has_surpassed_limit_already)
1618+
? get_search_distance_with_partial_acc(final_speed_limit)
1619+
: get_search_distance_with_decaying_acc();
1620+
search_dist += lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet);
1621+
15631622
lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true};
15641623
const double validate_time_horizon =
1565-
prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_;
1624+
t_h * prediction_time_horizon_rate_for_validate_lane_length_;
15661625

15671626
// lambda function to get possible paths for isolated lanelet
15681627
// isolated is often caused by lanelet with no connection e.g. shoulder-lane
@@ -1644,7 +1703,8 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
16441703
// Step4. add candidate reference paths to the all_ref_paths
16451704
const float path_prob = current_lanelet_data.probability;
16461705
const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) {
1647-
addReferencePaths(object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths);
1706+
addReferencePaths(
1707+
object, paths, path_prob, maneuver_prob, maneuver, all_ref_paths, final_speed_limit);
16481708
};
16491709
addReferencePathsLocal(left_paths, Maneuver::LEFT_LANE_CHANGE);
16501710
addReferencePathsLocal(right_paths, Maneuver::RIGHT_LANE_CHANGE);
@@ -1966,7 +2026,8 @@ void MapBasedPredictionNode::updateFuturePossibleLanelets(
19662026
void MapBasedPredictionNode::addReferencePaths(
19672027
const TrackedObject & object, const lanelet::routing::LaneletPaths & candidate_paths,
19682028
const float path_probability, const ManeuverProbability & maneuver_probability,
1969-
const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths)
2029+
const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
2030+
const double speed_limit)
19702031
{
19712032
if (!candidate_paths.empty()) {
19722033
updateFuturePossibleLanelets(object, candidate_paths);
@@ -1976,6 +2037,7 @@ void MapBasedPredictionNode::addReferencePaths(
19762037
predicted_path.probability = maneuver_probability.at(maneuver) * path_probability;
19772038
predicted_path.path = converted_path;
19782039
predicted_path.maneuver = maneuver;
2040+
predicted_path.speed_limit = speed_limit;
19792041
reference_paths.push_back(predicted_path);
19802042
}
19812043
}

0 commit comments

Comments
 (0)