前言

2021/12/30

前段时间一直在看Apollo的控制代码，因为工作较忙，只能抽时间整理下代码笔记，可能稍显粗糙，部分图片手绘，作为日后调试之参照。以后有时间再优化排版，再把涉及到的其他概念补上。看完这篇文章，你会对apollo横向控制有一个整体的概念。

1.车辆动力学模型推导

2.横向误差动力学模型推导

3.状态方程系数矩阵的离散化

车辆状态方程是连续空间，但是计算控制是离散控制，需要将微分方程转化成差分方程，所以要对系数矩阵进行离散化。

4.LQR横向控制原理

5.方向盘转角前馈控制推导

为了使横向稳态误差为0 e1ss -> 0

6.横向控制原理框图

7.横向控制关键代码解析

纵向控制主要看纵向控制器LatController类的实现，本节将对

apollo/modules/control/controller/lat_controller.h

apollo/modules/control/controller/lat_controller.cc

两个源文件代码进行逐行解析，可以结合代码和上述框图一起看

7.1 lat_controller.h

//modules/control/controller/lat_controller.h
//声明了LatController横向控制器类#pragma once#include <fstream>
#include <memory>
#include <string>#include "Eigen/Core"
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/filters/digital_filter_coefficients.h"
#include "modules/common/filters/mean_filter.h"
#include "modules/control/common/interpolation_1d.h"
#include "modules/control/common/leadlag_controller.h"
#include "modules/control/common/mrac_controller.h"
#include "modules/control/common/trajectory_analyzer.h"
#include "modules/control/controller/controller.h"//类声明位于命名空间apollo::control::
namespace apollo {
namespace control {//LatController类 实现LQR控制器，继承控制器Controller类，计算目标方向盘转角
class LatController : public Controller {public://类构造函数LatController();//类析构函数virtual ~LatController();//初始化横向控制器的控制配置参数，返回初始化的状态码common::Status Init(std::shared_ptr<DependencyInjector> injector,const ControlConf *control_conf) override;//基于车辆当前状态和目标轨迹计算期望的方向盘转角//参数：localization车辆定位信息//参数：chassis车辆状态信息，如速度，加速度等//参数：trajectory规划发布的轨迹//参数：cmd控制指令，该函数根据前三项参数计算出的控制指令放在cmd里//返回计算的状态码common::Status ComputeControlCommand(const localization::LocalizationEstimate *localization,const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,ControlCommand *cmd) override;//复位横向控制器函数//返回复位的状态码//override表明该函数接口是在基类中被声明为虚函数，在该类中实现common::Status Reset() override;//停止横向控制器函数//override表明该函数接口是在基类中被声明为虚函数，在该类中实现void Stop() override;//横向控制器的名字//返回横向控制器名字字符串std::string Name() const override;protected://更新车辆状态方程中的车辆状态矩阵X=[e1 e1' e2 e2'] e1,e2分别代表横向，航向误差void UpdateState(SimpleLateralDebug *debug);//倒车模式下才生效？倒车模式下，heading要进行转化，倒车模式默认关闭void UpdateDrivingOrientation();//更新车辆状态方程系数矩阵A及其离散形式Advoid UpdateMatrix();//扩展并更新考虑preview_window的系数矩阵A,横向控制preview_window是关闭的void UpdateMatrixCompound();//根据道路曲率计算前馈控制量double ComputeFeedForward(double ref_curvature) const;//计算横向误差函数，输入车辆x,y,theta,v,theta',a,以及轨迹信息，计算出来的结果都放到最后一个参数debug中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);//加载控制配置参数文件bool LoadControlConf(const ControlConf *control_conf);//初始化横向控制中的滤波器void InitializeFilters(const ControlConf *control_conf);//加载横向的增益调度表void LoadLatGainScheduler(const LatControllerConf &lat_controller_conf);//这个函数主要是在屏幕上打印一些车辆参数的信息void LogInitParameters();//将debug和chassis信息放入记录日志里void ProcessLogs(const SimpleLateralDebug *debug,const canbus::Chassis *chassis);//关闭横向日志文件void CloseLogFile();//车辆控制配置const ControlConf *control_conf_ = nullptr;//车辆本身参数common::VehicleParam vehicle_param_;//规划轨迹分析代理，该类用于实现提取轨迹信息TrajectoryAnalyzer trajectory_analyzer_;//下列参数是车辆的物理参数，从上面的控制配置control_conf_读取出来读到这些数据成员里//控制周期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;//车辆绕z轴的转动惯量double iz_ = 0.0;//方向盘转角和前轮转角之比double steer_ratio_ = 0.0;//方向盘单边的最大转角double steer_single_direction_max_degree_ = 0.0;//最大允许横向加速度double max_lat_acc_ = 0.0;// number of control cycles look ahead (preview controller)//向前预览的控制周期的数量int preview_window_ = 0;//预瞄控制相关参数//低速前进预瞄距离，针对非R档double lookahead_station_low_speed_ = 0.0;//低速倒车预瞄距离，针对R档double lookback_station_low_speed_ = 0.0;//高速前进预瞄距离，针对非R档double lookahead_station_high_speed_ = 0.0;//高速倒车预瞄距离，针对R档double lookback_station_high_speed_ = 0.0;//不考虑预览窗口的车辆状态矩阵X的维度，[e1 e1' e2 e2']初始化维度为4//e1,e2分别为横向误差，航向误差const int basic_state_size_ = 4;// 车辆状态方程系数矩阵A x'=Ax+Bu+B1*Psi_des'  Psi_des‘期望的heading角变化率Eigen::MatrixXd matrix_a_;// 车辆状态方程系数矩阵A的离散形式Ad,就是将A用双线性变换法离散Eigen::MatrixXd matrix_ad_;//车辆状态矩阵考虑preview预览之后的扩展阵//横向控制preview没有打开可以忽略这个adc，adc就是ad  4x4Eigen::MatrixXd matrix_adc_;// control matrix//车辆状态方程系数矩阵B  控制量的系数矩阵Eigen::MatrixXd matrix_b_;  4x1// 系数矩阵B的离散形式 BdEigen::MatrixXd matrix_bd_;  4x1// 系数矩阵考虑preview之后的扩展阵Eigen::MatrixXd matrix_bdc_;// 状态反馈矩阵K   u=-kx  LQR求解出最优的K  K=[k0 k1 k2 k3] 1x4Eigen::MatrixXd matrix_k_; // control authority weighting matrix//LQR算法中控制量的权重系数矩阵R  这里只有一个控制量就是前轮转角，所以R 1x1Eigen::MatrixXd matrix_r_;//LQR算法中状态反馈量的权重系数矩阵Q  这里有4个状态反馈量[e1 e1' e2 e2']^T，所以Q 4x4对角阵，主要就是对角线上是权重系数Eigen::MatrixXd matrix_q_;//更新后的Q矩阵 如果打开增益调度表 那么要不同车速下可以配置不同的Q矩阵，所以要根据车速更新QEigen::MatrixXd matrix_q_updated_;//车辆状态方程系数矩阵A中与v有关的时变项形如" 常数/v "，将常数提取出来放在矩阵matrix_a_coeff_里，每个周期处以v更新Eigen::MatrixXd matrix_a_coeff_;//车辆状态矩阵[e1 e1' e2 e2'], e1,e2分别为横向误差，航向误差Eigen::MatrixXd matrix_state_;//LQR控制算法求解器的参数，最大迭代次数，从控制配置里读int lqr_max_iteration_ = 0;//LQR控制算法求解器的参数，求解的精度，从控制配置里读double lqr_eps_ = 0.0;//数字滤波器类对象，这里是用于对方向盘转角控制指令进行滤波common::DigitalFilter digital_filter_;//插值表类对象，这里是用于根据车速插值车辆的增益调度表，不同v下，车辆横向误差乘以不同比例std::unique_ptr<Interpolation1D> lat_err_interpolation_;//插值表类对象，这里是用于根据车速插值车辆的增益调度表，不同v下，车辆航向误差乘以不同比例std::unique_ptr<Interpolation1D> heading_err_interpolation_;//均值滤波器类对象//横向误差均值滤波器common::MeanFilter lateral_error_filter_;//航向误差均值滤波器common::MeanFilter heading_error_filter_;//超前滞后控制器，在主回路上串联校正环节bool enable_leadlag_ = false;LeadlagController leadlag_controller_;//模型参考自适应控制MRAC，这里没有开启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_;//如果打开相应开关，就始终将车辆当前时间向前加0.8秒在轨迹上对应的点作为目标点double query_relative_time_;//上一时刻方向盘控制命令值double pre_steer_angle_ = 0.0;//上一时刻方向盘实际转角double pre_steering_position_ = 0.0;//最小速度保护，车辆状态方程系数矩阵A中有好几项分母中含有v的，//若v为0或者过小时会引发冲击或者错误，因此在更新系数矩阵时v小于保护速度就用保护速度代入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;//定义低高速的切换临界点，低速的边界，有些控制参数采用低速高速两套，低速边界默认设置为3m/sdouble low_speed_bound_ = 0.0;//低速窗口，主要是为了在高低速参数切换时防止过于生硬，又在这个窗口范围内进行线性插值double low_speed_window_ = 0.0;//当前车辆的航向角double driving_orientation_ = 0.0;//injector_是一个用来获取车辆状态信息的对象std::shared_ptr<DependencyInjector> injector_;
};}  // namespace control
}  // namespace apollo

