Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(trajectory_follower): improve yaw control on curve #2338

Closed
wants to merge 2 commits into from
Closed

feat(trajectory_follower): improve yaw control on curve #2338

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1be9406
feat(trajectory_follower): improve yaw control on curve
kosuke55 Nov 21, 2022
715cbd7
remove smooth_k
kosuke55 Nov 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 16 additions & 18 deletions control/trajectory_follower/design/mpc_lateral_controller-design.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,24 +84,22 @@ can be calculated by providing the current steer, velocity, and pose to function
The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the
AutonomouStuff Lexus RX 450h for under 40 km/h driving.

| Name | Type | Description | Default value |
| :------------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------ | :------------ |
| show_debug_info | bool | display debug info | false |
| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 |
| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true |
| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 |
| path_smoothing_times | int | number of times of applying path smoothing filter | 1 |
| curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 |
| curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 |
| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 |
| admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 |
| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 |
| stop_state_entry_ego_speed <sup>\*1</sup> | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 |
| stop_state_entry_target_speed <sup>\*1</sup> | double | threshold value of the target speed used to the stop state entry condition | 0.0 |
| converged_steer_rad | double | threshold value of the steer convergence | 0.1 |
| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true |
| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 |
| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 |
| Name | Type | Description | Default value |
| :------------------------------------------- | :----- | :--------------------------------------------------------------------------------------------- | :------------ |
| show_debug_info | bool | display debug info | false |
| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 |
| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true |
| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 |
| path_smoothing_times | int | number of times of applying path smoothing filter | 1 |
| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 |
| admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 |
| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 |
| stop_state_entry_ego_speed <sup>\*1</sup> | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 |
| stop_state_entry_target_speed <sup>\*1</sup> | double | threshold value of the target speed used to the stop state entry condition | 0.0 |
| converged_steer_rad | double | threshold value of the steer convergence | 0.1 |
| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true |
| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 |
| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 |

(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state.

Expand Down
3 changes: 1 addition & 2 deletions control/trajectory_follower/include/trajectory_follower/mpc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -399,8 +399,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
void setReferenceTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
const double traj_resample_dist, const bool enable_path_smoothing,
const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
const int curvature_smoothing_num_ref_steer);
const int path_filter_moving_ave_num);
/**
* @brief set the vehicle model of this MPC
*/
Expand Down
5 changes: 0 additions & 5 deletions control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,11 +89,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
bool m_enable_path_smoothing;
//!< @brief param of moving average filter for path smoothing
int m_path_filter_moving_ave_num;
//!< @brief point-to-point index distance for curvature calculation for trajectory //NOLINT
int m_curvature_smoothing_num_traj;
//!< @brief point-to-point index distance for curvature calculation for reference steer command
//!< //NOLINT
int m_curvature_smoothing_num_ref_steer;
//!< @brief path resampling interval [m]
double m_traj_resample_dist;

Expand Down
3 changes: 1 addition & 2 deletions control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,15 +45,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory
std::vector<double> yaw; //!< @brief yaw pose yaw vector
std::vector<double> vx; //!< @brief vx velocity vx vector
std::vector<double> k; //!< @brief k curvature k vector
std::vector<double> smooth_k; //!< @brief k smoothed-curvature k vector
std::vector<double> relative_time; //!< @brief relative_time duration time from start point

