百度Apollo5.0控制模块代码学习（四）横向控制
1 车辆动力学模型
关于车辆的方向盘动力学模型的推导过程可参考：
https://blog.csdn.net/u013914471/article/details/83018664；
https://blog.csdn.net/u013914471/article/details/83748571；
动力学模型如下：
其中，各个参数涵义如下：
e 1 e_{1} e1：横向误差；
e 1 ˙ \dot{e_{1}} e1˙：横向误差变化率；
e 2 e_{2} e2：航向误差；
e 2 ˙ \dot{e_{2}} e2˙：航向误差变化率；
C α f C_{\alpha f} Cαf：前轮侧偏钢度；
C α r C_{\alpha r} Cαr：后轮侧偏刚度；
l f l_{f} lf：前悬长度；
l r l_{r} lr：后悬长度；
V x V_{x} Vx：纵向车速；
φ ˙ d e s \dot{\varphi}_{des} φ˙des：期望转角速度；
I z I_{z} Iz：车辆绕z轴的转动惯量；
δ \delta δ：前轮转角；
$$
其中，状态矩阵系数：
控制矩阵系数：
2 横向控制
横向控制由两个控制器组成：前馈开环控制器和反馈闭环控制器。
2.1 前馈控制器
前馈控制器的引入是为了消除跟踪误差，因为车辆的系统状态方程一般为：
其中u为前轮转角 δ \delta δ，但是由于道路存在曲率ref_curvature（纵向控制中为Kappa），车辆方向盘动力学公式为：
其中 B 2 B_{2} B2 φ ˙ d e s \dot{\varphi}_{des} φ˙des就为道路曲率存在引起的误差，因此引入和道路曲率相关的前馈控制器以消除跟踪误差，计算依据为：
v
其中Kv=
R=1/ref_curvature；
k3=matrix_k(0,2)，（k为LQR求得）；
a y a_{y} ay=v. ψ ˙ \dot{\psi } ψ˙=v.v/R，（其中 ψ ˙ \dot{\psi } ψ˙为车辆角速度）；
具体代码如下：
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 = VehicleStateProvider::Instance()->linear_velocity();
double steer_angle_feedforwardterm;
if (VehicleStateProvider::Instance()->gear() ==
canbus::Chassis::GEAR_REVERSE) {
steer_angle_feedforwardterm = 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;
}
2.2 反馈控制
其中Q的调整步骤按照apollo/docs/howto
/how_to_tune_control_parameters_cn.md中的方式调整参数，具体如下：
### 横向控制器的整定
横向控制器设计用于最小调谐力。“所有”车辆的基础横向控制器调谐步骤如下：
1. 将`matrix_q` 中所有元素设置为零.
2. 增加`matrix_q`中的第三个元素，它定义了航向误差加权，以最小化航向误差。
3. 增加`matrix_q`的第一个元素，它定义横向误差加权以最小化横向误差。
#### 林肯MKZ调谐
对于Lincoln MKZ，有四个元素指的是状态加权矩阵Q的对角线元素：
- 横向误差加权
- 横向误差率加权
- 航向误差加权
- 航向差错率加权
通过在横向控制器调谐中列出的基本横向控制器调整步骤来调整加权参数。下面是一个例子。`
lat_controller_conf {
matrix_q: 0.05
matrix_q: 0.0
matrix_q: 1.0
matrix_q: 0.0
}
以下代码是对控制器的初始化，包括：
1，加载控制参数配置信息；
2，按照公式对matrix_a初始化；
3，对matrix_b进行初始化；
4，初始化Q，R；
5，加载横向误差及航向角误差下的标定表（车速-传动比标定表）；
6，如果允许滞后超前控制，则加载相应参数；
Status LatController::Init(const ControlConf *control_conf) {
control_conf_ = control_conf;
if (!LoadControlConf(control_conf_)) {
AERROR << "failed to load control conf";
return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
"failed to load control_conf");
}
// Matrix init operations.
const int matrix_size = basic_state_size_ + preview_window_;
matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);
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_;
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_;
matrix_b_ = Matrix::Zero(basic_state_size_, 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);
matrix_k_ = Matrix::Zero(1, matrix_size);
matrix_r_ = Matrix::Identity(1, 1);
matrix_q_ = Matrix::Zero(matrix_size, matrix_size);
int q_param_size = control_conf_->lat_controller_conf().matrix_q_size();
int reverse_q_param_size =
control_conf_->lat_controller_conf().reverse_matrix_q_size();
if (matrix_size != q_param_size || matrix_size != reverse_q_param_size) {
const auto error_msg =
StrCat("lateral controller error: matrix_q size: ", q_param_size,
"lateral controller error: reverse_maxtrix_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) = control_conf_->lat_controller_conf().matrix_q(i);
}
matrix_q_updated_ = matrix_q_;
InitializeFilters(control_conf_);
auto &lat_controller_conf = control_conf_->lat_controller_conf();
LoadLatGainScheduler(lat_controller_conf);
LogInitParameters();
bool enable_leadlag = control_conf_->lat_controller_conf()
.enable_reverse_leadlag_compensation();
if (enable_leadlag) {
leadlag_controller_.Init(lat_controller_conf.reverse_leadlag_conf(), ts_);
}
return Status::OK();
}
下面代码是计算控制命令，输入为车辆位置，底盘信息，规划路径，输出为控制命令，即方向盘转动角度。
Status LatController::ComputeControlCommand(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
ControlCommand *cmd)
DEFINE_bool(use_navigation_mode, false, “Use relative position in navigation mode”);在导航模式中使用相对位置；
DEFINE_bool(enable_navigation_mode_position_update, true, “Enable position update for navigation mode”);启用导航模式的位置更新；
在Apollo的工程中找到以下两句话：
“附注：导航模块是在Apollo 2.5版本引入的满足低成本测试的特性。在该模式下，Baidu或Google地图展现的是车辆的绝对位置，而主视图中展现的是车辆的相对位置。”
“In navigation mode, the coordinate system is FLU and relative position of the ego-car is (0, 0). But, absolute position of the ego-car is needed in MAP_NAVIGATOR.”
相对位置即车辆主视图中的位置，在导航模式下，车的相对位置为（0，0），但是在地图定位中需要的是车的绝对位置。
关于四元数与欧拉角的关系，可参考：
https://blog.csdn.net/xiaoma_bk/article/details/79082629
如果启动导航模式，则需要根据车辆目前的定位更新车辆的以下信息：
1，下一时刻规划点的时间与当前轨迹时间（常值-1）之差；
2，x，y坐标；
3，车辆的航向角，如果定位信息中车辆航向角为0，需要根据四元数求取欧拉角；
如果时间之差大于某一值，则初始化车辆位置信息，航向角为上面更新的2和3，当前轨迹时间更新为下一时刻的规划点时间；
否则更新目前车辆位置和初始车辆位置之差（位置，速度，航向角等差值）。
（这段的意思是如果车辆和下一时刻距离较远，则目前车辆的位置就是车辆的初始化位置，否则，直接计算车辆当前和初始化位置的差值）。
对于所有的规划点，根据当前位置和车速，更新位置（x，y），航向角，
即：新位置=旧位置+位置偏差（车走过的路程）
if (FLAGS_use_navigation_mode &&
FLAGS_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::WrapAngle(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);
});
}
}
将轨迹规划中的车辆后轴中心坐标转换为质心坐标。
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 (((FLAGS_trajectory_transform_to_com_reverse &&
vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) ||
(FLAGS_trajectory_transform_to_com_drive &&
vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE)) &&
(std::fabs(vehicle_state->linear_velocity()) <= low_speed_bound_)) {
trajectory_analyzer_.TrajectoryTransformToCOM(lr_);
}
如果档位为倒档，需要重新建立倒车时的车辆动力学模型，此时相应的前后轮侧偏刚度取负数，其他值不变。
之后，更新车辆的状态矩阵系数及控制矩阵系数。
if (vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) {
cf_ = -control_conf_->lat_controller_conf().cf();
cr_ = -control_conf_->lat_controller_conf().cr();
matrix_a_(0, 1) = 0.0;
matrix_a_coeff_(0, 2) = 1.0;
} else {
cf_ = control_conf_->lat_controller_conf().cf();
cr_ = control_conf_->lat_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_;
matrix_b_(1, 0) = cf_ / mass_;
matrix_b_(3, 0) = lf_ * cf_ / iz_;
matrix_bd_ = matrix_b_ * ts_;
更新状态变量，包括：横向偏差，横向偏差变化率，转向角偏差，转向角偏差变化率。
如果启动导航模式，则计算偏差时车辆当前位置设置为（0，0），否则，车辆当前位置坐标为根据车辆后轴坐标系转换的坐标位置。、
车辆朝向为定位模块确定的四元数坐标，
void LatController::UpdateState(SimpleLateralDebug *debug) {
auto vehicle_state = VehicleStateProvider::Instance();
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);
} 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);
}
// State matrix update;
// First four elements are fixed;
if (control_conf_->lat_controller_conf().enable_look_ahead_back_control() &&
std::fabs(vehicle_state->linear_velocity()) <= low_speed_bound_) {
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;
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;
}
}
以下为计算横向偏差的具体实现：
如果仅允许寻找时间最近的规划点，则跟踪点就是离（当前时间+寻找相对时间0.8s）时间最近的规划点。
否则，判断，是否允许导航模式且不允许采用导航模式更新，则跟踪点就是离（当前时间+寻找相对时间0.8s）时间最近的规划点。
否则，跟踪点就是位置距离目前的车辆最近的规划点。
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) {
TrajectoryPoint target_point;
if (FLAGS_query_time_nearest_point_only) {
target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
Clock::NowInSeconds() + query_relative_time_);
} else {
if (FLAGS_use_navigation_mode &&
!FLAGS_enable_navigation_mode_position_update) {
target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
Clock::NowInSeconds() + query_relative_time_);
} else {
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();
debug->mutable_current_target_point()->mutable_path_point()->set_x(
target_point.path_point().x());
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());
double lateral_error = cos_target_heading * dy - sin_target_heading * dx;
if (FLAGS_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());
double heading_error =
common::math::NormalizeAngle(theta - debug->ref_heading());
if (FLAGS_enable_navigation_mode_error_filter) {
heading_error = heading_error_filter_.Update(heading_error);
}
debug->set_heading_error(heading_error);
在特殊驾驶情况下，使用前视/后视窗口作为反馈信号估计航向误差。
此时前视点就是时间距离（追踪点时间戳+前视距离/纵向速度差）；
航向误差反馈=追踪点航向误差+追踪点航向角-前视点航向角；
前视点横向位置偏差的求解方法类似。
double heading_error_feedback;
if (VehicleStateProvider::Instance()->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 (VehicleStateProvider::Instance()->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 (VehicleStateProvider::Instance()->gear() ==
canbus::Chassis::GEAR_REVERSE) {
lateral_error_dot = -lateral_error_dot;
lateral_error_dot_dot = -lateral_error_dot_dot;
}
}
debug->set_lateral_error_rate(lateral_error_dot);
debug->set_lateral_acceleration(lateral_error_dot_dot);
debug->set_lateral_jerk(
(debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
previous_lateral_acceleration_ = debug->lateral_acceleration();
if (VehicleStateProvider::Instance()->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());
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::UpdateMatrix() {
double v;
// At reverse driving, replace the lateral translational motion dynamics with
// the corresponding kinematic models
if (VehicleStateProvider::Instance()->gear() ==
canbus::Chassis::GEAR_REVERSE) {
v = std::min(VehicleStateProvider::Instance()->linear_velocity(),
-minimum_speed_protection_);
matrix_a_(0, 2) = matrix_a_coeff_(0, 2) * v;
} else {
v = std::max(VehicleStateProvider::Instance()->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());
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;
}
}
}
设置矩阵Q；
// Adjust matrix_q_updated when in reverse gear
int q_param_size = control_conf_->lat_controller_conf().matrix_q_size();
int reverse_q_param_size =
control_conf_->lat_controller_conf().reverse_matrix_q_size();
if (VehicleStateProvider::Instance()->gear() ==
canbus::Chassis::GEAR_REVERSE) {
for (int i = 0; i < reverse_q_param_size; ++i) {
matrix_q_(i, i) =
control_conf_->lat_controller_conf().reverse_matrix_q(i);
}
} else {
for (int i = 0; i < q_param_size; ++i) {
matrix_q_(i, i) = control_conf_->lat_controller_conf().matrix_q(i);
}
}
解LQR方程，求取matrix_k，；
同时为为高速转向添加增益调度程序，即Q矩阵中（0，0），（2，2）系数乘以车辆当前线速度。为什么这么做？
// Add gain scheduler for higher speed steering
if (FLAGS_enable_gain_scheduler) {
matrix_q_updated_(0, 0) =
matrix_q_(0, 0) * lat_err_interpolation_->Interpolate(
std::fabs(vehicle_state->linear_velocity()));
matrix_q_updated_(2, 2) =
matrix_q_(2, 2) * heading_err_interpolation_->Interpolate(
std::fabs(vehicle_state->linear_velocity()));
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_);
}
求出K矩阵之后，需要求最优控制解，即K*state。
// feedback = - K * state
// Convert vehicle steer angle from rad to degree and then to steer degree
// then to 100% 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());
在所需速度域上增加横向误差的反馈控制
// Clamp the steer angle to -100.0 to 100.0
double steer_angle = 0.0;
double steer_angle_feedback_augment = 0.0;
bool enable_leadlag = control_conf_->lat_controller_conf()
.enable_reverse_leadlag_compensation();
// Augment the feedback control on lateral error at the desired speed domain
if (enable_leadlag) {
if (FLAGS_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;
}
}
方向盘转角=反馈方向盘控制转角+前馈方向盘控制转角+转向角反馈增强量；
同时所求出的方向盘转角进行限制。
steer_angle = steer_angle_feedback + steer_angle_feedforward +
steer_angle_feedback_augment;
以给定的最大横向加速度限制转向指令
// Limit the steering command with the given maximum lateral acceleration
if (FLAGS_set_steer_limit) {
const double 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;
在当前速度下夹紧转向角和转向限制；
对方向盘转角进行滤波；
// 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);
}
steer_angle = digital_filter_.Filter(steer_angle);
steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);
如果当前车速满足锁定方向盘车速条件时，则方向盘角度=前一时刻的方向盘角度。
if (std::abs(vehicle_state->linear_velocity()) < FLAGS_lock_steer_speed &&
(vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE ||
vehicle_state->gear() == canbus::Chassis::GEAR_REVERSE) &&
chassis->driving_mode() == canbus::Chassis::COMPLETE_AUTO_DRIVE) {
steer_angle = pre_steer_angle_;
}
对方向盘角度的变化率进行限制；
const double steer_diff_with_max_rate =
vehicle_param_.max_steer_angle_rate() * ts_ * 180 / M_PI /
steer_single_direction_max_degree_ * 100;
if (FLAGS_enable_maximum_steer_rate_limit) {
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));
pre_steer_angle_ = cmd->steering_target();
cmd->set_steering_rate(steer_diff_with_max_rate / ts_);
} else {
pre_steer_angle_ = steer_angle;
cmd->set_steering_target(steer_angle);
cmd->set_steering_rate(FLAGS_steer_angle_rate);
}