前面文章：
Apollo9.0 PNC源码学习之Control模块（一）
Apollo9.0 PNC源码学习之Control模块（二）
Apollo9.0 PNC源码学习之Control模块（三）
Apollo9.0 PNC源码学习之Control模块（四）
本文讲解百度Apollo基于LQR的横向控制器

1 LQR原理讲解

LQR：线性二次调节器，设计状态反馈控制器的方法
系统：$\dot{x}=Ax+Bu$
线性反馈控制器：$u=-Kx$
$$\begin{array}{l}\dot{x}=Ax-BKx=\underset{A_{cl}}{\underbrace{(A-BK)}}x\end{array}$$

让系统稳定的条件是矩阵$A_{cl}$的特征值实部均为负数。因此我们可以手动选择几个满足上述条件的特征值，然后反解出K，从而得到控制器。

代价函数J:
$$J={\int }_0^{\infty }{x}^{T}Qx+u^{T}Ru\mathrm{d}t$$

在系统稳定的前提下，通过设计合适的K，让代价函数J最小。
Q大：希望状态变量x更快收敛
R大：希望输入量u收敛更快，以更小的代价实现系统稳定

1.1 连续时间LQR推导

具体推导参见博客：线性二次型调节器(LQR)原理详解

求解连续时间LQR反馈控制器参数K的过程：
（1）设计参数矩阵Q、R
（2）求解Riccati方程$A^{T}P+PA-PB{R}^{-1}{B}^{T}P+Q=0$得到P
（3）计算$K=R^{-1}{B}^{T}P$得到反馈控制量$u=-kx$

1.2 离散时间LQR推导

离散情况下的LQR推导有最小二乘法和动态规划算法
详细推导见博客：连续时间LQR和离散时间LQR笔记
离散系统：
$x(k+1)=Ax(k)+Bu(k)$
代价函数：
设计步骤：
① 确定迭代范围N
② 设置迭代初始值$P_N=Q$
③ $t=N,...,1$，从后向前循环迭代求解离散时间的代数Riccati方程

$P_{t-1}=Q+A^T{P}_{t}A-A^T{P}_{t}B(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t}A$

④ $t=0,...,N$循环计算反馈系数$K_t=(R+B^T{P}_{t+1}B{)}^{-1}{B}^T{P}_{t+1}A$ 得到控制量$u_t=-K_t{x}_t$

2 百度Apollo基于LQR的横向控制

2.1 前言

control-controller-lat-based-lqr-controller 插件包是基于 LQR 控制算法进行车辆横向控制的控制器实现。
横向控制：沿着参考切线的法线方向，控制车辆的横向位置和车身朝向角按照规划轨迹线的方向行驶。经典的横向控制方法有：PID、LQR、MPC、Stanley、Purepursuit

横向控制算法
（1）PID：鲁棒性较差，对路径无要求，转弯不会内切，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径曲率较小及低速的跟踪场景

（2）Pure pursuit：鲁棒性较好，对路径无要求，转弯内切速度增加变得严重，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径连续或不连续或者低速的跟踪场景

（3）Stanley：鲁棒性好，对路径要求曲率连续，转弯不会内切，速度增加会有一定超调，速度增加稳态误差变大，适用场景：路径平滑的中低速跟踪场景

（4）LQR：鲁棒性较差，对路径要求曲率连续，不会转弯内切，曲率快速变化时超调严重，稳态误差小，除非速度特别大，适用场景：路径平滑的中高速城市驾驶跟踪场景

2.2 百度Apollo的LQR框架

ComputeControlCommand：
主要的计算逻辑在ComputeControlCommand方法，输入是规划轨迹线(规划)，车体位置信息(定位)，车体姿态信息，车体底盘信息，根据车辆转弯的动力学建模，建立车辆动力学的微分方程，设计[横向位置误差，横向位置误差变化率，航向角误差，航向角误差变化率]^T为控制系统的状态变量，可以通过观测值（计算误差）进行状态变量的获取，推到出系统状态转移矩阵A和控制矩阵B，将连续时间作用的系统状态方程离散化，设计 Q ，R 矩阵，进行LQR计算求解Riccati方程得到反馈矩阵K，根据反馈矩阵K计算出的控制量为 steer_angle_feedback，另一部分是车辆前馈 steer_angle_feedforward ，横向误差还设计了超前滞后调节器，对横向进行补偿，最终将三者相加，作为一次控制量输入到车辆，完成 1 次控制