7.2 lat_controller.cc

//modules/control/controller/lat_controller.cc
//Line37-45//控制器都是命名空间apollo::control::下定义的namespace apollo {namespace control {//使用了这些模块的类，故障码，状态码，轨迹点，车辆状态信息，矩阵运算，apollo时钟using apollo::common::ErrorCode;using apollo::common::Status;using apollo::common::TrajectoryPoint;using apollo::common::VehicleStateProvider;using Matrix = Eigen::MatrixXd;using apollo::cyber::Clock;//Line49-58//生成横向日志文件名称，名称与时间相关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);}//Line69-77//在指定的日志文件内写入各列数据标题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//Line80-88//LatController类的构造函数，打开横向日志文件LatController::LatController() : name_("LQR-based Lateral Controller") {//若打开csv_debug开关，enable_csv_debug去control_gflags.cc里查找//FLAGS_就是gflags库的用法，从相关.cc文件中直接取出全局变量值，默认关闭if (FLAGS_enable_csv_debug) {//获取日志文件名称steer_log_file_.open(GetLogFileName());//设定写入数据的精度为6位小数steer_log_file_ << std::fixed;steer_log_file_ << std::setprecision(6);//写入数据标题WriteHeaders(steer_log_file_);}AINFO << "Using " << name_;}//Line90//LatController类的析构函数，关闭横向日志文件LatController::~LatController() { CloseLogFile(); }//Line92-147//加载控制配置文件"control_conf.pb.txt"//加载车辆参数配置文件"vehicle_param.pb.txt"bool LatController::LoadControlConf(const ControlConf *control_conf) {//控制配置对象control_conf为空指针？if (!control_conf) {AERROR << "[LatController] control_conf == nullptr";return false;}//LatController类内成员车辆参数配置vehicle_param_加载车辆参数配置vehicle_param_ =common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();//LatController类内成员控制周期ts_加载纵向控制配置中的控制周期control_conf.pb.txt--lat_controller_conf--tsts_ = control_conf->lat_controller_conf().ts();//如果读取到的控制周期<=0则提示错误信息并返回if (ts_ <= 0.0) {AERROR << "[MPCController] Invalid control update interval.";return false;}//将控制配置中的前轮侧偏刚度(左右轮之和)cf读取到LatController类数据成员cf_cf_ = control_conf->lat_controller_conf().cf();//将控制配置中的后轮侧偏刚度(左右轮之和)cr读取到数据成员cr_cr_ = control_conf->lat_controller_conf().cr();//将控制配置中的预览窗口大小读取到数据成员preview_window_preview_window_ = control_conf->lat_controller_conf().preview_window();//将控制配置的低速预瞄距离读取到LatController类内数据成员lookahead_station_low_speed_lookahead_station_low_speed_ =control_conf->lat_controller_conf().lookahead_station();//lookback是倒车时的预瞄距离，与上面类似略过lookback_station_low_speed_ =control_conf->lat_controller_conf().lookback_station();//将控制配置的高速预瞄距离读取到LatController类内数据成员lookahead_station_high_speed_  lookahead_station_high_speed_ =control_conf->lat_controller_conf().lookahead_station_high_speed();//lookback是倒车时的预瞄距离，与上面类似略过lookback_station_high_speed_ =control_conf->lat_controller_conf().lookback_station_high_speed();//从车辆参数配置vehicle_param_读取轴距到LatController类数据成员wheelbase_wheelbase_ = vehicle_param_.wheel_base();//从车辆参数配置vehicle_param_读取转向传动比到LatController类数据成员steer_ratio_//转向传动比 = 方向盘转角/前轮转角steer_ratio_ = vehicle_param_.steer_ratio();//从车辆参数配置vehicle_param_读取单边方向盘最大转角转化成deg到LatController类数据成员//LatController类数据成员steer_single_direction_max_degree_steer_single_direction_max_degree_ =vehicle_param_.max_steer_angle() / M_PI * 180;//从控制配置control_conf加载最大允许的横向加速度到LatController类数据成员max_lat_acc_max_lat_acc_ = control_conf->lat_controller_conf().max_lateral_acceleration();//从控制配置control_conf_加载低高速边界到LatController类数据成员low_speed_bound_low_speed_bound_ = control_conf_->lon_controller_conf().switch_speed();//从控制配置control_conf_加载低速窗口到LatController类数据成员low_speed_window_//这个窗口主要应用在低高速切换之间的线性插值，凡是涉及低高速控制切换的，就在这个窗口做线性插值过渡低高速的控制参数low_speed_window_ =control_conf_->lon_controller_conf().switch_speed_window();//从控制配置control_conf加载左前悬的质量到临时常量变量mass_flconst double mass_fl = control_conf->lat_controller_conf().mass_fl();//从控制配置control_conf加载右前悬的质量到临时常量变量mass_frconst double mass_fr = control_conf->lat_controller_conf().mass_fr();//从控制配置control_conf加载左后悬的质量到临时常量变量mass_rlconst double mass_rl = control_conf->lat_controller_conf().mass_rl();//从控制配置control_conf加载右后悬的质量到临时常量变量mass_rrconst double mass_rr = control_conf->lat_controller_conf().mass_rr();//定义临时常量 前悬质量mass_front=左前悬质量+右前悬质量const double mass_front = mass_fl + mass_fr;//定义临时常量 后悬质量mass_front=左后悬质量+右后悬质量const double mass_rear = mass_rl + mass_rr;//计算 车辆总质量 = 前悬质量 + 后悬质量，计算结果存放到LatController类的数据成员 mass_//这里可以看出类内的对外部接口信息的别名通常都是小写英文加"_"//看到带下划线的变量下意识就可以想到是该类的数据成员mass_ = mass_front + mass_rear;//计算 前悬长度(前轴到车辆质心的距离) = 轴距 * (1 - 前悬质量 / 车辆总质量) 计算结果存放到LatController类的数据成员 lf_lf_ = wheelbase_ * (1.0 - mass_front / mass_);//计算 后悬长度(后轴到车辆质心的距离) = 轴距 * (1 - 后悬质量 / 车辆总质量) 计算结果存放到LatController类的数据成员 lr_lr_ = wheelbase_ * (1.0 - mass_rear / mass_);//计算 车辆绕z轴的转动惯量(又叫惯性矩) m*r^2//车辆绕z轴的转动惯量 = 前悬长度^2 * 前悬质量 + 后悬长度^2 * 后悬质量 计算结果存放到LatController类的数据成员iz_iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;//从控制配置control_conf_加载LQR迭代求解精度到LatController类数据成员lqr_eps_lqr_eps_ = control_conf->lat_controller_conf().eps();//从控制配置control_conf_加载LQR迭代求解最大次数到LatController类数据成员lqr_max_iteration_lqr_max_iteration_ = control_conf->lat_controller_conf().max_iteration();//若打开query_time_nearest_point_only模式，则会用到此参数，但是默认关闭//从控制配置control_conf_加载查询相对时间到LatController类数据成员query_relative_time_//若打开此种模式，则 目标点选为 当前时间+query_relative_time_ 这个时间在参考轨迹上对应的点//query_relative_time默认设置为0.8s，若打开此种模式就默认始终用将来0.8s的轨迹点作为目标点驱动当前车产生控制量向前走query_relative_time_ = control_conf->query_relative_time();//从控制配置control_conf_加载最小速度保护到LatController类数据成员minimum_speed_protection_//minimum_speed_protection的作用可以这样理解，因为车辆状态方程系数矩阵A(离散形式Ad)中均包含v作为分母的元素//若v=0，那么计算控制量时出现0出现在分母的情况，所以在控制配置里设置一个最小速度保护值//在更新系数矩阵A,Ad时，若v<最小保护速度，则v取最小保护速度，避免出现v=0作为分母的情况minimum_speed_protection_ = control_conf->minimum_speed_protection();return true;
}//Line149-166//处理日志数据函数，将储存在debug指针中的各种误差等信息写入横向日志文件//实际上默认enable_csv_debug日志debug开关关闭，在control_gflags.cc中设置打开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());if (FLAGS_enable_csv_debug) {steer_log_file_ << log_str << std::endl;}ADEBUG << "Steer_Control_Detail: " << log_str;}//Line168-175//打印初始化参数显示控制器名字，车辆总质量，惯性矩等参数void LatController::LogInitParameters() {AINFO << name_ << " begin.";AINFO << "[LatController parameters]"<< " mass_: " << mass_ << ","<< " iz_: " << iz_ << ","<< " lf_: " << lf_ << ","<< " lr_: " << lr_;}//Line177-188//该函数初始化横向控制中的滤波器void LatController::InitializeFilters(const ControlConf *control_conf) {// 低通滤波器LPF LowPassFilterstd::vector<double> den(3, 0.0); //初始化滤波器传递函数分母为[3,0]std::vector<double> num(3, 0.0); //初始化滤波器传递函数分子为[3,0]//调用common模块下的LpfCoefficients()函数//将控制周期ts_,从控制配置读取的cutoff_freq作为参数输入，计算得到滤波器的传递函数分子和分母//计算出的分子分母储存在den,num中，引用变量做实参，就是为了被函数修改然后传回来common::LpfCoefficients(ts_, control_conf->lat_controller_conf().cutoff_freq(), &den, &num);//上面计算出的分子，分母用来设置数字滤波器类对象digital_filter_//digital_filter_是LatController类的数据成员//digital_filter_用于对方向盘转角控制指令进行滤波digital_filter_.set_coefficients(den, num);//均值滤波器类对象lateral_error_filter_用来对反馈的横向误差进行均值滤波，简而言之就是移动窗口内的多个值取平均达到滤波的效果//均值滤波器lateral_error_filter_参数也是从控制配置control_conf读取(control_conf从配置文件control_conf.pb.txt中加载)//从控制配置读取均值滤波窗口大小(默认设置为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_用来对反馈的航向误差进行均值滤波，简而言之就是移动窗口内的多个值取平均达到滤波的效果//均值滤波器heading_error_filter_参数也是从控制配置control_conf读取(control_conf从配置文件control_conf.pb.txt中加载)//从控制配置读取均值滤波窗口大小(默认设置为10)设置均值滤波器类对象heading_error_filter_heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(control_conf->lat_controller_conf().mean_filter_window_size()));}//Line191-279//该函数完成横向LQR控制器的初始化工作//参数：injector车辆当前状态信息//参数：control_conf控制器参数配置Status LatController::Init(std::shared_ptr<DependencyInjector> injector,const ControlConf *control_conf) {//将控制器参数配置和车辆状态信息读取到LatController类数据成员control_conf_,injector_里//这样先初始化赋值后，类内其他成员函数可以直接访问控制配置和车辆参数control_conf_ = control_conf;injector_ = injector;//在if判断条件里调用布尔类型的加载控制配置的函数LoadControlConf()，将控制配置对象control_conf_中的参数再依次加载到//LatController类的各个数据成员上if (!LoadControlConf(control_conf_)) {AERROR << "failed to load control conf";return Status(ErrorCode::CONTROL_COMPUTE_ERROR,"failed to load control_conf");}//矩阵初始化操作//车辆状态方程 X = AX + B*delta前轮 + B1*Psi_des'   Psi_des是道路曲率变化率即期望的航向角变化率//车辆状态方程中矩阵的大小 = 基础状态矩阵大小 + 预览窗口大小//基础状态矩阵就是X=[e1 e1' e2 e2']^T   ^T表示转置 //e1横向误差，e2航向误差//基础状态矩阵大小basic_state_size_已经在声明时被初始化为4//横向控制预览窗口是0，所以matrix_size就是basic_state_size_=4const int matrix_size = basic_state_size_ + preview_window_;//初始化系数矩阵A为4*4的零矩阵matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);//车辆状态方程是连续形式，计算机控制需要转换成离散的差分方程形式//matrix_ad_是系数矩阵A的离散形式，A通过双线性变化法变成Ad//初始化系数矩阵Ad为4*4的零矩阵matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);//matrix_adc_是Ad Composed  就是由矩阵Ad和预览窗口一起扩展而成的矩阵//横向控制预览窗口是为0默认关闭preview，所以matrix_adc_就是matrix_ad_,所以这里也初始化为4*4零矩阵matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);/*A matrix (Gear Drive) 前进档的车辆状态方程系数矩阵Ac_f,c_r前后轮的侧偏刚度，为左右轮刚度之和l_f,l_r前后轴距m车辆总质量i_z车辆绕z轴的转动惯量v车辆的此刻的纵向速度，可以看出系数矩阵A是随着每时刻速度变化而变化的线性时变矩阵A矩阵中只要跟速度v有关的项都是时变项，其他都是常数项[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;]*///给矩阵A中的常数项(与v无关的项)进行赋值，A中常数项只有下面4个吗？//不是，因为矩阵A matrix_a_被初始化为4*4的零矩阵，0元素项不用再赋值//A矩阵中的非常数项在其他函数中每时刻更新//A矩阵第1行第2列是常数1.0matrix_a_(0, 1) = 1.0;//A矩阵第2行第3列是常数(c_f + c_r) / mmatrix_a_(1, 2) = (cf_ + cr_) / mass_;//A矩阵第3行第4列是常数1.0matrix_a_(2, 3) = 1.0;//A矩阵第4行第3列是常数(l_f * c_f - l_r * c_r) / i_zmatrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;//matrix_a_coeff_矩阵也初始化为4*4的零矩阵matrix_a_coeff_ = Matrix::Zero(matrix_size, matrix_size);//A矩阵中跟v相关的时变项，这些时变项分解成"常数项/v"的形式，然后提取出这个与v无关的常数项//放在矩阵matrix_a_coeff_的对应位置(例如若在A中是(1,1),在matrix_a_coeff_也放在(1,1))//A矩阵中第2行第2列提取出与v无关的常数项-(c_f + c_r) / mmatrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;//A矩阵中第2行第4列提取出与v无关的常数项(l_r * c_r - l_f * c_f) / mmatrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;//A矩阵中第4行第2列提取出与v无关的常数项(lr * cr - lf * cf) / i_zmatrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;//A矩阵中第4行第4列提取出与v无关的常数项(-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z)matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;/*车辆状态方程X = AX + B*delta前轮 + B1*Psi_des'中的矩阵B,B中元素可以看出都是常数项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);//矩阵B的离散化形式Bd初始化为4*1的0矩阵matrix_bd_ = Matrix::Zero(basic_state_size_, 1);//bdc就是bd copmposed 就是bd和预览窗口一起组合起来扩展成新的矩阵bdc//但是横向控制中preview窗口大小为0，所以预览功能相当于关闭，那么matrix_bdc_就是matrix_bd_matrix_bdc_ = Matrix::Zero(matrix_size, 1);//矩阵B为0的项就不用再赋值了，因为初始化为4*1 0矩阵了//矩阵B的第2行第1列为c_f / mmatrix_b_(1, 0) = cf_ / mass_;//矩阵B的第4行第1列为l_f * c_f / i_zmatrix_b_(3, 0) = lf_ * cf_ / iz_;//矩阵B的离散形式Bd就等于 B * tsmatrix_bd_ = matrix_b_ * ts_;//车辆状态矩阵X,[e1 e1' e2 e2']//车辆状态矩阵X初始化为4*1的0矩阵matrix_state_ = Matrix::Zero(matrix_size, 1);//状态反馈矩阵K=[k1 k2 k3 k4]分别对应[e1 e1' e2 e2']^T的各项误差的状态反馈系数//每时刻都会通过LQR求解到一个最优的K矩阵，然后用K矩阵来计算控制量//初始化K矩阵为1*4的0矩阵matrix_k_ = Matrix::Zero(1, matrix_size);//初始化R矩阵为1*1的单位阵，R矩阵就是LQR中目标函数中控制量平方和的权重系数//横向控制只有一个控制量就是前轮转角matrix_r_ = Matrix::Identity(1, 1);//初始化Q矩阵为4*4的0矩阵，Q矩阵是LQR中目标函数中各个状态量(X=[e1 e1' e2 e2'])平方和的权重系数//Q是一个对角阵，对角线上4个元素分别存放e1 e1' e2 e2'平方和在LQR目标函数中的权重系数，这里只是初始化一下matrix_q_ = Matrix::Zero(matrix_size, matrix_size);//定义临时变量q_param_size=control_conf控制配置里的matrix_q_size()，//但是在control_conf.pb.txt里并没有发现matrix_q_size这个参数，但是有matrix_q是repeated数组元素//这个就是Protobuf的用法，XXX_size()就是返回repeated类型数组XXX元素的个数//matrix_q在控制配置中有4项，就是Q矩阵对角线上的4个元素，对应车辆各个状态量在LQR目标函数中的权重系数//q_param_size默认为4int q_param_size = control_conf_->lat_controller_conf().matrix_q_size();//倒车的reverse_q_param_size就是配置文件中reverse_matrix_q元素个数int reverse_q_param_size =control_conf_->lat_controller_conf().reverse_matrix_q_size();//若车辆状态矩阵大小matrix_size ！= Q方阵的维度 或者 车辆状态矩阵大小matrix_size ！= 倒车Q方阵的维度//则报错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//可以看出实际上只考虑了横向误差和航向误差且航向误差的比重要比横向误差大很多，误差变化率Q阵中系数为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_updated_ = matrix_q_;//用LatController类数据成员控制配置control_conf_去初始化滤波器//初始化3个滤波器，1个低通滤波是对计算出方向盘转角控制指令进行滤波//另外两个滤波器是横向误差，航向误差的均值滤波器InitializeFilters(control_conf_);//定义1个临时变量lat_controller_conf,从控制配置control_conf_将//control_conf_里的lat_controller_conf读取到临时变量lat_controller_conf//lat_controller_conf实际上就是控制配置中的横向控制配置auto &lat_controller_conf = control_conf_->lat_controller_conf();//LoadLatGainScheduler加载增益调度表，就是横向误差和航向误差在车速不同时乘上个不同的比例//这个比例决定了实际时的控制效果，根据实际经验低速和高速应该采取不同的比例，低速比例较大，若高速//采用同样比例极有可能导致画龙现象，问题来了，对于一辆给定的车，争议调度表如何确定？LoadLatGainScheduler(lat_controller_conf);//这个函数主要是在屏幕上打印一些车辆参数的信息LogInitParameters();//是否使能超前滞后控制器从control_conf_里的lat_controller_conf里的enable_reverse_leadlag_compensation参数读取//默认是开启横向控制中的超前滞后控制器的，提升或者降低闭环反馈系统的响应速度enable_leadlag_ = control_conf_->lat_controller_conf().enable_reverse_leadlag_compensation();if (enable_leadlag_) {//若打开，用控制配置里的lat_controller_conf里reverse_leadlag_conf里的参数以及控制周期ts_去设置超前滞后控制器leadlag_controller_.Init(lat_controller_conf.reverse_leadlag_conf(), ts_);}//是否使能mrac模型参考自适应控制enable_mrac_从control_conf里的lat_controller_conf里的enable_steer_mrac_control读取//默认关闭，这一段直接跳过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_//这个使能开关从控制配置control_conf_里的lat_controller_conf里的enable_look_ahead_back_control参数读取//默认打开//lookahead前进预瞄，lookback倒车预瞄enable_look_ahead_back_control_ =control_conf_->lat_controller_conf().enable_look_ahead_back_control();//返回状态码okreturn Status::OK();}//Line281-285//这个函数关闭横向日志文件，本身FLAGS_enable_csv_debug用csv记录debug信息这个开关默认关闭的，直接跳过void LatController::CloseLogFile() {if (FLAGS_enable_csv_debug && steer_log_file_.is_open()) {steer_log_file_.close();}}//Line287-309//该函数加载增益调度表，从横向控制配置lat_controller_conf中读取增益调度表//参数：横向控制配置lat_controller_conf//本质上control_conf_就是从控制配置文件control_conf.pb.txt中读取到的//lat_controller_conf也就是control_conf_下的元素//横向控制配置中有两张增益调度表lat_err_gain_scheduler，heading_err_gain_scheduler//分别为横向误差的增益调度表和航向误差的增益调度表//最后都加载到了LatController类数据成员lat_err_interpolation_，heading_err_interpolation_里void LatController::LoadLatGainScheduler(const LatControllerConf &lat_controller_conf) {//从横向控制配置lat_controller_conf中读取横向误差的增益调度表放到临时变量lat_err_gain_schedulerconst auto &lat_err_gain_scheduler =lat_controller_conf.lat_err_gain_scheduler();//从横向控制配置lat_controller_conf中读取航向误差的增益调度表放到临时变量heading_err_gain_schedulerconst auto &heading_err_gain_scheduler =lat_controller_conf.heading_err_gain_scheduler();//屏幕上的打印信息，调度表加载成功AINFO << "Lateral control gain scheduler loaded";//定义了两张插值表xy1,xy2//Interpolation1D是Apollo自己定义的一维线性插值表类型，将已知离散点存入插值表，给出x就可以去表里找所在区间并插值计算y值Interpolation1D::DataType xy1, xy2;//遍历lat_controller_conf下面lat_err_gain_scheduler里的每一组调度表scheduler//每一组scheduler都包含speed,ratio两个值//将每一组scheduler的speed,ratio结对make_pair后存入插值表xy1for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}//遍历lat_controller_conf下面heading_err_gain_scheduler里的每一组调度表scheduler//每一组scheduler都包含speed,ratio两个值//将每一组scheduler的speed,ratio结对make_pair后存入插值表xy2for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}//首先将LatController类数据成员lat_err_interpolation_复位然后用xy1去初始化lat_err_interpolation_//heading_err_interpolation_同理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";}//Line311//该函数调用关闭横向日志文件的作用void LatController::Stop() { CloseLogFile(); }//Line313//该函数返回横向控制器的名称字符串std::string LatController::Name() const { return name_; }//Line315-645//该函数是横向控制中最核心的函数之一，计算控制质量放入*cmd中//参数：localization定位信息指针，数据结构见相应的类型定义//参数：chassis底盘反馈车辆状态信息指针，数据结构见相应的类型定义//参数：planning_published_trajectory规划发布的轨迹信息指针，数据结构见相应的类型定义//参数：cmd控制指令信息指针，该函数根据前3个参数计算到的结果存到cmd中，这个参数是用来存放计算结果的//cmd数据结构见相应的类型定义Status LatController::ComputeControlCommand(const localization::LocalizationEstimate *localization,const canbus::Chassis *chassis,const planning::ADCTrajectory *planning_published_trajectory,ControlCommand *cmd) {//通过LatController类数据成员injector_获取车辆当前状态信息放到临时变量vehicle_state里//vehicle_state的类型用auto根据等式右边自动指定auto vehicle_state = injector_->vehicle_state();//通过输入参数planning_published_trajectory获取车辆的期望轨迹存放到临时变量target_tracking_trajectoryauto target_tracking_trajectory = *planning_published_trajectory;//FLAGS_use_navigation_mode去modules/common/configs/config_gflags.cc取出//取出use_navigation_mode的值，是否打开导航模式，默认是false关闭的这一段直接跳过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();  //默认是false关闭的这一段直接跳过auto curr_vehicle_y = localization->pose().position().y();double curr_vehicle_heading = 0.0;const auto &orientation = localization->pose().orientation();//默认是false关闭的这一段直接跳过if (localization->pose().has_heading()) {curr_vehicle_heading = localization->pose().heading();} else {curr_vehicle_heading =common::math::QuaternionToHeading(orientation.qw(), orientation.qx(),//默认是false关闭的这一段直接跳过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; //默认是false关闭的这一段直接跳过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);//默认是false关闭的这一段直接跳过});}}//target_tracking_trajectory是从输入参数传进来的规划轨迹信息//将target_tracking_trajectory对象内容move移动到LatController类数据成员trajectory_analyzer_里trajectory_analyzer_ =std::move(TrajectoryAnalyzer(&target_tracking_trajectory));//将规划轨迹从后轴中心变换到质心，如果条件满足的话//FLAGS_trajectory_transform_to_com_reverse也是从modules/control/common/control_gflags.cc取出//取出trajectory_transform_to_com_reverse值，倒档是否需要转换到质心坐标的开关,默认false//这个if不满足，可以略过if (((FLAGS_trajectory_transform_to_com_reverse &&vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) ||//FLAGS_trajectory_transform_to_com_drive也是从modules/control/common/control_gflags.cc取出//取出trajectory_transform_to_com_drive值，前进档是否需要转换到质心坐标的开关,默认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) {/*A matrix (Gear Reverse) 倒档时的系数矩阵A[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;]*///这里可以理解，因为R档和前面加载LoadControlConf()函数里加载的符号不同需要更新//下面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 {/*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;]*///下面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) = 1.0;matrix_a_coeff_(0, 2) = 0.0;}//感觉有点多余，前面不是赋值了？//因为万一D档和R档切换，A矩阵的值是在变化的话，所以D档和R档时要按照各自的方式计算一下，因为cf_,cr_也更新了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_;//感觉有点多余，前面不是赋值了？//因为万一D档和R档切换，B矩阵的值是在变化的话，所以D档和R档时要按照各自的方式计算一下，因为cf_,cr_也更新了/*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_;//调用更新驾驶航向函数，也是要满足FLAGS_reverse_heading_control默认关闭，实际这个函数没啥用，略过UpdateDrivingOrientation();//定义临时变量SimpleLateralDebug类对象debug//SimpleLateralDebug类由modules/control/proto/control_cmd.proto文件生成，参见谷歌Protobuf使用//从名字就可以看出SimpleLateralDebug,"简单横向调试"，这个类就是用来调试横向控制的，里面用来储存一些横向控制过程量// 这个临时变量首先取出cmd里的debug?然后又Clear清空？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为0//关闭的情况下这行没什么用UpdateMatrixCompound();// Adjust matrix_q_updated when in reverse gear//当在R档时调整矩阵Q也就是LatControllr类成员matrix_q_//将控制配置里的reverse_matrix_q写入LatControllr类成员matrix_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) {for (int i = 0; i < reverse_q_param_size; ++i) {//R档加载控制配置里的reverse_matrix_qmatrix_q_(i, i) =control_conf_->lat_controller_conf().reverse_matrix_q(i);}} else {for (int i = 0; i < q_param_size; ++i) {//非R档加载控制配置里的matrix_qmatrix_q_(i, i) = control_conf_->lat_controller_conf().matrix_q(i);}}//对于更高速度的转向增加增益调度表//FLAGS_enable_gain_scheduler去modules/control/common/control_gflags.cc取出//取出control_gflags.cc中enable_gain_scheduler的值，默认是false，但是实际上很有用的//这里介绍一下if (FLAGS_enable_gain_scheduler) {  //如果打开增益调度表matrix_q_updated_(0, 0) =  //Q矩阵的(1,1)也就是横向误差平方和的权重系数, 第1行第1列//Q(1,1)=Q(1,1)*(用之前加载的横向误差调度增益表根据当前车速插值得到的ratio)matrix_q_(0, 0) * lat_err_interpolation_->Interpolate(std::fabs(vehicle_state->linear_velocity()));matrix_q_updated_(2, 2) =  //Q矩阵的(3,3)也就是航向误差平方和的权重系数, 第3行第3列//Q(3,3)=Q(3,3)*(用之前加载的航向误差调度增益表根据当前车速插值得到的ratio)matrix_q_(2, 2) * heading_err_interpolation_->Interpolate(std::fabs(vehicle_state->linear_velocity()));//求解LQR问题，求解到的最优状态反馈矩阵K放入matrix_k_中，最后一个引用变量作参数，明摆着就是要用对形参的修改改变实参，//常用来存放函数计算结果//参数：矩阵Ad,Bd,Q,R,lqr_eps_是LQR求解精度，lqr_max_iteration_是求解的最大迭代次数//参数：matrix_k_用来求解LQR控制方法计算出的最优状态反馈矩阵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// then to 100% ratio//状态反馈控制量 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计算前馈控制量//参数:debug中的道路曲率作为参数，前馈量的计算公式参见原理介绍const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());//初始化定义最终的方向盘控制转角为0.0double steer_angle = 0.0;//定义临时变量方向盘反馈增强初始化为0.0double steer_angle_feedback_augment = 0.0;//在期望的速度域增强横向误差的反馈控制if (enable_leadlag_) {//enable_leadlag_=control_conf->lat_controller_conf()->enable_reverse_leadlag_compensation()//见控制配置文件control_conf.pb.txt里enable_reverse_leadlag_compensation默认设置true//如果打开leadlag超前滞后控制器if (FLAGS_enable_feedback_augment_on_high_speed ||std::fabs(vehicle_state->linear_velocity()) < low_speed_bound_) {//如果车辆打开高速的反馈增强控制 或 车速小于低高速边界速度//low_speed_bound_是控制配置文件里的switch_speed()默认设置为3.0 m/s//enable_feedback_augment_on_high_speed的值见control_gflags.cc里，默认设置为false，高速不打开//满足上述条件,实际上就是低速时打开超前滞后控制器，然后这个超前滞后控制器只对横向误差进行增强控制//这一段不详解，计算出反馈增强控制方向盘转角百分数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, linerly interplolate the// 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;//这一部分计算方向盘转向的限制，根据最大的横向加速度限制计算,这一块主要是限制横向加速度//又涉及FLAGS_set_steer_limit 见control_gflags.cc里，FLAGS_set_steer_limit取出文件里定义的set_steer_limit的值，默认false//若限制横向加速度 最大方向盘转角百分数 = 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;//这一部分主要是对方向盘转动速率进行限制，如果FLAGS_enable_maximum_steer_rate_limit打开，默认不打开//如果打开取出车辆参数配置文件中的max_steer_angle_rate转化成百分数 /apollo/modules/common/data/vehicle_param.pb.txt //如果没打开，最大就限制为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_，重新计算方向盘转角控制量，并用方向盘转角和转速限制对转角控制量进行限幅//enable_mrac_ =control_conf_->lat_controller_conf().enable_steer_mrac_control()//也是看控制配置文件里的参数默认是false，略过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);// 对方向盘转角数字滤波，然后控制百分数又限制在正负100，百分数自然最大就是100steer_angle = digital_filter_.Filter(steer_angle);steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);//当处于D档或倒档 且 车速小于某一速度 且处于自驾模式时锁定方向盘，方向盘控制转角就保持上一次的命令值//FLAGS_lock_steer_speed 意思就是FLAGS_去XXXgflags.cc下取出lock_steer_speed的值//lock_steer_speed在modules/control/common/control_gflags.cc里定义为0.081m/sif (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_;}// 设定转角指令，再通过最大转角速率再次进行限制幅度，最多只能=上次的转角指令+/-最大转角速率 * Ts// 目前的代码是处在ComputeCommand函数,控制指令计算出来就放在cmd指针里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();// 为 logging and debugging计算一些额外信息// -k1*x1 x1为横向误差 这一项就是横向误差贡献的控制量百分数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;// -k2*x2 x2为横向误差率 这一项就是横向误差率贡献的控制量百分数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;// -k3*x3 x3为航向误差 这一项就是航向误差贡献的控制量百分数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;// -k4*x4 x4为航向误差率 这一项就是航向误差率贡献的控制量百分数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();}//Line647-653 mrac模型参考自适应控制的复位函数  //在控制配置文件control_conf.pb.txt里lat_controller_conf下的//enable_steer_mrac_control，其默认关闭，是另外一种横向控制器Status LatController::Reset() {matrix_state_.setZero();if (enable_mrac_) {mrac_controller_.Reset();}return Status::OK();}//Line655-707 横向控制器的车辆状态矩阵函数 主要就是更新状态方程中的 X = [e1 e1' e2 e2'] e1,e2分别为横向误差和航向误差void LatController::UpdateState(SimpleLateralDebug *debug) {//该函数的输入SimpleLateralDebug类型指针*debug，其实是要填充这个指针传出来debug信息//该函数在lat_controller.cc上面的ComputeControlCommand()函数里被调用//SimpleLateralDebug是control_cmd.proto下的一个message类型，包含各种调试信息//injector_是lat_controller类的数据成员DependencyInjector类对象，vehicle_state()是DependencyInjector成员函数//用于返回车辆当前状态，injector_在control_component.cc里的Status ControlComponent::CheckInput()函数里被更新auto vehicle_state = injector_->vehicle_state();//是否使用navigation_mode,默认不使用，在config_gflags.cc里设置DEFINE_bool(use_navigation_mode, false,//默认不使用导航模式直接跳过此段代码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 { //不使用导航模式则执行这部分//将车辆状态的坐标系从后轴中心转换到质心如果满足条件，默认倒档不需转换，D档要转换//调用了vehicle_state的成员函数ComputeCOMPosition()，lr_是后轴中心到质心的距离，然后被com引用const auto &com = vehicle_state->ComputeCOMPosition(lr_);//将x,y,heading航向,v纵向速度，angular_vel航向变化率，纵向加速度，debug是调试信息的待填充指针，就是目前所在函数的参数//trajectory_analyzer_是TrajectoryAnalyzer类对象只要把轨迹点传进去，就可以实现轨迹分析的相关功能见trajectory_analyzer.h如//获取轨迹段序号，查找距离当前位置的时间最近点ref和距离最近点match,车辆后轴中心坐标转换到质心等功能//trajectory_analyzer_在Status LatController::ComputeControlCommand()函数中被赋值//ComputeLateralErrors()函数计算得到的error都放入message debug里，稍后会用debug来更新车辆状态矩阵xComputeLateralErrors(com.x(), com.y(), driving_orientation_,vehicle_state->linear_velocity(), vehicle_state->angular_velocity(),vehicle_state->linear_acceleration(), trajectory_analyzer_, debug);}// 状态矩阵更新;//enable_look_ahead_back_control_=control_conf_->lat_controller_conf().enable_look_ahead_back_control();是在配置文件里设定//control_conf.pb.txt里lat_controller_conf里的enable_look_ahead_back_control默认打开if (enable_look_ahead_back_control_) {  //当打开lookaheadback control//当打开这个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_headingmatrix_state_(0, 0) = debug->lateral_error_feedback();matrix_state_(2, 0) = debug->heading_error_feedback();} else {matrix_state_(0, 0) = debug->lateral_error();   //所以这个if else就是是否打开预瞄，若打开状态X里的误差项要叠加上到lookahead点误差matrix_state_(2, 0) = debug->heading_error();}matrix_state_(1, 0) = debug->lateral_error_rate();  //把debug中的横向误差率和航向误差率更新进状态矩阵中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;}}//Line709-729 更新矩阵函数   这里主要是更新系数矩阵A,Advoid LatController::UpdateMatrix() {double v;//倒档，代替横向平动动力学用对应的运动学模型 R档的暂且不管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;  //R档时A第一行第3列可不为0} else {//v为车辆纵向速度和最小速度保护里的最大值。v = std::max(injector_->vehicle_state()->linear_velocity(),minimum_speed_protection_);matrix_a_(0, 2) = 0.0; //非R档A矩阵的1行3列为0}matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;  //更新A矩阵中与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()); //定义了一个单位阵，A阵列数*A阵列数matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *   //计算A矩阵的离散化形式Ad，用双线性变换法(matrix_i + ts_ * 0.5 * matrix_a_);}//Line731-742 更新组装矩阵Adc 其实就是Ad考虑preview后扩展的结果//因为横向preview_window=0 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;}}}//Line744-766 计算横向控制的前馈量steer_angle_feedforwardterm(D档和R档计算方式不一样)//kv=lr*m/2/Cf/L - lf*m/2/Cr/L   //k2就是LQR中Q矩阵中的航向误差权重系数//delta_ff=L*kappa_ref+kv*v^2*kappa_ref-k2(lr*kappa_ref-lf*m*v^2*kappa_ref/(2CrL))double LatController::ComputeFeedForward(double ref_curvature) const {const 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;}//Line768-906 计算横向误差，放入debug指针中，供其他函数如计算控制命令定义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) {//参数详解 输入xy坐标，车辆当前heading,纵向速度v,heading变化率，纵向加速度，//轨迹相关信息trajectory_analyzer对象用于提供轨迹的参考点，匹配点，lookahead点等信息//debug指针用于存放这里计算到的误差供其他函数调用，//SimpleLateralDebug是在control/proto/control_cmd.proto下定义的messgae类TrajectoryPoint target_point; //TrajectoryPoint是apollo/modules/common/proto/pnc_point.proto里定义的一个message类//TrajectoryPoint类包含轨迹点的v,acc,jerk,相对时间，前轮转向角等信息 //这里定义了一个TrajectoryPoint类对象if (FLAGS_query_time_nearest_point_only) {   //如果只将车辆当前的时间向前加固定时间长度后在轨迹上对应点作为目标点，gflags老套路去//modules/control/common/control_gflags.cc路径下取出query_time_nearest_point_only值//默认是false，target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(  //如果是Clock::NowInSeconds() + query_relative_time_);//query_relative_time_ = control_conf->query_relative_time();这个值是从控制配置文件中读取，默认0.8秒//query_time_nearest_point_only的意识就是始终将车辆当前时间向前加0.8秒在轨迹上对应的点作为目标点//在时域上向前看0.8秒作为目标点} else {  //如果不采用这种向前看0.8秒作为目标点的方法，默认不采用的if (FLAGS_use_navigation_mode && //去modules/common/configs/config_gflags.cc里看use_navigation_mode是默认false//默认不采用导航模式，直接看else!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);}}const double dx = x - target_point.path_point().x(); //dx就是当前车辆和目标点的x坐标之差const double dy = y - target_point.path_point().y(); //dy就是当前车辆和目标点的y坐标之差debug->mutable_current_target_point()->mutable_path_point()->set_x(target_point.path_point().x());   //将目标点的x坐标存入debug指针，debug.current_target_point.path_point.x,指针要用->来赋值成员debug->mutable_current_target_point()->mutable_path_point()->set_y(target_point.path_point().y());   //将目标点的y坐标存入debug指针，debug.current_target_point.path_point.y，指针要用->来赋值成员//mutable_current_target_point()是protobuf生成的message类的成员函数，//可以对message的数据成员进行赋值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()); //计算目标点处的heading角的正余弦值const double sin_target_heading = std::sin(target_point.path_point().theta());double lateral_error = cos_target_heading * dy - sin_target_heading * dx; //根据目标点处的heading角正余弦值计算横向误差if (FLAGS_enable_navigation_mode_error_filter) {  //如果打开导航模式误差滤波器，modules/control/common/control_gflags.cc里//默认时关闭的，导航模式及滤波器都关闭，直接略过lateral_error = lateral_error_filter_.Update(lateral_error);}debug->set_lateral_error(lateral_error); //将横向误差填入debug指针中的debug.lateral_errordebug->set_ref_heading(target_point.path_point().theta());  //将目标点的heading角填入debug指针的debug.ref_headingdouble heading_error =       //计算航向误差，车辆当前航向角theta-ref_heading角，然后再调用NormalizeAngle()函数将//角度标准化，对于车辆航向角，通常在-pi-pi之间，所以将航向角误差标准化一下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);    //将航向误差存放进debug指针 debug.heading_error//在低-高速切换窗口，现行插值预瞄距离为了更柔和的预测切换？double lookahead_station = 0.0;   //ahead是针对非倒档double lookback_station = 0.0;    //back是针对倒档if (std::fabs(linear_v) >= low_speed_bound_) { //如果速度的绝对值>=low_speed_bound_(低速边界) //这个也是在control_conf配置文件中定义的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));}//lookahead(前进预瞄)，lookback(倒车预瞄)//估计考虑预瞄距离的航向误差heading_error_feedbackdouble heading_error_feedback;if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {heading_error_feedback = heading_error; //倒档的话，heading_error_feedback就是heading_error} else {  //非倒档就是前进档//目标点的相对时间再加上预瞄时间(预瞄距离/车辆纵向速度)作为总相对时间//然后去trajectory_analyzer轨迹信息上根据总相对时间找出预瞄点auto lookahead_point = trajectory_analyzer.QueryNearestPointByRelativeTime(target_point.relative_time() +lookahead_station ///在估计预瞄时间时纵向速度若小于0.1就按0.1(std::max(std::fabs(linear_v), 0.1) * std::cos(heading_error)));heading_error_feedback = common::math::NormalizeAngle(  //heading_error=车辆当前heading-参考点heading//heading_error_feedback=heading_error+参考点heading-预瞄点heading=车辆当前heading-预瞄点headingheading_error + target_point.path_point().theta() -lookahead_point.path_point().theta());}debug->set_heading_error_feedback(heading_error_feedback); //将heading_error_feedback存入指针debug.heading_error_feedback//估计考虑预瞄距离的横向误差lateral_error_feedbackdouble lateral_error_feedback;if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {lateral_error_feedback = //倒档的lateral_error_feedback=lateral_error-倒车预瞄距离*sin(heading_error)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);}debug->set_lateral_error_feedback(lateral_error_feedback); //将lateral_error_feedback存入指针debug.lateral_error_feedbackauto 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) {   //测试倒车航向控制，modules/control/common/control_gflags.cc里//reverse_heading_control默认是false直接略过if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {lateral_error_dot = -lateral_error_dot;lateral_error_dot_dot = -lateral_error_dot_dot;}}debug->set_lateral_error_rate(lateral_error_dot);   //将计算得到的横向误差率填入debug指针debug.lateral_error_ratedebug->set_lateral_acceleration(lateral_error_dot_dot); //将计算得到的横向误差加速度填入debug指针debug.lateral_accelerationdebug->set_lateral_jerk( //利用横向加速度差分得到横向加加速度jerk放入debug指针debug.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); //angular_v是ComputeLateralError函数传进来的参数heading的变化率，若倒车debug.heading_rate为//负的angular_v} else {debug->set_heading_rate(angular_v); //若不倒车debug.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指针中debug->set_heading_jerk((debug->heading_acceleration() - previous_heading_acceleration_) / ts_);//目标点航向角的加加速度，同样差分法，结果放入debug指针中debug->set_ref_heading_jerk((debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /ts_);//heading角误差变化率的加加速度，同样差分法，结果放入debug指针中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();//设置目标点的曲率kappa从target_point获得，target_point是由trajectory_analyzer成员函数找到的目标点对象//结果放入debug指针中debug->set_curvature(target_point.path_point().kappa());}//Line908-end 更新驾驶航向？void LatController::UpdateDrivingOrientation() {auto vehicle_state = injector_->vehicle_state();//更新车辆状态driving_orientation_ = vehicle_state->heading();//driving_orientation_是lat_controller类数据成员matrix_bd_ = matrix_b_ * ts_; //横向控制状态方程B离散化为Bd//reverse_heading_control默认是false直接略过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

