apollo::control::LatController类 参考

LQR-Based lateral controller, to compute steering target. 更多...

#include <lat_controller.h>

类 apollo::control::LatController 继承关系图:

apollo::control::LatController 的协作图:

Public 成员函数
	LatController ()
	constructor
virtual	~LatController ()
	destructor
common::Status	Init (std::shared_ptr< DependencyInjector > injector) override
	initialize Lateral Controller
common::Status	ComputeControlCommand (const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory, ControlCommand *cmd) override
	compute steering target based on current vehicle status and target trajectory
common::Status	Reset () override
	reset Lateral Controller
void	Stop () override
	stop Lateral controller
std::string	Name () const override
	Lateral controller name
Public 成员函数 继承自 apollo::control::ControlTask
	ControlTask ()=default
	constructor
virtual	~ControlTask ()=default
	destructor

Protected 成员函数
void	UpdateState (SimpleLateralDebug *debug, const canbus::Chassis *chassis)
void	UpdateDrivingOrientation ()
void	UpdateMatrix ()
void	UpdateMatrixCompound ()
double	ComputeFeedForward (double ref_curvature) const
void	ComputeLateralErrors (const double x, const double y, const double theta, const double linear_v, const double angular_v, const double linear_a, const TrajectoryAnalyzer &trajectory_analyzer, SimpleLateralDebug *debug, const canbus::Chassis *chassis)
bool	LoadControlConf ()
void	InitializeFilters ()
void	LoadLatGainScheduler ()
void	LogInitParameters ()
void	ProcessLogs (const SimpleLateralDebug *debug, const canbus::Chassis *chassis)
void	CloseLogFile ()
Protected 成员函数 继承自 apollo::control::ControlTask
template<typename T >
bool	LoadConfig (T *config)
bool	LoadCalibrationTable (calibration_table *calibration_table_conf)

Protected 属性
LatBaseLqrControllerConf	lat_based_lqr_controller_conf_
common::VehicleParam	vehicle_param_
TrajectoryAnalyzer	trajectory_analyzer_
double	ts_ = 0.0
double	cf_ = 0.0
double	cr_ = 0.0
double	wheelbase_ = 0.0
double	mass_ = 0.0
double	lf_ = 0.0
double	lr_ = 0.0
double	iz_ = 0.0
double	steer_ratio_ = 0.0
double	steer_single_direction_max_degree_ = 0.0
double	max_lat_acc_ = 0.0
int	preview_window_ = 0
double	lookahead_station_low_speed_ = 0.0
double	lookback_station_low_speed_ = 0.0
double	lookahead_station_high_speed_ = 0.0
double	lookback_station_high_speed_ = 0.0
const int	basic_state_size_ = 4
Eigen::MatrixXd	matrix_a_
Eigen::MatrixXd	matrix_ad_
Eigen::MatrixXd	matrix_adc_
Eigen::MatrixXd	matrix_b_
Eigen::MatrixXd	matrix_bd_
Eigen::MatrixXd	matrix_bdc_
Eigen::MatrixXd	matrix_k_
Eigen::MatrixXd	matrix_r_
Eigen::MatrixXd	matrix_q_
Eigen::MatrixXd	matrix_q_updated_
Eigen::MatrixXd	matrix_a_coeff_
Eigen::MatrixXd	matrix_state_
int	lqr_max_iteration_ = 0
double	lqr_eps_ = 0.0
common::DigitalFilter	digital_filter_
std::unique_ptr< Interpolation1D >	lat_err_interpolation_
std::unique_ptr< Interpolation1D >	heading_err_interpolation_
common::MeanFilter	lateral_error_filter_
common::MeanFilter	heading_error_filter_
bool	enable_leadlag_ = false
LeadlagController	leadlag_controller_
bool	enable_mrac_ = false
MracController	mrac_controller_
bool	enable_look_ahead_back_control_ = false
double	previous_lateral_acceleration_ = 0.0
double	previous_heading_rate_ = 0.0
double	previous_ref_heading_rate_ = 0.0
double	previous_heading_acceleration_ = 0.0
double	previous_ref_heading_acceleration_ = 0.0
std::ofstream	steer_log_file_
const std::string	name_
double	query_relative_time_
double	pre_steer_angle_ = 0.0
double	pre_steering_position_ = 0.0
double	minimum_speed_protection_ = 0.1
double	current_trajectory_timestamp_ = -1.0
double	init_vehicle_x_ = 0.0
double	init_vehicle_y_ = 0.0
double	init_vehicle_heading_ = 0.0
double	low_speed_bound_ = 0.0
double	low_speed_window_ = 0.0
double	driving_orientation_ = 0.0
std::shared_ptr< DependencyInjector >	injector_

详细描述

LQR-Based lateral controller, to compute steering target.

For more details, please refer to "Vehicle dynamics and control." Rajamani, Rajesh. Springer Science & Business Media, 2011.

在文件 lat_controller.h 第 57 行定义.

构造及析构函数说明

LatController()

apollo::control::LatController::LatController

(

)

constructor

在文件 lat_controller.cc 第 75 行定义.

                             : name_("LQR-based Lateral Controller") {
  if (FLAGS_enable_csv_debug) {
    steer_log_file_.open(GetLogFileName());
    steer_log_file_ << std::fixed;
    steer_log_file_ << std::setprecision(6);
    WriteHeaders(steer_log_file_);
  }
  AINFO << "Using " << name_;
}

~LatController()

apollo::control::LatController::~LatController ( )

virtual

destructor

在文件 lat_controller.cc 第 85 行定义.

{ CloseLogFile(); }

成员函数说明

CloseLogFile()

void apollo::control::LatController::CloseLogFile ( )

protected

在文件 lat_controller.cc 第 271 行定义.

                                 {
  if (FLAGS_enable_csv_debug && steer_log_file_.is_open()) {
    steer_log_file_.close();
  }
}

ComputeControlCommand()

Status apollo::control::LatController::ComputeControlCommand ( const localization::LocalizationEstimate *  localization, const canbus::Chassis *  chassis, const planning::ADCTrajectory *  trajectory, ControlCommand *  cmd  )

overridevirtual

compute steering target based on current vehicle status and target trajectory

参数

localization	vehicle location
chassis	vehicle status e.g., speed, acceleration
trajectory	trajectory generated by planning
cmd	control command

返回

Status computation status

实现了 apollo::control::ControlTask.

