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.

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

fix(mpc): precise resampling around ego for stabling control (#5003) #857

Merged
merged 1 commit into from
Sep 22, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -447,7 +447,8 @@ class MPC
* @param param Trajectory filtering parameters.
*/
void setReferenceTrajectory(
const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param);
const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
const Odometry & current_kinematics);

/**
* @brief Reset the previous result of MPC.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -181,7 +181,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
* @brief Set the current trajectory using the received message.
* @param msg Received trajectory message.
*/
void setTrajectory(const Trajectory & msg);
void setTrajectory(const Trajectory & msg, const Odometry & current_kinematics);

/**
* @brief Check if the received data is valid.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<do
* @return The pair contains the successful flag and the resultant resampled trajectory
*/
std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
const MPCTrajectory & input, const double resample_interval_dist);
const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
const double ego_offset_to_segment);

/**
* @brief linearly interpolate the given trajectory assuming a base indexing and a new desired
Expand All @@ -122,14 +123,14 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj);

/**
* @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing
* @param [in] start_idx index of the trajectory point from which to start smoothing
* @param [in] start_vel initial velocity to set at the start_idx
* @param [in] start_seg_idx segment index of the trajectory point from which to start smoothing
* @param [in] start_vel initial velocity to set at the start_seg_idx
* @param [in] acc_lim limit on the acceleration
* @param [in] tau constant to control the smoothing (high-value = very smooth)
* @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity
*/
void dynamicSmoothingVelocity(
const size_t start_idx, const double start_vel, const double acc_lim, const double tau,
const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
MPCTrajectory & traj);

/**
Expand Down
17 changes: 12 additions & 5 deletions control/mpc_lateral_controller/src/mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -173,13 +173,20 @@ Float32MultiArrayStamped MPC::generateDiagData(
}

void MPC::setReferenceTrajectory(
const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param)
const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
const Odometry & current_kinematics)
{
const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
ego_nearest_yaw_threshold);
const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment(
trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);

const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);

// resampling
const auto [success_resample, mpc_traj_resampled] =
MPCUtils::resampleMPCTrajectoryByDistance(mpc_traj_raw, param.traj_resample_dist);
const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
if (!success_resample) {
warn_throttle("[setReferenceTrajectory] spline error when resampling by distance");
return;
Expand Down Expand Up @@ -389,13 +396,13 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter(
return input;
}

const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
ego_nearest_yaw_threshold);

MPCTrajectory output = input;
MPCUtils::dynamicSmoothingVelocity(
nearest_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
m_param.velocity_time_constant, output);

auto last_point = output.back();
Expand Down
9 changes: 5 additions & 4 deletions control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@ trajectory_follower::LateralOutput MpcLateralController::run(
trajectory_follower::InputData const & input_data)
{
// set input data
setTrajectory(input_data.current_trajectory);
setTrajectory(input_data.current_trajectory, input_data.current_odometry);

m_current_kinematic_state = input_data.current_odometry;
m_current_steering = input_data.current_steering;
Expand Down Expand Up @@ -319,7 +319,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd)

bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data)
{
setTrajectory(input_data.current_trajectory);
setTrajectory(input_data.current_trajectory, input_data.current_odometry);
m_current_kinematic_state = input_data.current_odometry;
m_current_steering = input_data.current_steering;

Expand All @@ -339,7 +339,8 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_
return true;
}

void MpcLateralController::setTrajectory(const Trajectory & msg)
void MpcLateralController::setTrajectory(
const Trajectory & msg, const Odometry & current_kinematics)
{
m_current_trajectory = msg;

Expand All @@ -353,7 +354,7 @@ void MpcLateralController::setTrajectory(const Trajectory & msg)
return;
}

m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param);
m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics);

// update trajectory buffer to check the trajectory shape change.
m_trajectory_buffer.push_back(m_current_trajectory);
Expand Down
109 changes: 73 additions & 36 deletions control/mpc_lateral_controller/src/mpc_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,19 @@

namespace autoware::motion::control::mpc_lateral_controller
{
namespace
{
double calcLongitudinalOffset(
const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back,
const geometry_msgs::msg::Point & p_target)
{
const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};
const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};

return segment_vec.dot(target_vec) / segment_vec.norm();
}
} // namespace

namespace MPCUtils
{
using tier4_autoware_utils::calcDistance2d;
Expand Down Expand Up @@ -77,7 +90,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<do
}

std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
const MPCTrajectory & input, const double resample_interval_dist)
const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
const double ego_offset_to_segment)
{
MPCTrajectory output;

Expand All @@ -92,7 +106,18 @@ std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
}

std::vector<double> output_arclength;
for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) {
// To accurately sample the ego point, resample separately in the forward direction and the
// backward direction from the current position.
for (double s = std::clamp(
input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0,
input_arclength.back() - 1e-6);
0 <= s; s -= resample_interval_dist) {
output_arclength.push_back(s);
}
std::reverse(output_arclength.begin(), output_arclength.end());
for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) +
resample_interval_dist;
s < input_arclength.back(); s += resample_interval_dist) {
output_arclength.push_back(s);
}

Expand Down Expand Up @@ -274,13 +299,17 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj)
}

void dynamicSmoothingVelocity(
const size_t start_idx, const double start_vel, const double acc_lim, const double tau,
const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
MPCTrajectory & traj)
{
double curr_v = start_vel;
traj.vx.at(start_idx) = start_vel;
// set current velocity in both start and end point of the segment
traj.vx.at(start_seg_idx) = start_vel;
if (1 < traj.vx.size()) {
traj.vx.at(start_seg_idx + 1) = start_vel;
}

for (size_t i = start_idx + 1; i < traj.size(); ++i) {
for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) {
const double ds = calcDistance2d(traj, i, i - 1);
const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits<double>::epsilon());
const double a = tau / std::max(tau + dt, std::numeric_limits<double>::epsilon());
Expand Down Expand Up @@ -320,29 +349,40 @@ bool calcNearestPoseInterp(
return true;
}

auto calcSquaredDist = [](const Pose & p, const MPCTrajectory & t, const size_t idx) {
const double dx = p.position.x - t.x.at(idx);
const double dy = p.position.y - t.y.at(idx);
return dx * dx + dy * dy;
};

/* get second nearest index = next to nearest_index */
const size_t next = static_cast<size_t>(
std::min(static_cast<int>(*nearest_index) + 1, static_cast<int>(traj_size) - 1));
const size_t prev =
static_cast<size_t>(std::max(static_cast<int>(*nearest_index) - 1, static_cast<int>(0)));
const double dist_to_next = calcSquaredDist(self_pose, traj, next);
const double dist_to_prev = calcSquaredDist(self_pose, traj, prev);
const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev;

