创建LonController继承自父类Controller
用到的类：PIDController、LeadlagController
成员函数：
public:
LonController() //尝试打开log文件，并输出表头到log文件
virtual ~LonController() //关闭log文件
Init() //加载conf参数(ts,enable_leadlag,station_pid_conf,low_speed_conf),初始化位置跟踪pid和速度跟踪pid
ComputeControlCommand() //根据当前车辆状态和目标轨迹计算刹车/油门值，输入为自车定位信息，自车底盘车辆状态（速度/加速度），规划输出的轨迹，看起来定位信息好像是没有用到，底盘信息用到了speed_mps()，规划信息用到了header().sequence_num,gear(判断是否是倒挡)，trajectory_type,trajectory_point_size,trajectory_point,
Reset() //speed和station的pid进行初始化
Stop() //关闭log文件
Name() //return name_;
protected:
ComputeLongitudinalErrors()
GetPathRemain()
private:
SetDigitalFilterPitchAngle()
LoadControlCalibrationTable()
SetDigitalFilter()
CloseLogFile()
成员变量：
const localization::LocalizationEstimate *localization_
const canbus::Chassis *chassis_
std::unique_ptr< Interpolation2D> control_interpolation_
const planning::ADCTrajectory *trajectory_message_
std::unique_ptr< TrajectoryAnalyzer> trajectory_analyzer_
std::string name_
bool controller_initialized_
double previous_acceleration_
double previous_acceleration_reference_
PIDController speed_pid_controller_
PIDController station_pid_controller_
LeadlagController speed_leadlag_controller_
LeadlagController station_leadlag_controller_
FILE *speed_log_file_
common::DigitalFilter digital_filter_pitch_angle_
const ControlConf *control_conf_ = nullptr
common::VehicleParam vehicle_param_
std::shared_ptr< DependencyInjector> injector_
QueryMatchedPathPoint()
//trajectory_analyzer.cc文件中的函数，查询匹配的路径点
//查询原理：输入是本车当前的x和y位置，
PathPoint TrajectoryAnalyzer::QueryMatchedPathPoint(const double x,
const double y) const {
CHECK_GT(trajectory_points_.size(), 0U);
double d_min = PointDistanceSquare(trajectory_points_.front(), x, y); //初始化路径上的点和本车位置之间的距离，为路径点第一个点和本车之间的直线距离的平方
size_t index_min = 0;
for (size_t i = 1; i < trajectory_points_.size(); ++i) { //计算所有路径点到本车之间的距离，找到离本车最近的路径点。
double d_temp = PointDistanceSquare(trajectory_points_[i], x, y);
if (d_temp < d_min) {
d_min = d_temp;
index_min = i;
}
}
//下边的操作没看懂？这处上面的代码不就能找到最近点了吗，下边的代码有什么作用？？？
size_t index_start = index_min == 0 ? index_min : index_min - 1;
size_t index_end =
index_min + 1 == trajectory_points_.size() ? index_min : index_min + 1;
const double kEpsilon = 0.001;
if (index_start == index_end ||
std::fabs(trajectory_points_[index_start].path_point().s() -
trajectory_points_[index_end].path_point().s()) <= kEpsilon) {
return TrajectoryPointToPathPoint(trajectory_points_[index_start]);
}
return FindMinDistancePoint(trajectory_points_[index_start],
trajectory_points_[index_end], x, y);
}
ToTrajectoryFrame()：参考《Optimal trajectory generation for dynamic street scenarios in a Frenét 》文中的方法，通过局部笛卡尔坐标系中的x,y,theta,v计算出对应的SL坐标系下的s,s’,d,d’。如果做规划时不考虑用Frenet坐标，则可以不考虑这部分的内容。
// reference: Optimal trajectory generation for dynamic street scenarios in a
// Frenét Frame,
// Moritz Werling, Julius Ziegler, Sören Kammel and Sebastian Thrun, ICRA 2010
// similar to the method in this paper without the assumption the "normal"
// vector
// (from vehicle position to ref_point position) and reference heading are
// perpendicular.
void TrajectoryAnalyzer::ToTrajectoryFrame(const double x, const double y,
const double theta, const double v,
const PathPoint &ref_point,
double *ptr_s, double *ptr_s_dot,
double *ptr_d,
double *ptr_d_dot) const {
double dx = x - ref_point.x();
double dy = y - ref_point.y();
double cos_ref_theta = std::cos(ref_point.theta());
double sin_ref_theta = std::sin(ref_point.theta());
// the sin of diff angle between vector (cos_ref_theta, sin_ref_theta) and
// (dx, dy)
double cross_rd_nd = cos_ref_theta * dy - sin_ref_theta * dx;
*ptr_d = cross_rd_nd;
// the cos of diff angle between vector (cos_ref_theta, sin_ref_theta) and
// (dx, dy)
double dot_rd_nd = dx * cos_ref_theta + dy * sin_ref_theta;
*ptr_s = ref_point.s() + dot_rd_nd;
double delta_theta = theta - ref_point.theta();
double cos_delta_theta = std::cos(delta_theta);
double sin_delta_theta = std::sin(delta_theta);
*ptr_d_dot = v * sin_delta_theta;
double one_minus_kappa_r_d = 1 - ref_point.kappa() * (*ptr_d);
if (one_minus_kappa_r_d <= 0.0) {
AERROR << "TrajectoryAnalyzer::ToTrajectoryFrame "
"found fatal reference and actual difference. "
"Control output might be unstable:"
<< " ref_point.kappa:" << ref_point.kappa()
<< " ref_point.x:" << ref_point.x()
<< " ref_point.y:" << ref_point.y() << " car x:" << x
<< " car y:" << y << " *ptr_d:" << *ptr_d
<< " one_minus_kappa_r_d:" << one_minus_kappa_r_d;
// currently set to a small value to avoid control crash.
one_minus_kappa_r_d = 0.01;
}
*ptr_s_dot = v * cos_delta_theta / one_minus_kappa_r_d;
}
QueryNestPointByAbsoluteTime()和QueryNearestPointByRelativeTime():
计算上次规划模块发出规划路径和目前control计算之间的相对时间delta_t，和规划路径的相对时间进行比对，当规划路径的相对时间>=delta_t时，选择那个点作为跟踪点，这就要求规划模块输出规划路径的时候还要输出每个点和起始点之间的相对时间
TrajectoryPoint TrajectoryAnalyzer::QueryNearestPointByAbsoluteTime(
const double t) const {
return QueryNearestPointByRelativeTime(t - header_time_);
}
TrajectoryPoint TrajectoryAnalyzer::QueryNearestPointByRelativeTime(
const double t) const {
auto func_comp = [](const TrajectoryPoint &point,
const double relative_time) {
return point.relative_time() < relative_time;
};
auto it_low = std::lower_bound(trajectory_points_.begin(),
trajectory_points_.end(), t, func_comp);
//若查找出的是轨迹第一个点的话返回第一个元素，如果是轨迹最后一个点的话返回最后一个元素，如果是轨迹中间点的话，看看离当前查找到的点和前一个点哪个近，哪个近就跟踪哪个点
if (it_low == trajectory_points_.begin()) {
return trajectory_points_.front();
}
if (it_low == trajectory_points_.end()) {
return trajectory_points_.back();
}
if (FLAGS_query_forward_time_point_only) {
return *it_low;
} else {
auto it_lower = it_low - 1;
if (it_low->relative_time() - t < t - it_lower->relative_time()) {
return *it_low;
}
return *it_lower;
}
}
math_utils.cc --> NormalizeAngle()：把角度正则化到[-pi,pi]范围内
std::fmod() 浮点数取模/余
编程语言中负数取余的问题
double NormalizeAngle(const double angle) {
double a = std::fmod(angle + M_PI, 2.0 * M_PI);
if (a < 0.0) { //c++和java取余数好像是余数可以为负数，python取余数好像是不可以
a += (2.0 * M_PI);
}
return a - M_PI;
}
ComputeLongitudinalErrors()：计算调试时看的误差。
kappa好像是曲率的意思
//计算纵向误差,分解后的车辆运动到Frenet框架上
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( //把局部笛卡尔坐标系的本车定位状态映射到SL坐标系的坐标s,s',d,d'。
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 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);
debug->mutable_current_matched_point()->mutable_path_point()->set_x(
matched_point.x());
debug->mutable_current_matched_point()->mutable_path_point()->set_y(
matched_point.y());
debug->mutable_current_reference_point()->mutable_path_point()->set_x(
reference_point.path_point().x());
debug->mutable_current_reference_point()->mutable_path_point()->set_y(
reference_point.path_point().y());
debug->mutable_preview_reference_point()->mutable_path_point()->set_x(
preview_point.path_point().x());
debug->mutable_preview_reference_point()->mutable_path_point()->set_y(
preview_point.path_point().y());
ADEBUG << "matched point:" << matched_point.DebugString();
ADEBUG << "reference point:" << reference_point.DebugString();
ADEBUG << "preview point:" << preview_point.DebugString();
//把角度正则化到[-pi,pi]范围内后再进行sin,cos的计算，正则角度范围有什么好处呢？不正则难道就不能用sin,cos计算了？
double heading_error = common::math::NormalizeAngle(vehicle_state->heading() -
matched_point.theta());
double lon_speed = vehicle_state->linear_velocity() * std::cos(heading_error); //纵向速度分量
double lon_acceleration =
vehicle_state->linear_acceleration() * std::cos(heading_error); //纵向加速度分量
double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
vehicle_state->linear_velocity() *
std::sin(heading_error); //没看明白算的是什么？
debug->set_station_reference(reference_point.path_point().s());
debug->set_current_station(s_matched);
debug->set_station_error(reference_point.path_point().s() - s_matched);
debug->set_speed_reference(reference_point.v());
debug->set_current_speed(lon_speed);
debug->set_speed_error(reference_point.v() - s_dot_matched);
debug->set_acceleration_reference(reference_point.a());
debug->set_current_acceleration(lon_acceleration);
debug->set_acceleration_error(reference_point.a() -
lon_acceleration / one_minus_kappa_lat_error);
double jerk_reference = //计算jerk应该也用不到控制里，可能也就是作为调试信号看结果吧。
(debug->acceleration_reference() - previous_acceleration_reference_) / ts;
double lon_jerk =
(debug->current_acceleration() - previous_acceleration_) / ts;
debug->set_jerk_reference(jerk_reference);
debug->set_current_jerk(lon_jerk);
debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
previous_acceleration_reference_ = debug->acceleration_reference();
previous_acceleration_ = debug->current_acceleration();
debug->set_preview_station_error(preview_point.path_point().s() - s_matched);
debug->set_preview_speed_error(preview_point.v() - s_dot_matched);
debug->set_preview_speed_reference(preview_point.v());
debug->set_preview_acceleration_reference(preview_point.a());
}
leadlag__controller --> LeadlagController::Control
double LeadlagController::Control(const double error, const double dt) {
// check if the c2d transform passed during the initilization
//检查c2d转换是否在初始化期间通过
if (!transfromc2d_enabled_) {
TransformC2d(dt);
if (!transfromc2d_enabled_) {
AWARN << "C2d transform failed; will work as a unity compensator, dt: "
<< dt; //C2d转换失败，将用作统一补偿器
return error; // treat the Lead/Lag as a unity proportional controller
} //将超前/滞后视为统一比例控制器
}
// check if the current sampling time is valid，检查当前采样时间是否有效
if (dt <= 0.0) {
AWARN << "dt <= 0, will use the last output, dt: " << dt; //dt<=0,将使用最后一个输出
return previous_output_;
}
double output = 0.0;
innerstate_ = (error - previous_innerstate_ * kd0_) / kd1_; // calculate
// the inner (intermedia) state under the Direct form II for the Lead / Lag
// compensator factorization，计算超前/滞后补偿器分解的直接形式②下的内部（中间）状态
if (innerstate_ > innerstate_saturation_high_) {
innerstate_ = innerstate_saturation_high_;
innerstate_saturation_status_ = 1;
} else if (innerstate_ < innerstate_saturation_low_) {
innerstate_ = innerstate_saturation_low_;
innerstate_saturation_status_ = -1;
} else {
innerstate_saturation_status_ = 0;
}
output = innerstate_ * kn1_ + previous_innerstate_ * kn0_;
previous_innerstate_ = innerstate_;
previous_output_ = output;
return output;
}
ComputeControlCommand()：计算控制指令，
Status LonController::ComputeControlCommand(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
control::ControlCommand *cmd) {
localization_ = localization; //传入定位、底盘和路径规划信息
chassis_ = chassis;
trajectory_message_ = planning_published_trajectory;
if (!control_interpolation_) {
AERROR << "Fail to initialize calibration table.";
return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
"Fail to initialize calibration table.");
}
//做一些上层传入的信息的校验判断，防止上层修改代码，不符合控制要求导致的系统问题
if (trajectory_analyzer_ == nullptr ||
trajectory_analyzer_->seq_num() !=
trajectory_message_->header().sequence_num()) {
trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
}
const LonControllerConf &lon_controller_conf =
control_conf_->lon_controller_conf();
auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
debug->Clear();
double brake_cmd = 0.0;
double throttle_cmd = 0.0;
double ts = lon_controller_conf.ts();
double preview_time = lon_controller_conf.preview_window() * ts; //通过配置文件中的窗口数（间隔点个数）计算预瞄时间
bool enable_leadlag = //是否要启动超前滞后控制器，预瞄和超/滞后控制器有什么区别呢，感觉跟差不多似的
lon_controller_conf.enable_reverse_leadlag_compensation();
if (preview_time < 0.0) {
const auto error_msg =
absl::StrCat("Preview time set as: ", preview_time, " less than 0");
AERROR << error_msg;
return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
}
ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,
debug);
double station_error_limit = lon_controller_conf.station_error_limit();
double station_error_limited = 0.0;
if (FLAGS_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);
}
//前进档和后退档的控制参数也不相同。
if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {
station_pid_controller_.SetPID(
lon_controller_conf.reverse_station_pid_conf());
speed_pid_controller_.SetPID(lon_controller_conf.reverse_speed_pid_conf());
if (enable_leadlag) {
station_leadlag_controller_.SetLeadlag(
lon_controller_conf.reverse_station_leadlag_conf());
speed_leadlag_controller_.SetLeadlag(
lon_controller_conf.reverse_speed_leadlag_conf());
}
} else if (injector_->vehicle_state()->linear_velocity() <=
lon_controller_conf.switch_speed()) {
speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
} else {
speed_pid_controller_.SetPID(lon_controller_conf.high_speed_pid_conf());
}
double speed_offset =
station_pid_controller_.Control(station_error_limited, ts);
if (enable_leadlag) { //为什么通过位置PID算出来速度补偿之后，再把速度补偿放到超前/滞后补偿器中呢？难道超前/滞后补偿器所做的工作是类似对输入进行一定的放大缩小后输出？而且感觉好滤波器有一点点神似，看的不太明白
speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
}
double speed_controller_input = 0.0;
double speed_controller_input_limit =
lon_controller_conf.speed_controller_input_limit();
double speed_controller_input_limited = 0.0;
if (FLAGS_enable_speed_station_preview) {
speed_controller_input = speed_offset + debug->preview_speed_error();
} else {
speed_controller_input = speed_offset + debug->speed_error();
}
speed_controller_input_limited =
common::math::Clamp(speed_controller_input, -speed_controller_input_limit,
speed_controller_input_limit);
double acceleration_cmd_closeloop = 0.0;
//把速度误差和位置误差算出的速度补偿叠加一起，放到speed_pid_controller_计算期望加速度
acceleration_cmd_closeloop =
speed_pid_controller_.Control(speed_controller_input_limited, ts);
debug->set_pid_saturation_status(
speed_pid_controller_.IntegratorSaturationStatus());
if (enable_leadlag) {
acceleration_cmd_closeloop =
speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
debug->set_leadlag_saturation_status(
speed_leadlag_controller_.InnerstateSaturationStatus());
}
//获取IMU的俯仰角，转换成对应的纵向加速度，a_x = g * sin(pitch)
double slope_offset_compensation = digital_filter_pitch_angle_.Filter(
GRA_ACC * std::sin(injector_->vehicle_state()->pitch()));
if (std::isnan(slope_offset_compensation)) {
slope_offset_compensation = 0;
}
debug->set_slope_offset_compensation(slope_offset_compensation);
// apollo规划的时候把加速度也规划出来了，不知道真的是有没有用，他规划加速度的原理是什么？考虑了跟随目标的加速度了？或者是速度曲线的求导？这块可以以后好好研究一下。
double acceleration_cmd =
acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
FLAGS_enable_slope_offset * debug->slope_offset_compensation();
debug->set_is_full_stop(false);
GetPathRemain(debug);
// At near-stop stage, replace the brake control command with the standstill
// acceleration if the former is even softer than the latter
//在接近停止阶段，如果前者比后者更软，则将制动控制命令替换为静止加速。
//当满足一定条件时，加速度将发固定值，让车辆平稳减速到停止。就是debug这里面的数据真的是信息太多了，得好好看看，列一个表吧
if ((trajectory_message_->trajectory_type() ==
apollo::planning::ADCTrajectory::NORMAL) &&
((std::fabs(debug->preview_acceleration_reference()) <=
control_conf_->max_acceleration_when_stopped() &&
std::fabs(debug->preview_speed_reference()) <=
vehicle_param_.max_abs_speed_when_stopped()) ||
std::abs(debug->path_remain()) <
control_conf_->max_path_remain_when_stopped())) {
acceleration_cmd =
(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
? std::max(acceleration_cmd,
-lon_controller_conf.standstill_acceleration())
: std::min(acceleration_cmd,
lon_controller_conf.standstill_acceleration());
ADEBUG << "Stop location reached";
debug->set_is_full_stop(true);
}
double throttle_lowerbound =
std::max(vehicle_param_.throttle_deadzone(),
lon_controller_conf.throttle_minimum_action());
double brake_lowerbound =
std::max(vehicle_param_.brake_deadzone(),
lon_controller_conf.brake_minimum_action());
double calibration_value = 0.0;
double acceleration_lookup =
(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
? -acceleration_cmd
: acceleration_cmd;
if (FLAGS_use_preview_speed_for_table) {
calibration_value = control_interpolation_->Interpolate(
std::make_pair(debug->preview_speed_reference(), acceleration_lookup));
} else {
calibration_value = control_interpolation_->Interpolate(
std::make_pair(chassis_->speed_mps(), acceleration_lookup));
}
if (acceleration_lookup >= 0) {
if (calibration_value >= 0) {
throttle_cmd = std::max(calibration_value, throttle_lowerbound);
} else {
throttle_cmd = throttle_lowerbound;
}
brake_cmd = 0.0;
} else {
throttle_cmd = 0.0;
if (calibration_value >= 0) {
brake_cmd = brake_lowerbound;
} else {
brake_cmd = std::max(-calibration_value, brake_lowerbound);
}
}
debug->set_station_error_limited(station_error_limited);
debug->set_speed_offset(speed_offset);
debug->set_speed_controller_input_limited(speed_controller_input_limited);
debug->set_acceleration_cmd(acceleration_cmd);
debug->set_throttle_cmd(throttle_cmd);
debug->set_brake_cmd(brake_cmd);
debug->set_acceleration_lookup(acceleration_lookup);
debug->set_speed_lookup(chassis_->speed_mps());
debug->set_calibration_value(calibration_value);
debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);
if (FLAGS_enable_csv_debug && speed_log_file_ != nullptr) {
fprintf(speed_log_file_,
"%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f,"
"%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d,\r\n",
debug->station_reference(), debug->station_error(),
station_error_limited, debug->preview_station_error(),
debug->speed_reference(), debug->speed_error(),
speed_controller_input_limited, debug->preview_speed_reference(),
debug->preview_speed_error(),
debug->preview_acceleration_reference(), acceleration_cmd_closeloop,
acceleration_cmd, debug->acceleration_lookup(),
debug->speed_lookup(), calibration_value, throttle_cmd, brake_cmd,
debug->is_full_stop());
}
// if the car is driven by acceleration, disgard the cmd->throttle and brake
cmd->set_throttle(throttle_cmd);
cmd->set_brake(brake_cmd);
cmd->set_acceleration(acceleration_cmd);
if (std::fabs(injector_->vehicle_state()->linear_velocity()) <=
vehicle_param_.max_abs_speed_when_stopped() ||
chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
cmd->set_gear_location(trajectory_message_->gear());
} else {
cmd->set_gear_location(chassis->gear_location());
}
return Status::OK();
}
lon_controller.h
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file
* @brief Defines the LonController class.
*/
#pragma once
#include <memory>
#include <string>
#include <vector>
#include "modules/common/configs/proto/vehicle_config.pb.h"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/filters/digital_filter_coefficients.h"
#include "modules/control/common/interpolation_2d.h"
#include "modules/control/common/leadlag_controller.h"
#include "modules/control/common/pid_controller.h"
#include "modules/control/common/trajectory_analyzer.h"
#include "modules/control/controller/controller.h"
/**
* @namespace apollo::control
* @brief apollo::control
*/
namespace apollo {
namespace control {
/**
* @class LonController
*
* @brief Longitudinal controller, to compute brake / throttle values.
*/
class LonController : public Controller {
public:
/**
* @brief constructor
*/
LonController();
/**
* @brief destructor
*/
virtual ~LonController();
/**
* @brief initialize Longitudinal Controller
* @param control_conf control configurations
* @return Status initialization status
*/
common::Status Init(std::shared_ptr<DependencyInjector> injector,
const ControlConf *control_conf) override;
/**
* @brief compute brake / throttle values based on current vehicle status
* and target trajectory
* @param localization vehicle location
* @param chassis vehicle status e.g., speed, acceleration
* @param trajectory trajectory generated by planning
* @param cmd control command
* @return Status computation status
*/
common::Status ComputeControlCommand(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
control::ControlCommand *cmd) override;
/**
* @brief reset longitudinal controller
* @return Status reset status
*/
common::Status Reset() override;
/**
* @brief stop longitudinal controller
*/
void Stop() override;
/**
* @brief longitudinal controller name
* @return string controller name in string
*/
std::string Name() const override;
protected:
void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,
const double preview_time, const double ts,
SimpleLongitudinalDebug *debug);
void GetPathRemain(SimpleLongitudinalDebug *debug);
private:
void SetDigitalFilterPitchAngle(const LonControllerConf &lon_controller_conf);
void LoadControlCalibrationTable(
const LonControllerConf &lon_controller_conf);
void SetDigitalFilter(double ts, double cutoff_freq,
common::DigitalFilter *digital_filter);
void CloseLogFile();
const localization::LocalizationEstimate *localization_ = nullptr;
const canbus::Chassis *chassis_ = nullptr;
std::unique_ptr<Interpolation2D> control_interpolation_;
const planning::ADCTrajectory *trajectory_message_ = nullptr;
std::unique_ptr<TrajectoryAnalyzer> trajectory_analyzer_;
std::string name_;
bool controller_initialized_ = false;
double previous_acceleration_ = 0.0;
double previous_acceleration_reference_ = 0.0;
PIDController speed_pid_controller_;
PIDController station_pid_controller_;
LeadlagController speed_leadlag_controller_;
LeadlagController station_leadlag_controller_;
FILE *speed_log_file_ = nullptr;
common::DigitalFilter digital_filter_pitch_angle_;
const ControlConf *control_conf_ = nullptr;
// vehicle parameter
common::VehicleParam vehicle_param_;
std::shared_ptr<DependencyInjector> injector_;
};
} // namespace control
} // namespace apollo
lon_controller.cc
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/control/controller/lon_controller.h"
#include <algorithm>
#include <utility>
#include "absl/strings/str_cat.h"
#include "cyber/common/log.h"
#include "cyber/time/time.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/control/common/control_gflags.h"
#include "modules/localization/common/localization_gflags.h"
namespace apollo {
namespace control {
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
using apollo::cyber::Time;
constexpr double GRA_ACC = 9.8;
LonController::LonController() //构造函数，其：代表给name_赋值，不过后面的Contr...找不到在哪，是个啥东西，应该是个返回字符串的函数吧。
: name_(ControlConf_ControllerType_Name(ControlConf::LON_CONTROLLER)) {
if (FLAGS_enable_csv_debug) { //FLAGS_enable_csv_debug这个变量直接搜是找不到的，好像是用的google的gflags工具(支持从环境变量和配置文件中读取参数)，此工具的介绍在本文末尾参考链接,在control_gflags.h文件中去除"FLAGS_"直接搜"enable_csv_enable"就可以搜到该变量
time_t rawtime;
char name_buffer[80];
std::time(&rawtime);
std::tm time_tm;
localtime_r(&rawtime, &time_tm);
strftime(name_buffer, 80, "/tmp/speed_log__%F_%H%M%S.csv", &time_tm);
speed_log_file_ = fopen(name_buffer, "w");
if (speed_log_file_ == nullptr) {
AERROR << "Fail to open file:" << name_buffer;
FLAGS_enable_csv_debug = false;
}
if (speed_log_file_ != nullptr) {
fprintf(speed_log_file_,
"station_reference,"
"station_error,"
"station_error_limited,"
"preview_station_error,"
"speed_reference,"
"speed_error,"
"speed_error_limited,"
"preview_speed_reference,"
"preview_speed_error,"
"preview_acceleration_reference,"
"acceleration_cmd_closeloop,"
"acceleration_cmd,"
"acceleration_lookup,"
"speed_lookup,"
"calibration_value,"
"throttle_cmd,"
"brake_cmd,"
"is_full_stop,"
"\r\n");
fflush(speed_log_file_); //fflush()的作用是用来刷新缓冲区
}
AINFO << name_ << " used.";
}
}
void LonController::CloseLogFile() {
if (FLAGS_enable_csv_debug) {
if (speed_log_file_ != nullptr) {
fclose(speed_log_file_);
speed_log_file_ = nullptr;
}
}
}
void LonController::Stop() { CloseLogFile(); }
LonController::~LonController() { CloseLogFile(); }
Status LonController::Init(std::shared_ptr<DependencyInjector> injector,
const ControlConf *control_conf) {
control_conf_ = control_conf;
if (control_conf_ == nullptr) {
controller_initialized_ = false;
AERROR << "get_longitudinal_param() nullptr";
return Status(ErrorCode::CONTROL_INIT_ERROR,
"Failed to load LonController conf");
}
injector_ = injector;
const LonControllerConf &lon_controller_conf =
control_conf_->lon_controller_conf();
double ts = lon_controller_conf.ts();
bool enable_leadlag =
lon_controller_conf.enable_reverse_leadlag_compensation();
station_pid_controller_.Init(lon_controller_conf.station_pid_conf());
speed_pid_controller_.Init(lon_controller_conf.low_speed_pid_conf());
if (enable_leadlag) {
station_leadlag_controller_.Init(
lon_controller_conf.reverse_station_leadlag_conf(), ts);
speed_leadlag_controller_.Init(
lon_controller_conf.reverse_speed_leadlag_conf(), ts);
}
vehicle_param_.CopyFrom(
common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());
SetDigitalFilterPitchAngle(lon_controller_conf);
LoadControlCalibrationTable(lon_controller_conf);
controller_initialized_ = true;
return Status::OK();
}
void LonController::SetDigitalFilterPitchAngle(
const LonControllerConf &lon_controller_conf) {
double cutoff_freq =
lon_controller_conf.pitch_angle_filter_conf().cutoff_freq();
double ts = lon_controller_conf.ts();
SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
}
void LonController::LoadControlCalibrationTable(
const LonControllerConf &lon_controller_conf) {
const auto &control_table = lon_controller_conf.calibration_table();
AINFO << "Control calibration table loaded";
AINFO << "Control calibration table size is "
<< control_table.calibration_size();
Interpolation2D::DataType xyz;
for (const auto &calibration : control_table.calibration()) {
xyz.push_back(std::make_tuple(calibration.speed(),
calibration.acceleration(),
calibration.command()));
}
control_interpolation_.reset(new Interpolation2D);
ACHECK(control_interpolation_->Init(xyz))
<< "Fail to load control calibration table";
}
Status LonController::ComputeControlCommand(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
control::ControlCommand *cmd) {
localization_ = localization;
chassis_ = chassis;
trajectory_message_ = planning_published_trajectory;
if (!control_interpolation_) {
AERROR << "Fail to initialize calibration table.";
return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
"Fail to initialize calibration table.");
}
if (trajectory_analyzer_ == nullptr ||
trajectory_analyzer_->seq_num() !=
trajectory_message_->header().sequence_num()) {
trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));
}
const LonControllerConf &lon_controller_conf =
control_conf_->lon_controller_conf();
auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();
debug->Clear();
double brake_cmd = 0.0;
double throttle_cmd = 0.0;
double ts = lon_controller_conf.ts();
double preview_time = lon_controller_conf.preview_window() * ts;
bool enable_leadlag =
lon_controller_conf.enable_reverse_leadlag_compensation();
if (preview_time < 0.0) {
const auto error_msg =
absl::StrCat("Preview time set as: ", preview_time, " less than 0");
AERROR << error_msg;
return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
}
ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,
debug);
double station_error_limit = lon_controller_conf.station_error_limit();
double station_error_limited = 0.0;
if (FLAGS_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);
}
if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {
station_pid_controller_.SetPID(
lon_controller_conf.reverse_station_pid_conf());
speed_pid_controller_.SetPID(lon_controller_conf.reverse_speed_pid_conf());
if (enable_leadlag) {
station_leadlag_controller_.SetLeadlag(
lon_controller_conf.reverse_station_leadlag_conf());
speed_leadlag_controller_.SetLeadlag(
lon_controller_conf.reverse_speed_leadlag_conf());
}
} else if (injector_->vehicle_state()->linear_velocity() <=
lon_controller_conf.switch_speed()) {
speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
} else {
speed_pid_controller_.SetPID(lon_controller_conf.high_speed_pid_conf());
}
double speed_offset =
station_pid_controller_.Control(station_error_limited, ts);
if (enable_leadlag) {
speed_offset = station_leadlag_controller_.Control(speed_offset, ts);
}
double speed_controller_input = 0.0;
double speed_controller_input_limit =
lon_controller_conf.speed_controller_input_limit();
double speed_controller_input_limited = 0.0;
if (FLAGS_enable_speed_station_preview) {
speed_controller_input = speed_offset + debug->preview_speed_error();
} else {
speed_controller_input = speed_offset + debug->speed_error();
}
speed_controller_input_limited =
common::math::Clamp(speed_controller_input, -speed_controller_input_limit,
speed_controller_input_limit);
double acceleration_cmd_closeloop = 0.0;
acceleration_cmd_closeloop =
speed_pid_controller_.Control(speed_controller_input_limited, ts);
debug->set_pid_saturation_status(
speed_pid_controller_.IntegratorSaturationStatus());
if (enable_leadlag) {
acceleration_cmd_closeloop =
speed_leadlag_controller_.Control(acceleration_cmd_closeloop, ts);
debug->set_leadlag_saturation_status(
speed_leadlag_controller_.InnerstateSaturationStatus());
}
double slope_offset_compensation = digital_filter_pitch_angle_.Filter(
GRA_ACC * std::sin(injector_->vehicle_state()->pitch()));
if (std::isnan(slope_offset_compensation)) {
slope_offset_compensation = 0;
}
debug->set_slope_offset_compensation(slope_offset_compensation);
double acceleration_cmd =
acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
FLAGS_enable_slope_offset * debug->slope_offset_compensation();
debug->set_is_full_stop(false);
GetPathRemain(debug);
// At near-stop stage, replace the brake control command with the standstill
// acceleration if the former is even softer than the latter
if ((trajectory_message_->trajectory_type() ==
apollo::planning::ADCTrajectory::NORMAL) &&
((std::fabs(debug->preview_acceleration_reference()) <=
control_conf_->max_acceleration_when_stopped() &&
std::fabs(debug->preview_speed_reference()) <=
vehicle_param_.max_abs_speed_when_stopped()) ||
std::abs(debug->path_remain()) <
control_conf_->max_path_remain_when_stopped())) {
acceleration_cmd =
(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
? std::max(acceleration_cmd,
-lon_controller_conf.standstill_acceleration())
: std::min(acceleration_cmd,
lon_controller_conf.standstill_acceleration());
ADEBUG << "Stop location reached";
debug->set_is_full_stop(true);
}
double throttle_lowerbound =
std::max(vehicle_param_.throttle_deadzone(),
lon_controller_conf.throttle_minimum_action());
double brake_lowerbound =
std::max(vehicle_param_.brake_deadzone(),
lon_controller_conf.brake_minimum_action());
double calibration_value = 0.0;
double acceleration_lookup =
(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
? -acceleration_cmd
: acceleration_cmd;
if (FLAGS_use_preview_speed_for_table) {
calibration_value = control_interpolation_->Interpolate(
std::make_pair(debug->preview_speed_reference(), acceleration_lookup));
} else {
calibration_value = control_interpolation_->Interpolate(
std::make_pair(chassis_->speed_mps(), acceleration_lookup));
}
if (acceleration_lookup >= 0) {
if (calibration_value >= 0) {
throttle_cmd = std::max(calibration_value, throttle_lowerbound);
} else {
throttle_cmd = throttle_lowerbound;
}
brake_cmd = 0.0;
} else {
throttle_cmd = 0.0;
if (calibration_value >= 0) {
brake_cmd = brake_lowerbound;
} else {
brake_cmd = std::max(-calibration_value, brake_lowerbound);
}
}
debug->set_station_error_limited(station_error_limited);
debug->set_speed_offset(speed_offset);
debug->set_speed_controller_input_limited(speed_controller_input_limited);
debug->set_acceleration_cmd(acceleration_cmd);
debug->set_throttle_cmd(throttle_cmd);
debug->set_brake_cmd(brake_cmd);
debug->set_acceleration_lookup(acceleration_lookup);
debug->set_speed_lookup(chassis_->speed_mps());
debug->set_calibration_value(calibration_value);
debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);
if (FLAGS_enable_csv_debug && speed_log_file_ != nullptr) {
fprintf(speed_log_file_,
"%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f,"
"%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d,\r\n",
debug->station_reference(), debug->station_error(),
station_error_limited, debug->preview_station_error(),
debug->speed_reference(), debug->speed_error(),
speed_controller_input_limited, debug->preview_speed_reference(),
debug->preview_speed_error(),
debug->preview_acceleration_reference(), acceleration_cmd_closeloop,
acceleration_cmd, debug->acceleration_lookup(),
debug->speed_lookup(), calibration_value, throttle_cmd, brake_cmd,
debug->is_full_stop());
}
// if the car is driven by acceleration, disgard the cmd->throttle and brake
cmd->set_throttle(throttle_cmd);
cmd->set_brake(brake_cmd);
cmd->set_acceleration(acceleration_cmd);
if (std::fabs(injector_->vehicle_state()->linear_velocity()) <=
vehicle_param_.max_abs_speed_when_stopped() ||
chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
cmd->set_gear_location(trajectory_message_->gear());
} else {
cmd->set_gear_location(chassis->gear_location());
}
return Status::OK();
}
Status LonController::Reset() {
speed_pid_controller_.Reset();
station_pid_controller_.Reset();
return Status::OK();
}
std::string LonController::Name() const { return name_; }
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 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);
debug->mutable_current_matched_point()->mutable_path_point()->set_x(
matched_point.x());
debug->mutable_current_matched_point()->mutable_path_point()->set_y(
matched_point.y());
debug->mutable_current_reference_point()->mutable_path_point()->set_x(
reference_point.path_point().x());
debug->mutable_current_reference_point()->mutable_path_point()->set_y(
reference_point.path_point().y());
debug->mutable_preview_reference_point()->mutable_path_point()->set_x(
preview_point.path_point().x());
debug->mutable_preview_reference_point()->mutable_path_point()->set_y(
preview_point.path_point().y());
ADEBUG << "matched point:" << matched_point.DebugString();
ADEBUG << "reference point:" << reference_point.DebugString();
ADEBUG << "preview point:" << preview_point.DebugString();
double heading_error = common::math::NormalizeAngle(vehicle_state->heading() -
matched_point.theta());
double lon_speed = vehicle_state->linear_velocity() * std::cos(heading_error);
double lon_acceleration =
vehicle_state->linear_acceleration() * std::cos(heading_error);
double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
vehicle_state->linear_velocity() *
std::sin(heading_error);
debug->set_station_reference(reference_point.path_point().s());
debug->set_current_station(s_matched);
debug->set_station_error(reference_point.path_point().s() - s_matched);
debug->set_speed_reference(reference_point.v());
debug->set_current_speed(lon_speed);
debug->set_speed_error(reference_point.v() - s_dot_matched);
debug->set_acceleration_reference(reference_point.a());
debug->set_current_acceleration(lon_acceleration);
debug->set_acceleration_error(reference_point.a() -
lon_acceleration / one_minus_kappa_lat_error);
double jerk_reference =
(debug->acceleration_reference() - previous_acceleration_reference_) / ts;
double lon_jerk =
(debug->current_acceleration() - previous_acceleration_) / ts;
debug->set_jerk_reference(jerk_reference);
debug->set_current_jerk(lon_jerk);
debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
previous_acceleration_reference_ = debug->acceleration_reference();
previous_acceleration_ = debug->current_acceleration();
debug->set_preview_station_error(preview_point.path_point().s() - s_matched);
debug->set_preview_speed_error(preview_point.v() - s_dot_matched);
debug->set_preview_speed_reference(preview_point.v());
debug->set_preview_acceleration_reference(preview_point.a());
}
void LonController::SetDigitalFilter(double ts, double cutoff_freq,
common::DigitalFilter *digital_filter) {
std::vector<double> denominators;
std::vector<double> numerators;
common::LpfCoefficients(ts, cutoff_freq, &denominators, &numerators);
digital_filter->set_coefficients(denominators, numerators);
}
// TODO(all): Refactor and simplify
void LonController::GetPathRemain(SimpleLongitudinalDebug *debug) {
int stop_index = 0;
static constexpr double kSpeedThreshold = 1e-3;
static constexpr double kForwardAccThreshold = -1e-2;
static constexpr double kBackwardAccThreshold = 1e-1;
static constexpr double kParkingSpeed = 0.1;
if (trajectory_message_->gear() == canbus::Chassis::GEAR_DRIVE) {
while (stop_index < trajectory_message_->trajectory_point_size()) {
auto ¤t_trajectory_point =
trajectory_message_->trajectory_point(stop_index);
if (fabs(current_trajectory_point.v()) < kSpeedThreshold &&
current_trajectory_point.a() > kForwardAccThreshold &&
current_trajectory_point.a() < 0.0) {
break;
}
++stop_index;
}
} else {
while (stop_index < trajectory_message_->trajectory_point_size()) {
auto ¤t_trajectory_point =
trajectory_message_->trajectory_point(stop_index);
if (current_trajectory_point.v() < kSpeedThreshold &&
current_trajectory_point.a() < kBackwardAccThreshold &&
current_trajectory_point.a() > 0.0) {
break;
}
++stop_index;
}
}
if (stop_index == trajectory_message_->trajectory_point_size()) {
--stop_index;
if (fabs(trajectory_message_->trajectory_point(stop_index).v()) <
kParkingSpeed) {
ADEBUG << "the last point is selected as parking point";
} else {
ADEBUG << "the last point found in path and speed > speed_deadzone";
}
}
debug->set_path_remain(
trajectory_message_->trajectory_point(stop_index).path_point().s() -
debug->current_station());
}
} // namespace control
} // namespace apollo
google gflags:gflags(google开源的一套命令行参数解析工具)和百度apollo - Apollo代码解析：3. 命令行参数传递google gflags
137
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