在文件 lat_controller.cc 第 304 行定义.

                         {
  auto vehicle_state = injector_->vehicle_state();
  auto previous_lon_debug = injector_->Get_previous_lon_debug_info();
  auto target_tracking_trajectory = *planning_published_trajectory;
 
  if (FLAGS_use_navigation_mode &&
      lat_based_lqr_controller_conf_.enable_navigation_mode_position_update()) {
    auto time_stamp_diff =
        planning_published_trajectory->header().timestamp_sec() -
        current_trajectory_timestamp_;
 
    auto curr_vehicle_x = localization->pose().position().x();
    auto curr_vehicle_y = localization->pose().position().y();
 
    double curr_vehicle_heading = 0.0;
    const auto &orientation = localization->pose().orientation();
    if (localization->pose().has_heading()) {
      curr_vehicle_heading = localization->pose().heading();
    } else {
      curr_vehicle_heading =
          common::math::QuaternionToHeading(orientation.qw(), orientation.qx(),
                                            orientation.qy(), orientation.qz());
    }
 
    // new planning trajectory
    if (time_stamp_diff > 1.0e-6) {
      init_vehicle_x_ = curr_vehicle_x;
      init_vehicle_y_ = curr_vehicle_y;
      init_vehicle_heading_ = curr_vehicle_heading;
 
      current_trajectory_timestamp_ =
          planning_published_trajectory->header().timestamp_sec();
    } else {
      auto x_diff_map = curr_vehicle_x - init_vehicle_x_;
      auto y_diff_map = curr_vehicle_y - init_vehicle_y_;
      auto theta_diff = curr_vehicle_heading - init_vehicle_heading_;
 
      auto cos_map_veh = std::cos(init_vehicle_heading_);
      auto sin_map_veh = std::sin(init_vehicle_heading_);
 
      auto x_diff_veh = cos_map_veh * x_diff_map + sin_map_veh * y_diff_map;
      auto y_diff_veh = -sin_map_veh * x_diff_map + cos_map_veh * y_diff_map;
 
      auto cos_theta_diff = std::cos(-theta_diff);
      auto sin_theta_diff = std::sin(-theta_diff);
 
      auto tx = -(cos_theta_diff * x_diff_veh - sin_theta_diff * y_diff_veh);
      auto ty = -(sin_theta_diff * x_diff_veh + cos_theta_diff * y_diff_veh);
 
      auto ptr_trajectory_points =
          target_tracking_trajectory.mutable_trajectory_point();
      std::for_each(
          ptr_trajectory_points->begin(), ptr_trajectory_points->end(),
          [&cos_theta_diff, &sin_theta_diff, &tx, &ty,
           &theta_diff](common::TrajectoryPoint &p) {
            auto x = p.path_point().x();
            auto y = p.path_point().y();
            auto theta = p.path_point().theta();
 
            auto x_new = cos_theta_diff * x - sin_theta_diff * y + tx;
            auto y_new = sin_theta_diff * x + cos_theta_diff * y + ty;
            auto theta_new = common::math::NormalizeAngle(theta - theta_diff);
 
            p.mutable_path_point()->set_x(x_new);
            p.mutable_path_point()->set_y(y_new);
            p.mutable_path_point()->set_theta(theta_new);
          });
    }
  }
 
  trajectory_analyzer_ =
      std::move(TrajectoryAnalyzer(&target_tracking_trajectory));
 
  // Transform the coordinate of the planning trajectory from the center of the
  // rear-axis to the center of mass, if conditions matched
  if (((lat_based_lqr_controller_conf_.trajectory_transform_to_com_reverse() &&
        vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) ||
       (lat_based_lqr_controller_conf_.trajectory_transform_to_com_drive() &&
        vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE)) &&
      enable_look_ahead_back_control_) {
    trajectory_analyzer_.TrajectoryTransformToCOM(lr_);
  }
 
  // Re-build the vehicle dynamic models at reverse driving (in particular,
  // replace the lateral translational motion dynamics with the corresponding
  // kinematic models)
  if (vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) {
    /*
    A matrix (Gear Reverse)
    [0.0, 0.0, 1.0 * v 0.0;
     0.0, (-(c_f + c_r) / m) / v, (c_f + c_r) / m,
     (l_r * c_r - l_f * c_f) / m / v;
     0.0, 0.0, 0.0, 1.0;
     0.0, ((lr * cr - lf * cf) / i_z) / v, (l_f * c_f - l_r * c_r) / i_z,
     (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
    */
    cf_ = -lat_based_lqr_controller_conf_.cf();
    cr_ = -lat_based_lqr_controller_conf_.cr();
    matrix_a_(0, 1) = 0.0;
    matrix_a_coeff_(0, 2) = 1.0;
  } else {
    /*
    A matrix (Gear Drive)
    [0.0, 1.0, 0.0, 0.0;
     0.0, (-(c_f + c_r) / m) / v, (c_f + c_r) / m,
     (l_r * c_r - l_f * c_f) / m / v;
     0.0, 0.0, 0.0, 1.0;
     0.0, ((lr * cr - lf * cf) / i_z) / v, (l_f * c_f - l_r * c_r) / i_z,
     (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
    */
    cf_ = lat_based_lqr_controller_conf_.cf();
    cr_ = lat_based_lqr_controller_conf_.cr();
    matrix_a_(0, 1) = 1.0;
    matrix_a_coeff_(0, 2) = 0.0;
  }
  matrix_a_(1, 2) = (cf_ + cr_) / mass_;
  matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
  matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
  matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
  matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
  matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;
 
  /*
  b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
  */
  matrix_b_(1, 0) = cf_ / mass_;
  matrix_b_(3, 0) = lf_ * cf_ / iz_;
  matrix_bd_ = matrix_b_ * ts_;
 
  UpdateDrivingOrientation();
 
  SimpleLateralDebug *debug = cmd->mutable_debug()->mutable_simple_lat_debug();
  debug->Clear();
 
  // Update state = [Lateral Error, Lateral Error Rate, Heading Error, Heading
  // Error Rate, preview lateral error1 , preview lateral error2, ...]
  UpdateState(debug, chassis);
 
  UpdateMatrix();
 
  // Compound discrete matrix with road preview model
  UpdateMatrixCompound();
 
  // Adjust matrix_q_updated when in reverse gear
  int q_param_size = lat_based_lqr_controller_conf_.matrix_q_size();
  int reverse_q_param_size =
      lat_based_lqr_controller_conf_.reverse_matrix_q_size();
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
    for (int i = 0; i < reverse_q_param_size; ++i) {
      matrix_q_(i, i) = lat_based_lqr_controller_conf_.reverse_matrix_q(i);
    }
  } else {
    for (int i = 0; i < q_param_size; ++i) {
      matrix_q_(i, i) = lat_based_lqr_controller_conf_.matrix_q(i);
    }
  }
 
  uint num_iteration;
  double result_diff;
  // Add gain scheduler for higher speed steering
  if (FLAGS_enable_gain_scheduler) {
    matrix_q_updated_(0, 0) =
        matrix_q_(0, 0) * lat_err_interpolation_->Interpolate(
                              std::fabs(vehicle_state->linear_velocity()));
    matrix_q_updated_(2, 2) =
        matrix_q_(2, 2) * heading_err_interpolation_->Interpolate(
                              std::fabs(vehicle_state->linear_velocity()));
    common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,
                                  matrix_r_, lqr_eps_, lqr_max_iteration_,
                                  &matrix_k_, &num_iteration, &result_diff);
  } else {
    common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_,
                                  matrix_r_, lqr_eps_, lqr_max_iteration_,
                                  &matrix_k_, &num_iteration, &result_diff);
  }
 
  ADEBUG << "LQR num_iteration is " << num_iteration
        << ", max iteration threshold is " << lqr_max_iteration_
        << "; result_diff is " << result_diff;
 
  // feedback = - K * state
  // Convert vehicle steer angle from rad to degree and then to steer degree
  // then to 100% ratio
  const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /
                                      M_PI * steer_ratio_ /
                                      steer_single_direction_max_degree_ * 100;
 
  const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());
 
  double steer_angle = 0.0;
  double steer_angle_feedback_augment = 0.0;
  // Augment the feedback control on lateral error at the desired speed domain
  if (enable_leadlag_) {
    if (lat_based_lqr_controller_conf_
            .enable_feedback_augment_on_high_speed() ||
        std::fabs(vehicle_state->linear_velocity()) < low_speed_bound_) {
      steer_angle_feedback_augment =
          leadlag_controller_.Control(-matrix_state_(0, 0), ts_) * 180 / M_PI *
          steer_ratio_ / steer_single_direction_max_degree_ * 100;
      if (std::fabs(vehicle_state->linear_velocity()) >
          low_speed_bound_ - low_speed_window_) {
        // Within the low-high speed transition window, linerly interplolate the
        // augment control gain for "soft" control switch
        steer_angle_feedback_augment = common::math::lerp(
            steer_angle_feedback_augment, low_speed_bound_ - low_speed_window_,
            0.0, low_speed_bound_, std::fabs(vehicle_state->linear_velocity()));
      }
    }
  }
  steer_angle = steer_angle_feedback + steer_angle_feedforward +
                steer_angle_feedback_augment;
 
  // Compute the steering command limit with the given maximum lateral
  // acceleration
  const double steer_limit =
      FLAGS_set_steer_limit ? std::atan(max_lat_acc_ * wheelbase_ /
                                        (vehicle_state->linear_velocity() *
                                         vehicle_state->linear_velocity())) *
                                  steer_ratio_ * 180 / M_PI /
                                  steer_single_direction_max_degree_ * 100
                            : 100.0;
 
  const double steer_diff_with_max_rate =
      lat_based_lqr_controller_conf_.enable_maximum_steer_rate_limit()
          ? vehicle_param_.max_steer_angle_rate() * ts_ * 180 / M_PI /
                steer_single_direction_max_degree_ * 100
          : 100.0;
 
  const double steering_position = chassis->steering_percentage();
 
  // Re-compute the steering command if the MRAC control is enabled, with steer
  // angle limitation and steer rate limitation
  if (enable_mrac_) {
    const int mrac_model_order =
        lat_based_lqr_controller_conf_.steer_mrac_conf().mrac_model_order();
    Matrix steer_state = Matrix::Zero(mrac_model_order, 1);
    steer_state(0, 0) = chassis->steering_percentage();
    if (mrac_model_order > 1) {
      steer_state(1, 0) = (steering_position - pre_steering_position_) / ts_;
    }
    if (std::fabs(vehicle_state->linear_velocity()) >
        FLAGS_minimum_speed_resolution) {
      mrac_controller_.SetStateAdaptionRate(1.0);
      mrac_controller_.SetInputAdaptionRate(1.0);
    } else {
      mrac_controller_.SetStateAdaptionRate(0.0);
      mrac_controller_.SetInputAdaptionRate(0.0);
    }
    steer_angle = mrac_controller_.Control(
        steer_angle, steer_state, steer_limit, steer_diff_with_max_rate / ts_);
    // Set the steer mrac debug message
    MracDebug *mracdebug = debug->mutable_steer_mrac_debug();
    Matrix steer_reference = mrac_controller_.CurrentReferenceState();
    mracdebug->set_mrac_model_order(mrac_model_order);
    for (int i = 0; i < mrac_model_order; ++i) {
      mracdebug->add_mrac_reference_state(steer_reference(i, 0));
      mracdebug->add_mrac_state_error(steer_state(i, 0) -
                                      steer_reference(i, 0));
      mracdebug->mutable_mrac_adaptive_gain()->add_state_adaptive_gain(
          mrac_controller_.CurrentStateAdaptionGain()(i, 0));
    }
    mracdebug->mutable_mrac_adaptive_gain()->add_input_adaptive_gain(
        mrac_controller_.CurrentInputAdaptionGain()(0, 0));
    mracdebug->set_mrac_reference_saturation_status(
        mrac_controller_.ReferenceSaturationStatus());
    mracdebug->set_mrac_control_saturation_status(
        mrac_controller_.ControlSaturationStatus());
  }
  pre_steering_position_ = steering_position;
  debug->set_steer_mrac_enable_status(enable_mrac_);
 
  // Clamp the steer angle with steer limitations at current speed
  double steer_angle_limited =
      common::math::Clamp(steer_angle, -steer_limit, steer_limit);
  steer_angle = steer_angle_limited;
  debug->set_steer_angle_limited(steer_angle_limited);
 
  // Limit the steering command with the designed digital filter
  steer_angle = digital_filter_.Filter(steer_angle);
  steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);
 
  // Check if the steer is locked and hence the previous steer angle should be
  // executed
  if (injector_->vehicle_state()->gear() != canbus::Chassis::GEAR_REVERSE) {
    if ((std::abs(vehicle_state->linear_velocity()) <
             lat_based_lqr_controller_conf_.lock_steer_speed() ||
         previous_lon_debug->path_remain() <= 0) &&
        vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE &&
        chassis->driving_mode() == canbus::Chassis::COMPLETE_AUTO_DRIVE) {
      ADEBUG << "Into lock steer, path_remain is "
             << previous_lon_debug->path_remain() << "linear_velocity is "
             << vehicle_state->linear_velocity();
      steer_angle = pre_steer_angle_;
    }
  }
 
  // Set the steer commands
  cmd->set_steering_target(common::math::Clamp(
      steer_angle, pre_steer_angle_ - steer_diff_with_max_rate,
      pre_steer_angle_ + steer_diff_with_max_rate));
  cmd->set_steering_rate(FLAGS_steer_angle_rate);
 
  pre_steer_angle_ = cmd->steering_target();
 
  // compute extra information for logging and debugging
  const double steer_angle_lateral_contribution =
      -matrix_k_(0, 0) * matrix_state_(0, 0) * 180 / M_PI * steer_ratio_ /
      steer_single_direction_max_degree_ * 100;
 
  const double steer_angle_lateral_rate_contribution =
      -matrix_k_(0, 1) * matrix_state_(1, 0) * 180 / M_PI * steer_ratio_ /
      steer_single_direction_max_degree_ * 100;
 
  const double steer_angle_heading_contribution =
      -matrix_k_(0, 2) * matrix_state_(2, 0) * 180 / M_PI * steer_ratio_ /
      steer_single_direction_max_degree_ * 100;
 
  const double steer_angle_heading_rate_contribution =
      -matrix_k_(0, 3) * matrix_state_(3, 0) * 180 / M_PI * steer_ratio_ /
      steer_single_direction_max_degree_ * 100;
 
  debug->set_heading(driving_orientation_);
  debug->set_steer_angle(steer_angle);
  debug->set_steer_angle_feedforward(steer_angle_feedforward);
  debug->set_steer_angle_lateral_contribution(steer_angle_lateral_contribution);
  debug->set_steer_angle_lateral_rate_contribution(
      steer_angle_lateral_rate_contribution);
  debug->set_steer_angle_heading_contribution(steer_angle_heading_contribution);
  debug->set_steer_angle_heading_rate_contribution(
      steer_angle_heading_rate_contribution);
  debug->set_steer_angle_feedback(steer_angle_feedback);
  debug->set_steer_angle_feedback_augment(steer_angle_feedback_augment);
  debug->set_steering_position(steering_position);
  debug->set_ref_speed(vehicle_state->linear_velocity());
 
  ProcessLogs(debug, chassis);
  return Status::OK();
}