const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index);
const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index);
const double dx3 = traj.x.at(*nearest_index) - traj.x.at(second_nearest_index);
const double dy3 = traj.y.at(*nearest_index) - traj.y.at(second_nearest_index);
const double c_sq = dx3 * dx3 + dy3 * dy3;
const auto [prev, next] = [&]() -> std::pair<size_t, size_t> {
if (*nearest_index == 0) {
return std::make_pair(0, 1);
}
if (*nearest_index == traj_size - 1) {
return std::make_pair(traj_size - 2, traj_size - 1);
}

geometry_msgs::msg::Point nearest_traj_point;
nearest_traj_point.x = traj.x.at(*nearest_index);
nearest_traj_point.y = traj.y.at(*nearest_index);
geometry_msgs::msg::Point next_nearest_traj_point;
next_nearest_traj_point.x = traj.x.at(*nearest_index + 1);
next_nearest_traj_point.y = traj.y.at(*nearest_index + 1);

const double signed_length =
calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position);
if (signed_length <= 0) {
return std::make_pair(*nearest_index - 1, *nearest_index);
}
return std::make_pair(*nearest_index, *nearest_index + 1);
}();

geometry_msgs::msg::Point next_traj_point;
next_traj_point.x = traj.x.at(next);
next_traj_point.y = traj.y.at(next);
geometry_msgs::msg::Point prev_traj_point;
prev_traj_point.x = traj.x.at(prev);
prev_traj_point.y = traj.y.at(prev);
const double traj_seg_length =
tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point);
/* if distance between two points are too close */
if (c_sq < 1.0E-5) {
if (traj_seg_length < 1.0E-5) {
nearest_pose->position.x = traj.x.at(*nearest_index);
nearest_pose->position.y = traj.y.at(*nearest_index);
nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));
Expand All @@ -351,18 +391,15 @@ bool calcNearestPoseInterp(
}

/* linear interpolation */
const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0);
nearest_pose->position.x =
alpha * traj.x.at(*nearest_index) + (1 - alpha) * traj.x.at(second_nearest_index);
nearest_pose->position.y =
alpha * traj.y.at(*nearest_index) + (1 - alpha) * traj.y.at(second_nearest_index);
const double tmp_yaw_err =
normalizeRadian(traj.yaw.at(*nearest_index) - traj.yaw.at(second_nearest_index));
const double nearest_yaw =
normalizeRadian(traj.yaw.at(second_nearest_index) + alpha * tmp_yaw_err);
const double ratio = std::clamp(
calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length,
0.0, 1.0);
nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next);
nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next);
const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next));
const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err);
nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw);
*nearest_time = alpha * traj.relative_time.at(*nearest_index) +
(1 - alpha) * traj.relative_time.at(second_nearest_index);
*nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next);
return true;
}

Expand Down
22 changes: 16 additions & 6 deletions control/mpc_lateral_controller/test/test_mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,9 @@ class MPCTest : public ::testing::Test
mpc.initializeSteeringPredictor();

// Init trajectory
mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param);
const auto current_kinematics =
makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0);
mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
}

nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity)
Expand Down Expand Up @@ -221,7 +223,9 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)

// Init parameters and reference trajectory
initializeMPC(mpc);
mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param);
const auto current_kinematics =
makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0);
mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics);

// Calculate MPC
AckermannLateralCommand ctrl_cmd;
Expand All @@ -237,7 +241,8 @@ TEST_F(MPCTest, OsqpCalculate)
{
MPC mpc;
initializeMPC(mpc);
mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param);
const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0);
mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);

std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
std::make_shared<KinematicsBicycleModel>(wheelbase, steer_limit, steer_tau);
Expand All @@ -262,7 +267,9 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
{
MPC mpc;
initializeMPC(mpc);
mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param);
const auto current_kinematics =
makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0);
mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics);

std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
std::make_shared<KinematicsBicycleModel>(wheelbase, steer_limit, steer_tau);
Expand Down Expand Up @@ -300,7 +307,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
// Init filters
mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
// Init trajectory
mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param);
const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0);
mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics);
// Calculate MPC
AckermannLateralCommand ctrl_cmd;
Trajectory pred_traj;
Expand All @@ -315,7 +323,9 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
{
MPC mpc;
initializeMPC(mpc);
mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param);
const auto current_kinematics =
makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0);
mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics);

std::shared_ptr<VehicleModelInterface> vehicle_model_ptr =
std::make_shared<KinematicsBicycleModelNoDelay>(wheelbase, steer_limit);
Expand Down
Loading