Skip to content

21.3.0

Choose a tag to compare

View all tags
@github-actions github-actions released this 22 Dec 10:32
21.3.0
18edff9

Description

Abstract

This PR fixes critical issues in FollowWaypointController. It implements physics-based jerk-constrained acceleration, improves distance integration, and handles edge cases. These changes eliminate acceleration oscillations, improve waypoint arrival accuracy, and ensure smooth deceleration after trajectory completion.

Note: This PR builds on top of refactor PR. The refactor PR simplified FollowTrajectoryAction architecture. This PR fixes fundamental acceleration optimization issues.

Background

The FollowWaypointController had two critical problems:

  1. Acceleration oscillations and contradictory limits - The algorithm produced oscillating acceleration values between steps. getAccelerationLimits() often returned invalid ranges where local_max_acceleration < local_min_acceleration. Both problems came from not properly handling jerk constraints (rate of acceleration change).

  2. Distance calculation errors - Used simple Euler integration (distance += speed * time). This is inaccurate when acceleration changes. It caused systematic underestimation of distances.

These issues caused unreliable trajectory following. The contradictory limits occurred frequently. This showed fundamental algorithmic problems that needed proper solutions, not workarounds.

Additionally, this PR implements missing functionality (final waypoint braking TODO from refactor PR) and improves code clarity through refactoring.

Details

1. Fix Acceleration Oscillations And Contradictory Limits (Critical)

Issue Visualization

Before fix: Severe oscillations in acceleration (±3 m/s² jerk oscillations)

plot_before

After fix: Smooth, continuous deceleration respecting jerk constraints

plot_after

Root Cause

The core problem was incorrect getAccelerationLimits() implementation. It computed acceleration bounds without proper jerk constraints.

The fundamental flaw:

Old implementation used time_for_non_acceleration (time to reduce acceleration to 0.0) in physically incorrect formulas:

// Old code - treats acceleration as constant during deceleration
return std::max(-speed / time_for_non_acceleration, local_min_acceleration);

Why this is wrong: It assumes constant acceleration during deceleration. But acceleration actually changes (ramps with jerk limits). This produced contradictory limits (local_max < local_min), oscillations, and prediction errors.

Solution: Physics-Based Acceleration Computation

New method: accelerationWithJerkConstraint()

Replaces the flawed formula with correct physics-based calculation:

auto FollowWaypointController::accelerationWithJerkConstraint(
  const double current_speed, const double target_speed, const double acceleration_rate) const
  -> double
{
  const auto delta_speed = target_speed - current_speed;
  const auto jerk_magnitude = std::abs(acceleration_rate);

  // Estimate steps needed: N = round(sqrt(2·|Δv| / j) / dt)
  const auto minimum_discrete_steps =
    std::max(1.0, std::round(std::sqrt(2 * std::abs(delta_speed) / jerk_magnitude) / step_time));

  // Base acceleration + jerk correction for acceleration ramping
  const auto base_acceleration = delta_speed / (minimum_discrete_steps * step_time);
  const auto sign = (delta_speed >= 0.0) ? 1.0 : -1.0;
  const auto jerk_correction =
    sign * jerk_magnitude * step_time * (minimum_discrete_steps - 1.0) / 2.0;

  return base_acceleration + jerk_correction;
}

How it works:

  1. Estimates steps N needed to reach target speed
  2. Computes base acceleration
  3. Adds jerk correction for acceleration ramping

Full mathematical derivation is in method documentation in follow_waypoint_controller.hpp.

Usage in getAccelerationLimits():

// NEW implementation
const auto min_acceleration_zero_speed =
    accelerationWithJerkConstraint(speed, 0.0, max_deceleration_rate);
const auto max_acceleration_target_speed =
    accelerationWithJerkConstraint(speed, target_speed, max_acceleration_rate);

const auto local_min_acceleration = std::max(
    std::max(min_acceleration_zero_speed, -max_deceleration),
    acceleration - max_deceleration_rate * step_time);
const auto local_max_acceleration = std::min(
    std::min(max_acceleration_target_speed, max_acceleration),
    acceleration + max_acceleration_rate * step_time);

Impact:

  • Eliminates contradictory limits - No more local_max < local_min. The old workaround that returned {local_max, local_max} was removed. Replaced with exception: 
    // NEW: Can safely throw exception because condition no longer occurs
if (local_max_acceleration + local_epsilon < local_min_acceleration - local_epsilon) {
  throw ControllerError("Contradictory acceleration limits...");
}
  • Eliminates oscillations - Consistent bounds across steps → consistent acceleration selection
  • Respects physics - All acceleration changes bounded by jerk limits
  • Enables validation - The exception was never thrown in extensive testing. This proves the algorithm is now correct.

2. Distance Integration Improvements

Core Change: Euler → Trapezoidal Integration

Replaced Euler integration (uses initial speed only) with trapezoidal integration (uses average speed). This eliminates systematic underestimation errors. Improves accuracy of waypoint arrival predictions.