ComputeFeedForward()

double apollo::control::LatController::ComputeFeedForward ( double  ref_curvature ) const

protected

在文件 lat_controller.cc 第 747 行定义.

                                                                   {
  const double kv =
      lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
 
  // Calculate the feedforward term of the lateral controller; then change it
  // from rad to %
  const double v = injector_->vehicle_state()->linear_velocity();
  double steer_angle_feedforwardterm;
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE &&
      !lat_based_lqr_controller_conf_.reverse_use_dynamic_model()) {
    steer_angle_feedforwardterm =
        lat_based_lqr_controller_conf_.reverse_feedforward_ratio() *
        wheelbase_ * ref_curvature * 180 / M_PI * steer_ratio_ /
        steer_single_direction_max_degree_ * 100;
  } else {
    steer_angle_feedforwardterm =
        (wheelbase_ * ref_curvature + kv * v * v * ref_curvature -
         matrix_k_(0, 2) *
             (lr_ * ref_curvature -
              lf_ * mass_ * v * v * ref_curvature / 2 / cr_ / wheelbase_)) *
        180 / M_PI * steer_ratio_ / steer_single_direction_max_degree_ * 100;
  }
 
  return steer_angle_feedforwardterm;
}

ComputeLateralErrors()

void apollo::control::LatController::ComputeLateralErrors ( const double  x, const double  y, const double  theta, const double  linear_v, const double  angular_v, const double  linear_a, const TrajectoryAnalyzer &  trajectory_analyzer, SimpleLateralDebug *  debug, const canbus::Chassis *  chassis  )

