/*lat_controller.cpp*/
namespace apollo {
namespace control {

using apollo::common::ErrorCode;//故障碼
using apollo::common::Status;//狀態(tài)碼
using apollo::common::TrajectoryPoint;//軌跡點(diǎn)
using apollo::common::VehicleStateProvider;//車輛狀態(tài)信息
using Matrix = Eigen::MatrixXd;//矩陣庫
using apollo::cyber::Clock;//Apollo時(shí)鐘

namespace {
//日志文件名稱，名稱與時(shí)間相關(guān)
std::string GetLogFileName() {
  time_t raw_time;
  char name_buffer[80];
  std::time(&raw_time);
  std::tm time_tm;
  localtime_r(&raw_time, &time_tm);
  strftime(name_buffer, 80, "/tmp/steer_log_simple_optimal_%F_%H%M%S.csv",
           &time_tm);
  return std::string(name_buffer);
}
//在指定的日志文件內(nèi)寫入各列數(shù)據(jù)標(biāo)題
void WriteHeaders(std::ofstream &file_stream) {
  file_stream << "current_lateral_error,"
              << "current_ref_heading,"
              << "current_heading,"
              << "current_heading_error,"
              << "heading_error_rate,"
              << "lateral_error_rate,"
              << "current_curvature,"
              << "steer_angle,"
              << "steer_angle_feedforward,"
              << "steer_angle_lateral_contribution,"
              << "steer_angle_lateral_rate_contribution,"
              << "steer_angle_heading_contribution,"
              << "steer_angle_heading_rate_contribution,"
              << "steer_angle_feedback,"
              << "steering_position,"
              << "v" << std::endl;
}
}  // namespace
//構(gòu)造函數(shù)
LatController::LatController() : 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);//保留六位小數(shù)
    WriteHeaders(steer_log_file_);//寫入數(shù)據(jù)標(biāo)題
  }
  AINFO << "Using " << name_;
}
//析構(gòu)函數(shù) 關(guān)閉日志文件
LatController::~LatController() { CloseLogFile(); }
//加載控制配置 "control_conf.pb.txt"
//車輛參數(shù) "vehicle_param.pb.txt"
bool LatController::LoadControlConf(const ControlConf *control_conf) {
  if (!control_conf) {
    AERROR << "[LatController] control_conf == nullptr";
    return false;
  }
  vehicle_param_ =
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
//LatController類內(nèi)成員控制周期ts_加載縱向控制配置中的控制周期control_conf.pb.txt--lat_controller_conf--ts
  ts_ = control_conf->lat_controller_conf().ts();
  if (ts_ <= 0.0) {
    AERROR << "[MPCController] Invalid control update interval.";
    return false;
  }
  //control_conf.pb.txt--lat_controller_conf--cf加載的值賦值到cf_ 前輪側(cè)偏剛度（左右輪之和）
  cf_ = control_conf->lat_controller_conf().cf();
  //后輪側(cè)偏剛度之和
  cr_ = control_conf->lat_controller_conf().cr();
  //預(yù)覽窗口的大小
  preview_window_ = control_conf->lat_controller_conf().preview_window();
  //低速前進(jìn)預(yù)瞄距離
  lookahead_station_low_speed_ =
      control_conf->lat_controller_conf().lookahead_station();
  //低速倒車預(yù)瞄距離
  lookback_station_low_speed_ =
      control_conf->lat_controller_conf().lookback_station();
  //高速前進(jìn)預(yù)瞄距離
  lookahead_station_high_speed_ =
      control_conf->lat_controller_conf().lookahead_station_high_speed();
  //高速倒車預(yù)瞄距離
  lookback_station_high_speed_ =
      control_conf->lat_controller_conf().lookback_station_high_speed();
  //軸距
  wheelbase_ = vehicle_param_.wheel_base();
  //轉(zhuǎn)向傳動(dòng)比 = 方向盤轉(zhuǎn)角/前輪轉(zhuǎn)角
  steer_ratio_ = vehicle_param_.steer_ratio();
  //單邊方向盤最大轉(zhuǎn)角轉(zhuǎn)化成deg
  steer_single_direction_max_degree_ =
      vehicle_param_.max_steer_angle() / M_PI * 180;
  //最大允許的橫向加速度
  max_lat_acc_ = control_conf->lat_controller_conf().max_lateral_acceleration();
  //低速切換到高速的邊界
  low_speed_bound_ = control_conf_->lon_controller_conf().switch_speed();
  //低速窗口
  //主要應(yīng)用在低高速切換之間的線性插值，凡是涉及低高速控制切換的，就在這個(gè)窗口做線性插值過渡低高速的控制參數(shù)
  low_speed_window_ =
      control_conf_->lon_controller_conf().switch_speed_window();
  //左前懸的質(zhì)量
  const double mass_fl = control_conf->lat_controller_conf().mass_fl();
  //右前懸的質(zhì)量
  const double mass_fr = control_conf->lat_controller_conf().mass_fr();
  //左后懸的質(zhì)量
  const double mass_rl = control_conf->lat_controller_conf().mass_rl();
  //右后懸的質(zhì)量
  const double mass_rr = control_conf->lat_controller_conf().mass_rr();
  //前懸質(zhì)量
  const double mass_front = mass_fl + mass_fr;
  //后懸質(zhì)量
  const double mass_rear = mass_rl + mass_rr;
  //車輛總質(zhì)量
  mass_ = mass_front + mass_rear;
  //前懸長度
  lf_ = wheelbase_ * (1.0 - mass_front / mass_);
  //后懸長度
  lr_ = wheelbase_ * (1.0 - mass_rear / mass_);

  //轉(zhuǎn)動(dòng)慣量
  iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;
  //LQR求解精度
  lqr_eps_ = control_conf->lat_controller_conf().eps();
  //迭代最大次數(shù)
  lqr_max_iteration_ = control_conf->lat_controller_conf().max_iteration();
  //若打開query_time_nearest_point_only模式，則會(huì)用到此參數(shù)，但是默認(rèn)關(guān)閉
  //若打開此種模式，則目標(biāo)點(diǎn)選為當(dāng)前時(shí)間+query_relative_time_ 這個(gè)時(shí)間在參考軌跡上對(duì)應(yīng)的點(diǎn)
  //query_relative_time默認(rèn)設(shè)置為0.8s，若打開此種模式就默認(rèn)始終用將來0.8s的軌跡點(diǎn)作為目標(biāo)點(diǎn)驅(qū)動(dòng)當(dāng)前車產(chǎn)生控制量向前走
  query_relative_time_ = control_conf->query_relative_time();
  //最小速度保護(hù)，避免v=0作為分母情況出現(xiàn)
  minimum_speed_protection_ = control_conf->minimum_speed_protection();

  return true;
}
//處理日志數(shù)據(jù)函數(shù)，將儲(chǔ)存在debug中的各種誤差信息寫入日志文件里
void LatController::ProcessLogs(const SimpleLateralDebug *debug,
                                const canbus::Chassis *chassis) {
  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());
  //默認(rèn)enable_csv_debug日志debug開關(guān)關(guān)閉
  if (FLAGS_enable_csv_debug) {
    steer_log_file_ << log_str << std::endl;
  }
  ADEBUG << "Steer_Control_Detail: " << log_str;
}
//打印初始化參數(shù)
//顯示控制器的名稱 
//顯示參數(shù)：車輛總質(zhì)量、轉(zhuǎn)動(dòng)慣量、前懸長度、后懸長度
void LatController::LogInitParameters() {
  AINFO << name_ << " begin.";
  AINFO << "[LatController parameters]"
        << " mass_: " << mass_ << ","
        << " iz_: " << iz_ << ","
        << " lf_: " << lf_ << ","
        << " lr_: " << lr_;
}
//初始化濾波器
void LatController::InitializeFilters(const ControlConf *control_conf) {
  //低通濾波器
  std::vector<double> den(3, 0.0);//初始化濾波器傳遞函數(shù)分母[3,0]
  std::vector<double> num(3, 0.0);//初始化濾波器傳遞函數(shù)分子為[3,0]
  //將控制周期和cutoff_freq()作為參數(shù)輸入，計(jì)算得到濾波器傳遞函數(shù)分子分母，注意此處引用變量作為實(shí)參，就是為了被函數(shù)修改之后傳回來。
  common::LpfCoefficients(
      ts_, control_conf->lat_controller_conf().cutoff_freq(), &den, &num);
  //上面計(jì)算出的分子，分母用來設(shè)置數(shù)字濾波器類對(duì)象digital_filter_，用于對(duì)方向盤轉(zhuǎn)角控制指令進(jìn)行濾波
  digital_filter_.set_coefficients(den, num);
  //對(duì)反饋的橫向誤差進(jìn)行均值濾波，簡而言之就是移動(dòng)窗口內(nèi)的多個(gè)值取平均達(dá)到濾波的效果
  //從控制配置讀取均值濾波窗口大小(默認(rèn)設(shè)置為10)設(shè)置均值濾波器類對(duì)象lateral_error_filter_
  lateral_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
      control_conf->lat_controller_conf().mean_filter_window_size()));
  heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
      control_conf->lat_controller_conf().mean_filter_window_size()));
}
//橫向控制器LQR的初始化工作
//injector車輛當(dāng)前狀態(tài)信息
Status LatController::Init(std::shared_ptr<DependencyInjector> injector,
                           const ControlConf *control_conf) {
  control_conf_ = control_conf;
  injector_ = injector;
  //如果加載失敗。。。。
  if (!LoadControlConf(control_conf_)) {
    AERROR << "failed to load control conf";
    return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
                  "failed to load control_conf");
  }
  //矩陣初始化操作
  //車輛狀態(tài)方程中矩陣的大小 = 基礎(chǔ)狀態(tài)矩陣大小 + 預(yù)覽窗口大小
  const int matrix_size = basic_state_size_ + preview_window_;//4+0
  matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);//4*4零矩陣
  //計(jì)算機(jī)控制需要轉(zhuǎn)換成離散的差分方程形式
  //matrix_ad_是系數(shù)矩陣A的離散形式，A通過雙線性變化法變成Ad
  matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);
  /*
  A matrix (Gear Drive) 前進(jìn)擋的狀態(tài)方程系數(shù)矩陣A
  [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;]
  */
  //此處apolloA矩陣有誤
  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_矩陣也初始化為4*4的零矩陣
  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
  */
  //初始化矩陣B為4*1的零矩陣
  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_;
  //矩陣B的離散形式Bd就等于 B * ts
  matrix_bd_ = matrix_b_ * ts_;
  //車輛狀態(tài)矩陣X,[e1 e1' e2 e2']
  matrix_state_ = Matrix::Zero(matrix_size, 1);
  //初始化K矩陣為1*4的0矩陣
  matrix_k_ = Matrix::Zero(1, matrix_size);
  //初始化R矩陣為1*1的單位陣
  matrix_r_ = Matrix::Identity(1, 1);
  //初始化Q矩陣為4*4的0矩陣，Q矩陣是LQR中目標(biāo)函數(shù)中各個(gè)狀態(tài)量(X=[e1 e1' e2 e2'])平方和的權(quán)重系數(shù)
  matrix_q_ = Matrix::Zero(matrix_size, matrix_size);
  //q_param_size默認(rèn)為4
  int q_param_size = control_conf_->lat_controller_conf().matrix_q_size();
  //倒車的reverse_q_param_size
  int reverse_q_param_size =
      control_conf_->lat_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);
  }
  //加載控制配置中matrix_q(0),matrix_q(1),matrix_q(2),matrix_q(3)
  //默認(rèn)分別為0.05，0.0，1.0，0.0
  //加載到LatController類數(shù)據(jù)成員matrix_q_即LQR的Q矩陣中
  for (int i = 0; i < q_param_size; ++i) {
    matrix_q_(i, i) = control_conf_->lat_controller_conf().matrix_q(i);
  }
  //更新后的Q矩陣
  matrix_q_updated_ = matrix_q_;
  //初始化3個(gè)濾波器，1個(gè)低通濾波是對(duì)計(jì)算出方向盤轉(zhuǎn)角控制指令進(jìn)行濾波
  InitializeFilters(control_conf_);
  //橫向控制配置
  auto &lat_controller_conf = control_conf_->lat_controller_conf();
  //加載增益調(diào)度表，就是橫向誤差和航向誤差在車速不同時(shí)乘上個(gè)不同的比例
  //這個(gè)比例決定了實(shí)際時(shí)的控制效果，根據(jù)實(shí)際經(jīng)驗(yàn)低速和高速應(yīng)該采取不同的比例，低速比例較大
  LoadLatGainScheduler(lat_controller_conf);
  //打印一些車輛參數(shù)的信息
  LogInitParameters();

  enable_leadlag_ = control_conf_->lat_controller_conf()
                     .enable_reverse_leadlag_compensation();
  //是否打開超前滯后器
  if (enable_leadlag_) {
  leadlag_controller_.Init(lat_controller_conf.reverse_leadlag_conf(), ts_);
  }

  enable_mrac_ =
      control_conf_->lat_controller_conf().enable_steer_mrac_control();
  if (enable_mrac_) {
    mrac_controller_.Init(lat_controller_conf.steer_mrac_conf(),
                          vehicle_param_.steering_latency_param(), ts_);
  }