All locations updated:

  1. PredictedEntityStatus::step() (prediction in waypoint controller):
// OLD: traveled_distance += speed * step_time;
// NEW:
traveled_distance += (current_speed + desired_speed) * 0.5 * step_time;
  1. follow_trajectory.cpp - pose update:
// OLD: updated_status.pose.position += current_velocity * step_time;
// NEW:
updated_status.pose.position += (current_velocity + desired_velocity) * 0.5 * step_time;

Mathematical justification: For constant acceleration:

  • Trapezoidal method d = (v₀ + v₁)/2 · t is exact (error = 0)
  • Euler method d = v₀·t underestimates by ½·a·t²

Data Structure Change: PredictedState → PredictedEntityStatus

Motivation: Enable using same distance calculation functions as FollowTrajectoryAction during prediction. This allows algorithm validation with lanelet-aware mode and production performance with fast 1D kinematics.

Before:

struct PredictedState {
  double acceleration, speed, traveled_distance, travel_time;

  auto moveStraight(const double step_acceleration, const double step_time) -> void {
    speed += acceleration * step_time;
    traveled_distance += speed * step_time;  // Euler integration - inaccurate
  }
}

After:

struct PredictedEntityStatus {
  static constexpr bool use_distance_along_lanelet = false;

  template <typename UpdateEntityStatusFunc, typename DistanceAlongLaneletFunc>
  auto step(const double step_acceleration, const double step_time, ...) -> void {
    const auto current_speed = entity_status_.action_status.twist.linear.x;
    const auto desired_speed = current_speed + step_acceleration * step_time;

    if (use_distance_along_lanelet) {
      // High accuracy mode: use lanelet-aware distance calculation
      entity_status_ = update_entity_status(entity_status_, desired_velocity);
      traveled_distance += distance_along_lanelet(current_position, entity_status_.pose.position);
    } else {
      // Fast mode: 1D kinematic model with trapezoidal integration
      entity_status_.action_status.twist.linear.x = desired_speed;
      entity_status_.action_status.accel.linear.x = (desired_speed - current_speed) / step_time;
      traveled_distance += (current_speed + desired_speed) * 0.5 * step_time;
    }
    travel_time += step_time;
  }

private:
  traffic_simulator_msgs::msg::EntityStatus entity_status_;
}

Key changes:

  1. Now encapsulates full EntityStatus instead of just scalar values (acceleration, speed)
  2. Uses same functions as FollowTrajectoryAction - update_entity_status() and distance_along_lanelet()
  3. Dual-mode operation via use_distance_along_lanelet flag:
    • true: High-accuracy lanelet-aware distance (for validation/debugging)
    • false: Fast 1D kinematics with trapezoidal integration (default/production)

Validation Testing (400 Autoware runs)

For detailed test methodology and complete results, see discussion in RJD-1921.

Summary:

  • Lanelet-aware mode: 399/400 passed, 92.5% stop before target (avg 0.023mm), 1686x slower (0.119712s/step)
  • 1D kinematics (default): 400/400 passed, 99% overshoot (avg 0.707mm, max 1.919mm), fast (0.000071s/step)

Decision: Use 1D kinematics by default for performance (1686x faster). Accuracy is acceptable (all passed, <2mm overshoot << 100mm tolerance). Robust overshoot handling prevents issues. Lanelet-aware mode validates algorithm correctness. It remains available for debugging.

Function signature changes:

To support new PredictedEntityStatus, function signatures changed from scalar parameters to EntityStatus:

// OLD
auto getAcceleration(const double remaining_distance, const double acceleration, const double speed) const;

// NEW
auto getAcceleration(
  const double remaining_distance,
  const traffic_simulator_msgs::msg::EntityStatus & entity_status,
  const std::function<...> & update_entity_status,
  const std::function<...> & distance_along_lanelet) const;

Similar changes in getPredictedStopEntityStatusWithoutConsideringTime() and getPredictedWaypointArrivalState().

3. Edge Case Handling via clampAcceleration()

Context: FollowTrajectoryAction and FollowWaypointController can be invoked at any time with any entity state and any trajectory. The entity may have:

  • Negative speed (moving backward from previous action)
  • Speed exceeding trajectory's target speed (speed from previous action)

Design requirement: Controller must gracefully handle these abnormal initial conditions. It must smoothly transition entity to valid motion. It should not fail or produce unrealistic behavior.

Key architectural change: clampAcceleration() was moved earlier in call chain. It now handles abnormal states before calling getAccelerationLimits().

Why this matters: getAccelerationLimits() assumes 0 <= speed <= target_speed. Pre-filtering handles abnormal states (negative speed, overspeed) before calling it. This prevents exceptions.

Handles two edge cases:

  1. Negative speed: Apply jerk-limited acceleration to reach speed = 0.0 with acc = 0.0
  2. Overspeed: Apply jerk-limited deceleration to reach target_speed with acc = 0.0

Both ensure smooth recovery. No violation of preconditions. No abrupt changes.

Implementation:

