Skip to content

Commit

Permalink
update steering angle calculation method
Browse files Browse the repository at this point in the history
  • Loading branch information
@brkay54
brkay54 committed Aug 12, 2022
1 parent 45bb849 commit 1c4c2a5
Show file tree
Hide file tree
Showing 5 changed files with 90 additions and 107 deletions.
2 changes: 1 addition & 1 deletion launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@
input_delay: 0.24 # steering input delay time for delay compensation
vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s]
steer_lim_deg: 40.0 # steering angle limit [deg]
steer_rate_lim_dps: 5.0 # steering angle rate limit [deg/s]
steer_rate_lim_dps: 15.0 # steering angle rate limit [deg/s]
acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss]
velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s]

Expand Down
6 changes: 3 additions & 3 deletions ...fig/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,10 @@
post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m]

# steering angle rate limit parameters
max_steering_angle_rate: 5.0 # steering angle rate [degree/s]
max_steering_angle_rate: 15.0 # maximum steering angle rate [degree/s]
resample_ds: 0.1 # distance between trajectory points [m]
lookup_distance: 2.0 # look-up distance of Trajectory point for calculation of steering angle limit [m]
threshold_trigger: 2.0 # threshold steering degree limit to trigger steeringRateLimit [degree]
curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m]
curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m]

# system
over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point
6 changes: 3 additions & 3 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,10 @@ class SmootherBase
double max_steering_angle_rate; // max steering angle rate [degree/s]
double wheel_base; // wheel base [m]
double sample_ds; // distance between trajectory points [m]
double lookup_dist; // look-up distance of Trajectory point for calculation of steering angle
// limit [m]
double curvature_threshold; // look-up distance of Trajectory point for calculation of steering angle
// limit [m]
double
threshold_trigger; // threshold steering degree limit to trigger steeringRateLimit [degree]
curvature_calculation_distance; // threshold steering degree limit to trigger steeringRateLimit [degree]
resampling::ResampleParam resample_param;
};

Expand Down
6 changes: 3 additions & 3 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,9 +180,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt);
update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance);
update_param("resample_ds", p.sample_ds);
update_param("lookup_distance", p.lookup_dist);
update_param("curvature_threshold", p.curvature_threshold);
update_param("max_steering_angle_rate", p.max_steering_angle_rate);
update_param("threshold_trigger", p.threshold_trigger);
update_param("curvature_calculation_distance", p.curvature_calculation_distance);
smoother_->setParam(p);
}

Expand Down Expand Up @@ -524,7 +524,7 @@ bool MotionVelocitySmootherNode::smoothVelocity(

// Resample trajectory with ego-velocity based interval distance
auto traj_resampled = smoother_->resampleTrajectory(
*traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x,
*traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
current_pose_ptr_->pose, node_param_.delta_yaw_threshold);
if (!traj_resampled) {
RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
Expand Down
177 changes: 80 additions & 97 deletions planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,9 @@ SmootherBase::SmootherBase(rclcpp::Node & node)
p.min_jerk = node.declare_parameter("normal.min_jerk", -0.1);
p.max_lateral_accel = node.declare_parameter("max_lateral_accel", 0.2);
p.sample_ds = node.declare_parameter("resample_ds", 0.5);
p.lookup_dist = node.declare_parameter("lookup_distance", 2.0);
p.curvature_threshold = node.declare_parameter("curvature_threshold", 0.2);
p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate", 5.0);
p.threshold_trigger = node.declare_parameter("threshold_trigger", 3.0);
p.curvature_calculation_distance = node.declare_parameter("curvature_calculation_distance", 1.0);
p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve", 3.5);
p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve", 0.0);
p.min_curve_velocity = node.declare_parameter("min_curve_velocity", 1.38);
Expand All @@ -67,7 +67,41 @@ double SmootherBase::getMinDecel() const { return base_param_.min_decel; }
double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; }

double SmootherBase::getMinJerk() const { return base_param_.min_jerk; }
std::vector<double> calcCurvatureFrom3Points(
const TrajectoryPoints & trajectory, size_t idx_dist)
{
using tier4_autoware_utils::calcCurvature;
using tier4_autoware_utils::getPoint;

if (trajectory.size() < 3) {
const std::vector<double> k_arr(trajectory.size(), 0.0);
return k_arr;
}

// if the idx size is not enough, change the idx_dist
const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));

if (idx_dist < 1) {
throw std::logic_error("idx_dist less than 1 is not expected");
}

// calculate curvature by circle fitting from three points
std::vector<double> k_arr;
// for first point curvature = 0;
k_arr.push_back(0.0);
for (size_t i = 1; i + 1 < trajectory.size(); i++) {

const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));
const auto p1 = getPoint(trajectory.at(i));
const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i)));
k_arr.push_back(calcCurvature(p0, p1, p2));
}
// for last point curvature = 0;
k_arr.push_back(0.0);

return k_arr;
}
boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
const TrajectoryPoints & input, [[maybe_unused]] const double v0,
[[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const
Expand Down Expand Up @@ -122,6 +156,7 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
}
double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity);

if (enable_smooth_limit) {
if (i >= latacc_min_vel_arr.size()) return output;
v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i));
Expand All @@ -133,119 +168,67 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
return output;
}



boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
const TrajectoryPoints & input) const
{
if (input.empty()) {
return boost::none;
}

if (input.size() < 2) {
if (input.size() < 3) {
return boost::optional<TrajectoryPoints>(
input); // cannot calculate the desired velocity. do nothing.
}

// Interpolate with constant interval distance for lateral acceleration calculation.
std::vector<double> out_arclength;
const std::vector<double> in_arclength = trajectory_utils::calcArclengthArray(input);
for (double s = 0; s < in_arclength.back(); s += base_param_.sample_ds) {
const auto traj_length = motion_utils::calcArcLength(input);
for (double s = 0; s < traj_length; s += base_param_.sample_ds) {
out_arclength.push_back(s);
}
auto output = trajectory_utils::applyLinearInterpolation(in_arclength, input, out_arclength);
if (!output) {
RCLCPP_WARN(
rclcpp::get_logger("smoother").get_child("smoother_base"),
"interpolation failed at steering angle rate limit.");
return boost::none;
}

std::vector<double> arc_length_array(output->size() - 1);

for (size_t i = 0; output->size() > i + 1; i++) {
/*
* calculate the arc length of 2 trajectory points by using orientation of vehicle
*/

const double heading_diff =
std::fabs(tf2::getYaw(output->at(i + 1).pose.orientation - output->at(i).pose.orientation));
const double radius = base_param_.sample_ds / (2.0 * sin(heading_diff / 2.0));
const double arc_length = radius * heading_diff;
arc_length_array.at(i) = arc_length;

/* Fill the steering tire angle w.r.t. orientation
*
* delta_orientation / delta_time = velocity * tan(steering_tire_angle) / wheelbase
*
* calculate desired steering_tire_angle w.r.t. delta_orientation, velocity, and wheelbase
*
*/

output->at(i).front_wheel_angle_rad = static_cast<float>(std::atan(
(tf2::getYaw(output->at(i + 1).pose.orientation - output->at(i).pose.orientation) /
arc_length_array.at(i)) *
base_param_.wheel_base));
}
output->back().front_wheel_angle_rad = 0.0;

const size_t lookup_index = base_param_.lookup_dist / base_param_.sample_ds;

for (size_t i = 0; i < output->size(); i++) {
const size_t base = output->size() - 1 - i;

if (lookup_index > i) {
continue;
}
double ds = 0.0;
double tot_vel = 0.0;
double tot_steering_diff = 0.0;
double tot_steering = 0.0;

for (size_t k = 0; k < lookup_index; k++) {
ds += arc_length_array.at(base + k);
tot_vel += (output->at(base + k).longitudinal_velocity_mps +
output->at(base + k + 1).longitudinal_velocity_mps) /
2.0;
tot_steering += (output->at(base + k).front_wheel_angle_rad +
output->at(base + k + 1).front_wheel_angle_rad) /
2.0;
tot_steering_diff += fabs(
output->at(base + k).front_wheel_angle_rad -
output->at(1 + k + base).front_wheel_angle_rad);
}

double mean_steer = tot_steering / static_cast<double>(lookup_index);

if (fabs(mean_steer) > tier4_autoware_utils::deg2rad(base_param_.threshold_trigger)) {
const double mean_vel = tot_vel / static_cast<double>(lookup_index);
const double dt = std::max(ds / mean_vel, std::numeric_limits<double>::epsilon());

const double dt_steering =
tot_steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate);
const double mean_steering_diff = tot_steering_diff / static_cast<double>(lookup_index);
const auto output_traj =
motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength);
auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
output.back() = input.back(); // keep the final speed.

if (dt_steering > dt) {
const double target_mean_vel = (ds / dt_steering);
const size_t idx_dist =
static_cast<size_t>(std::max(static_cast<int>((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1));

double tmp_vel =
target_mean_vel * fabs(
mean_steering_diff / (output->at(base).front_wheel_angle_rad -
output->at(base + 1).front_wheel_angle_rad));
if (tmp_vel < output->at(base).longitudinal_velocity_mps) {
output->at(base).longitudinal_velocity_mps = tmp_vel;
} else {
if (target_mean_vel < output->at(base).longitudinal_velocity_mps) {
output->at(base).longitudinal_velocity_mps = target_mean_vel;
// Calculate curvature assuming the trajectory points interval is constant
const auto curvature_v = calcCurvatureFrom3Points(output, idx_dist);

for (size_t i = 0; i + 1 < output.size(); i++) {

if(fabs(curvature_v.at(i)) > base_param_.curvature_threshold){

// calculate the just 2 steering angle
output.at(i).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i));
output.at(i + 1).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i + 1));

const double mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0;
const double dt = std::max(base_param_.sample_ds / mean_vel, std::numeric_limits<double>::epsilon());
const double steering_diff = fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad);
const double dt_steering =
steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate);

if (dt_steering > dt) {
const double target_mean_vel = (base_param_.sample_ds / dt_steering);
for(size_t k = 0; k < 2 ; k++){
double temp_vel = output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel);
if (temp_vel < output.at(i + k).longitudinal_velocity_mps) {
output.at(i + k).longitudinal_velocity_mps = temp_vel;
} else {
if (target_mean_vel < output.at(i + k).longitudinal_velocity_mps) {
output.at(i + k).longitudinal_velocity_mps = target_mean_vel;
}
}
if (output.at(i + k).longitudinal_velocity_mps < base_param_.min_curve_velocity) {
output.at(i + k).longitudinal_velocity_mps = base_param_.min_curve_velocity;
}
}
}
// else{
// if(target_mean_vel < output->at(base + k).longitudinal_velocity_mps) {
// output->at(base + k).longitudinal_velocity_mps = target_mean_vel;
// }
// }
if (output->at(base).longitudinal_velocity_mps < base_param_.min_curve_velocity) {
output->at(base).longitudinal_velocity_mps = base_param_.min_curve_velocity;
}
}
}
}

return output;
Expand Down

0 comments on commit 1c4c2a5

Please sign in to comment.