//是否能使前進(jìn)倒車的預(yù)瞄控制enable_look_ahead_back_control_
  enable_look_ahead_back_control_ =
      control_conf_->lat_controller_conf().enable_look_ahead_back_control();
  //返回狀態(tài)碼
  return Status::OK();
}
//關(guān)閉日志文件
void LatController::CloseLogFile() {

  if (FLAGS_enable_csv_debug && steer_log_file_.is_open()) {
    steer_log_file_.close();
  }
}
//加載增益調(diào)度表
void LatController::LoadLatGainScheduler(
    const LatControllerConf &lat_controller_conf) {
  //橫向誤差增益表
  const auto &lat_err_gain_scheduler =
      lat_controller_conf.lat_err_gain_scheduler();
  //朝向誤差增益表
  const auto &heading_err_gain_scheduler =
      lat_controller_conf.heading_err_gain_scheduler();
  AINFO << "Lateral control gain scheduler loaded";
  //定義了兩張插值表xy1,xy2
  Interpolation1D::DataType xy1, xy2;
  //遍歷調(diào)度表 將每一組橫向誤差調(diào)度表的speed,ratio結(jié)對(duì)make_pair存入插值表xy1
  for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {
    xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
  //將每組朝向誤差調(diào)度表的speed,ratio結(jié)對(duì)make_pair存入插值表
  for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {
    xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  }
  //將lat_err_interpolation_復(fù)位，然后用xy1初始化lat_err_interpolation_
  lat_err_interpolation_.reset(new Interpolation1D);
  ACHECK(lat_err_interpolation_->Init(xy1))
      << "Fail to load lateral error gain scheduler";
  //將heading_err_interpolation_復(fù)位，然后用xy1初始化heading_err_interpolation_
  heading_err_interpolation_.reset(new Interpolation1D);
  ACHECK(heading_err_interpolation_->Init(xy2))
      << "Fail to load heading error gain scheduler";
}
//關(guān)閉橫向日志文件
void LatController::Stop() { CloseLogFile(); }
//返回橫向控制器的名稱字符串
std::string LatController::Name() const { return name_; }
//計(jì)算控制指令
Status LatController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization,
    const canbus::Chassis *chassis,
    const planning::ADCTrajectory *planning_published_trajectory,
    ControlCommand *cmd) {
  //通過injector_獲取當(dāng)前車輛狀態(tài)信息存儲(chǔ)在vehicle_state
  auto vehicle_state = injector_->vehicle_state();
  //車輛期望軌跡存放到target_tracking_trajectory
  auto target_tracking_trajectory = *planning_published_trajectory;
  //是否打開導(dǎo)航模式，默認(rèn)關(guān)閉
  if (FLAGS_use_navigation_mode &&
      FLAGS_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);
          });
    }
  }
  //target_tracking_trajectory是從輸入?yún)?shù)傳進(jìn)來的規(guī)劃軌跡信息
	  //將target_tracking_trajectory對(duì)象內(nèi)容move移動(dòng)到LatController類數(shù)據(jù)成員trajectory_analyzer_里
  trajectory_analyzer_ =
      std::move(TrajectoryAnalyzer(&target_tracking_trajectory));

  //將規(guī)劃軌跡從后軸中心變換到質(zhì)心，如果條件滿足的話，倒檔是否需要轉(zhuǎn)換到質(zhì)心坐標(biāo)的開關(guān)//這個(gè)if不滿足，可以略過
  if (((FLAGS_trajectory_transform_to_com_reverse &&
        vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) ||
 //前進(jìn)檔是否需要轉(zhuǎn)換到質(zhì)心坐標(biāo)的開關(guān),默認(rèn)false
   (FLAGS_trajectory_transform_to_com_drive &&
        vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE)) &&
      enable_look_ahead_back_control_) {
    trajectory_analyzer_.TrajectoryTransformToCOM(lr_);
  }

  //倒擋重建車輛的動(dòng)力學(xué)模型，尤其是橫向的動(dòng)力學(xué)模型
  if (vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) {
    /*
    倒檔時(shí)的系數(shù)矩陣A
    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;]
    */
    //下面4項(xiàng)都是D檔，R檔A中會(huì)變化的項(xiàng)，D檔和R檔這4項(xiàng)不同
    cf_ = -control_conf_->lat_controller_conf().cf();
    cr_ = -control_conf_->lat_controller_conf().cr();
    matrix_a_(0, 1) = 0.0;
    matrix_a_coeff_(0, 2) = 1.0;
  } else {
    /*
    前進(jìn)檔的系數(shù)矩陣A
    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;]
    */
    //前進(jìn)擋的一些參數(shù)：前輪側(cè)偏剛度等
    cf_ = control_conf_->lat_controller_conf().cf();
    cr_ = control_conf_->lat_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_;
  //調(diào)用更新駕駛航向函數(shù)
  UpdateDrivingOrientation();
  //簡單橫向調(diào)試
  SimpleLateralDebug *debug = cmd->mutable_debug()->mutable_simple_lat_debug();
  debug->Clear();

  //更新車輛狀態(tài)矩陣X=[e1 e1' e2 e2']
  //首先該函數(shù)UpdateState()內(nèi)部調(diào)用了ComputeLateralErrors()函數(shù)得到的各種誤差信息存放到debug中
  //然后又用debug去更新車輛狀態(tài)矩陣X即matrix_state_
  UpdateState(debug);
  //主要是更新車輛狀態(tài)方程系數(shù)矩陣A及其離散形式中與速度相關(guān)的時(shí)變項(xiàng)
  UpdateMatrix();

  //更新以及組裝離散矩陣Ad,Bd和預(yù)覽窗口模型，預(yù)覽窗在橫向控制中都關(guān)閉了，控制配置里preview_window為0
  UpdateMatrixCompound();

  //R檔調(diào)整矩陣Q
  int q_param_size = control_conf_->lat_controller_conf().matrix_q_size();
  int reverse_q_param_size =
      control_conf_->lat_controller_conf().reverse_matrix_q_size();
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
  //R檔加載控制配置里的reverse_matrix_q
    for (int i = 0; i < reverse_q_param_size; ++i) {
      matrix_q_(i, i) =
          control_conf_->lat_controller_conf().reverse_matrix_q(i);
    }
  } else {
  //非R檔加載控制配置里的matrix_q
    for (int i = 0; i < q_param_size; ++i) {
      matrix_q_(i, i) = control_conf_->lat_controller_conf().matrix_q(i);
    }
  }

  //對(duì)于更高速度的轉(zhuǎn)向增加增益調(diào)度表
  //取出control_gflags.cc中enable_gain_scheduler的值，默認(rèn)是false，但是實(shí)際上很有用
  //如果打開增益調(diào)度表
  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()));
