Apollo控制代码学习（一）_apollo代码

前言

2023/6/19
学习记录，也希望读者能多批评指正，共同学习进步
Apollo默认纵向控制器是PID，横向控制器默认是LQR：

1.controller.h

该文件定义其他控制器的基类Controller，学习记录见代码：

//定义父类Controller
#pragma once 
//预处理指令，确保头文件只被编译一次，可在任何编译器中使用。当编译器遇到#pragma once
//会检查当前文件是否被编译过，是则跳过编译；#pragma once比#ifndef更快，因为只检查一次
#include <memory>//使用智能指针
#include <string>

#include "modules/common/status/status.h"
#include "modules/control/common/dependency_injector.h"
#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
#include "modules/control/proto/control_conf.pb.h" //加载控制配置的文件
#include "modules/common_msgs/localization_msgs/localization.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"

//命名空间 apollo::control
namespace apollo {
namespace control {

//Controller所有控制器的基类
class Controller {
 public:
  Controller() = default; //构造器
  virtual ~Controller() = default; //析构器 trivial-type 机械地清理内存
  
  //初始化控制器 参数：控制配置参数 返回状态码
  /*=0表示这个成员函数是纯虚函数，也就是没有定义，只有接口，由继承类来具体定义它的行为*/
  virtual common::Status Init(std::shared_ptr<DependencyInjector> injector,
                              const ControlConf *control_conf) = 0;

  //根据当前车辆状态和目标轨迹经计算目标方向盘转角
  //参数：车辆当前位置location；chassis车辆速度、加速度；规划轨迹trajectory;前三个参数计算的控制指令放在cmd中
  virtual common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      control::ControlCommand *cmd) = 0;

  //重置控制器 返回重置的状态码
  virtual common::Status Reset() = 0;

  //返回控制器的名称
  //成员函数后面加const，表示this是一个指向常量的指针
  virtual std::string Name() const = 0; 
  //停止控制器
  virtual void Stop() = 0;
};

}  // namespace control
}  // namespace apollo
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

2. lat_controller.h

定义横向控制器类，学习记录如下

#pragma once

#include <fstream> //创建文件流
#include <memory> //智能指针
#include <string>

#include "Eigen/Core" //包含Matrix和Array类，基础的线性代数运算和数组操作
#include "modules/common_msgs/config_msgs/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"//继承的控制父类

namespace apollo {
namespace control {
//定义了横向控制器，继承Controller类
//基于LQR的横向控制器计算转向角
class LatController : public Controller {
 public:
  LatController();//构造函数
  
  virtual ~LatController();//析构函数加上virtual是为了防止内存泄漏

  //初始化横向控制器 参数：控制参数 返回初始状态码 override重写同名函数
  //override表明该函数接口是在基类中被声明为虚函数，在该类中实现
  common::Status Init(std::shared_ptr<DependencyInjector> injector,
                      const ControlConf *control_conf) override;
  //基于当前车辆状态和目标轨迹计算控制指令
  common::Status ComputeControlCommand(
      const localization::LocalizationEstimate *localization,
      const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
      ControlCommand *cmd) override;
  //重置横向控制器 返回重置的状态码
  common::Status Reset() override;
  //停止横向控制器
  void Stop() override;
  //横向控制器名称
  std::string Name() const override;
 protected:
  //更新车辆状态方程中的车辆状态矩阵X=[e1 e1' e2 e2']
  void UpdateState(SimpleLateralDebug *debug);
  //倒车模式，默认关闭
  void UpdateDrivingOrientation();
  //更新车辆状态方程系数矩阵A以及离散形式A1
  void UpdateMatrix();
  //更新预览窗口的系数矩阵A，预览窗口默认关闭的
  void UpdateMatrixCompound();
  //根据道路曲率计算前馈控制量
  double ComputeFeedForward(double ref_curvature) const;
  //计算横向误差函数
  //参数：x,y,theta,v,angular_v,a,轨迹信息
  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();
  // vehicle
  //车辆控制配置
  const ControlConf *control_conf_ = nullptr;
  //车辆参数
  common::VehicleParam vehicle_param_;
  // a proxy to analyze the planning trajectory
  //分析规划轨迹的代理，提取轨迹信息
  TrajectoryAnalyzer trajectory_analyzer_;
  //下面是车辆的物理参数
  //控制周期
  double ts_ = 0.0;
  //前轮侧偏刚度 左右轮之和
  double cf_ = 0.0;
  //后轮侧偏刚度 左右轮之和
  double cr_ = 0.0;
  //轴距 L = Lf+Lr
  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;
  //向前预览的控制周期的数量
  int preview_window_ = 0;

  //预瞄控制相关参数
  //低速前进预瞄距离
  double lookahead_station_low_speed_ = 0.0;
  //低速倒车预瞄距离
  double lookback_station_low_speed_ = 0.0;
  //高速前进预瞄距离
  double lookahead_station_high_speed_ = 0.0;
  //高速倒车预瞄距离
  double lookback_station_high_speed_ = 0.0;
  //不考虑预览窗口的车辆状态矩阵X的维度4
  //[e1 e1' e2 e2']
  const int basic_state_size_ = 4;
  //车辆状态矩阵A x'=Ax+Bu+B1*Psi_des'  Psi_des‘期望的heading角变化率
  Eigen::MatrixXd matrix_a_;
  //车辆状态方程系数矩阵A的离散形式A'
  Eigen::MatrixXd matrix_ad_;
  //车辆状态矩阵考虑preview预览之后的扩展阵
  Eigen::MatrixXd matrix_adc_;
  //控制量的系数矩阵B
  Eigen::MatrixXd matrix_b_;
  //控制量的系数矩阵B(离散)
  Eigen::MatrixXd matrix_bd_;
  //考虑preview预览之后的扩展阵的控制系数矩阵
  Eigen::MatrixXd matrix_bdc_;
  //状态反馈矩阵 u = -kx
  //LQR求解出最优的K  K=[k0 k1 k2 k3] 1x4
  Eigen::MatrixXd matrix_k_;
  //控制量前轮转角的权重系数矩阵R 1x1 
  Eigen::MatrixXd matrix_r_;
  //状态反馈量的权重系数矩阵Q [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']
  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;
  //最小速度保护
  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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216

参考文章：
智能指针
Eigen