自动驾驶——车辆动力学模型

/*lat_controller.cpp*/namespace apollo {namespace control {using apollo::common::ErrorCode;//故障码using apollo::common::Status;//状态码using apollo::common::TrajectoryPoint;//轨迹点using apollo::common::VehicleStateProvider;//车辆状态信息using Matrix = Eigen::MatrixXd;//矩阵库using apollo::cyber::Clock;//Apollo时钟namespace {//日志文件名称，名称与时间相关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);}//在指定的日志文件内写入各列数据标题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//构造函数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);//保留六位小数WriteHeaders(steer_log_file_);//写入数据标题}AINFO << "Using " << name_;}//析构函数 关闭日志文件LatController::~LatController() {CloseLogFile(); }//加载控制配置 "control_conf.pb.txt"//车辆参数 "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类内成员控制周期ts_加载纵向控制配置中的控制周期control_conf.pb.txt--lat_controller_conf--tsts_ = 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_ 前轮侧偏刚度（左右轮之和）cf_ = control_conf->lat_controller_conf().cf();//后轮侧偏刚度之和cr_ = control_conf->lat_controller_conf().cr();//预览窗口的大小preview_window_ = control_conf->lat_controller_conf().preview_window();//低速前进预瞄距离lookahead_station_low_speed_ =control_conf->lat_controller_conf().lookahead_station();//低速倒车预瞄距离lookback_station_low_speed_ =control_conf->lat_controller_conf().lookback_station();//高速前进预瞄距离lookahead_station_high_speed_ =control_conf->lat_controller_conf().lookahead_station_high_speed();//高速倒车预瞄距离lookback_station_high_speed_ =control_conf->lat_controller_conf().lookback_station_high_speed();//轴距wheelbase_ = vehicle_param_.wheel_base();//转向传动比 = 方向盘转角/前轮转角steer_ratio_ = vehicle_param_.steer_ratio();//单边方向盘最大转角转化成degsteer_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();//低速窗口//主要应用在低高速切换之间的线性插值，凡是涉及低高速控制切换的，就在这个窗口做线性插值过渡低高速的控制参数low_speed_window_ =control_conf_->lon_controller_conf().switch_speed_window();//左前悬的质量const double mass_fl = control_conf->lat_controller_conf().mass_fl();//右前悬的质量const double mass_fr = control_conf->lat_controller_conf().mass_fr();//左后悬的质量const double mass_rl = control_conf->lat_controller_conf().mass_rl();//右后悬的质量const double mass_rr = control_conf->lat_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_);//转动惯量iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;//LQR求解精度lqr_eps_ = control_conf->lat_controller_conf().eps();//迭代最大次数lqr_max_iteration_ = control_conf->lat_controller_conf().max_iteration();//若打开query_time_nearest_point_only模式，则会用到此参数，但是默认关闭//若打开此种模式，则目标点选为当前时间+query_relative_time_ 这个时间在参考轨迹上对应的点//query_relative_time默认设置为0.8s，若打开此种模式就默认始终用将来0.8s的轨迹点作为目标点驱动当前车产生控制量向前走query_relative_time_ = control_conf->query_relative_time();//最小速度保护，避免v=0作为分母情况出现minimum_speed_protection_ = control_conf->minimum_speed_protection();return true;}//处理日志数据函数，将储存在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());//默认enable_csv_debug日志debug开关关闭if (FLAGS_enable_csv_debug) {steer_log_file_ << log_str << std::endl;}ADEBUG << "Steer_Control_Detail: " << log_str;}//打印初始化参数//显示控制器的名称 //显示参数：车辆总质量、转动惯量、前悬长度、后悬长度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);//初始化滤波器传递函数分母[3,0]std::vector<double> num(3, 0.0);//初始化滤波器传递函数分子为[3,0]//将控制周期和cutoff_freq()作为参数输入，计算得到滤波器传递函数分子分母，注意此处引用变量作为实参，就是为了被函数修改之后传回来。common::LpfCoefficients(ts_, control_conf->lat_controller_conf().cutoff_freq(), &den, &num);//上面计算出的分子，分母用来设置数字滤波器类对象digital_filter_，用于对方向盘转角控制指令进行滤波digital_filter_.set_coefficients(den, num);//对反馈的横向误差进行均值滤波，简而言之就是移动窗口内的多个值取平均达到滤波的效果//从控制配置读取均值滤波窗口大小(默认设置为10)设置均值滤波器类对象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车辆当前状态信息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");}//矩阵初始化操作//车辆状态方程中矩阵的大小 = 基础状态矩阵大小 + 预览窗口大小const int matrix_size = basic_state_size_ + preview_window_;//4+0matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);//4*4零矩阵//计算机控制需要转换成离散的差分方程形式//matrix_ad_是系数矩阵A的离散形式，A通过双线性变化法变成Admatrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);/*A matrix (Gear Drive) 前进挡的状态方程系数矩阵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 * tsmatrix_bd_ = matrix_b_ * ts_;//车辆状态矩阵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中目标函数中各个状态量(X=[e1 e1' e2 e2'])平方和的权重系数matrix_q_ = Matrix::Zero(matrix_size, matrix_size);//q_param_size默认为4int q_param_size = control_conf_->lat_controller_conf().matrix_q_size();//倒车的reverse_q_param_sizeint 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)//默认分别为0.05，0.0，1.0，0.0//加载到LatController类数据成员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个滤波器，1个低通滤波是对计算出方向盘转角控制指令进行滤波InitializeFilters(control_conf_);//横向控制配置auto &lat_controller_conf = control_conf_->lat_controller_conf();//加载增益调度表，就是横向误差和航向误差在车速不同时乘上个不同的比例//这个比例决定了实际时的控制效果，根据实际经验低速和高速应该采取不同的比例，低速比例较大LoadLatGainScheduler(lat_controller_conf);//打印一些车辆参数的信息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_);}//是否能使前进倒车的预瞄控制enable_look_ahead_back_control_enable_look_ahead_back_control_ =control_conf_->lat_controller_conf().enable_look_ahead_back_control();//返回状态码return Status::OK();}//关闭日志文件void LatController::CloseLogFile() {if (FLAGS_enable_csv_debug && steer_log_file_.is_open()) {steer_log_file_.close();}}//加载增益调度表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,xy2Interpolation1D::DataType xy1, xy2;//遍历调度表 将每一组横向误差调度表的speed,ratio结对make_pair存入插值表xy1for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}//将每组朝向误差调度表的speed,ratio结对make_pair存入插值表for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}//将lat_err_interpolation_复位，然后用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_复位，然后用xy1初始化heading_err_interpolation_heading_err_interpolation_.reset(new Interpolation1D);ACHECK(heading_err_interpolation_->Init(xy2))<< "Fail to load heading error gain scheduler";}//关闭横向日志文件void LatController::Stop() {CloseLogFile(); }//返回横向控制器的名称字符串std::string LatController::Name() const {return name_; }//计算控制指令Status LatController::ComputeControlCommand(const localization::LocalizationEstimate *localization,const canbus::Chassis *chassis,const planning::ADCTrajectory *planning_published_trajectory,ControlCommand *cmd) {//通过injector_获取当前车辆状态信息存储在vehicle_stateauto vehicle_state = injector_->vehicle_state();//车辆期望轨迹存放到target_tracking_trajectoryauto target_tracking_trajectory = *planning_published_trajectory;//是否打开导航模式，默认关闭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 trajectoryif (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是从输入参数传进来的规划轨迹信息//将target_tracking_trajectory对象内容move移动到LatController类数据成员trajectory_analyzer_里trajectory_analyzer_ =std::move(TrajectoryAnalyzer(&target_tracking_trajectory));//将规划轨迹从后轴中心变换到质心，如果条件满足的话，倒档是否需要转换到质心坐标的开关//这个if不满足，可以略过if (((FLAGS_trajectory_transform_to_com_reverse &&vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) ||//前进档是否需要转换到质心坐标的开关,默认false(FLAGS_trajectory_transform_to_com_drive &&vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE)) &&enable_look_ahead_back_control_) {trajectory_analyzer_.TrajectoryTransformToCOM(lr_);}//倒挡重建车辆的动力学模型，尤其是横向的动力学模型if (vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) {/*倒档时的系数矩阵AA 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项都是D档，R档A中会变化的项，D档和R档这4项不同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 {/*前进档的系数矩阵AA 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_ = 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_;//调用更新驾驶航向函数UpdateDrivingOrientation();//简单横向调试SimpleLateralDebug *debug = cmd->mutable_debug()->mutable_simple_lat_debug();debug->Clear();//更新车辆状态矩阵X=[e1 e1' e2 e2']//首先该函数UpdateState()内部调用了ComputeLateralErrors()函数得到的各种误差信息存放到debug中//然后又用debug去更新车辆状态矩阵X即matrix_state_UpdateState(debug);//主要是更新车辆状态方程系数矩阵A及其离散形式中与速度相关的时变项UpdateMatrix();//更新以及组装离散矩阵Ad,Bd和预览窗口模型，预览窗在横向控制中都关闭了，控制配置里preview_window为0UpdateMatrixCompound();//R档调整矩阵Qint 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_qfor (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_qfor (int i = 0; i < q_param_size; ++i) {matrix_q_(i, i) = control_conf_->lat_controller_conf().matrix_q(i);}}//对于更高速度的转向增加增益调度表//取出中enable_gain_scheduler的值，默认是false，但是实际上很有用//如果打开增益调度表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问题，求解到的最优状态反馈矩阵K放入matrix_k_中，最后一个引用变量作参数，明摆着就是要用对形参的修改改变实参，//常用来存放函数计算结果 //这个if和else都是调用SolveLQRProblem函数，其中不同就只有一个是matrix_q_updated_是考虑调度增益表的Q，Q与车速有关//一个是matrix_q_不考虑增益调度表的Q矩阵，Q与车速无关，看你是否打开调度增益表//根据经验打开的话更容易获得更好的控制性能，否则低速适用的Q用到高速往往容易出现画龙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//状态反馈控制量 u = -K*X//将计算出的控制量(车辆的前轮转角)从rad转化为deg,然后再乘上转向传动比steer_ratio_转化成方向盘转角//最后再根据单边的方向盘最大转角转化控制为百分数 -100-100//这里计算出的是反馈的控制量const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /M_PI * steer_ratio_ /steer_single_direction_max_degree_ * 100;//定义临时常量steer_angle_feedforward存放前馈控制量//调用函数ComputeFeedForward计算前馈控制量const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());double steer_angle = 0.0;double steer_angle_feedback_augment = 0.0;//在期望的速度域增强横向误差的反馈控制//如果打开leadlag超前滞后控制器if (enable_leadlag_) {//如果车辆打开高速的反馈增强控制 或 车速小于低高速边界速度if (FLAGS_enable_feedback_augment_on_high_speed ||std::fabs(vehicle_state->linear_velocity()) < low_speed_bound_) {//满足上述条件,实际上就是低速时打开超前滞后控制器，然后这个超前滞后控制器只对横向误差进行增强控制//计算出反馈增强控制方向盘转角百分数steer_angle_feedback_augmentsteer_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, 线性插值增强控制// augment control gain for "soft" control switchsteer_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;//根据最大的横向加速度限制计算方向盘转向的限制//若限制横向加速度 最大方向盘转角百分数 = atan(ay_max * L / v^2) * steerratio * 180/pi /max_steer_ang * 100//根据上述公式可以从限制的最大横向加速度计算出最大的方向盘转角控制百分数//若无限制 最大方向盘转角百分数 = 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;//这一部分主要是对方向盘转动速率进行限制，默认关闭//如果没打开，最大就限制为100//一个周期方向盘转角最大增量 = 最大方向盘角速度 * 控制周期//此刻方向盘转角控制量只能在范围内：上一时刻方向盘转角控制量 +/- 一个周期方向盘转角最大增量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;//方向盘实际转角//方向盘实际转角从chassis信息读，canbus从车辆can线上读到发出来的const double steering_position = chassis->steering_percentage();//如果打开MRAC模型参考自适应控制 enable_mrac_//重新计算方向盘转角控制量，并用方向盘转角和转速限制对转角控制量进行限幅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 messageMracDebug *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_控制配置文件里的参数默认是false，略过//将当前转角设置为上一时刻的转角pre_steering_position_ = steering_position;//将enable_mrac_是否打开信息加入debug调试信息结构体debug->set_steer_mrac_enable_status(enable_mrac_);//根据当前车速对下发方向盘转角进行限幅，横向加速度的限制转化到此刻方向盘转角限制就会引入车速//steer_limit通过横向最大加速度或者方向盘最大转角限制//限幅后的方向盘转角steer_angle_limiteddouble steer_angle_limited =common::math::Clamp(steer_angle, -steer_limit, steer_limit);steer_angle = steer_angle_limited;//方向盘转角信息写入debug结构体中debug->set_steer_angle_limited(steer_angle_limited);//对方向盘转角数字滤波，然后控制百分数又限制在正负100steer_angle = digital_filter_.Filter(steer_angle);steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);//当处于D档或倒档且车速小于转向自锁速度且处于自驾模式时锁定方向盘，方向盘控制转角就保持上一次的命令值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_;}//设定转角指令，再通过最大转角速率再次进行限制幅度 最多只能=上次的转角指令+/-最大转角速率 * Tscmd->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();//日志和调试计算的一些额外信息//横向误差贡献的控制量百分数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里 //驾驶朝向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());//将debug和chassis信息放入记录日志里ProcessLogs(debug, chassis);return Status::OK();}//mrac模型参考自适应控制的复位函数//enable_steer_mrac_control，其默认关闭，是另外一种横向控制器Status LatController::Reset() {matrix_state_.setZero();if (enable_mrac_) {mrac_controller_.Reset();}return Status::OK();}//横向控制器的车辆状态矩阵函数SimpleLateralDebug是control_cmd.proto下的一个message类型，包含各种调试信息//用于返回车辆当前状态void LatController::UpdateState(SimpleLateralDebug *debug) {auto vehicle_state = injector_->vehicle_state();//是否使用navigation_modeif (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 matchedconst 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);}// 状态矩阵更新;//当打开这个e1和e3分别赋值横向反馈误差和航向反馈误差，和下面else两行的区别在哪里？//若打开lookahead（D档）,lookback(R档)则x中的e1,e3就为考虑了lookahead的误差//lateral_error_feedback = lateral_error + 参考点到lookahead点的横向误差//heading_error_feedback = heading_error + ref_heading - lookahead点的heading 实际上就是匹配点到lookahead点的航向差 //heading_error = 车辆heading - ref_headingif (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();// 这一部分是更新状态矩阵中的preview项，但是preview_window默认为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;}}//更新矩阵函数，主要是更新系数矩阵A,Advoid LatController::UpdateMatrix() {double v;//倒档代替横向平动动力学用对应的运动学模型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为车辆纵向速度和最小速度保护里的最大值v = std::max(injector_->vehicle_state()->linear_velocity(),minimum_speed_protection_);matrix_a_(0, 2) = 0.0;}//更新A矩阵中与v有关的项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());//计算A矩阵的离散化形式Ad，用双线性变换法matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *(matrix_i + ts_ * 0.5 * matrix_a_);}//更新组装矩阵Adc,就是Ad考虑preview后扩展的结果void LatController::UpdateMatrixCompound() {// Initialize preview matrixmatrix_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;}}}//计算横向控制的前馈量double LatController::ComputeFeedForward(double ref_curvature) const {//kv=lr*m/2/Cf/L - lf*m/2/Cr/Lconst double kv =lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;// 计算前馈项并且转化成百分数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;}//计算横向误差，放入debug指针中//参数：xy坐标，车辆当前heading,纵向速度v,heading变化率，纵向加速度//轨迹相关信息trajectory_analyzer对象用于提供轨迹的参考点，匹配点，lookahead点等信息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类包含轨迹点的v,acc,jerk,相对时间，前轮转向角等信息TrajectoryPoint target_point;//如果只将车辆当前的时间向前加固定时间长度后在轨迹上对应点作为目标点if (FLAGS_query_time_nearest_point_only) {//在时域上向前看0.8秒作为目标点target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(Clock::NowInSeconds() + query_relative_time_);} else {//默认不采用导航模式，直接看elseif (FLAGS_use_navigation_mode &&!FLAGS_enable_navigation_mode_position_update) {target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(Clock::NowInSeconds() + query_relative_time_);} else {//默认不采用导航模式,则目标点取轨迹上距离当前车辆xy坐标点最近的点，默认目标点就是取距离最近点target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);}}//dx就是当前车辆和目标点的x坐标之差const double dx = x - target_point.path_point().x();//dy就是当前车辆和目标点的y坐标之差const double dy = y - target_point.path_point().y();//将目标点的x坐标存入debug指针debug->mutable_current_target_point()->mutable_path_point()->set_x(target_point.path_point().x());//将目标点的y坐标存入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();//计算目标点处的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());//根据目标点处的heading角正余弦值计算横向误差double lateral_error = cos_target_heading * dy - sin_target_heading * dx;if (FLAGS_enable_navigation_mode_error_filter) {//如果打开导航模式误差滤波器，modules/control/common/里//默认时关闭的，导航模式及滤波器都关闭，直接略过lateral_error = lateral_error_filter_.Update(lateral_error);}debug->set_lateral_error(lateral_error);//将目标点的heading角填入debug指针的debug.ref_headingdebug->set_ref_heading(target_point.path_point().theta());//计算航向误差//车辆当前航向角theta-ref_heading角//角度标准化double heading_error =common::math::NormalizeAngle(theta - debug->ref_heading());//如果导航模式滤波器打开对航向偏差进行滤波if (FLAGS_enable_navigation_mode_error_filter) {heading_error = heading_error_filter_.Update(heading_error);}debug->set_heading_error(heading_error);//在低-高速切换窗口，现行插值预瞄距离为了更柔和的预测切换double lookahead_station = 0.0;//Ddouble lookback_station = 0.0;//R//速度大于低速边界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_;//就采用低速的预瞄距离包括非R档和R档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));}//估计考虑预瞄距离的航向误差heading_error_feedbackdouble heading_error_feedback;if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {heading_error_feedback = heading_error;} else {auto lookahead_point = trajectory_analyzer.QueryNearestPointByRelativeTime(//目标点的相对时间再加上预瞄时间(预瞄距离/车辆纵向速度)作为总相对时间//然后去trajectory_analyzer轨迹信息上根据总相对时间找出预瞄点target_point.relative_time() +lookahead_station ///在估计预瞄时间时纵向速度若小于0.1就按0.1(std::max(std::fabs(linear_v), 0.1) * std::cos(heading_error)));//heading_error=车辆当前heading-参考点heading//heading_error_feedback=heading_error+参考点heading-预瞄点heading=车辆当前heading-预瞄点headingheading_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);//估计考虑预瞄距离的横向误差lateral_error_feedbackdouble lateral_error_feedback;if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {//倒档的lateral_error_feedback=lateral_error-倒车预瞄距离*sin(heading_error)lateral_error_feedback =lateral_error - lookback_station * std::sin(heading_error);} else {//前进档的lateral_error_feedback=lateral_error+前进预瞄距离*sin(heading_error)lateral_error_feedback =lateral_error + lookahead_station * std::sin(heading_error);}//将lateral_error_feedback存入指针debugdebug->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;}}//将计算得到的横向误差率填入debug指针debug->set_lateral_error_rate(lateral_error_dot);//将计算得到的横向误差加速度填入debug指针debug->set_lateral_acceleration(lateral_error_dot_dot);//利用横向加速度差分得到横向加加速度jerk放入debug指针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) {//angular_v是ComputeLateralError函数传进来的参数heading的变化率，若倒车debug.heading_rate为负的angular_vdebug->set_heading_rate(-angular_v);} else {debug->set_heading_rate(angular_v);}//参考的航向角变化率=目标点纵向速度/目标点转弯半径，绕Z轴w=v/R，下面的kappa就是曲率 结果放入debug指针中debug->set_ref_heading_rate(target_point.path_point().kappa() *target_point.v());//航向角误差率=车辆的航向角变化率-目标点航向角变化率，结果放入debug指针中debug->set_heading_error_rate(debug->heading_rate() -debug->ref_heading_rate());//航向角变化的加速度就用差分法，这一时刻航向角变化率减去上一时刻之差然后再除以采样周期ts，结果放入debug指针中debug->set_heading_acceleration((debug->heading_rate() - previous_heading_rate_) / ts_);//目标点航向角变化的加速度也用差分法计算得到，结果放入debug指针中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();//heading角的加加速度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_);//heading角误差变化率的加加速度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();//kappadebug->set_curvature(target_point.path_point().kappa());}//更新驾驶航向void LatController::UpdateDrivingOrientation() {//更新车辆状态auto vehicle_state = injector_->vehicle_state();//朝向driving_orientation_ = vehicle_state->heading();//横向控制状态方程B离散化为Bdmatrix_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 modematrix_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 modelsif (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_;

反馈计算

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

前馈计算：

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

参考【运动控制】Apollo6.0的leadlag_controller解析

控制误差计算

横向距离误差计算

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;

横向误差变化率计算

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

航向角误差计算

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

航向角误差变化率计算

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代码学习(三)—车辆动力学模型