//求解LQR問題，求解到的最優(yōu)狀態(tài)反饋矩陣K放入matrix_k_中，最后一個(gè)引用變量作參數(shù)，明擺著就是要用對(duì)形參的修改改變實(shí)參，
//常用來存放函數(shù)計(jì)算結(jié)果 
//這個(gè)if和else都是調(diào)用SolveLQRProblem函數(shù)，其中不同就只有一個(gè)是matrix_q_updated_是考慮調(diào)度增益表的Q，Q與車速有關(guān)
	    //一個(gè)是matrix_q_不考慮增益調(diào)度表的Q矩陣，Q與車速無關(guān)，看你是否打開調(diào)度增益表
	    //根據(jù)經(jīng)驗(yàn)打開的話更容易獲得更好的控制性能，否則低速適用的Q用到高速往往容易出現(xiàn)畫龍
common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,
                                  matrix_r_, lqr_eps_, lqr_max_iteration_,
                                  &matrix_k_);
  } else {
    common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_,
                                  matrix_r_, lqr_eps_, lqr_max_iteration_,
                                  &matrix_k_);
  }

  // feedback = - K * state
  // Convert vehicle steer angle from rad to degree and then to steer degree
  //狀態(tài)反饋控制量 u = -K*X
  //將計(jì)算出的控制量(車輛的前輪轉(zhuǎn)角)從rad轉(zhuǎn)化為deg,然后再乘上轉(zhuǎn)向傳動(dòng)比steer_ratio_轉(zhuǎn)化成方向盤轉(zhuǎn)角
  //最后再根據(jù)單邊的方向盤最大轉(zhuǎn)角轉(zhuǎn)化控制為百分?jǐn)?shù) -100-100
  //這里計(jì)算出的是反饋的控制量
  const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /
                                      M_PI * steer_ratio_ /
                                      steer_single_direction_max_degree_ * 100;
  //定義臨時(shí)常量steer_angle_feedforward存放前饋控制量
  //調(diào)用函數(shù)ComputeFeedForward計(jì)算前饋控制量
  const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());

  double steer_angle = 0.0;
  double steer_angle_feedback_augment = 0.0;
  //在期望的速度域增強(qiáng)橫向誤差的反饋控制
  //如果打開leadlag超前滯后控制器
  if (enable_leadlag_) {
  //如果車輛打開高速的反饋增強(qiáng)控制 或 車速小于低高速邊界速度
    if (FLAGS_enable_feedback_augment_on_high_speed ||
        std::fabs(vehicle_state->linear_velocity()) < low_speed_bound_) {
  //滿足上述條件,實(shí)際上就是低速時(shí)打開超前滯后控制器，然后這個(gè)超前滯后控制器只對(duì)橫向誤差進(jìn)行增強(qiáng)控制
  //計(jì)算出反饋增強(qiáng)控制方向盤轉(zhuǎn)角百分?jǐn)?shù)steer_angle_feedback_augment
      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, 線性插值增強(qiáng)控制
        // 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()));
      }
    }
  }
  //方向盤轉(zhuǎn)角控制量=反饋控制量+前饋控制量+增強(qiáng)反饋控制量（超前滯后控制器）
  steer_angle = steer_angle_feedback + steer_angle_feedforward +
                steer_angle_feedback_augment;

  //根據(jù)最大的橫向加速度限制計(jì)算方向盤轉(zhuǎn)向的限制
  //若限制橫向加速度 最大方向盤轉(zhuǎn)角百分?jǐn)?shù) = atan(ay_max * L / v^2) * steerratio * 180/pi /max_steer_ang * 100
  //根據(jù)上述公式可以從限制的最大橫向加速度計(jì)算出最大的方向盤轉(zhuǎn)角控制百分?jǐn)?shù)
  //若無限制 最大方向盤轉(zhuǎn)角百分?jǐn)?shù) = 100 
  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;
 //這一部分主要是對(duì)方向盤轉(zhuǎn)動(dòng)速率進(jìn)行限制，默認(rèn)關(guān)閉
 //如果沒打開，最大就限制為100
 //一個(gè)周期方向盤轉(zhuǎn)角最大增量 = 最大方向盤角速度 * 控制周期
 //此刻方向盤轉(zhuǎn)角控制量只能在范圍內(nèi)：上一時(shí)刻方向盤轉(zhuǎn)角控制量 +/- 一個(gè)周期方向盤轉(zhuǎn)角最大增量
  const double steer_diff_with_max_rate =
      FLAGS_enable_maximum_steer_rate_limit
          ? vehicle_param_.max_steer_angle_rate() * ts_ * 180 / M_PI /
                steer_single_direction_max_degree_ * 100
          : 100.0;
