前言
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
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
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