protected

在文件 lat_controller.cc 第 773 行定义.

                                  {
  TrajectoryPoint target_point;
 
  if (lat_based_lqr_controller_conf_.query_time_nearest_point_only()) {
    target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
        Clock::NowInSeconds() + query_relative_time_);
  } else {
    if (FLAGS_use_navigation_mode &&
        !lat_based_lqr_controller_conf_
             .enable_navigation_mode_position_update()) {
      target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
          Clock::NowInSeconds() + query_relative_time_);
    } else {
      target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
    }
  }
  const double dx = x - target_point.path_point().x();
  const double dy = y - target_point.path_point().y();
 
  debug->mutable_current_target_point()->mutable_path_point()->set_x(
      target_point.path_point().x());
  debug->mutable_current_target_point()->mutable_path_point()->set_y(
      target_point.path_point().y());
 
  ADEBUG << "x point: " << x << " y point: " << y;
  ADEBUG << "match point information : " << target_point.ShortDebugString();
 
  const double cos_target_heading = std::cos(target_point.path_point().theta());
  const double sin_target_heading = std::sin(target_point.path_point().theta());
 
  double lateral_error = cos_target_heading * dy - sin_target_heading * dx;
  if (lat_based_lqr_controller_conf_.enable_navigation_mode_error_filter()) {
    lateral_error = lateral_error_filter_.Update(lateral_error);
  }
 
  debug->set_lateral_error(lateral_error);
 
  debug->set_ref_heading(target_point.path_point().theta());
  double heading_error =
      common::math::NormalizeAngle(theta - debug->ref_heading());
  if (lat_based_lqr_controller_conf_.enable_navigation_mode_error_filter()) {
    heading_error = heading_error_filter_.Update(heading_error);
  }
  debug->set_heading_error(heading_error);
 
  // Within the low-high speed transition window, linerly interplolate the
  // lookahead/lookback station for "soft" prediction window switch
  double lookahead_station = 0.0;
  double lookback_station = 0.0;
  if (std::fabs(linear_v) >= low_speed_bound_) {
    lookahead_station = lookahead_station_high_speed_;
    lookback_station = lookback_station_high_speed_;
  } else if (std::fabs(linear_v) < low_speed_bound_ - low_speed_window_) {
    lookahead_station = lookahead_station_low_speed_;
    lookback_station = lookback_station_low_speed_;
  } else {
    lookahead_station = common::math::lerp(
        lookahead_station_low_speed_, low_speed_bound_ - low_speed_window_,
        lookahead_station_high_speed_, low_speed_bound_, std::fabs(linear_v));
    lookback_station = common::math::lerp(
        lookback_station_low_speed_, low_speed_bound_ - low_speed_window_,
        lookback_station_high_speed_, low_speed_bound_, std::fabs(linear_v));
  }
 
  // Estimate the heading error with look-ahead/look-back windows as feedback
  // signal for special driving scenarios
  double heading_error_feedback;
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
    heading_error_feedback = heading_error;
  } else {
    auto lookahead_point = trajectory_analyzer.QueryNearestPointByRelativeTime(
        target_point.relative_time() +
        lookahead_station /
            (std::max(std::fabs(linear_v), 0.1) * std::cos(heading_error)));
    heading_error_feedback = common::math::NormalizeAngle(
        heading_error + target_point.path_point().theta() -
        lookahead_point.path_point().theta());
  }
  debug->set_heading_error_feedback(heading_error_feedback);
 
  // Estimate the lateral error with look-ahead/look-back windows as feedback
  // signal for special driving scenarios
  double lateral_error_feedback;
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
    lateral_error_feedback =
        lateral_error - lookback_station * std::sin(heading_error);
  } else {
    lateral_error_feedback =
        lateral_error + lookahead_station * std::sin(heading_error);
  }
  debug->set_lateral_error_feedback(lateral_error_feedback);
 
  auto lateral_error_dot = linear_v * std::sin(heading_error);
  auto lateral_error_dot_dot = linear_a * std::sin(heading_error);
  if (FLAGS_reverse_heading_control) {
    if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
      lateral_error_dot = -lateral_error_dot;
      lateral_error_dot_dot = -lateral_error_dot_dot;
    }
  }
  auto centripetal_acceleration =
      linear_v * linear_v / wheelbase_ *
      std::tan(chassis->steering_percentage() / 100 *
               vehicle_param_.max_steer_angle() / steer_ratio_);
  debug->set_lateral_error_rate(lateral_error_dot);
  debug->set_lateral_acceleration(lateral_error_dot_dot);
  debug->set_lateral_centripetal_acceleration(centripetal_acceleration);
  debug->set_lateral_jerk(
      (debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
  previous_lateral_acceleration_ = debug->lateral_acceleration();
 
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
    debug->set_heading_rate(-angular_v);
  } else {
    debug->set_heading_rate(angular_v);
  }
  debug->set_ref_heading_rate(target_point.path_point().kappa() *
                              target_point.v());
  debug->set_heading_error_rate(debug->heading_rate() -
                                debug->ref_heading_rate());
 
  debug->set_heading_acceleration(
      (debug->heading_rate() - previous_heading_rate_) / ts_);
  debug->set_ref_heading_acceleration(
      (debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);
  debug->set_heading_error_acceleration(debug->heading_acceleration() -
                                        debug->ref_heading_acceleration());
  previous_heading_rate_ = debug->heading_rate();
  previous_ref_heading_rate_ = debug->ref_heading_rate();
 
  debug->set_heading_jerk(
      (debug->heading_acceleration() - previous_heading_acceleration_) / ts_);
  debug->set_ref_heading_jerk(
      (debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /
      ts_);
  debug->set_heading_error_jerk(debug->heading_jerk() -
                                debug->ref_heading_jerk());
  previous_heading_acceleration_ = debug->heading_acceleration();
  previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
 
  debug->set_curvature(target_point.path_point().kappa());
}

Init()

Status apollo::control::LatController::Init ( std::shared_ptr< DependencyInjector >  injector )

overridevirtual

initialize Lateral Controller

参数

control_conf

control configurations

返回

Status initialization status

实现了 apollo::control::ControlTask.

在文件 lat_controller.cc 第 177 行定义.

                                                                     {
  if (!ControlTask::LoadConfig<LatBaseLqrControllerConf>(
          &lat_based_lqr_controller_conf_)) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_INIT_ERROR,
                  "failed to load lat control_conf");
  }
  injector_ = injector;
  if (!LoadControlConf()) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                  "failed to load control_conf");
  }
  // Matrix init operations.
  const int matrix_size = basic_state_size_ + preview_window_;
  matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);
  /*
  A matrix (Gear Drive)
  [0.0, 1.0, 0.0, 0.0;
   0.0, (-(c_f + c_r) / m) / v, (c_f + c_r) / m,
   (l_r * c_r - l_f * c_f) / m / v;
   0.0, 0.0, 0.0, 1.0;
   0.0, ((lr * cr - lf * cf) / i_z) / v, (l_f * c_f - l_r * c_r) / i_z,
   (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
  */
  matrix_a_(0, 1) = 1.0;
  matrix_a_(1, 2) = (cf_ + cr_) / mass_;
  matrix_a_(2, 3) = 1.0;
  matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
 
  matrix_a_coeff_ = Matrix::Zero(matrix_size, matrix_size);
  matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
  matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
  matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
  matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;
 
  /*
  b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
  */
  matrix_b_ = Matrix::Zero(basic_state_size_, 1);
  matrix_bd_ = Matrix::Zero(basic_state_size_, 1);
  matrix_bdc_ = Matrix::Zero(matrix_size, 1);
  matrix_b_(1, 0) = cf_ / mass_;
  matrix_b_(3, 0) = lf_ * cf_ / iz_;
  matrix_bd_ = matrix_b_ * ts_;
 
  matrix_state_ = Matrix::Zero(matrix_size, 1);
  matrix_k_ = Matrix::Zero(1, matrix_size);
  matrix_r_ = Matrix::Identity(1, 1);
  matrix_q_ = Matrix::Zero(matrix_size, matrix_size);
 
  int q_param_size = lat_based_lqr_controller_conf_.matrix_q_size();
  int reverse_q_param_size =
      lat_based_lqr_controller_conf_.reverse_matrix_q_size();
  if (matrix_size != q_param_size || matrix_size != reverse_q_param_size) {
    const auto error_msg = absl::StrCat(
        "lateral controller error: matrix_q size: ", q_param_size,
        "lateral controller error: reverse_matrix_q size: ",
        reverse_q_param_size,
        " in parameter file not equal to matrix_size: ", matrix_size);
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
  }
 
  for (int i = 0; i < q_param_size; ++i) {
    matrix_q_(i, i) = lat_based_lqr_controller_conf_.matrix_q(i);
  }
 
  matrix_q_updated_ = matrix_q_;
  InitializeFilters();
  LoadLatGainScheduler();
  LogInitParameters();
 
  enable_leadlag_ =
      lat_based_lqr_controller_conf_.enable_reverse_leadlag_compensation();
  if (enable_leadlag_) {
    leadlag_controller_.Init(
        lat_based_lqr_controller_conf_.reverse_leadlag_conf(), ts_);
  }
 
  enable_mrac_ = lat_based_lqr_controller_conf_.enable_steer_mrac_control();
  if (enable_mrac_) {
    mrac_controller_.Init(lat_based_lqr_controller_conf_.steer_mrac_conf(),
                          vehicle_param_.steering_latency_param(), ts_);
  }
 
  enable_look_ahead_back_control_ =
      lat_based_lqr_controller_conf_.enable_look_ahead_back_control();
 
  return Status::OK();
}

