Skip to content

Commit

Permalink
fix(mpc): precise resampling around ego for stabling control (autowar…
Browse files Browse the repository at this point in the history
…efoundation#5003)

* fix(mpc): precise resampling around ego

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* update test

Signed-off-by: Takayuki Murooka <[email protected]>

* Update control/mpc_lateral_controller/src/mpc_utils.cpp

Co-authored-by: Takamasa Horibe <[email protected]>

* Update control/mpc_lateral_controller/src/mpc_utils.cpp

Co-authored-by: Takamasa Horibe <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>
Co-authored-by: Takamasa Horibe <[email protected]>
  • Loading branch information
takayuki5168 and TakaHoribe authored Sep 21, 2023
1 parent 7956761 commit 08c809c
Show file tree
Hide file tree
Showing 7 changed files with 114 additions and 57 deletions.
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 @@ -187,7 +187,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 @@ -175,13 +175,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 @@ -391,13 +398,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

0 comments on commit 08c809c

Please sign in to comment.