ComputeLateralErrors() ：
核心思想是根据车辆当前位置通过最近距离计算方法找到当前车辆位置距离规划轨迹线上的最近点，然后将实际坐标投影到轨迹线上的 match_point，这里匹配点就是横向的参考轨迹点，则车辆的横向位置误差即为投影变化的L方向距离，横向距离有一个方向的假设，左转为正，横向误差正数表示当前车辆偏离参考轨迹点左侧，轨迹线上的曲率正数表示左转，转向角输出正数，控制车辆为左转车轮。航向角误差为车辆当前航向角-参考轨迹点航向角，横向位置误差变化率为车身姿态的运动速度在横向的投影分量，航向角变化率为车身姿态的横摆角速度

LQR控制器方法定义在modules/common/math/linear_quadratic_regulator.cc，静态方法有 SolveLQRProblem （Riccati方程矩阵差计算），通过设定系统迭代的容差值（eps）和最大迭代次数（max_iteration），改变精度和求解时间

control/controllers/lat_based_lqr_controller/
├── conf/                                                  // 控制器配置参数文件
├── docs/                                                  // 文档相关
├── lateral_controller_test/                               // 单元测试数据
├── proto
│   ├── BUILD
│   └── lat_based_lqr_controller_conf.proto                // 控制器配置参数定义
├── BUILD                                                  // 规则构建文件
├── cyberfile.xml                                          // 插件包管理配置文件
├── lat_controller.cc                                      // 控制器实现文件
├── lat_controller.h                                       // 控制器实现文件
├── lat_controller_test.cc                                 // 控制器单元测试文件
├── plugins.xml                                            // 插件配置文件
└── README_cn.md                                           // 说明文档

3 LQR源码解析

linear_quadratic_regulator.h

#pragma once#include "Eigen/Core"namespace apollo {
namespace common {
namespace math {/*** @brief 求解离散时间线性二次型问题的求解器.* @param A 系统动态矩阵* @param B 控制矩阵* @param Q 系统状态的成本矩阵* @param R 控制输出的成本矩阵* @param M 状态和控制之间的交叉项，即 x'Qx + u'Ru + 2x'Mu* @param tolerance 求解离散代数黎卡提方程的数值公差* @param max_num_iteration 求解黎卡提的最大迭代次数* @param ptr_K 反馈控制矩阵（指针）*/
void SolveLQRProblem(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,const Eigen::MatrixXd &M, const double tolerance,const uint max_num_iteration, Eigen::MatrixXd *ptr_K);/*** @brief 求解离散时间线性二次型问题的求解器.* @param A 系统动态矩阵* @param B 控制矩阵* @param Q 系统状态的成本矩阵* @param R 控制输出的成本矩阵* @param tolerance 求解离散代数黎卡提方程的数值公差* @param max_num_iteration 求解黎卡提的最大迭代次数* @param ptr_K 反馈控制矩阵（指针）*/
void SolveLQRProblem(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R,const double tolerance, const uint max_num_iteration,Eigen::MatrixXd *ptr_K);}  // namespace math
}  // namespace common
}  // namespace apollo

linear_quadratic_regulator.cc