InitializeFilters()

void apollo::control::LatController::InitializeFilters ( )

protected

在文件 lat_controller.cc 第 164 行定义.

                                      {
  // Low pass filter
  std::vector<double> den(3, 0.0);
  std::vector<double> num(3, 0.0);
  common::LpfCoefficients(ts_, lat_based_lqr_controller_conf_.cutoff_freq(),
                          &den, &num);
  digital_filter_.set_coefficients(den, num);
  lateral_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
      lat_based_lqr_controller_conf_.mean_filter_window_size()));
  heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
      lat_based_lqr_controller_conf_.mean_filter_window_size()));
}

LoadControlConf()

bool apollo::control::LatController::LoadControlConf ( )

protected

在文件 lat_controller.cc 第 87 行定义.

                                    {
  vehicle_param_ =
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
 
  ts_ = lat_based_lqr_controller_conf_.ts();
  if (ts_ <= 0.0) {
    AERROR << "[LatController] Invalid control update interval.";
    return false;
  }
  cf_ = lat_based_lqr_controller_conf_.cf();
  cr_ = lat_based_lqr_controller_conf_.cr();
  preview_window_ = lat_based_lqr_controller_conf_.preview_window();
  lookahead_station_low_speed_ =
      lat_based_lqr_controller_conf_.lookahead_station();
  lookback_station_low_speed_ =
      lat_based_lqr_controller_conf_.lookback_station();
  lookahead_station_high_speed_ =
      lat_based_lqr_controller_conf_.lookahead_station_high_speed();
  lookback_station_high_speed_ =
      lat_based_lqr_controller_conf_.lookback_station_high_speed();
  wheelbase_ = vehicle_param_.wheel_base();
  steer_ratio_ = vehicle_param_.steer_ratio();
  steer_single_direction_max_degree_ =
      vehicle_param_.max_steer_angle() / M_PI * 180;
  max_lat_acc_ = lat_based_lqr_controller_conf_.max_lateral_acceleration();
  low_speed_bound_ = lat_based_lqr_controller_conf_.switch_speed();
  low_speed_window_ = lat_based_lqr_controller_conf_.switch_speed_window();
 
  const double mass_fl = lat_based_lqr_controller_conf_.mass_fl();
  const double mass_fr = lat_based_lqr_controller_conf_.mass_fr();
  const double mass_rl = lat_based_lqr_controller_conf_.mass_rl();
  const double mass_rr = lat_based_lqr_controller_conf_.mass_rr();
  const double mass_front = mass_fl + mass_fr;
  const double mass_rear = mass_rl + mass_rr;
  mass_ = mass_front + mass_rear;
 
  lf_ = wheelbase_ * (1.0 - mass_front / mass_);
  lr_ = wheelbase_ * (1.0 - mass_rear / mass_);
 
  // moment of inertia
  iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;
 
  lqr_eps_ = lat_based_lqr_controller_conf_.eps();
  lqr_max_iteration_ = lat_based_lqr_controller_conf_.max_iteration();
 
  query_relative_time_ = lat_based_lqr_controller_conf_.query_relative_time();
 
  minimum_speed_protection_ = FLAGS_minimum_speed_protection;
 
  return true;
}

