本文详细介绍了如何从零开始搭建实现Apollo9.0中的控制模块，该模块负责处理环境感知和路径规划的数据，通过实时计算提供精确的车辆控制指令，对无人驾驶车辆的安全性和性能至关重要。
《从0开始搭建实现apollo9.0》系列五 Control模块解读
control模块作为自动驾驶软件系统的最下游，根据环境感知模块和路径规划模块提供的信息，对车辆的油门、刹车和方向盘进行控制，以确保车辆能够安全、稳定地行驶。它通过接收来自这些模块的实时数据，计算出合适的控制指令，并发送给车辆的执行机构，从而实现对车辆的精确控制。也是无人驾驶车辆设计过程中最底层、最重要的环节，对于车辆的安全行驶和性能表现具有决定性的影响。
控制解耦为横向和纵向；
lqr controller
主要的计算逻辑在 ComputeControlCommand 函数内
输入是规划轨迹线（apollo::planning::ADCTrajectory），车体位置信息（apollo::localization::LocalizationEstimate），车体姿态信息（VehicleStateProvider），车体底盘信息（apollo::canbus::Chassis）等。
状态量为[横向位置误差，横向位置误差变化率，航向角误差，航向角误差变化率]；
控制量为cmd->steering_target()，
根据车辆转弯的动力学建模，建立车辆动力学的微分方程，可以通过观测值（计算误差）进行状态变量的获取，推到出系统状态转移矩阵（A）和控制矩阵（B），将连续时间作用的系统状态方程离散化，设计 Q ，R 矩阵，进行 LQR 计算求解 Riccati 方程得到反馈矩阵K；
根据反馈矩阵K计算出的控制量为 steer_angle_feedback ， 另一部分是车辆前馈 steer_angle_feedforward ，横向误差还设计了超前滞后调节器，对横向进行补偿，最终将三者相加，作为一次控制量输入到车辆，完成 1 次控制。
参考：
https://blog.csdn.net/tauyangdao/article/details/108058222
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.
//矩阵初始化A、B、k、R、Q、state
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);
/*
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;]
*/
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_;
/*
b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
*/
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 = lat_based_lqr_controller_conf_.matrix_q_size();
int reverse_q_param_size =
lat_based_lqr_controller_conf_.reverse_matrix_q_size();
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);
}
matrix_q_updated_ = matrix_q_;
InitializeFilters();
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();
}
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;
//FLAGS_use_navigation_mode为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);
});
}
}
//轨迹分析器初始化,将参考轨迹转换成TrajectoryAnalyzer类
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;
}
//矩阵A、B赋值，matrix_a_coeff_为矩阵A中与速度有关的项，bd为B*t
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_;
//判断是否是倒车模式
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, ...]
UpdateState(debug, chassis);
//前瞻预瞄法enable_look_ahead_back_control_
// 横向误差、航向角误差由预瞄点和车辆位置计算
// 横向误差变化率、航向角误差变化率由轨迹参考点和车辆位置计算
UpdateMatrix();
// Compound discrete matrix with road preview model
UpdateMatrixCompound();
// Adjust matrix_q_updated when in reverse gear
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) {
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) {
matrix_q_(i, i) = lat_based_lqr_controller_conf_.matrix_q(i);
}
}
// Add gain scheduler for higher speed steering
//FLAGS_enable_gain_scheduler：false：Enable gain scheduler for higher vehicle speed
//（0，0）为lat_err，（2，2）为heading_err
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为1*4，state为4*1
// feedback = - K * state
// Convert vehicle steer angle from rad to degree and then to steer degree
// then to 100% ratio
//LQR控制器反馈结果：steer_angle_feedback
const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /
M_PI * steer_ratio_ /
steer_single_direction_max_degree_ * 100;
//参考路径前馈结果：steer_angle_feedforward
const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());
double steer_angle = 0.0;
double steer_angle_feedback_augment = 0.0;
// Augment the feedback control on lateral error at the desired speed domain
//超前-滞后补偿器计算结果：steer_angle_feedback_augment
//enable_leadlag_ = false;
if (enable_leadlag_) {
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
// 转角位置限制：转角命令不能超过极限位置
// 转角变化率限制：转角的变化率不能超过转角变化的最大值
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;
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
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;
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
// 转角滤波处理：转角命令进行低通滤波处理，平滑输出命令
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
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
输出控制指令至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->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();
}
pid controller
control-controller-lon-based-pid-controller 插件包是基于 PID 控制器进行车辆纵向控制计算的控制器实现，车辆纵向控制是在 Frenet 坐标系下，沿着道路参考线切线的方向，控制车辆的位置、速度、加速度按照规划轨迹线的参考位置、参考速度行驶。车辆纵向控制的算法有很多种，本插件是设计一种基于PID控制器的控制方法。
主要的计算逻辑在 ComputeControlCommand 函数；
输入是规划轨迹线（apollo::planning::ADCTrajectory），车体位置信息（apollo::localization::LocalizationEstimate），车体姿态信息（VehicleStateProvider），车体底盘信息（apollo::canbus::Chassis）等；
通过求解纵向误差（位置误差，速度误差）、期望的车辆加速度，加上两级的PID控制器的控制量，可以选择是否需要增加坡度加速度补偿；
纵向误差计算
纵向误差计算在 ComputeLongitudinalErrors() 方法中，核心思想是通过绝对时间的查询方法找到规划轨迹线上的点作为控制参考的目标点，然后通过最优距离计算方法（apollo::control::TrajectoryAnalyzer::QueryMatchedPathPoint）找到当前车辆位置在轨迹线上的最佳匹配点（规划线上的点matched_point），将车辆当前位置的坐标（VRF坐标，车体坐标系），投影到最佳匹配轨迹点坐标系（Frenet），求出当前车辆位置投影到S轴（Frenet纵向方向）的距离（PathPoint.s)（这个距离是相对于规划起点定义的），这样纵向误差的计算就在相同的规划轨迹线的坐标系下进行计算，即实际的纵向位置（Frenet:S轴）误差=规划轨迹线的参考点纵向距离（TrajectoryPoint.s）-当前车辆投影在轨迹线上的匹配点纵向距离（s_matched.s），轨迹线参考坐标的处理参考 apollo::control::TrajectoryAnalyzer::ToTrajectoryFrame 方法（modules/control/control_component/control_task_base/common/trajectory_analyzer.cc）。
PID控制器
PID控制器全称是比例-积分-微分控制器（Proportional–Integral–Derivative controller），方法定义在 modules/control/control_component/control_task_base/common/pid_controller.cc ，PIDController的方法有Init（初始化），SetPID（设置p,i,d参数），Reset（重置），Reset_integral（积分量重置），Control（PID计算）。PID是根据误差的大小，误差的积分，误差的微分求和，作用至控制系统，达到消除误差的目标。 PID 有比例参数 kp ，积分参数 ki ，微分参数 kd ，不同的参数调节作用和效果不同，调试时可以先调kp，从小到大逐渐增加，在适当增加ki消除稳态误差，有必要时增加kd进行超前条件，提升作用系统的调节速度。先调速度环 speed_pid ，再适当增加位置环 station_pid 。
在初始化函数中：
设置了双环pid的初始化，参数载入，俯仰角滤波，和标定表载入；
位置pid
输入：位置偏差 = 规划位置 - 实际位置
输出：速度补偿
速度pid
输入：速度偏差 = 速度误差（规划速度 - 实际速度） + 速度补偿
输出：加速度补偿
station_pid_controller_.Init(
lon_based_pidcontroller_conf_.station_pid_conf());
speed_pid_controller_.Init(
lon_based_pidcontroller_conf_.low_speed_pid_conf());
vehicle_param_.CopyFrom(
common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
SetDigitalFilterPitchAngle();
InitControlCalibrationTable();
ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,
debug);
double station_error_limit =
lon_based_pidcontroller_conf_.station_error_limit();
double station_error_limited = 0.0;
if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {
station_error_limited =
common::math::Clamp(debug->preview_station_error(),
-station_error_limit, station_error_limit);
} else {
station_error_limited = common::math::Clamp(
debug->station_error(), -station_error_limit, station_error_limit);
}
难点在于匹配点、预瞄点计算；
void LonController::ComputeLongitudinalErrors(
const TrajectoryAnalyzer *trajectory_analyzer, const double preview_time,
const double ts, SimpleLongitudinalDebug *debug) {
// the decomposed vehicle motion onto Frenet frame
// s: longitudinal accumulated distance along reference trajectory
// s_dot: longitudinal velocity along reference trajectory
// d: lateral distance w.r.t. reference trajectory
// d_dot: lateral distance change rate, i.e. dd/dt
double s_matched = 0.0;
double s_dot_matched = 0.0;
double d_matched = 0.0;
double d_dot_matched = 0.0;
auto vehicle_state = injector_->vehicle_state();
auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
vehicle_state->x(), vehicle_state->y());
trajectory_analyzer->ToTrajectoryFrame(
vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
vehicle_state->linear_velocity(), matched_point, &s_matched,
&s_dot_matched, &d_matched, &d_dot_matched);
// double current_control_time = Time::Now().ToSecond();
double current_control_time = ::apollo::cyber::Clock::NowInSeconds();
double preview_control_time = current_control_time + preview_time;
TrajectoryPoint reference_point =
trajectory_analyzer->QueryNearestPointByAbsoluteTime(
current_control_time);
TrajectoryPoint preview_point =
trajectory_analyzer->QueryNearestPointByAbsoluteTime(
preview_control_time);
期望加速度 = 加速度补偿 + 预瞄点参考加速度 + 坡度加速度补偿
车辆有加速度控制和油门/刹车百分比控制两种方式，所以最终的输出还需要根据实际的需要进行转换，之前canbus模块也提到过；
加速度控制：限幅滤波后即可输出；
油门/刹车百分比控制：根据标定表进行匹配计算后发出；
control-controller-mpc-controller
MPC 的基本原理可以分为三个步骤：预测模型、滚动优化、反馈校正；
基于 MPC 控制算法进行车辆横纵向联合控制的控制器实现，通过该算法求解，可以同时输出控制车辆的方向盘转角、油门、刹车指令等。
首先是模型建立：
状态向量： 横向误差， 航向角误差、 横向误差变化速率、 航向角误差变化率， 纵向位置误差，纵向速度误差；
控制量：车辆转角， 加速度控制量；
滚动优化：
1、线性化；
2、离散化；
3、
扫一扫
3587
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