#include <limits>#include "Eigen/Dense"#include "cyber/common/log.h"
#include "modules/common/math/linear_quadratic_regulator.h"namespace apollo {
namespace common {
namespace math {using Matrix = Eigen::MatrixXd;// solver with cross term
// 求解带有交叉项的求解器
void SolveLQRProblem(const Matrix &A, const Matrix &B, const Matrix &Q,const Matrix &R, const Matrix &M, const double tolerance,const uint max_num_iteration, Matrix *ptr_K) {// 检查ABQRif (A.rows() != A.cols() || B.rows() != A.rows() || Q.rows() != Q.cols() ||Q.rows() != A.rows() || R.rows() != R.cols() || R.rows() != B.cols() ||M.rows() != Q.rows() || M.cols() != R.cols()) {AERROR << "LQR solver: one or more matrices have incompatible dimensions.";return;}// 转置矩阵Matrix AT = A.transpose();Matrix BT = B.transpose();Matrix MT = M.transpose();// 求解离散时间代数黎卡提方程（DARE）// 计算矩阵差分黎卡提方程，初始化P和QMatrix P = Q;uint num_iteration = 0;double diff = std::numeric_limits<double>::max();while (num_iteration++ < max_num_iteration && diff > tolerance) {Matrix P_next =AT * P * A -(AT * P * B + M) * (R + BT * P * B).inverse() * (BT * P * A + MT) + Q;// check the difference between P and P_next// 检查P和P_next之间的差异diff = fabs((P_next - P).maxCoeff());P = P_next;}if (num_iteration >= max_num_iteration) {// LQR求解器无法收敛到一个解决方案，最后一次连续结果差异是:ADEBUG << "LQR solver cannot converge to a solution, ""last consecutive result diff is: "<< diff;} else { // LQR求解器在迭代次数：时收敛，最大连续结果差异是：ADEBUG << "LQR solver converged at iteration: " << num_iteration<< ", max consecutive result diff.: " << diff;}*ptr_K = (R + BT * P * B).inverse() * (BT * P * A + MT);
}void SolveLQRProblem(const Matrix &A, const Matrix &B, const Matrix &Q,const Matrix &R, const double tolerance,const uint max_num_iteration, Matrix *ptr_K) {// 创建大小合适的零矩阵M：// M.rows() == Q.rows() && M.cols() == R.cols()Matrix M = Matrix::Zero(Q.rows(), R.cols());SolveLQRProblem(A, B, Q, R, M, tolerance, max_num_iteration, ptr_K);
}}  // namespace math
}  // namespace common
}  // namespace apollo

4 横向控制LQR源码解析

lat_controller.h

#pragma once#include <fstream>
#include <memory>
#include <string>#include "Eigen/Core"#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/lat_based_lqr_controller/proto/lat_based_lqr_controller_conf.pb.h"#include "cyber/plugin_manager/plugin_manager.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/control_component/controller_task_base/common/interpolation_1d.h"
#include "modules/control/control_component/controller_task_base/common/leadlag_controller.h"
#include "modules/control/control_component/controller_task_base/common/mrac_controller.h"
#include "modules/control/control_component/controller_task_base/common/trajectory_analyzer.h"
#include "modules/control/control_component/controller_task_base/control_task.h"namespace apollo {
namespace control {class LatController : public ControlTask {public:LatController();virtual ~LatController();// 初始化横向控制器common::Status Init(std::shared_ptr<DependencyInjector> injector) 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'] e1,e2分别代表横向，航向误差void UpdateState(SimpleLateralDebug *debug, const canbus::Chassis *chassis);// logic for reverse driving mode// 更新车辆朝向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,const canbus::Chassis *chassis);// 加载控制配置bool LoadControlConf();// 初始化横向控制中的滤波器void InitializeFilters();// 加载横向的增益调度表void LoadLatGainScheduler();// 这个函数主要是在屏幕上打印一些车辆参数的信息void LogInitParameters();// 将debug和chassis信息放入记录日志里void ProcessLogs(const SimpleLateralDebug *debug,const canbus::Chassis *chassis);// 关闭横向日志文件void CloseLogFile();// vehicle 车辆控制配置LatBaseLqrControllerConf lat_based_lqr_controller_conf_;// 车辆本身参数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;// 向前预览的控制周期的数量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_;// 车辆状态方程系数矩阵B  控制量的系数矩阵 4 x 1Eigen::MatrixXd matrix_b_;// 系数矩阵B的离散形式 BdEigen::MatrixXd matrix_bd_;// 系数矩阵考虑preview之后的扩展阵Eigen::MatrixXd matrix_bdc_;// 状态反馈矩阵K   u=-kx  LQR求解出最优的K  K=[k0 k1 k2 k3] 1x4Eigen::MatrixXd matrix_k_;// 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_;// parameters for lqr solver; number of iterations// lqr最大迭代次数int lqr_max_iteration_ = 0;// parameters for lqr solver; threshold for computation// 求解精度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_;// MeanFilter heading_rate_filter_;common::MeanFilter lateral_error_filter_; //横向误差均值滤波器common::MeanFilter heading_error_filter_; //航向误差均值滤波器// Lead/Lag controllerbool enable_leadlag_ = false; //超前滞后控制器，在主回路上串联校正环节LeadlagController leadlag_controller_;// Mrac controllerbool enable_mrac_ = false;MracController mrac_controller_;// Look-ahead controllerbool enable_look_ahead_back_control_ = false; //预瞄控制器，这里开启了// for compute the differential valute to estimate acceleration/lon_jerk// 上一时刻的横向加速度，主要为了差分计算横向加加速度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;// 若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;// 定义低高速的切换临界点，低速的边界double low_speed_bound_ = 0.0;// 低速窗口double low_speed_window_ = 0.0;// 当前车辆的航向角double driving_orientation_ = 0.0;// 获取车辆状态信息的对象std::shared_ptr<DependencyInjector> injector_;
};// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::LatController,ControlTask)}  // namespace control
}  // namespace apollo