auto FollowWaypointController::clampAcceleration(
  const double candidate_acceleration, const double acceleration, const double speed) const -> double
{
  // Edge case 1: Negative speed (moving backward)
  if (speed + (acceleration + max_acceleration_rate * step_time) * step_time < 0.0) {
    return std::min(
      accelerationWithJerkConstraint(speed, 0.0, max_acceleration_rate), max_acceleration);

  // Edge case 2: Overspeed (speed > target_speed)
  } else if (speed + (acceleration - max_deceleration_rate * step_time) * step_time > target_speed) {
    return std::max(
      accelerationWithJerkConstraint(speed, target_speed, max_deceleration_rate),
      -max_deceleration);

  // Normal case: clamp to acceleration limits
  } else {
    auto [local_min_acceleration, local_max_acceleration] = getAccelerationLimits(acceleration, speed);
    return std::clamp(candidate_acceleration, local_min_acceleration, local_max_acceleration);
  }
}

4. Refactor Acceleration Candidate Selection Logic

Old implementation:

if (const auto distance_diff = remaining_distance - predicted_state_opt->traveled_distance;
    (distance_diff >= 0 || min_distance_diff < 0) &&
    (std::abs(distance_diff) < std::abs(min_distance_diff))) {
  min_distance_diff = distance_diff;
  best_acceleration = candidate_acceleration;
}

Hard to read. Logic hidden in combined conditions.

New implementation:

const auto distance_diff = remaining_distance - predicted_state_opt->traveled_distance;

const bool current_candidate_stops_before_target = distance_diff >= 0;
const bool best_candidate_stops_before_target = min_distance_diff >= 0;

bool should_update_best_candidate = false;
if (current_candidate_stops_before_target && best_candidate_stops_before_target) {
  /// Both stop before target - choose closer to target
  should_update_best_candidate = (distance_diff < min_distance_diff);
} else if (current_candidate_stops_before_target && !best_candidate_stops_before_target) {
  /// Current stops before, best overshoots - always prefer stopping before
  should_update_best_candidate = true;
} else if (!current_candidate_stops_before_target && !best_candidate_stops_before_target) {
  /// Both overshoot - choose smaller overshoot
  should_update_best_candidate = (std::abs(distance_diff) < std::abs(min_distance_diff));
}

if (should_update_best_candidate) {
  min_distance_diff = distance_diff;
  best_acceleration = candidate_acceleration;
}

Impact: Same logic but clearer. Each case explicitly documented. Easier to understand and maintain.

5. Implement Missing Final Waypoint Braking

Context: Refactor PR left TODO case - vehicle within distance threshold but still moving toward final waypoint.

Old behavior (refactor PR):

if (distance <= distance_threshold) {
  /// @todo distance within threshold but vehicle still moving - apply constrained braking
}

Vehicle continues moving. No braking applied. Eventually stops but without controlled deceleration.

New behavior: Apply jerk-constrained braking when vehicle enters distance threshold while still moving.

Implementation:

Added constrained_brake_velocity() helper:

const auto constrained_brake_velocity = [&behavior_parameter, step_time](
                                          const double speed, const auto & orientation) {
  constexpr double target_breaking_speed = 0.0;
  const auto controller =
    FollowWaypointController(behavior_parameter, step_time, true, target_breaking_speed);
  const auto deceleration = std::max(
    controller.accelerationWithJerkConstraint(
      speed, target_breaking_speed, behavior_parameter.dynamic_constraints.max_deceleration_rate),
    -behavior_parameter.dynamic_constraints.max_deceleration);
  return scalarToDirectionVector(speed + deceleration * step_time, orientation);
};

Applied in final waypoint handling:

if (distance <= distance_threshold) {
  log_waypoint_action("Within threshold but moving", "Brake");
  return update_entity_status(
    entity_status,
    constrained_brake_velocity(
      entity_status.action_status.twist.linear.x, entity_status.pose.orientation));
}

Impact: Vehicle smoothly decelerates to full stop when reaching final waypoint. Uses physics-based jerk constraints. No abrupt changes. Completes trajectory sequence gracefully.

References

Development context: These fixes address fundamental issues discovered during investigation of waypoint overshoot problems in FollowTrajectoryAction. While refactor PR simplified arrival detection logic, underlying acceleration optimization issues required separate attention and proper physics-based solutions.

For detailed analysis, test results, and development notes, see discussion in RJD-1921

Known Limitations

  1. Distance calculation mode - use_distance_along_lanelet = false by default in PredictedEntityStatus. The 1D kinematic model is:

    • Much faster (1686x in testing) than lanelet-aware calculation
    • Less accurate (99% overshoot rate with avg 0.707mm vs 92.5% stop before with avg 0.023mm for lanelet-aware)

  2. Jerk constraint approximation - The discrete-time jerk constraint uses N = round(sqrt(2·|Δv| / j) / dt). This is an approximation. For very small time steps or speed changes, rounding errors may accumulate. In practice, this has not caused issues with typical step_time = 0.03333s and speed ranges 0-20 m/s.

Related Issues

Assets 2
Loading