LoadLatGainScheduler()

void apollo::control::LatController::LoadLatGainScheduler ( )

protected

在文件 lat_controller.cc 第 277 行定义.

                                         {
  const auto &lat_err_gain_scheduler =
      lat_based_lqr_controller_conf_.lat_err_gain_scheduler();
  const auto &heading_err_gain_scheduler =
      lat_based_lqr_controller_conf_.heading_err_gain_scheduler();
  AINFO << "Lateral control gain scheduler loaded";
  Interpolation1D::DataType xy1, xy2;
  for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {
    xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
  for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {
    xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
 
  lat_err_interpolation_.reset(new Interpolation1D);
  ACHECK(lat_err_interpolation_->Init(xy1))
      << "Fail to load lateral error gain scheduler";
 
  heading_err_interpolation_.reset(new Interpolation1D);
  ACHECK(heading_err_interpolation_->Init(xy2))
      << "Fail to load heading error gain scheduler";
}

LogInitParameters()

void apollo::control::LatController::LogInitParameters ( )

protected

在文件 lat_controller.cc 第 158 行定义.

                                      {
  AINFO << name_ << " begin.";
  AINFO << "[LatController parameters]" << " mass_: " << mass_ << ","
        << " iz_: " << iz_ << "," << " lf_: " << lf_ << "," << " lr_: " << lr_;
}

Name()

std::string apollo::control::LatController::Name ( ) const

overridevirtual

Lateral controller name

返回

string controller name in string

实现了 apollo::control::ControlTask.

在文件 lat_controller.cc 第 302 行定义.

{ return name_; }

ProcessLogs()

void apollo::control::LatController::ProcessLogs ( const SimpleLateralDebug *  debug, const canbus::Chassis *  chassis  )

protected

在文件 lat_controller.cc 第 139 行定义.

                                                              {
  const std::string log_str = absl::StrCat(
      debug->lateral_error(), ",", debug->ref_heading(), ",", debug->heading(),
      ",", debug->heading_error(), ",", debug->heading_error_rate(), ",",
      debug->lateral_error_rate(), ",", debug->curvature(), ",",
      debug->steer_angle(), ",", debug->steer_angle_feedforward(), ",",
      debug->steer_angle_lateral_contribution(), ",",
      debug->steer_angle_lateral_rate_contribution(), ",",
      debug->steer_angle_heading_contribution(), ",",
      debug->steer_angle_heading_rate_contribution(), ",",
      debug->steer_angle_feedback(), ",", chassis->steering_percentage(), ",",
      injector_->vehicle_state()->linear_velocity());
  if (FLAGS_enable_csv_debug) {
    steer_log_file_ << log_str << std::endl;
  }
  ADEBUG << "Steer_Control_Detail: " << log_str;
}

Reset()

Status apollo::control::LatController::Reset ( )

overridevirtual

reset Lateral Controller

返回

Status reset status

实现了 apollo::control::ControlTask.

在文件 lat_controller.cc 第 647 行定义.

                            {
  matrix_state_.setZero();
  if (enable_mrac_) {
    mrac_controller_.Reset();
  }
  return Status::OK();
}

Stop()

void apollo::control::LatController::Stop ( )

overridevirtual

stop Lateral controller

实现了 apollo::control::ControlTask.

在文件 lat_controller.cc 第 300 行定义.

{ CloseLogFile(); }

UpdateDrivingOrientation()

void apollo::control::LatController::UpdateDrivingOrientation ( )

protected

在文件 lat_controller.cc 第 920 行定义.

                                             {
  auto vehicle_state = injector_->vehicle_state();
  driving_orientation_ = vehicle_state->heading();
  matrix_bd_ = matrix_b_ * ts_;
  // Reverse the driving direction if the vehicle is in reverse mode
  if (FLAGS_reverse_heading_control) {
    if (vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) {
      driving_orientation_ =
          common::math::NormalizeAngle(driving_orientation_ + M_PI);
      // Update Matrix_b for reverse mode
      matrix_bd_ = -matrix_b_ * ts_;
      ADEBUG << "Matrix_b changed due to gear direction";
    }
  }
}

UpdateMatrix()

void apollo::control::LatController::UpdateMatrix ( )

protected

在文件 lat_controller.cc 第 711 行定义.

                                 {
  double v;
  // At reverse driving, replace the lateral translational motion dynamics with
  // the corresponding kinematic models
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE &&
      !lat_based_lqr_controller_conf_.reverse_use_dynamic_model()) {
    v = std::min(injector_->vehicle_state()->linear_velocity(),
                 -minimum_speed_protection_);
    matrix_a_(0, 2) = matrix_a_coeff_(0, 2) * v;
  } else {
    v = std::max(injector_->vehicle_state()->linear_velocity(),
                 minimum_speed_protection_);
    matrix_a_(0, 2) = 0.0;
  }
  matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
  matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
  matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
  matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;
  Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
  matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *
               (matrix_i + ts_ * 0.5 * matrix_a_);
}

UpdateMatrixCompound()

void apollo::control::LatController::UpdateMatrixCompound ( )

protected

在文件 lat_controller.cc 第 734 行定义.

                                         {
  // Initialize preview matrix
  matrix_adc_.block(0, 0, basic_state_size_, basic_state_size_) = matrix_ad_;
  matrix_bdc_.block(0, 0, basic_state_size_, 1) = matrix_bd_;
  if (preview_window_ > 0) {
    matrix_bdc_(matrix_bdc_.rows() - 1, 0) = 1;
    // Update A matrix;
    for (int i = 0; i < preview_window_ - 1; ++i) {
      matrix_adc_(basic_state_size_ + i, basic_state_size_ + 1 + i) = 1;
    }
  }
}

UpdateState()

void apollo::control::LatController::UpdateState ( SimpleLateralDebug *  debug, const canbus::Chassis *  chassis  )

protected

在文件 lat_controller.cc 第 655 行定义.

                                                              {
  auto vehicle_state = injector_->vehicle_state();
  if (FLAGS_use_navigation_mode) {
    ComputeLateralErrors(
        0.0, 0.0, driving_orientation_, vehicle_state->linear_velocity(),
        vehicle_state->angular_velocity(), vehicle_state->linear_acceleration(),
        trajectory_analyzer_, debug, chassis);
  } else {
    // Transform the coordinate of the vehicle states from the center of the
    // rear-axis to the center of mass, if conditions matched
    const auto &com = vehicle_state->ComputeCOMPosition(lr_);
    ComputeLateralErrors(com.x(), com.y(), driving_orientation_,
                         vehicle_state->linear_velocity(),
                         vehicle_state->angular_velocity(),
                         vehicle_state->linear_acceleration(),
                         trajectory_analyzer_, debug, chassis);
  }
 
  // State matrix update;
  // First four elements are fixed;
  if (enable_look_ahead_back_control_) {
    matrix_state_(0, 0) = debug->lateral_error_feedback();
    matrix_state_(2, 0) = debug->heading_error_feedback();
  } else {
    matrix_state_(0, 0) = debug->lateral_error();
    matrix_state_(2, 0) = debug->heading_error();
  }
  matrix_state_(1, 0) = debug->lateral_error_rate();
  matrix_state_(3, 0) = debug->heading_error_rate();
 
  // Next elements are depending on preview window size;
  for (int i = 0; i < preview_window_; ++i) {
    const double preview_time = ts_ * (i + 1);
    const auto preview_point =
        trajectory_analyzer_.QueryNearestPointByRelativeTime(preview_time);
 
    const auto matched_point = trajectory_analyzer_.QueryNearestPointByPosition(
        preview_point.path_point().x(), preview_point.path_point().y());
 
    const double dx =
        preview_point.path_point().x() - matched_point.path_point().x();
    const double dy =
        preview_point.path_point().y() - matched_point.path_point().y();
 
    const double cos_matched_theta =
        std::cos(matched_point.path_point().theta());
    const double sin_matched_theta =
        std::sin(matched_point.path_point().theta());
    const double preview_d_error =
        cos_matched_theta * dy - sin_matched_theta * dx;
 
    matrix_state_(basic_state_size_ + i, 0) = preview_d_error;
  }
}

类成员变量说明

basic_state_size_

const int apollo::control::LatController::basic_state_size_ = 4

protected

在文件 lat_controller.h 第 181 行定义.

cf_

double apollo::control::LatController::cf_ = 0.0

protected

在文件 lat_controller.h 第 147 行定义.

cr_

double apollo::control::LatController::cr_ = 0.0

protected

在文件 lat_controller.h 第 149 行定义.

current_trajectory_timestamp_

double apollo::control::LatController::current_trajectory_timestamp_ = -1.0

protected

在文件 lat_controller.h 第 255 行定义.

digital_filter_

common::DigitalFilter apollo::control::LatController::digital_filter_

protected

在文件 lat_controller.h 第 212 行定义.

driving_orientation_

double apollo::control::LatController::driving_orientation_ = 0.0

protected

在文件 lat_controller.h 第 267 行定义.

enable_leadlag_

bool apollo::control::LatController::enable_leadlag_ = false

protected

在文件 lat_controller.h 第 223 行定义.

enable_look_ahead_back_control_

bool apollo::control::LatController::enable_look_ahead_back_control_ = false

protected

在文件 lat_controller.h 第 231 行定义.

enable_mrac_

bool apollo::control::LatController::enable_mrac_ = false

protected

在文件 lat_controller.h 第 227 行定义.

heading_err_interpolation_

std::unique_ptr<Interpolation1D> apollo::control::LatController::heading_err_interpolation_

protected

在文件 lat_controller.h 第 216 行定义.

heading_error_filter_

common::MeanFilter apollo::control::LatController::heading_error_filter_

protected

在文件 lat_controller.h 第 220 行定义.

init_vehicle_heading_

double apollo::control::LatController::init_vehicle_heading_ = 0.0

protected

在文件 lat_controller.h 第 261 行定义.

init_vehicle_x_

double apollo::control::LatController::init_vehicle_x_ = 0.0

protected

在文件 lat_controller.h 第 257 行定义.

init_vehicle_y_

double apollo::control::LatController::init_vehicle_y_ = 0.0

protected

在文件 lat_controller.h 第 259 行定义.

injector_

std::shared_ptr<DependencyInjector> apollo::control::LatController::injector_

protected

在文件 lat_controller.h 第 269 行定义.

iz_

double apollo::control::LatController::iz_ = 0.0

protected

在文件 lat_controller.h 第 159 行定义.

lat_based_lqr_controller_conf_

LatBaseLqrControllerConf apollo::control::LatController::lat_based_lqr_controller_conf_

protected

在文件 lat_controller.h 第 135 行定义.

lat_err_interpolation_

std::unique_ptr<Interpolation1D> apollo::control::LatController::lat_err_interpolation_

protected

在文件 lat_controller.h 第 214 行定义.

lateral_error_filter_

common::MeanFilter apollo::control::LatController::lateral_error_filter_

protected

在文件 lat_controller.h 第 219 行定义.

leadlag_controller_

LeadlagController apollo::control::LatController::leadlag_controller_

protected

在文件 lat_controller.h 第 224 行定义.

lf_

double apollo::control::LatController::lf_ = 0.0

protected

在文件 lat_controller.h 第 155 行定义.

lookahead_station_high_speed_

double apollo::control::LatController::lookahead_station_high_speed_ = 0.0

protected

在文件 lat_controller.h 第 176 行定义.

lookahead_station_low_speed_

double apollo::control::LatController::lookahead_station_low_speed_ = 0.0

protected

在文件 lat_controller.h 第 174 行定义.

lookback_station_high_speed_

double apollo::control::LatController::lookback_station_high_speed_ = 0.0

protected

在文件 lat_controller.h 第 177 行定义.

lookback_station_low_speed_

double apollo::control::LatController::lookback_station_low_speed_ = 0.0

protected

在文件 lat_controller.h 第 175 行定义.

low_speed_bound_

double apollo::control::LatController::low_speed_bound_ = 0.0

protected

在文件 lat_controller.h 第 263 行定义.

low_speed_window_

double apollo::control::LatController::low_speed_window_ = 0.0

protected

在文件 lat_controller.h 第 265 行定义.

lqr_eps_

double apollo::control::LatController::lqr_eps_ = 0.0

protected

在文件 lat_controller.h 第 210 行定义.

lqr_max_iteration_

int apollo::control::LatController::lqr_max_iteration_ = 0

protected

在文件 lat_controller.h 第 208 行定义.

lr_

double apollo::control::LatController::lr_ = 0.0

protected

在文件 lat_controller.h 第 157 行定义.

mass_

double apollo::control::LatController::mass_ = 0.0

protected

在文件 lat_controller.h 第 153 行定义.

matrix_a_

Eigen::MatrixXd apollo::control::LatController::matrix_a_

protected

在文件 lat_controller.h 第 183 行定义.

matrix_a_coeff_

Eigen::MatrixXd apollo::control::LatController::matrix_a_coeff_

protected

在文件 lat_controller.h 第 203 行定义.

matrix_ad_

Eigen::MatrixXd apollo::control::LatController::matrix_ad_

protected

在文件 lat_controller.h 第 185 行定义.

matrix_adc_

Eigen::MatrixXd apollo::control::LatController::matrix_adc_

protected

在文件 lat_controller.h 第 187 行定义.

matrix_b_

Eigen::MatrixXd apollo::control::LatController::matrix_b_

protected

在文件 lat_controller.h 第 189 行定义.

matrix_bd_

Eigen::MatrixXd apollo::control::LatController::matrix_bd_

protected

在文件 lat_controller.h 第 191 行定义.

matrix_bdc_

Eigen::MatrixXd apollo::control::LatController::matrix_bdc_

protected

在文件 lat_controller.h 第 193 行定义.

matrix_k_

Eigen::MatrixXd apollo::control::LatController::matrix_k_

protected

在文件 lat_controller.h 第 195 行定义.

matrix_q_

Eigen::MatrixXd apollo::control::LatController::matrix_q_

protected

在文件 lat_controller.h 第 199 行定义.

matrix_q_updated_

Eigen::MatrixXd apollo::control::LatController::matrix_q_updated_

protected

在文件 lat_controller.h 第 201 行定义.

matrix_r_

Eigen::MatrixXd apollo::control::LatController::matrix_r_

protected

在文件 lat_controller.h 第 197 行定义.

matrix_state_

Eigen::MatrixXd apollo::control::LatController::matrix_state_

protected

在文件 lat_controller.h 第 205 行定义.

max_lat_acc_

double apollo::control::LatController::max_lat_acc_ = 0.0

protected

在文件 lat_controller.h 第 166 行定义.

minimum_speed_protection_

double apollo::control::LatController::minimum_speed_protection_ = 0.1

protected

在文件 lat_controller.h 第 253 行定义.

mrac_controller_

MracController apollo::control::LatController::mrac_controller_

protected

在文件 lat_controller.h 第 228 行定义.

name_

const std::string apollo::control::LatController::name_

protected

在文件 lat_controller.h 第 245 行定义.

pre_steer_angle_

double apollo::control::LatController::pre_steer_angle_ = 0.0

protected

在文件 lat_controller.h 第 249 行定义.

pre_steering_position_

double apollo::control::LatController::pre_steering_position_ = 0.0

protected

在文件 lat_controller.h 第 251 行定义.

preview_window_

int apollo::control::LatController::preview_window_ = 0

protected

在文件 lat_controller.h 第 169 行定义.

previous_heading_acceleration_

double apollo::control::LatController::previous_heading_acceleration_ = 0.0

protected

在文件 lat_controller.h 第 239 行定义.

previous_heading_rate_

double apollo::control::LatController::previous_heading_rate_ = 0.0

protected

在文件 lat_controller.h 第 236 行定义.

previous_lateral_acceleration_

double apollo::control::LatController::previous_lateral_acceleration_ = 0.0

protected

在文件 lat_controller.h 第 234 行定义.

previous_ref_heading_acceleration_

double apollo::control::LatController::previous_ref_heading_acceleration_ = 0.0

protected

在文件 lat_controller.h 第 240 行定义.

previous_ref_heading_rate_

double apollo::control::LatController::previous_ref_heading_rate_ = 0.0

protected

在文件 lat_controller.h 第 237 行定义.

query_relative_time_

double apollo::control::LatController::query_relative_time_

protected

在文件 lat_controller.h 第 247 行定义.

steer_log_file_

std::ofstream apollo::control::LatController::steer_log_file_

protected

在文件 lat_controller.h 第 243 行定义.

steer_ratio_

double apollo::control::LatController::steer_ratio_ = 0.0

protected

在文件 lat_controller.h 第 161 行定义.

steer_single_direction_max_degree_

double apollo::control::LatController::steer_single_direction_max_degree_ = 0.0

protected

在文件 lat_controller.h 第 163 行定义.

trajectory_analyzer_

TrajectoryAnalyzer apollo::control::LatController::trajectory_analyzer_

protected

在文件 lat_controller.h 第 141 行定义.

ts_

double apollo::control::LatController::ts_ = 0.0

protected

在文件 lat_controller.h 第 145 行定义.

vehicle_param_

common::VehicleParam apollo::control::LatController::vehicle_param_

protected

在文件 lat_controller.h 第 138 行定义.

wheelbase_

double apollo::control::LatController::wheelbase_ = 0.0

protected

在文件 lat_controller.h 第 151 行定义.

---

该类的文档由以下文件生成:

• modules/control/controllers/lat_based_lqr_controller/lat_controller.h
• modules/control/controllers/lat_based_lqr_controller/lat_controller.cc