//方向盤實(shí)際轉(zhuǎn)角
//方向盤實(shí)際轉(zhuǎn)角從chassis信息讀，canbus從車輛can線上讀到發(fā)出來的
  const double steering_position = chassis->steering_percentage();

  //如果打開MRAC模型參考自適應(yīng)控制 enable_mrac_
  //重新計(jì)算方向盤轉(zhuǎn)角控制量，并用方向盤轉(zhuǎn)角和轉(zhuǎn)速限制對(duì)轉(zhuǎn)角控制量進(jìn)行限幅
  if (enable_mrac_) {
    const int mrac_model_order = control_conf_->lat_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()) >
        control_conf_->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());
  }//enable_mrac_控制配置文件里的參數(shù)默認(rèn)是false，略過
  //將當(dāng)前轉(zhuǎn)角設(shè)置為上一時(shí)刻的轉(zhuǎn)角
  pre_steering_position_ = steering_position;
  //將enable_mrac_是否打開信息加入debug調(diào)試信息結(jié)構(gòu)體
  debug->set_steer_mrac_enable_status(enable_mrac_);
  //根據(jù)當(dāng)前車速對(duì)下發(fā)方向盤轉(zhuǎn)角進(jìn)行限幅，橫向加速度的限制轉(zhuǎn)化到此刻方向盤轉(zhuǎn)角限制就會(huì)引入車速
 //steer_limit通過橫向最大加速度或者方向盤最大轉(zhuǎn)角限制
 //限幅后的方向盤轉(zhuǎn)角steer_angle_limited
  double steer_angle_limited =
      common::math::Clamp(steer_angle, -steer_limit, steer_limit);
  steer_angle = steer_angle_limited;
  //方向盤轉(zhuǎn)角信息寫入debug結(jié)構(gòu)體中
  debug->set_steer_angle_limited(steer_angle_limited);

  //對(duì)方向盤轉(zhuǎn)角數(shù)字濾波，然后控制百分?jǐn)?shù)又限制在正負(fù)100
  steer_angle = digital_filter_.Filter(steer_angle);
  steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);

  //當(dāng)處于D檔或倒檔且車速小于轉(zhuǎn)向自鎖速度且處于自駕模式時(shí)鎖定方向盤，方向盤控制轉(zhuǎn)角就保持上一次的命令值
  if (std::abs(vehicle_state->linear_velocity()) < FLAGS_lock_steer_speed &&
      (vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE ||
       vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) &&
      chassis->driving_mode() == canbus::Chassis::COMPLETE_AUTO_DRIVE) {
    steer_angle = pre_steer_angle_;
  }

  //設(shè)定轉(zhuǎn)角指令，再通過最大轉(zhuǎn)角速率再次進(jìn)行限制幅度 最多只能=上次的轉(zhuǎn)角指令+/-最大轉(zhuǎn)角速率 * Ts
  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));
  //將此刻的方向盤命令值賦給上一時(shí)刻方向盤命令值
  cmd->set_steering_rate(FLAGS_steer_angle_rate);

  pre_steer_angle_ = cmd->steering_target();

  //日志和調(diào)試計(jì)算的一些額外信息
  //橫向誤差貢獻(xiàn)的控制量百分?jǐn)?shù)
  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;
  //橫向誤差率貢獻(xiàn)的控制量百分?jǐn)?shù)
  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;
  //航向誤差貢獻(xiàn)的控制量百分?jǐn)?shù)
  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;
  //航向誤差率貢獻(xiàn)的控制量百分?jǐn)?shù)
  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;
  //將信息放進(jìn)指針debug里 
  //駕駛朝向
  debug->set_heading(driving_orientation_);
  //轉(zhuǎn)向角
  debug->set_steer_angle(steer_angle);
  //轉(zhuǎn)向角反饋
  debug->set_steer_angle_feedforward(steer_angle_feedforward);
  //轉(zhuǎn)向角橫向貢獻(xiàn)
  debug->set_steer_angle_lateral_contribution(steer_angle_lateral_contribution);
  //轉(zhuǎn)向角橫向貢獻(xiàn)率
  debug->set_steer_angle_lateral_rate_contribution(
    
   steer_angle_lateral_rate_contribution);
   //轉(zhuǎn)向角朝向貢獻(xiàn)
  debug->set_steer_angle_heading_contribution(steer_angle_heading_contribution);
  //轉(zhuǎn)向角朝向貢獻(xiàn)率
  debug->set_steer_angle_heading_rate_contribution(
      steer_angle_heading_rate_contribution);
  //轉(zhuǎn)向角反饋
  debug->set_steer_angle_feedback(steer_angle_feedback);
  //轉(zhuǎn)向角增強(qiáng)反饋
  debug->set_steer_angle_feedback_augment(steer_angle_feedback_augment);
  //當(dāng)前轉(zhuǎn)向角
  debug->set_steering_position(steering_position);
  debug->set_ref_speed(vehicle_state->linear_velocity());
  //將debug和chassis信息放入記錄日志里
  ProcessLogs(debug, chassis);
  return Status::OK();
}
//mrac模型參考自適應(yīng)控制的復(fù)位函數(shù)
//enable_steer_mrac_control，其默認(rèn)關(guān)閉，是另外一種橫向控制器
Status LatController::Reset() {
  matrix_state_.setZero();
  if (enable_mrac_) {
    mrac_controller_.Reset();
  }
  return Status::OK();
}
//橫向控制器的車輛狀態(tài)矩陣函數(shù)
SimpleLateralDebug是control_cmd.proto下的一個(gè)message類型，包含各種調(diào)試信息
//用于返回車輛當(dāng)前狀態(tài)
void LatController::UpdateState(SimpleLateralDebug *debug) {
  auto vehicle_state = injector_->vehicle_state();
  //是否使用navigation_mode
  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);
  } 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);
  }

  // 狀態(tài)矩陣更新;
  //當(dāng)打開這個(gè)e1和e3分別賦值橫向反饋誤差和航向反饋誤差，和下面else兩行的區(qū)別在哪里？
  //若打開lookahead（D檔）,lookback(R檔)則x中的e1,e3就為考慮了lookahead的誤差
  //lateral_error_feedback = lateral_error + 參考點(diǎn)到lookahead點(diǎn)的橫向誤差
  //heading_error_feedback = heading_error + ref_heading - lookahead點(diǎn)的heading 實(shí)際上就是匹配點(diǎn)到lookahead點(diǎn)的航向差 
  //heading_error = 車輛heading - ref_heading
  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();

  // 這一部分是更新狀態(tài)矩陣中的preview項(xiàng)，但是preview_window默認(rèn)為0忽略
  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;
  }
}
//更新矩陣函數(shù)，主要是更新系數(shù)矩陣A,Ad
void LatController::UpdateMatrix() {
  double v;
  //倒檔代替橫向平動(dòng)動(dòng)力學(xué)用對(duì)應(yīng)的運(yùn)動(dòng)學(xué)模型
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
    v = std::min(injector_->vehicle_state()->linear_velocity(),
                 -minimum_speed_protection_);
    matrix_a_(0, 2) = matrix_a_coeff_(0, 2) * v;
  } else {
  //v為車輛縱向速度和最小速度保護(hù)里的最大值
    v = std::max(injector_->vehicle_state()->linear_velocity(),
                 minimum_speed_protection_);
    matrix_a_(0, 2) = 0.0;
  }
  //更新A矩陣中與v有關(guān)的項(xiàng)
  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;
  //定義了單位陣（A列xA列)
  Matrix matrix_i =Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
  //計(jì)算A矩陣的離散化形式Ad，用雙線性變換法
  matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *
               (matrix_i + ts_ * 0.5 * matrix_a_);
}
//更新組裝矩陣Adc,就是Ad考慮preview后擴(kuò)展的結(jié)果
void LatController::UpdateMatrixCompound() {
  // 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;
    }
  }
}
//計(jì)算橫向控制的前饋量
double LatController::ComputeFeedForward(double ref_curvature) const {
//kv=lr*m/2/Cf/L - lf*m/2/Cr/L
  const double kv =
      lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;

  // 計(jì)算前饋項(xiàng)并且轉(zhuǎn)化成百分?jǐn)?shù)
  const double v = injector_->vehicle_state()->linear_velocity();
  double steer_angle_feedforwardterm;
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
    steer_angle_feedforwardterm = 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;
}
//計(jì)算橫向誤差，放入debug指針中
//參數(shù)：xy坐標(biāo)，車輛當(dāng)前heading,縱向速度v,heading變化率，縱向加速度
//軌跡相關(guān)信息trajectory_analyzer對(duì)象用于提供軌跡的參考點(diǎn)，匹配點(diǎn)，lookahead點(diǎn)等信息
void 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) {
  //TrajectoryPoint類包含軌跡點(diǎn)的v,acc,jerk,相對(duì)時(shí)間，前輪轉(zhuǎn)向角等信息
  TrajectoryPoint target_point;
//如果只將車輛當(dāng)前的時(shí)間向前加固定時(shí)間長度后在軌跡上對(duì)應(yīng)點(diǎn)作為目標(biāo)點(diǎn)
  if (FLAGS_query_time_nearest_point_only) {
  //在時(shí)域上向前看0.8秒作為目標(biāo)點(diǎn)
    target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
        Clock::NowInSeconds() + query_relative_time_);
  } else {
  //默認(rèn)不采用導(dǎo)航模式，直接看else
    if (FLAGS_use_navigation_mode &&
        !FLAGS_enable_navigation_mode_position_update) {
      target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
          Clock::NowInSeconds() + query_relative_time_);
    } else {
    //默認(rèn)不采用導(dǎo)航模式,則目標(biāo)點(diǎn)取軌跡上距離當(dāng)前車輛xy坐標(biāo)點(diǎn)最近的點(diǎn)，默認(rèn)目標(biāo)點(diǎn)就是取距離最近點(diǎn)
      target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
    }
  }
  //dx就是當(dāng)前車輛和目標(biāo)點(diǎn)的x坐標(biāo)之差
  const double dx = x - target_point.path_point().x();
  //dy就是當(dāng)前車輛和目標(biāo)點(diǎn)的y坐標(biāo)之差
  const double dy = y - target_point.path_point().y();