lat_controller.cc

#include "modules/control/controllers/lat_based_lqr_controller/lat_controller.h"#include <algorithm>
#include <iomanip>
#include <utility>
#include <vector>#include "Eigen/LU"
#include "absl/strings/str_cat.h"#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "modules/common/configs/config_gflags.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/linear_quadratic_regulator.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/control/control_component/common/control_gflags.h"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;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());// 设定写入数据的精度为6位小数steer_log_file_ << std::fixed;steer_log_file_ << std::setprecision(6);// 写入数据标题WriteHeaders(steer_log_file_);}AINFO << "Using " << name_;
}
// 关闭日志文件
LatController::~LatController() { CloseLogFile(); }
//加载车辆参数配置文件
bool LatController::LoadControlConf() {vehicle_param_ =common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();ts_ = lat_based_lqr_controller_conf_.ts();if (ts_ <= 0.0) {AERROR << "[LatController] Invalid control update interval.";return false;}cf_ = lat_based_lqr_controller_conf_.cf();cr_ = lat_based_lqr_controller_conf_.cr();preview_window_ = lat_based_lqr_controller_conf_.preview_window();lookahead_station_low_speed_ =lat_based_lqr_controller_conf_.lookahead_station();lookback_station_low_speed_ =lat_based_lqr_controller_conf_.lookback_station();lookahead_station_high_speed_ =lat_based_lqr_controller_conf_.lookahead_station_high_speed();lookback_station_high_speed_ =lat_based_lqr_controller_conf_.lookback_station_high_speed();wheelbase_ = vehicle_param_.wheel_base();steer_ratio_ = vehicle_param_.steer_ratio();steer_single_direction_max_degree_ =vehicle_param_.max_steer_angle() / M_PI * 180;max_lat_acc_ = lat_based_lqr_controller_conf_.max_lateral_acceleration();low_speed_bound_ = lat_based_lqr_controller_conf_.switch_speed();low_speed_window_ = lat_based_lqr_controller_conf_.switch_speed_window();const double mass_fl = lat_based_lqr_controller_conf_.mass_fl();const double mass_fr = lat_based_lqr_controller_conf_.mass_fr();const double mass_rl = lat_based_lqr_controller_conf_.mass_rl();const double mass_rr = lat_based_lqr_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_);// moment of inertia// 车辆绕z轴的转动惯量iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;lqr_eps_ = lat_based_lqr_controller_conf_.eps();lqr_max_iteration_ = lat_based_lqr_controller_conf_.max_iteration();query_relative_time_ = lat_based_lqr_controller_conf_.query_relative_time();minimum_speed_protection_ = FLAGS_minimum_speed_protection;return true;
}
// 处理日志函数
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;
}
// 打印初始化参数显示控制器名字，车辆总质量，惯性矩等参数
void LatController::LogInitParameters() {AINFO << name_ << " begin.";AINFO << "[LatController parameters]"<< " mass_: " << mass_ << ","<< " iz_: " << iz_ << ","<< " lf_: " << lf_ << ","<< " lr_: " << lr_;
}
// 该函数初始化横向控制中的滤波器
void LatController::InitializeFilters() {// Low pass filterstd::vector<double> den(3, 0.0);std::vector<double> num(3, 0.0);common::LpfCoefficients(ts_, lat_based_lqr_controller_conf_.cutoff_freq(),&den, &num);digital_filter_.set_coefficients(den, num);lateral_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(lat_based_lqr_controller_conf_.mean_filter_window_size()));heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(lat_based_lqr_controller_conf_.mean_filter_window_size()));
}
// 该函数完成横向LQR控制器的初始化工作
Status LatController::Init(std::shared_ptr<DependencyInjector> injector) {if (!ControlTask::LoadConfig<LatBaseLqrControllerConf>(&lat_based_lqr_controller_conf_)) {AERROR << "failed to load control conf";return Status(ErrorCode::CONTROL_INIT_ERROR,"failed to load lat control_conf");}injector_ = injector;if (!LoadControlConf()) {AERROR << "failed to load control conf";return Status(ErrorCode::CONTROL_COMPUTE_ERROR,"failed to load control_conf");}// Matrix init operations.//横向控制预览窗口是0，所以matrix_size就是basic_state_size_=4const int matrix_size = basic_state_size_ + preview_window_;matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);// 4x4matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);// 4x4matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);// 4x4/*A 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;]*/// A 矩阵（前进档）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_;// 车辆状态方程系数矩阵A中与v有关的时变项形如" 常数/v "，将常数提取出来放在矩阵matrix_a_coeff_里，每个周期处以v更新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*/matrix_b_ = Matrix::Zero(basic_state_size_, 1); // 4 x 1matrix_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_;matrix_bd_ = matrix_b_ * ts_;matrix_state_ = Matrix::Zero(matrix_size, 1);// 4 x 1matrix_k_ = Matrix::Zero(1, matrix_size);  // 1 x 4matrix_r_ = Matrix::Identity(1, 1);        // 1 x 1matrix_q_ = Matrix::Zero(matrix_size, matrix_size); // 4 x 4int q_param_size = lat_based_lqr_controller_conf_.matrix_q_size();int reverse_q_param_size =lat_based_lqr_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);}for (int i = 0; i < q_param_size; ++i) {matrix_q_(i, i) = lat_based_lqr_controller_conf_.matrix_q(i);}// 更新后的Q矩阵matrix_q_updated_matrix_q_updated_ = matrix_q_;// 初始化3个滤波器，1个低通滤波是对计算出方向盘转角控制指令进行滤波// 另外两个滤波器是横向误差，航向误差的均值滤波器InitializeFilters();// LoadLatGainScheduler加载增益调度表，就是横向误差和航向误差在车速不同时乘上个不同的比例// 这个比例决定了实际时的控制效果，根据实际经验低速和高速应该采取不同的比例，低速比例较大，若高速// 采用同样比例极有可能导致画龙现象LoadLatGainScheduler();// 这个函数主要是在屏幕上打印一些车辆参数的信息LogInitParameters();// 默认是开启横向控制中的超前滞后控制器的，提升或者降低闭环反馈系统的响应速度enable_leadlag_ =lat_based_lqr_controller_conf_.enable_reverse_leadlag_compensation();if (enable_leadlag_) {leadlag_controller_.Init(lat_based_lqr_controller_conf_.reverse_leadlag_conf(), ts_);}// 默认关闭enable_mrac_ = lat_based_lqr_controller_conf_.enable_steer_mrac_control();if (enable_mrac_) {mrac_controller_.Init(lat_based_lqr_controller_conf_.steer_mrac_conf(),vehicle_param_.steering_latency_param(), ts_);}// 默认打开 是否使能前进倒车时的预瞄控制enable_look_ahead_back_control_ =lat_based_lqr_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() {// 取横向误差的增益调度表放到临时变量lat_err_gain_schedulerconst auto &lat_err_gain_scheduler =lat_based_lqr_controller_conf_.lat_err_gain_scheduler();// 读取航向误差的增益调度表放到临时变量heading_err_gain_schedulerconst auto &heading_err_gain_scheduler =lat_based_lqr_controller_conf_.heading_err_gain_scheduler();// 屏幕上的打印信息，调度表加载成功AINFO << "Lateral control gain scheduler loaded";// Interpolation1D是Apollo自己定义的一维线性插值表类型，将已知离散点存入插值表，给出x就可以去表里找所在区间并插值计算y值Interpolation1D::DataType xy1, xy2;// 每一组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()));}for (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_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";
}
// 该函数调用关闭横向日志文件
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) {// 获取车辆状态auto vehicle_state = injector_->vehicle_state();// 先前的纵向调试结果auto previous_lon_debug = injector_->Get_previous_lon_debug_info();// 目标跟踪轨迹auto target_tracking_trajectory = *planning_published_trajectory;// 是否打开导航模式，默认是falseif (FLAGS_use_navigation_mode &&lat_based_lqr_controller_conf_.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是从输入参数传进来的规划轨迹信息trajectory_analyzer_ =std::move(TrajectoryAnalyzer(&target_tracking_trajectory));// Transform the coordinate of the planning trajectory from the center of the// rear-axis to the center of mass, if conditions matched// 将规划轨迹从后轴中心变换到质心if (((lat_based_lqr_controller_conf_.trajectory_transform_to_com_reverse() &&vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) ||(lat_based_lqr_controller_conf_.trajectory_transform_to_com_drive() &&vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE)) &&enable_look_ahead_back_control_) {trajectory_analyzer_.TrajectoryTransformToCOM(lr_);}// Re-build the vehicle dynamic models at reverse driving (in particular,// replace the lateral translational motion dynamics with the corresponding// kinematic models)// 倒档时重建车辆的动力学模型if (vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) {/*A 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;]*/cf_ = -lat_based_lqr_controller_conf_.cf();cr_ = -lat_based_lqr_controller_conf_.cr();matrix_a_(0, 1) = 0.0;matrix_a_coeff_(0, 2) = 1.0;} else {/*A 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_ = lat_based_lqr_controller_conf_.cf();cr_ = lat_based_lqr_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_;// 调用更新驾驶航向函数，也是要满足FLAGS_reverse_heading_control默认关闭，实际这个函数没啥用UpdateDrivingOrientation();// 简单横向调试SimpleLateralDebug *debug = cmd->mutable_debug()->mutable_simple_lat_debug();debug->Clear();// Update state = [Lateral Error, Lateral Error Rate, Heading Error, Heading// Error Rate, preview lateral error1 , preview lateral error2, ...]//更新车辆状态矩阵X=[e1 e1' e2 e2']//首先该函数UpdateState()内部调用了ComputeLateralErrors()函数得到的各种误差信息存放到debug中//然后又用debug去更新车辆状态矩阵X即matrix_state_UpdateState(debug, chassis);// 主要是更新车辆状态方程系数矩阵A及其离散形式中与速度相关的时变项UpdateMatrix();// Compound discrete matrix with road preview model 没啥用UpdateMatrixCompound();// Adjust matrix_q_updated when in reverse gear// 当在R档时调整矩阵Q也就是LatControllr类成员matrix_q_// 将控制配置里的reverse_matrix_q写入LatControllr类成员matrix_q_int q_param_size = lat_based_lqr_controller_conf_.matrix_q_size();int reverse_q_param_size =lat_based_lqr_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) = lat_based_lqr_controller_conf_.reverse_matrix_q(i);}} else {for (int i = 0; i < q_param_size; ++i) {// 非R档加载控制配置里的matrix_qmatrix_q_(i, i) = lat_based_lqr_controller_conf_.matrix_q(i);}}// Add gain scheduler for higher speed steering// 对于更高速度的转向增加增益调度表 默认是false，但是实际上很有用if (FLAGS_enable_gain_scheduler) {// Q矩阵的(1,1)也就是横向误差平方和的权重系数, 第1行第1列// Q(1,1)=Q(1,1)*(用之前加载的横向误差调度增益表根据当前车速插值得到的ratio)matrix_q_updated_(0, 0) =matrix_q_(0, 0) * lat_err_interpolation_->Interpolate(std::fabs(vehicle_state->linear_velocity()));// Q矩阵的(3,3)也就是航向误差平方和的权重系数, 第3行第3列// Q(3,3)=Q(3,3)*(用之前加载的航向误差调度增益表根据当前车速插值得到的ratio)matrix_q_updated_(2, 2) =matrix_q_(2, 2) * heading_err_interpolation_->Interpolate(std::fabs(vehicle_state->linear_velocity()));// 求解LQR问题，求解到的最优状态反馈矩阵K放入matrix_k_// 根据经验打开的话更容易获得更好的控制性能，否则低速适用的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// 将计算出的控制量(车辆的前轮转角)从rad转化为deg,然后再乘上转向传动比steer_ratio_转化成方向盘转角const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /M_PI * steer_ratio_ /steer_single_direction_max_degree_ * 100;// 存放前馈控制量 根据曲率计算前馈const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());// 初始化定义最终的方向盘控制转角为0.0double steer_angle = 0.0;// 定义临时变量方向盘反馈增强初始化为0.0double steer_angle_feedback_augment = 0.0;// Augment the feedback control on lateral error at the desired speed domainif (enable_leadlag_) {// 如果车辆打开高速的反馈增强控制 或 车速小于低高速边界速度3m/s// 实际上就是低速时打开超前滞后控制器，然后这个超前滞后控制器只对横向误差进行增强控制if (lat_based_lqr_controller_conf_.enable_feedback_augment_on_high_speed() ||std::fabs(vehicle_state->linear_velocity()) < low_speed_bound_) {steer_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;// Compute the steering command limit with the given maximum lateral// acceleration//若限制横向加速度 最大方向盘转角百分数 = atan(ay_max * L / v^2) * steerratio * 180/pi /max_steer_ang * 100//根据上述公式可以从限制的最大横向加速度计算出最大的方向盘转角控制百分数//若无限制 最大方向盘转角百分数 = 100const 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;// 一个周期方向盘转角最大增量 = 最大方向盘角速度 * 控制周期const double steer_diff_with_max_rate =lat_based_lqr_controller_conf_.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();// Re-compute the steering command if the MRAC control is enabled, with steer// angle limitation and steer rate limitation// 如果打开MRAC模型参考自适应控制 enable_mrac_，重新计算方向盘转角控制量，并用方向盘转角和转速限制对转角控制量进行限幅if (enable_mrac_) {const int mrac_model_order =lat_based_lqr_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()) >FLAGS_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());}// 将当前时刻方向盘的转角置为上一时刻的转角pre_steering_position_ = steering_position;// 将enable_mrac_是否打开信息加入debug调试信息结构体debug->set_steer_mrac_enable_status(enable_mrac_);// Clamp the steer angle with steer limitations at current speed// 根据当前车速对下发方向盘转角进行限幅，横向加速度的限制转化到此刻方向盘转角限制就会引入车double steer_angle_limited =common::math::Clamp(steer_angle, -steer_limit, steer_limit);steer_angle = steer_angle_limited;debug->set_steer_angle_limited(steer_angle_limited);// Limit the steering command with the designed digital filter// 对方向盘转角数字滤波，然后控制百分数又限制在正负100，百分数自然最大就是100steer_angle = digital_filter_.Filter(steer_angle);steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);// Check if the steer is locked and hence the previous steer angle should be// executed//当处于D档或倒档 且 车速小于某一速度 且处于自驾模式时锁定方向盘，方向盘控制转角就保持上一次的命令值 0.081m/sif (injector_->vehicle_state()->gear() != canbus::Chassis::GEAR_REVERSE) {if ((std::abs(vehicle_state->linear_velocity()) <lat_based_lqr_controller_conf_.lock_steer_speed() ||previous_lon_debug->path_remain() <= 0) &&vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE &&chassis->driving_mode() == canbus::Chassis::COMPLETE_AUTO_DRIVE) {ADEBUG << "Into lock steer, path_remain is "<< previous_lon_debug->path_remain() << "linear_velocity is "<< vehicle_state->linear_velocity();steer_angle = pre_steer_angle_;}}// Set the steer commands// 设定转角指令，再通过最大转角速率再次进行限制幅度，最多只能=上次的转角指令+/-最大转角速率 * 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();// compute extra information for logging and debuggingconst 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());ProcessLogs(debug, chassis);return Status::OK();
}
// 默认关闭，是另外一种横向控制器
Status LatController::Reset() {matrix_state_.setZero();if (enable_mrac_) {mrac_controller_.Reset();}return Status::OK();
}
// 横向控制器的车辆状态矩阵函数 主要就是更新状态方程中的 X = [e1 e1' e2 e2']
void LatController::UpdateState(SimpleLateralDebug *debug,const canbus::Chassis *chassis) {auto vehicle_state = injector_->vehicle_state();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, chassis);} 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, chassis);}// State matrix update;// First four elements are fixed;// 默认打开if (enable_look_ahead_back_control_) {// 若打开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();matrix_state_(2, 0) = debug->heading_error();}matrix_state_(1, 0) = debug->lateral_error_rate();matrix_state_(3, 0) = debug->heading_error_rate();// Next elements are depending on preview window size;// 这一部分是更新状态矩阵中的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,Ad
void LatController::UpdateMatrix() {double v;// At reverse driving, replace the lateral translational motion dynamics with// the corresponding kinematic models// 倒档，代替横向平动动力学用对应的运动学模型 R档的暂且不管if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE &&!lat_based_lqr_controller_conf_.reverse_use_dynamic_model()) {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());// 计算A矩阵的离散化形式Ad，用双线性变换法matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *(matrix_i + ts_ * 0.5 * matrix_a_);
}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;}}
}
// 计算前馈
// 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_;// 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 &&!lat_based_lqr_controller_conf_.reverse_use_dynamic_model()) {steer_angle_feedforwardterm =lat_based_lqr_controller_conf_.reverse_feedforward_ratio() *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
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,const canbus::Chassis *chassis) {TrajectoryPoint target_point;if (lat_based_lqr_controller_conf_.query_time_nearest_point_only()) {target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(Clock::NowInSeconds() + query_relative_time_);} else { // 如果不采用这种向前看0.8秒作为目标点的方法，默认不采用的if (FLAGS_use_navigation_mode &&!lat_based_lqr_controller_conf_.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();const double dy = y - target_point.path_point().y();// 当前点xdebug->mutable_current_target_point()->mutable_path_point()->set_x(target_point.path_point().x());// 当前点ydebug->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());// 根据目标点处的heading角正余弦值计算横向误差double lateral_error = cos_target_heading * dy - sin_target_heading * dx;if (lat_based_lqr_controller_conf_.enable_navigation_mode_error_filter()) {lateral_error = lateral_error_filter_.Update(lateral_error);}// 横向误差debug->set_lateral_error(lateral_error);// 目标点航向debug->set_ref_heading(target_point.path_point().theta());// 计算航向误差，车辆当前航向角theta-ref_heading角double heading_error =common::math::NormalizeAngle(theta - debug->ref_heading());if (lat_based_lqr_controller_conf_.enable_navigation_mode_error_filter()) {heading_error = heading_error_filter_.Update(heading_error);}// 航向误差debug->set_heading_error(heading_error);// Within the low-high speed transition window, linerly interplolate the// lookahead/lookback station for "soft" prediction window switchdouble lookahead_station = 0.0;double lookback_station = 0.0;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_;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));}// Estimate the heading error with look-ahead/look-back windows as feedback// signal for special driving scenariosdouble heading_error_feedback;if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {heading_error_feedback = heading_error;} else {auto lookahead_point = trajectory_analyzer.QueryNearestPointByRelativeTime(target_point.relative_time() +lookahead_station /(std::max(std::fabs(linear_v), 0.1) * std::cos(heading_error)));heading_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);// Estimate the lateral error with look-ahead/look-back windows as feedback// signal for special driving scenariosdouble lateral_error_feedback;if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {lateral_error_feedback =lateral_error - lookback_station * std::sin(heading_error);} else { // 前进档lateral_error_feedback =lateral_error + lookahead_station * std::sin(heading_error);}// 横向误差反馈debug->set_lateral_error_feedback(lateral_error_feedback);auto lateral_error_dot = linear_v * std::sin(heading_error);auto lateral_error_dot_dot = linear_a * std::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;}}auto centripetal_acceleration =linear_v * linear_v / wheelbase_ *std::tan(chassis->steering_percentage() / 100 *vehicle_param_.max_steer_angle() / steer_ratio_);// 横向误差率debug->set_lateral_error_rate(lateral_error_dot);// 横向加速度debug->set_lateral_acceleration(lateral_error_dot_dot);debug->set_lateral_centripetal_acceleration(centripetal_acceleration);// 横向jerkdebug->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) {debug->set_heading_rate(-angular_v);} else {debug->set_heading_rate(angular_v);}// 参考的航向角变化率=目标点纵向速度/目标点转弯半径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());// 航向角变化的加速度就用差分法，这一时刻航向角变化率减去上一时刻之差然后再处以采样周期tsdebug->set_heading_acceleration((debug->heading_rate() - previous_heading_rate_) / ts_);// 目标点航向角变化的加速度也用差分法计算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();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_);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();debug->set_curvature(target_point.path_point().kappa());
}
// 更新驾驶航向
void LatController::UpdateDrivingOrientation() {auto vehicle_state = injector_->vehicle_state();driving_orientation_ = vehicle_state->heading();matrix_bd_ = matrix_b_ * ts_;// Reverse the driving direction if the vehicle is in reverse modeif (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