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

前言

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及其离散形式Ad
  void 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  4x4
  Eigen::MatrixXd matrix_adc_;
  // control matrix
  //车辆状态方程系数矩阵B  控制量的系数矩阵
  Eigen::MatrixXd matrix_b_;  4x1
  // 系数矩阵B的离散形式 Bd
  Eigen::MatrixXd matrix_bd_;  4x1
  // 系数矩阵考虑preview之后的扩展阵
  Eigen::MatrixXd matrix_bdc_;
  // 状态反馈矩阵K   u=-kx  LQR求解出最优的K  K=[k0 k1 k2 k3] 1x4
  Eigen::MatrixXd matrix_k_; 
  // control authority weighting matrix
  //LQR算法中控制量的权重系数矩阵R  这里只有一个控制量就是前轮转角，所以R 1x1
  Eigen::MatrixXd matrix_r_;
  //LQR算法中状态反馈量的权重系数矩阵Q  这里有4个状态反馈量[e1 e1' e2 e2']^T，所以Q 4x4对角阵，主要就是对角线上是权重系数
  Eigen::MatrixXd matrix_q_;
  //更新后的Q矩阵 如果打开增益调度表 那么要不同车速下可以配置不同的Q矩阵，所以要根据车速更新Q
  Eigen::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/s
  double 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--ts
	  ts_ = 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