//將目標(biāo)點(diǎn)的x坐標(biāo)存入debug指針
  debug->mutable_current_target_point()->mutable_path_point()->set_x(
      target_point.path_point().x());
 //將目標(biāo)點(diǎn)的y坐標(biāo)存入debug指針
  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();
//計(jì)算目標(biāo)點(diǎn)處的heading角的正余弦值
  const double cos_target_heading = std::cos(target_point.path_point().theta());
  
  const double sin_target_heading = std::sin(target_point.path_point().theta());
//根據(jù)目標(biāo)點(diǎn)處的heading角正余弦值計(jì)算橫向誤差
  double lateral_error = cos_target_heading * dy - sin_target_heading * dx;
  if (FLAGS_enable_navigation_mode_error_filter) {
  //如果打開導(dǎo)航模式誤差濾波器，modules/control/common/control_gflags.cc里
  //默認(rèn)時(shí)關(guān)閉的，導(dǎo)航模式及濾波器都關(guān)閉，直接略過
    lateral_error = lateral_error_filter_.Update(lateral_error);
  }

  debug->set_lateral_error(lateral_error);
//將目標(biāo)點(diǎn)的heading角填入debug指針的debug.ref_heading
  debug->set_ref_heading(target_point.path_point().theta());
  //計(jì)算航向誤差
  //車輛當(dāng)前航向角theta-ref_heading角
  //角度標(biāo)準(zhǔn)化
  double heading_error =
      common::math::NormalizeAngle(theta - debug->ref_heading());
  //如果導(dǎo)航模式濾波器打開對(duì)航向偏差進(jìn)行濾波
  if (FLAGS_enable_navigation_mode_error_filter) {
    heading_error = heading_error_filter_.Update(heading_error);
  }
  debug->set_heading_error(heading_error);

  //在低-高速切換窗口，現(xiàn)行插值預(yù)瞄距離為了更柔和的預(yù)測切換
  double lookahead_station = 0.0;//D
  double lookback_station = 0.0;//R
  //速度大于低速邊界
  if (std::fabs(linear_v) >= low_speed_bound_) {
    lookahead_station = lookahead_station_high_speed_;//若為高速就用高速的預(yù)瞄距離
    lookback_station = lookback_station_high_speed_;
  } 