/**
* @brief push_back for all values
*/
void push_back(
const double & xp, const double & yp, const double & zp, const double & yawp,
const double & vxp, const double & kp, const double & smooth_kp, const double & tp);
const double & vxp, const double & kp, const double & tp);
/**
* @brief clear for all values
*/
Expand Down
10 changes: 2 additions & 8 deletions control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,23 +141,17 @@ TRAJECTORY_FOLLOWER_PUBLIC void calcTrajectoryYawFromXY(
/**
* @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
* points when num = 1)
* @param [in] curvature_smoothing_num_traj index distance for 3 points for curvature calculation
* @param [in] curvature_smoothing_num_ref_steer index distance for 3 points for smoothed curvature
* calculation
* @param [inout] traj object trajectory
*/
TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature(
const size_t curvature_smoothing_num_traj, const size_t curvature_smoothing_num_ref_steer,
MPCTrajectory * traj);
TRAJECTORY_FOLLOWER_PUBLIC bool calcTrajectoryCurvature(MPCTrajectory * traj);
/**
* @brief Calculate path curvature by 3-points circle fitting with smoothing num (use nearest 3
* points when num = 1)
* @param [in] curvature_smoothing_num index distance for 3 points for curvature calculation
* @param [in] traj input trajectory
* @return vector of curvatures at each point of the given trajectory
*/
TRAJECTORY_FOLLOWER_PUBLIC std::vector<double> calcTrajectoryCurvature(
const size_t curvature_smoothing_num, const MPCTrajectory & traj);
TRAJECTORY_FOLLOWER_PUBLIC std::vector<double> calcTrajectoryCurvature(const MPCTrajectory & traj);
/**
* @brief calculate nearest pose on MPCTrajectory with linear interpolation
* @param [in] traj reference trajectory
Expand Down
33 changes: 11 additions & 22 deletions control/trajectory_follower/src/mpc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,16 +119,13 @@ bool MPC::calculateMPC(
const double yaw = traj.yaw[i] + yaw_error;
const double vx = traj.vx[i];
const double k = traj.k[i];
const double smooth_k = traj.smooth_k[i];
const double relative_time = traj.relative_time[i];
mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, smooth_k, relative_time);
mpc_predicted_traj.push_back(x, y, z, yaw, vx, k, relative_time);
}
trajectory_follower::MPCUtils::convertToAutowareTrajectory(mpc_predicted_traj, predicted_traj);

/* prepare diagnostic message */
const double nearest_k = reference_trajectory.k[static_cast<size_t>(mpc_data.nearest_idx)];
const double nearest_smooth_k =
reference_trajectory.smooth_k[static_cast<size_t>(mpc_data.nearest_idx)];
const double steer_cmd = ctrl_cmd.steering_tire_angle;
const double wb = m_vehicle_model_ptr->getWheelbase();

Expand All @@ -143,7 +140,7 @@ bool MPC::calculateMPC(
// [2] feedforward steering value
append_diag_data(mpc_matrix.Uref_ex(0));
// [3] feedforward steering value raw
append_diag_data(std::atan(nearest_smooth_k * wb));
append_diag_data(std::atan(nearest_k * wb));
// [4] current steering angle
append_diag_data(mpc_data.steer);
// [5] lateral error
Expand All @@ -163,14 +160,12 @@ bool MPC::calculateMPC(
// [12] angvel from measured steer
append_diag_data(current_velocity * tan(mpc_data.steer) / wb);
// [13] angvel from path curvature
append_diag_data(current_velocity * nearest_smooth_k);
// [14] nearest path curvature (used for feedforward)
append_diag_data(nearest_smooth_k);
// [15] nearest path curvature (not smoothed)
append_diag_data(current_velocity * nearest_k);
// [14] nearest path curvature
append_diag_data(nearest_k);
// [16] predicted steer
// [15] predicted steer
append_diag_data(mpc_data.predicted_steer);
// [17] angvel from predicted steer
// [16] angvel from predicted steer
append_diag_data(current_velocity * tan(mpc_data.predicted_steer) / wb);

return true;
Expand All @@ -179,8 +174,7 @@ bool MPC::calculateMPC(
void MPC::setReferenceTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
const double traj_resample_dist, const bool enable_path_smoothing,
const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
const int curvature_smoothing_num_ref_steer)
const int path_filter_moving_ave_num)
{
trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory
trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory
Expand Down Expand Up @@ -219,12 +213,11 @@ void MPC::setReferenceTrajectory(

/* calculate yaw angle */
trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift);
mpc_traj_smoothed.yaw.back() = mpc_traj_raw.yaw.back();
trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw);

/* calculate curvature */
trajectory_follower::MPCUtils::calcTrajectoryCurvature(
static_cast<size_t>(curvature_smoothing_num_traj),
static_cast<size_t>(curvature_smoothing_num_ref_steer), &mpc_traj_smoothed);
trajectory_follower::MPCUtils::calcTrajectoryCurvature(&mpc_traj_smoothed);

/* add end point with vel=0 on traj for mpc prediction */
{
Expand All @@ -233,9 +226,7 @@ void MPC::setReferenceTrajectory(
const double t_end = t.relative_time.back() + t_ext;
const double v_end = 0.0;
t.vx.back() = v_end; // set for end point
t.push_back(
t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t.smooth_k.back(),
t_end);
t.push_back(t.x.back(), t.y.back(), t.z.back(), t.yaw.back(), v_end, t.k.back(), t_end);
}

if (!mpc_traj_smoothed.size()) {
Expand Down Expand Up @@ -505,7 +496,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
output.vx.back() = v_end; // set for end point
output.push_back(
output.x.back(), output.y.back(), output.z.back(), output.yaw.back(), v_end, output.k.back(),
output.smooth_k.back(), t_end);
t_end);
return output;
}

Expand Down Expand Up @@ -555,7 +546,6 @@ MPCMatrix MPC::generateMPCMatrix(
const double ref_vx_squared = ref_vx * ref_vx;

const double ref_k = reference_trajectory.k[static_cast<size_t>(i)] * sign_vx;
const double ref_smooth_k = reference_trajectory.smooth_k[static_cast<size_t>(i)] * sign_vx;

/* get discrete state matrix A, B, C, W */
m_vehicle_model_ptr->setVelocity(ref_vx);
Expand Down Expand Up @@ -601,7 +591,6 @@ MPCMatrix MPC::generateMPCMatrix(
m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;

/* get reference input (feed-forward) */
m_vehicle_model_ptr->setCurvature(ref_smooth_k);
m_vehicle_model_ptr->calculateReferenceInput(Uref);
if (std::fabs(Uref(0, 0)) < DEG2RAD * m_param.zero_ff_steer_deg) {
Uref(0, 0) = 0.0; // ignore curvature noise
Expand Down
6 changes: 1 addition & 5 deletions control/trajectory_follower/src/mpc_lateral_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,6 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
m_mpc.m_ctrl_period = node_->get_parameter("ctrl_period").as_double();
m_enable_path_smoothing = node_->declare_parameter<bool>("enable_path_smoothing");
m_path_filter_moving_ave_num = node_->declare_parameter<int>("path_filter_moving_ave_num");
m_curvature_smoothing_num_traj = node_->declare_parameter<int>("curvature_smoothing_num_traj");
m_curvature_smoothing_num_ref_steer =
node_->declare_parameter<int>("curvature_smoothing_num_ref_steer");
m_traj_resample_dist = node_->declare_parameter<double>("traj_resample_dist");
m_mpc.m_admissible_position_error = node_->declare_parameter<double>("admissible_position_error");
m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter<double>("admissible_yaw_error_rad");
Expand Down Expand Up @@ -300,8 +297,7 @@ void MpcLateralController::setTrajectory(
}

m_mpc.setReferenceTrajectory(
*msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer);
*msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num);

// update trajectory buffer to check the trajectory shape change.
m_trajectory_buffer.push_back(*m_current_trajectory_ptr);
Expand Down
7 changes: 2 additions & 5 deletions control/trajectory_follower/src/mpc_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,15 +24,14 @@ namespace trajectory_follower
{
void MPCTrajectory::push_back(
const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp,
const double & kp, const double & smooth_kp, const double & tp)
const double & kp, const double & tp)
{
x.push_back(xp);
y.push_back(yp);
z.push_back(zp);
yaw.push_back(yawp);
vx.push_back(vxp);
k.push_back(kp);
smooth_k.push_back(smooth_kp);
relative_time.push_back(tp);
}

Expand All @@ -44,16 +43,14 @@ void MPCTrajectory::clear()
yaw.clear();
vx.clear();
k.clear();
smooth_k.clear();
relative_time.clear();
}

size_t MPCTrajectory::size() const
{
if (
x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() &&
x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() &&
x.size() == relative_time.size()) {
x.size() == vx.size() && x.size() == k.size() && x.size() == relative_time.size()) {
return x.size();
} else {
std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl;
Expand Down
Loading