Apollo control模块横向控制原理及核心代码逐行解析相关推荐

Apollo control模块纵向控制原理及核心代码逐行解析
前言 2021/12/30 前段时间一直在看Apollo的控制代码,因为工作较忙,只能抽时间整理下代码笔记,可能稍显粗糙,部分图片手绘,作为日后调试之参照.以后有时间再优化排版,再把涉及到的其他概念补 ...
DophinScheduler ui部分核心代码详细解析——重中之重的src文件夹里究竟有何种玄机
2021SC@SDUSC 文章目录一.整体结构二.具体细节 1.components 2.images 3.js 1.dag-canvas 2.contextMenu 3.nodeStatus.j ...
DophinScheduler server部分核心代码详细解析——掌控任务和进程的呼吸与脉搏：log、monitor与registry
2021SC@SDUSC 文章目录一.整体结构二.具体分析 1.log 1.LoggerRequestProcessor 2.LoggerServer 3.MasterLogFilter 2.mo ...
DophinScheduler ui部分核心代码详细解析续集——前端组件的代码综合分析
2021SC@SDUSC 文章目录一.总体思路二.js代码分析 1.pages 2.router 3.store 4.login 三.module 四.view 上文链接: DophinSched ...
DophinScheduler server部分核心代码详细解析——统领全局调度全场的服务器server部分究竟干了什么？
2021SC@SDUSC 文章目录一.整体结构二.细节分析 1.builder 2.entity 1.DataxTaskExecutionContext 2.DependenceTaskExecu ...
DophinScheduler ui部分核心代码详细解析——底层逻辑与具体实现
2021SC@SDUSC 文章目录一.前端 1.\_test\_ 1.Counter.js 2.test.spec.js 2.build 3.node_modules 4.src 一.前端 1._t ...
Hugging Face实战（NLP实战/Transformer实战/预训练模型/分词器/模型微调/模型自动选择/PyTorch版本/代码逐行解析）下篇之模型训练
模型训练的流程代码是不是特别特别多啊?有的童鞋看过Bert那个源码写的特别特别详细,参数贼多,运行一个模型百八十个参数的. Transformer对NLP的理解是一个大道至简的感觉,Hugging F ...
pointnet++代码逐行解析（一）——— train_classification
继续巩固PointNet++代码的实现这篇博客,把代码逐行注释一遍! pointnet++的所有代码和数据集都在github上,Pytorch代码:https://github.com/yanx27/ ...
apollo control模块中车身动力学模型推导
声明:本文主要参考了follow轻尘的https://blog.csdn.net/u013914471/article/details/82968608,在此多谢该博主的文章的指引,本文主要是自己学习 ...

Apollo control模块横向控制原理及核心代码逐行解析

前言