//若縱向速度絕對(duì)值小于低速邊界-低速窗口
else if (std::fabs(linear_v) < low_speed_bound_ - low_speed_window_) {
    lookahead_station = lookahead_station_low_speed_;//就采用低速的預(yù)瞄距離包括非R檔和R檔
    lookback_station = lookback_station_low_speed_;
  } else {
  //若縱向速度絕對(duì)值小于低速邊界又大于（低速邊界-低速窗口）就插值計(jì)算預(yù)瞄距離
    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));
  }

  //估計(jì)考慮預(yù)瞄距離的航向誤差heading_error_feedback
  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(
    //目標(biāo)點(diǎn)的相對(duì)時(shí)間再加上預(yù)瞄時(shí)間(預(yù)瞄距離/車輛縱向速度)作為總相對(duì)時(shí)間
    //然后去trajectory_analyzer軌跡信息上根據(jù)總相對(duì)時(shí)間找出預(yù)瞄點(diǎn)
        target_point.relative_time() +
        lookahead_station /
        //在估計(jì)預(yù)瞄時(shí)間時(shí)縱向速度若小于0.1就按0.1
            (std::max(std::fabs(linear_v), 0.1) * std::cos(heading_error)));
    //heading_error=車輛當(dāng)前heading-參考點(diǎn)heading
	    //heading_error_feedback=heading_error+參考點(diǎn)heading-預(yù)瞄點(diǎn)heading=車輛當(dāng)前heading-預(yù)瞄點(diǎn)heading
    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);

  //估計(jì)考慮預(yù)瞄距離的橫向誤差lateral_error_feedback
  double lateral_error_feedback;
  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
  //倒檔的lateral_error_feedback=lateral_error-倒車預(yù)瞄距離*sin(heading_error)
    lateral_error_feedback =
        lateral_error - lookback_station * std::sin(heading_error);
  } else {
//前進(jìn)檔的lateral_error_feedback=lateral_error+前進(jìn)預(yù)瞄距離*sin(heading_error)
    lateral_error_feedback =
        lateral_error + lookahead_station * std::sin(heading_error);
  }
  //將lateral_error_feedback存入指針debug
  debug->set_lateral_error_feedback(lateral_error_feedback);

  auto lateral_error_dot = linear_v * std::sin(heading_error);//橫向誤差率=縱向速度v*sin(heading_error)
  auto lateral_error_dot_dot = linear_a * std::sin(heading_error);//橫向誤差加速度=縱向加速度*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;
    }
  }
  //將計(jì)算得到的橫向誤差率填入debug指針
  debug->set_lateral_error_rate(lateral_error_dot);
  //將計(jì)算得到的橫向誤差加速度填入debug指針
  debug->set_lateral_acceleration(lateral_error_dot_dot);
  //利用橫向加速度差分得到橫向加加速度jerk放入debug指針
  debug->set_lateral_jerk(
      (debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
  //差分法需要迭代更新下上一時(shí)刻的橫向加速度
  previous_lateral_acceleration_ = debug->lateral_acceleration();

  if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
  //angular_v是ComputeLateralError函數(shù)傳進(jìn)來的參數(shù)heading的變化率，若倒車debug.heading_rate為負(fù)的angular_v
    debug->set_heading_rate(-angular_v);
  } else {
    debug->set_heading_rate(angular_v);
  }
  //參考的航向角變化率=目標(biāo)點(diǎn)縱向速度/目標(biāo)點(diǎn)轉(zhuǎn)彎半徑，繞Z軸w=v/R，下面的kappa就是曲率 結(jié)果放入debug指針中
  debug->set_ref_heading_rate(target_point.path_point().kappa() *
                              target_point.v());
  //航向角誤差率=車輛的航向角變化率-目標(biāo)點(diǎn)航向角變化率，結(jié)果放入debug指針中
  debug->set_heading_error_rate(debug->heading_rate() -
                                debug->ref_heading_rate());
//航向角變化的加速度就用差分法，這一時(shí)刻航向角變化率減去上一時(shí)刻之差然后再除以采樣周期ts，結(jié)果放入debug指針中
  debug->set_heading_acceleration(
      (debug->heading_rate() - previous_heading_rate_) / ts_);
   //目標(biāo)點(diǎn)航向角變化的加速度也用差分法計(jì)算得到，結(jié)果放入debug指針中
  debug->set_ref_heading_acceleration(
      (debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);
      //航向角誤差變化的加速度
  debug->set_heading_error_acceleration(debug->heading_acceleration() -
     //當(dāng)前時(shí)刻的航向角變化率     
 debug->ref_heading_acceleration());
  previous_heading_rate_ = debug->heading_rate();
  previous_ref_heading_rate_ = debug->ref_heading_rate();
//heading角的加加速度
  debug->set_heading_jerk(
      (debug->heading_acceleration() - previous_heading_acceleration_) / ts_);
      //目標(biāo)點(diǎn)航向角的加加速度
  debug->set_ref_heading_jerk(
      (debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /
      ts_);
      //heading角誤差變化率的加加速度
  debug->set_heading_error_jerk(debug->heading_jerk() -
                                debug->ref_heading_jerk());
   //迭代將當(dāng)前時(shí)刻的值賦給上一時(shí)刻，以便用差分法求導(dǎo)
  previous_heading_acceleration_ = debug->heading_acceleration();
  previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
//kappa
  debug->set_curvature(target_point.path_point().kappa());
}
//更新駕駛航向
void LatController::UpdateDrivingOrientation() {
//更新車輛狀態(tài)
  auto vehicle_state = injector_->vehicle_state();
  //朝向
  driving_orientation_ = vehicle_state->heading();
  //橫向控制狀態(tài)方程B離散化為Bd
  matrix_bd_ = matrix_b_ * ts_;
  //倒擋就更改方向180°
  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";
    }
  }
}

}  // namespace control
}  // namespace apollo

A矩陣離散化

void LatController::UpdateMatrix() {
  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) {
    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_);
}

B矩陣離散化

/*
  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_;

反饋計(jì)算

const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /
                                      M_PI * steer_ratio_ /
                                      steer_single_direction_max_degree_ * 100;

前饋計(jì)算：

double LatController::ComputeFeedForward(double ref_curvature) const {
  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) {
    steer_angle_feedforwardterm = 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;
}

超前滯后反饋：steer_angle_feedback_augment





參考【運(yùn)動(dòng)控制】Apollo6.0的leadlag_controller解析

控制誤差計(jì)算

橫向距離誤差計(jì)算

 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;

橫向誤差變化率計(jì)算

auto lateral_error_dot = linear_v * std::sin(heading_error);

航向角誤差計(jì)算

  double heading_error =
      common::math::NormalizeAngle(theta - debug->ref_heading());

航向角誤差變化率計(jì)算

  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());

參考：Apollo代碼學(xué)習(xí)(三)—車輛動(dòng)力學(xué)模型文章來源地址http://www.zghlxwxcb.cn/news/detail-653696.html

到了這里，關(guān)于自動(dòng)駕駛——車輛動(dòng)力學(xué)模型的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！