前面文章：
Apollo9.0 PNC源码学习之Control模块（一）
Apollo9.0 PNC源码学习之Control模块（二）
Apollo9.0 PNC源码学习之Control模块（三）
Apollo9.0 PNC源码学习之Control模块（四）
本文讲解百度Apollo基于LQR的横向控制器
1 LQR原理讲解
LQR：线性二次调节器，设计状态反馈控制器的方法
系统：
x
˙
=
A
x
+
B
u
\dot x=Ax+Bu
x˙=Ax+Bu
线性反馈控制器：
u
=
−
K
x
u=-Kx
u=−Kx
x
˙
=
A
x
−
B
K
x
=
(
A
−
B
K
)
⏟
A
c
l
x
\begin{array}{l} \dot{x}=A x-B K x=\underbrace{(A-B K)}_{A_{c l}} x\\ \end{array}
x˙=Ax−BKx=Acl
(A−BK)x
让系统稳定的条件是矩阵 A c l A_{cl} Acl的特征值实部均为负数。因此我们可以手动选择几个满足上述条件的特征值，然后反解出K，从而得到控制器。
代价函数J:
J
=
∫
0
∞
x
T
Q
x
+
u
T
R
u
d
t
J=\int_{0}^{\infty} x^{T} Q x+u^{T} R u \mathrm{~d} t
J=∫0∞xTQx+uTRu dt
在系统稳定的前提下，通过设计合适的K，让代价函数J最小。
Q大：希望状态变量x更快收敛
R大：希望输入量u收敛更快，以更小的代价实现系统稳定
1.1 连续时间LQR推导
具体推导参见博客：线性二次型调节器(LQR)原理详解
求解连续时间LQR反馈控制器参数K的过程：
（1）设计参数矩阵Q、R
（2）求解Riccati方程
A
T
P
+
P
A
−
P
B
R
−
1
B
T
P
+
Q
=
0
A^TP+PA-PBR^{-1}B^TP+Q=0
ATP+PA−PBR−1BTP+Q=0得到P
（3）计算
K
=
R
−
1
B
T
P
K=R^{-1}B^TP
K=R−1BTP得到反馈控制量
u
=
−
k
x
u=-kx
u=−kx
1.2 离散时间LQR推导
离散情况下的LQR推导有最小二乘法和动态规划算法
详细推导见博客：连续时间LQR和离散时间LQR笔记
离散系统：
x
(
k
+
1
)
=
A
x
(
k
)
+
B
u
(
k
)
x(k+1)=Ax(k)+Bu(k)
x(k+1)=Ax(k)+Bu(k)
代价函数：
设计步骤：
① 确定迭代范围N
② 设置迭代初始值
P
N
=
Q
P_N=Q
PN=Q
③
t
=
N
,
.
.
.
,
1
t=N,...,1
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 P_{t-1}=Q+A^TP_tA-A^TP_tB(R+B^TP_{t+1}B)^{-1}B^TP_tA Pt−1=Q+ATPtA−ATPtB(R+BTPt+1B)−1BTPtA
④ t = 0 , . . . , N t=0,...,N t=0,...,N循环计算反馈系数 K t = ( R + B T P t + 1 B ) − 1 B T P t + 1 A K_t=(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A Kt=(R+BTPt+1B)−1BTPt+1A 得到控制量 u t = − K t x t u_t=-K_tx_t ut=−Ktxt
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) {
// 检查ABQR
if (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和Q
Matrix 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及其离散形式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,
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 4x4
Eigen::MatrixXd matrix_adc_;
// 车辆状态方程系数矩阵B 控制量的系数矩阵 4 x 1
Eigen::MatrixXd matrix_b_;
// 系数矩阵B的离散形式 Bd
Eigen::MatrixXd matrix_bd_;
// 系数矩阵考虑preview之后的扩展阵
Eigen::MatrixXd matrix_bdc_;
// 状态反馈矩阵K u=-kx LQR求解出最优的K K=[k0 k1 k2 k3] 1x4
Eigen::MatrixXd matrix_k_;
// 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_;
// 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 controller
bool enable_leadlag_ = false; //超前滞后控制器，在主回路上串联校正环节
LeadlagController leadlag_controller_;
// Mrac controller
bool enable_mrac_ = false;
MracController mrac_controller_;
// Look-ahead controller
bool 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 filter
std::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_=4
const int matrix_size = basic_state_size_ + preview_window_;
matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);// 4x4
matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);// 4x4
matrix_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 1
matrix_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 1
matrix_k_ = Matrix::Zero(1, matrix_size); // 1 x 4
matrix_r_ = Matrix::Identity(1, 1); // 1 x 1
matrix_q_ = Matrix::Zero(matrix_size, matrix_size); // 4 x 4
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();
// 若车辆状态矩阵大小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_scheduler
const auto &lat_err_gain_scheduler =
lat_based_lqr_controller_conf_.lat_err_gain_scheduler();
// 读取航向误差的增益调度表放到临时变量heading_err_gain_scheduler
const 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后存入插值表xy1
for (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;
// 是否打开导航模式，默认是false
if (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 trajectory
if (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_q
for (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_q
matrix_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.0
double steer_angle = 0.0;
// 定义临时变量方向盘反馈增强初始化为0.0
double steer_angle_feedback_augment = 0.0;
// Augment the feedback control on lateral error at the desired speed domain
if (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 switch
steer_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
//根据上述公式可以从限制的最大横向加速度计算出最大的方向盘转角控制百分数
//若无限制 最大方向盘转角百分数 = 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;
// 一个周期方向盘转角最大增量 = 最大方向盘角速度 * 控制周期
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 message
MracDebug *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，百分数自然最大就是100
steer_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/s
if (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 debugging
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;
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 matched
const 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_heading
matrix_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 matrix
matrix_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();
// 当前点x
debug->mutable_current_target_point()->mutable_path_point()->set_x(
target_point.path_point().x());
// 当前点y
debug->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 switch
double 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 scenarios
double 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 scenarios
double 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);
// 横向jerk
debug->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());
// 航向角变化的加速度就用差分法，这一时刻航向角变化率减去上一时刻之差然后再处以采样周期ts
debug->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 mode
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 mode
matrix_bd_ = -matrix_b_ * ts_;
ADEBUG << "Matrix_b changed due to gear direction";
}
}
}
} // namespace control
} // namespace apollo
