Apollo9.0 PNC源码学习之Control模块（三）—

本文将对Apollo的纵向控制器进行讲解，看完本文，你将会对百度Apollo的纵向控制有更深的理解
前面文章：
Apollo9.0 PNC源码学习之Control模块（一）
Apollo9.0 PNC源码学习之Control模块（二）

1 纵向控制器简介

control-controller-lon-based-pid-controller 插件包是基于 PID 控制器进行车辆纵向控制计算的控制器实现，车辆纵向控制是在 Frenet 坐标系下，沿着道路参考线切线的方向，控制车辆的位置、速度、加速度按照规划轨迹线的参考位置、参考速度行驶

在这里插入图片描述 control-controller-lon-based-pid-controller 插件主要包含LonController的实现文件， LonController 是继承 ControlTask 的子类，实现 Init、ComputeControlCommand、Reset、Name 等方法。主要的计算逻辑在 ComputeControlCommand方法，输入是规划轨迹线（apollo::planning::ADCTrajectory），车体位置信息（apollo::localization::LocalizationEstimate），车体姿态信息（VehicleStateProvider），车体底盘信息（apollo::canbus::Chassis）等，通过求解纵向误差（位置误差，速度误差）、期望的车辆加速度，加上两级的PID控制器的控制量，可以选择是否需要增加坡度加速度补偿，如果车辆在接近规划轨迹线终点时，可以设置接近终点的加速度进行准确停车，如果车辆前方长时间停着行人，也可以配置对应的加速度作用车辆，求解完成加速度后，然后查询车辆标定表转化成车辆的控制接口如油门踏板或刹车踏板，输入给车辆，完成 1 次控制。

1.1 纵向误差计算

纵向误差计算在 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）

1.2 PID控制器

PID方法在之前文章中详细介绍过，PIDController的方法有：

Init（初始化）
SetPID（设置p,i,d参数）
Reset（重置）
Reset_integral（积分量重置）
Control（PID计算）

调试时可以先调kp，从小到大逐渐增加，在适当增加ki消除，有必要时增加kd，提升作用系统的调节速度。先调速度环 speed_pid ，再适当增加位置环 station_pid

1.3 车辆标定表

Apollo 支持的线控车辆主要的控制接口是基于油门、刹车接口，油门/刹车控制量与车辆的行驶速度是线性相关的，这个相关性车辆可能无法直接提供相应的数据或计算公式，为了通过线控接口比较准确的控制车辆速度，需要进行作用量与观测值的定量标定，获得相关性的关系。选取车辆在不同车速下，进行加速或减速直线行驶，记录当前时刻的车体纵向的加速度。如此往复，记录不同速度下，急加速，缓加速，急减速，缓减速等，形成一个标定表，如 modules/control/control_component/conf/calibration_table.pb.txt 所示，打印出如下图所示。
刹车标定表
在这里插入图片描述油门标定表

1.4 模块输入输出与配置

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

输入：

Channel名称	类型	描述
`/apollo/planning`	apollo::planning::ADCTrajectory	车辆规划轨迹线信息（轨迹点信息），control_component 订阅此消息， LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数
`/apollo/localization/pose`	apollo::localization::LocalizationEstimate	车辆定位信息（世界坐标系位置），control_component 订阅此消息，LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数
`/apollo/canbus/chassis`	apollo::canbus::Chassis	车辆底盘信息（底盘车速），control_component订阅此消息， LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数
-	apollo::common::VehicleState	车身姿态信息（车身俯仰角）
`/apollo/control`	apollo::control::ControlCommand	车辆控制指令（方向盘控制指令），control_component 订阅此消息， LonController 继承 ControlTask 基类方法 ComputeControlCommand 传入参数

输出：

Channel名称	类型	描述
`/apollo/control`	apollo::control::ControlCommand	车辆的控制指令：油门、刹车指令

配置文件：

文件路径	类型/结构	说明
`modules/control/control_component/conf/pipeline.pb.txt`	apollo::control::ControlPipeline	ControlComponent 的配置文件
`modules/control/control_component/conf/control.conf`	`command line flags`	命令行参数配置，配置全局的 flag 变量
`modules/control/controllers/lon_based_pid_controller/conf/controller_conf.pb.txt`	apollo::control::LonBasedPidControllerConf	PID 纵向控制器配置文件

Flags：

flagfile	类型	描述
`modules/control/control_component/common/control_gflags.cc`	`flags`	定义全局的 flag 变量在 LonController 使用，通过 control.conf 进行配置
`modules/control/control_component/common/control_gflags.h`	`declare`	flags 声明文件

2 纵向控制器源码解析

lon_controller.h

#pragma once#include <memory>
#include <string>
#include <vector>#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/lon_based_pid_controller/proto/lon_based_pid_controller_conf.pb.h"#include "cyber/cyber.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/control/control_component/controller_task_base/common/interpolation_2d.h"
#include "modules/control/control_component/controller_task_base/common/leadlag_controller.h"
#include "modules/control/control_component/controller_task_base/common/pid_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"
#include "modules/control/controllers/lon_based_pid_controller/util/check_pit.h"/*** @namespace apollo::control* @brief apollo::control*/
namespace apollo {
namespace control {// LonController类，纵向控制器, 来计算 brake/throttle 值
class LonController : public ControlTask {public:LonController();virtual ~LonController();// Init()函数初始化纵向控制器// 参数：injector车辆当前状态信息，将其读取到LonController类成员变量injector_里common::Status Init(std::shared_ptr<DependencyInjector> injector) 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*/// 计算 刹车/油门值基于当前车辆的状态和目标轨迹// 参数：车辆定位信息 指针localization// 参数：chassis车辆底盘反馈状态信息 指针chassis// 参数：规划发布的轨迹信息 指针trajectory// 参数：控制指令 指针cmd，实际上是计算出的控制指令往cmd里填充// 返回计算状态码common::Status ComputeControlCommand(const localization::LocalizationEstimate *localization,const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,control::ControlCommand *cmd) override;// 重置控制器common::Status Reset() override;// 停止纵向控制器void Stop() override;// override说明在基类中接口已经被声明为虚函数，调用的是派生类里的定义std::string Name() const override;protected:// 计算纵向误差函数// 参数：轨迹信息指针trajectory// 参数：预览时间preview_time// 参数：控制周期ts// 参数：调试信息指针debug用来存放计算的纵向误差信息// 该函数在control_component.cc中被调用void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,const double preview_time, const double ts,SimpleLongitudinalDebug *debug);// 根据规划发布的轨迹msg寻找轨迹点上接下来的第一个停车点// 停车点信息存到debug里，这个参数又是用来装结果的void GetPathRemain(SimpleLongitudinalDebug *debug);std::shared_ptr<DependencyInjector> injector_;private:// 设置Pitch车辆俯仰角// 参数：lon_controller_conf是纵向控制器配置类对象，该类由modules/control/proto/.proto文件生成// 从该对象中读取截至频率，控制周期等参数来设置pitch角滤波器// pitch角用来进行车辆的坡道补偿，默认坡道补偿是关闭的void SetDigitalFilterPitchAngle();// 加载控制标定表函数// 参数：lon_controller_conf是纵向控制器配置类对象，该类由modules/control/proto/.proto文件生成// 从纵向控制器配置对象中读取车速-加速度-控制百分数标定表void InitControlCalibrationTable();// 设置数字滤波器函数// 参数：控制周期ts// 参数：截至频率cutoff_freq// 参数：数字滤波器类对象digital_filter，前面两项参数就是为了设置这个对象void SetDigitalFilter(double ts, double cutoff_freq,common::DigitalFilter *digital_filter);// 到达目标是否停止bool IsStopByDestination(SimpleLongitudinalDebug *debug);// 遇到行人是否停止bool IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug);// 完全停止bool IsFullStopLongTerm(SimpleLongitudinalDebug *debug);// 设置刹车void SetParkingBrake(const LonBasedPidControllerConf *conf,control::ControlCommand *control_command);// 关闭日志文件函数void CloseLogFile();// 存放的是定位来的信息，初始化为空指针const localization::LocalizationEstimate *localization_ = nullptr;// 存放的是来自canbus的车辆状态反馈信息，初始化为空指针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_;//纵向上leadlag控制器默认关闭// 用于速度的控制LeadlagController speed_leadlag_controller_;// 用于位置的控制LeadlagController station_leadlag_controller_;// 用于存放纵向上的日志信息，默认也关闭csv日志FILE *speed_log_file_ = nullptr;// 用于对俯仰角pitch进行滤波common::DigitalFilter digital_filter_pitch_angle_;// 用于存放配置文件中加载进来的控制配置LonBasedPidControllerConf lon_based_pidcontroller_conf_;// 油门制动标定表calibration_table calibration_table_;// vehicle parameter// 用于存放车辆的实际尺寸参数common::VehicleParam vehicle_param_;bool is_stop_by_pedestrian_ = false;          // 是否被行人停止标志位bool is_stop_by_pedestrian_previous_ = false; // 上一次是否被行人停止标志位double start_time_ = 0.0;                     // 开始时间double wait_time_diff_ = 0.0;                 // 等待时间差bool is_full_stop_previous_ = false;          // 上一次是否完全停止标志位double is_full_stop_start_time_ = 0.0;        // 完全停止开始时间double is_full_stop_wait_time_diff_ = 0.0;    // 完全停止等待时间差bool epb_on_change_switch_ = true;            // EPB（紧急停车制动）开关变化标志位bool epb_off_change_switch_ = true;           int epb_change_count_ = 0;                    // EPB变化计数int smode_num_ = 0;                           // S模式数目double standstill_narmal_acceleration_ = 0.0; // 静止时的正常加速度double stop_gain_acceleration_ = 0.0;         // 停止时的加速度增益double max_path_remain_when_stopped_ = 0.0;   // 停止时的最大路径剩余bool parking_release_ = false;                // 停车释放标志位
};// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::LonController,ControlTask)}  // namespace control
}  // namespace apollo

lon_controller.cc

#include "modules/control/controllers/lon_based_pid_controller/lon_controller.h"#include <algorithm>
#include <utility>#include "absl/strings/str_cat.h"#include "cyber/common/log.h"
#include "cyber/time/clock.h"
#include "cyber/time/time.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/control/control_component/common/control_gflags.h"
// #include "modules/localization/common/localization_gflags.h"namespace apollo {
namespace control {using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleStateProvider;
using apollo::cyber::Time;
using apollo::external_command::CommandStatusType;
using apollo::planning::ADCTrajectory;
using apollo::planning::StopReasonCode;
// 定义常量重力加速度 9.8
constexpr double GRA_ACC = 9.8;LonController::LonController() : name_("PID-basesd Longitudinal Controller") {// node_.reset(new apollo::cyber::Node("lon_controller"));// 是否打开csv debug的功能，默认falseif (FLAGS_enable_csv_debug) {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,"    // 闭环加速度cmd"acceleration_cmd,"              // 加速度cmd"acceleration_lookup,"           // 加速度表现"acceleration_lookup_limit,""speed_lookup,"                  // 速度表现"calibration_value,"             // 标定表值"throttle_cmd,"                  // 油门"brake_cmd,"                     // 刹车"is_full_stop,"                  // 是否完全停止"\r\n");fflush(speed_log_file_);}AINFO << name_ << " used.";}
}
// 关闭log日志文件函数主要fclose实现，本身enable_csv_debug就没打开
void LonController::CloseLogFile() {if (FLAGS_enable_csv_debug) {if (speed_log_file_ != nullptr) {fclose(speed_log_file_);speed_log_file_ = nullptr;}}
}
// Stop()函数调用的就是上面的关闭日志文件函数
void LonController::Stop() { CloseLogFile(); }
// 上面的关闭日志文件函数
LonController::~LonController() { CloseLogFile(); }
// 纵向控制器的初始化函数
// 输入参数是DependencyInjector类对象指针injector主要是用来获取车辆状态信息的
Status LonController::Init(std::shared_ptr<DependencyInjector> injector) {// 加载纵向控制器参数if (!ControlTask::LoadConfig<LonBasedPidControllerConf>(&lon_based_pidcontroller_conf_)) {AERROR << "failed to load control conf";return Status(ErrorCode::CONTROL_INIT_ERROR,"failed to load lon control_conf");}// 加载油门制动标定表if (!ControlTask::LoadCalibrationTable(&calibration_table_)) {AERROR << "failed to load calibration table";return Status(ErrorCode::CONTROL_INIT_ERROR,"lon failed to load calibration table");}// 将传进来的参数车辆状态信息injector赋给LonController类数据成员injector_injector_ = injector;standstill_narmal_acceleration_ =-fabs(lon_based_pidcontroller_conf_.standstill_narmal_acceleration());stop_gain_acceleration_ =-fabs(lon_based_pidcontroller_conf_.stop_gain_acceleration());// 控制周期tsdouble ts = lon_based_pidcontroller_conf_.ts();// 是否打开超前滞后控制器 默认falsebool enable_leadlag =lon_based_pidcontroller_conf_.enable_reverse_leadlag_compensation();// 位置PID控制器的初始化从modules/control/conf/control_conf.pb.txt->lon_controller_conf->station_pid_conf读取station_pid_controller_.Init(lon_based_pidcontroller_conf_.station_pid_conf());// 速度控制器的初始化也是加载control_conf.pb.txt->lon_controller_conf里的相关参数speed_pid_controller_.Init(lon_based_pidcontroller_conf_.low_speed_pid_conf());//如果打开超前滞后控制器 默认关闭if (enable_leadlag) {station_leadlag_controller_.Init(lon_based_pidcontroller_conf_.reverse_station_leadlag_conf(), ts);speed_leadlag_controller_.Init(lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf(), ts);}// 车辆的参数从VehicleConfigHelper类的成员函数GetConfig()加载vehicle_param_.CopyFrom(common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param());// 从modules/control/conf/control_conf.pb.txt->lon_controller_conf里加载数字滤波器的参数// 这个滤波器是用于对pitch角(车辆的俯仰角)进行滤波，pitch角是用于对车辆控制时的坡道补偿SetDigitalFilterPitchAngle();// 从modules/control/conf/control_conf.pb.txt->lon_controller_conf里加载标定表(不同车速下，加速度到油门/制动标定表)// 加载的标定表放入control_interpolation_中InitControlCalibrationTable();// 纵向控制器被初始化的标志位置为truecontroller_initialized_ = true;return Status::OK();
}
// 设置数字滤波器俯仰角
void LonController::SetDigitalFilterPitchAngle() {// 配置中读到的参数滤波器的截止频率存到变量cutoff_freq中double cutoff_freq =lon_based_pidcontroller_conf_.pitch_angle_filter_conf().cutoff_freq();// 配置中读到的参数滤波器的采样周期存到变量ts中double ts = lon_based_pidcontroller_conf_.ts();// 将ts，cutoff_freq作为输入参数调用函数SetDigitalFilter()// 将参数全部设置到LonController类的数据成员滤波器类对象digital_filter_pitch_angle_中SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
}
// 初始化标定表
void LonController::InitControlCalibrationTable() {AINFO << "Control calibration table size is "<< calibration_table_.calibration_size();// 定义DataType类型变量xyz// Apollo类型定义typedef std::vector<std::tuple<double, double, double>> DataType;// DataType实际上就是一个三维容器，存放很多组对应的速度，加速度，油门/制动百分数数据Interpolation2D::DataType xyz;for (const auto &calibration : calibration_table_.calibration()) {xyz.push_back(std::make_tuple(calibration.speed(),calibration.acceleration(),calibration.command()));}// std::unique_ptr指针的清空复位，将LonController类数据成员control_interpolation_清空control_interpolation_.reset(new Interpolation2D);// 将标定表读取到LonController类的数据成员xyz_里(特定车速下的加速度对应的控制量百分数)ACHECK(control_interpolation_->Init(xyz))<< "Fail to load control calibration table";
}
// 计算纵向控制指令
// localization/chassis/planning_published_trajectory都是const只允许读取
Status LonController::ComputeControlCommand(const localization::LocalizationEstimate *localization,const canbus::Chassis *chassis,const planning::ADCTrajectory *planning_published_trajectory,control::ControlCommand *cmd) {// 将输入的定位，底盘信息分别读取到LonController类的数据成员localization_,chassis_    localization_ = localization;chassis_ = chassis;// 将输入的规划轨迹信息分别读取到LonController类的数据成员trajectory_message_ 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.");}// 如果规划轨迹信息指针为空或者轨迹分析器里的序号和轨迹message的序号不相等时复位if (trajectory_analyzer_ == nullptr ||trajectory_analyzer_->seq_num() !=trajectory_message_->header().sequence_num()) {trajectory_analyzer_.reset(new TrajectoryAnalyzer(trajectory_message_));}// 定义临时变量debug是cmd.debug.simple_lon_debug// 因为cmd是指针所以这样访问数据成员，cmd所属类型包含一个数据成员debug专门用来存放调试信息auto debug = cmd->mutable_debug()->mutable_simple_lon_debug();// 首先将debug清空debug->Clear();// 初始化刹车，油门控制命令为0double brake_cmd = 0.0;double throttle_cmd = 0.0;// 初始化采样周期ts从lon_controller_conf里读到，lon_controller_conf也是从control_conf.pb.txt中读取到的double ts = lon_based_pidcontroller_conf_.ts();// preview预览时间=lon_controller_conf读到的纵向预览窗口大小*采样时间tsdouble preview_time = lon_based_pidcontroller_conf_.preview_window() * ts;// 默认falsebool enable_leadlag =lon_based_pidcontroller_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);}// 调用计算纵向误差函数// trajectory_analyzer_.get()获得轨迹信息指针用于提供轨迹点的速度加速度，匹配点参考点等信息// debug计算得到的误差放入debug中ComputeLongitudinalErrors(trajectory_analyzer_.get(), preview_time, ts,debug);// 定义临时变量station_error_limit纵向位置误差限制，默认是2.0mdouble station_error_limit =lon_based_pidcontroller_conf_.station_error_limit();// 定义临时变量station_error_limited为限幅后的纵向位置误差double station_error_limited = 0.0;// 如果打开速度-位置预览，默认为true打开speed_station_preview// 预览点：当前时间加上预览时间在轨迹上对应的点，车辆将要到达的纵向位置// 参考点：当前时间在轨迹上对应的点，车辆此刻应该到达的纵向位置// 匹配点：当前时间在轨迹上对应的点，车辆此刻应该到达的纵向位置/*** 两种位置误差* 第一种, preview_station_error=预览点纵向位置 - 匹配点纵向位置* 第二种, station_error=参考点纵向位置-匹配点纵向位置*/if (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {// 如果打开此开关则采用第一种纵向位置误差，纵向误差的计算结果都是存在debug里station_error_limited =common::math::Clamp(debug->preview_station_error(),-station_error_limit, station_error_limit);} else {// 否则采用第二种纵向位置误差，纵向误差的计算结果都是存在debug里station_error_limited = common::math::Clamp(debug->station_error(), -station_error_limit, station_error_limit);}// 如果轨迹msg信息指针里的档位信息是R档if (trajectory_message_->gear() == canbus::Chassis::GEAR_REVERSE) {// 检查车是否在坑道里if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_,injector_->vehicle_state()->linear_velocity(),trajectory_message_->is_replan())) {ADEBUG << "in pit";station_pid_controller_.SetPID(lon_based_pidcontroller_conf_.pit_station_pid_conf());speed_pid_controller_.SetPID(lon_based_pidcontroller_conf_.pit_speed_pid_conf());} else { // 不在坑道里// 从配置文件加载lon_controller_conf里的倒车PID参数配置加载到位置PID控制器对象station_pid_controller_station_pid_controller_.SetPID(lon_based_pidcontroller_conf_.reverse_station_pid_conf());// 从配置文件加载lon_controller_conf里的倒车PID参数配置加载到速度PID控制器对象station_pid_controller_speed_pid_controller_.SetPID(lon_based_pidcontroller_conf_.reverse_speed_pid_conf());}// 如果打开enable_leadlag超前滞后控制器，默认是关闭if (enable_leadlag) {station_leadlag_controller_.SetLeadlag(lon_based_pidcontroller_conf_.reverse_station_leadlag_conf());speed_leadlag_controller_.SetLeadlag(lon_based_pidcontroller_conf_.reverse_speed_leadlag_conf());}} else if (injector_->vehicle_state()->linear_velocity() <=lon_based_pidcontroller_conf_.switch_speed()) { //若非R档 且 当前车速<=高低速切换的转换速度switch_speed// 检查是否在坑道里if (CheckPit::CheckInPit(debug, &lon_based_pidcontroller_conf_,injector_->vehicle_state()->linear_velocity(),trajectory_message_->is_replan())) {ADEBUG << "in pit";station_pid_controller_.SetPID(lon_based_pidcontroller_conf_.pit_station_pid_conf());speed_pid_controller_.SetPID(lon_based_pidcontroller_conf_.pit_speed_pid_conf());} else {station_pid_controller_.SetPID(lon_based_pidcontroller_conf_.station_pid_conf());// 速度PID控制器对象speed_pid_controller_加载控制配置文件中低速PID参数speed_pid_controller_.SetPID(lon_based_pidcontroller_conf_.low_speed_pid_conf());}} else {station_pid_controller_.SetPID(lon_based_pidcontroller_conf_.station_pid_conf());// 速度PID控制器对象speed_pid_controller_加载控制配置文件中高速PID参数speed_pid_controller_.SetPID(lon_based_pidcontroller_conf_.high_speed_pid_conf());}// 非R档情况下，定义临时变量speed_offset速度偏差// 速度偏差=位置控制器根据(限幅后位置误差，采样周期)计算出控制量即速度double speed_offset =station_pid_controller_.Control(station_error_limited, ts);if (enable_leadlag) {// 默认falsespeed_offset = station_leadlag_controller_.Control(speed_offset, ts);}// 定义一个临时变量速度控制器的输入speed_controller_input为0double speed_controller_input = 0.0;// 从配置文件加载速度控制器输入限幅double speed_controller_input_limit =lon_based_pidcontroller_conf_.speed_controller_input_limit();// 经过限幅之后的速度控制器的输入double speed_controller_input_limited = 0.0;// 打开speed_station_previewif (lon_based_pidcontroller_conf_.enable_speed_station_preview()) {// 速度控制器的输入 = 位置控制器计算出的speed_offset + 当前时间向前加上预览时间在轨迹上的对应点的速度和当前车速的偏差speed_controller_input = speed_offset + debug->preview_speed_error();} else { // 速度控制器的输入 = 位置控制器计算出的speed_offset + 参考点车速和当前车速的偏差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);// 定义临时变量acceleration_cmd_closeloop闭环加速度指令初始化为0double acceleration_cmd_closeloop = 0.0;// 闭环的加速度指令就等于速度PID控制器根据速度控制器的输入，以及采样周期去计算acceleration_cmd_closeloop =speed_pid_controller_.Control(speed_controller_input_limited, ts);// 将速度PID控制器中积分器的饱和状态设置到debug.pid_saturation_statusdebug->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());}// 汽车空档if (chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {speed_pid_controller_.Reset_integral();  // 重置积分station_pid_controller_.Reset_integral();// 重置积分}// 汽车俯仰角弧度double vehicle_pitch_rad =digital_filter_pitch_angle_.Filter(injector_->vehicle_state()->pitch());// 汽车俯仰角double vehicle_pitch =vehicle_pitch_rad * 180 / M_PI + FLAGS_pitch_offset_deg;ADEBUG << "[LON]vehicle_pitch is " << vehicle_pitch;// 将vehicle_pitch放入debug里debug->set_vehicle_pitch(vehicle_pitch);// TODO(ALL): confirm the slope_offset_compensation whether is positive or not// when vehicle move uphill// Resume: uphill: + , downhill: -// 定义斜坡补偿加速度 = use_opposite_slope_compensation *(重力加速度 * 车辆俯仰角的正弦值)得到斜坡加速度补偿double slope_offset_compensation =lon_based_pidcontroller_conf_.use_opposite_slope_compensation() *GRA_ACC *std::sin(vehicle_pitch_rad + FLAGS_pitch_offset_deg * M_PI / 180);// 判断坡道补偿加速度是否为非数NaN，当浮点数过小下溢就可能出现NaN非数if (std::isnan(slope_offset_compensation)) {slope_offset_compensation = 0;}// 将斜坡补偿加速度设置到debug里，debug.slope_offset_compensation=slope_offset_compensationdebug->set_slope_offset_compensation(slope_offset_compensation);// 总的加速度指令 = 闭环加速度指令 + 预览参考加速度 + 坡道补偿加速度(如果打开坡道补偿的话)double acceleration_cmd =acceleration_cmd_closeloop + debug->preview_acceleration_reference() +lon_based_pidcontroller_conf_.enable_slope_offset() *debug->slope_offset_compensation();// Check the steer command in reverse trajectory if the current steer target// is larger than previous target, free the acceleration command, wait for// the current steer targetdouble current_steer_interval =cmd->steering_target() - chassis->steering_percentage();// 转向检查 + 未知轨迹 + 当前转向间隔 > steer_cmd_intervalif (lon_based_pidcontroller_conf_.use_steering_check() &&(trajectory_message_->trajectory_type() ==apollo::planning::ADCTrajectory::UNKNOWN) &&std::abs(current_steer_interval) >lon_based_pidcontroller_conf_.steer_cmd_interval()) {ADEBUG << "steering_target is " << cmd->steering_target()<< " steering_percentage is " << chassis->steering_percentage();ADEBUG << "Steer cmd interval is larger than "<< lon_based_pidcontroller_conf_.steer_cmd_interval();// 重置速度位置PID的积分，加速度指令设置为0speed_pid_controller_.Reset_integral();station_pid_controller_.Reset_integral();acceleration_cmd = 0;// 等待转向 truedebug->set_is_wait_steer(true);} else { // 等待转向 falsedebug->set_is_wait_steer(false);}// current_steer_interval加入调试信息中debug->set_current_steer_interval(current_steer_interval);// At near-stop stage, replace the brake control command with the standstill// acceleration if the former is even softer than the latter// 设置debug的is_full_stop标志位为falsedebug->set_is_full_stop(false);debug->set_is_full_stop_soft(false);auto previous_full_stop =injector_->Get_previous_lon_debug_info()->is_full_stop();// 获取停车点的一个函数，后面介绍，找到当前规划模块发布的轨迹msg里的第一个v,a都小于一个很小值的点作为停车点// 找到的这个停车点的纵向位置和当前车辆纵向位置的偏差设置到debug里面去，debug.path_remain()GetPathRemain(debug);IsStopByDestination(debug);IsPedestrianStopLongTerm(debug);// 使用预览参考 + 预览加速度参考 <= 停止的最大加速度 + 预览速度参考 <= 停止的最大速度 + 轨迹不在开放空间if (lon_based_pidcontroller_conf_.use_preview_reference_check() &&(std::fabs(debug->preview_acceleration_reference()) <=FLAGS_max_acceleration_when_stopped) &&std::fabs(debug->preview_speed_reference()) <=vehicle_param_.max_abs_speed_when_stopped() &&trajectory_message_->trajectory_type() != ADCTrajectory::OPEN_SPACE) {// 因为目标地和行人停车if (debug->is_stop_reason_by_destination() ||debug->is_stop_reason_by_prdestrian()) {// 完全停车debug->set_is_full_stop(true);ADEBUG << "Into full stop within preview acc and reference speed, "<< "is_full_stop is " << debug->is_full_stop();} else {debug->set_is_full_stop_soft(true);ADEBUG << "Into full stop soft within preview acc and reference speed, "<< "is_full_stop_soft is " << debug->is_full_stop_soft();}}// 启用预览完全停车if (!previous_full_stop) { // 停车最大留用距离max_path_remain_when_stopped_ = FLAGS_max_path_remain_when_stopped;} else {max_path_remain_when_stopped_ =FLAGS_max_path_remain_when_stopped +lon_based_pidcontroller_conf_.full_stop_path_remain_gain();}// 前进档+还没有到达停车最大留用距离 || 倒档 + 还没有到达停车最大留用距离if (((trajectory_message_->gear() == Chassis::GEAR_DRIVE) &&debug->path_remain() < max_path_remain_when_stopped_) ||((trajectory_message_->gear() == Chassis::GEAR_REVERSE) &&debug->path_remain() > -max_path_remain_when_stopped_)) {ADEBUG << "Into full stop decision by path remain.";if (debug->is_stop_reason_by_destination() ||debug->is_stop_reason_by_prdestrian()) {debug->set_is_full_stop(true);// 当前路径留用距离ADEBUG << "Current path remain distance: " << debug->path_remain()<< " is within max_path_remain threshold: "<< max_path_remain_when_stopped_<< ", into full stop because vehicle is in destination: "<< debug->is_stop_reason_by_destination()<< " or pedestrian is in long time stop: "<< debug->is_stop_reason_by_prdestrian()<< "is_full_stop flag: " << debug->is_full_stop();} else {debug->set_is_full_stop_soft(true);ADEBUG << "Current path remain distance: " << debug->path_remain()<< " is within max_path_remain threshold: "<< max_path_remain_when_stopped_<< ", but not into full stop because stop not in destination: "<< debug->is_stop_reason_by_destination()<< " or pedestrian is not long time stop: "<< debug->is_stop_reason_by_prdestrian()<< "is_full_stop_soft flag: " << debug->is_full_stop_soft();}// 当前线速度 < 停止最大车速 && 因为行人停止if (injector_->vehicle_state()->linear_velocity() <vehicle_param_.max_abs_speed_when_stopped() &&debug->is_stop_reason_by_prdestrian()) {ADEBUG << "Current stop is for long time pedestrian stop, "<< debug->is_stop_reason_by_prdestrian();// 完全停车debug->set_is_full_stop(true);}} else {ADEBUG << "Not into full stop decision by path remain.";}// 完全停车if (debug->is_full_stop()) { //R档略过acceleration_cmd =(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)? std::max(acceleration_cmd, //standstill_acceleration默认为-0.3 m/s^2-lon_based_pidcontroller_conf_.standstill_acceleration()): std::min(acceleration_cmd, //非R档取控制器计算加速度指令，standstill减速度中最小值lon_based_pidcontroller_conf_.standstill_acceleration());// 速度、位置PID积分为0speed_pid_controller_.Reset_integral();station_pid_controller_.Reset_integral();}// 如果车辆已完全停止if (debug->is_full_stop_soft()) {// 如果当前档位不是倒车档if (chassis->gear_location() != canbus::Chassis::GEAR_REVERSE) {// 根据加速度命令的正负性确定下一步加速度命令acceleration_cmd =(acceleration_cmd >= 0)       ? standstill_narmal_acceleration_: (debug->path_remain() >= 0) ? acceleration_cmd: (trajectory_message_->trajectory_type() != ADCTrajectory::NORMAL)? (acceleration_cmd + stop_gain_acceleration_): (acceleration_cmd + standstill_narmal_acceleration_);} else {// 如果当前档位是倒车档// 根据加速度命令的正负性确定下一步加速度命令acceleration_cmd =(acceleration_cmd <= 0)       ? -standstill_narmal_acceleration_: (debug->path_remain() <= 0) ? acceleration_cmd: (trajectory_message_->trajectory_type() != ADCTrajectory::NORMAL)? (acceleration_cmd - stop_gain_acceleration_): (acceleration_cmd - standstill_narmal_acceleration_);}// 重置速度PID控制器的积分项speed_pid_controller_.Reset_integral();// 重置位置PID控制器的积分项station_pid_controller_.Reset_integral();}// 定义油门指令的下边界double throttle_lowerbound =std::max(vehicle_param_.throttle_deadzone(),lon_based_pidcontroller_conf_.throttle_minimum_action());// 定义刹车指令的下边界double brake_lowerbound =std::max(vehicle_param_.brake_deadzone(),lon_based_pidcontroller_conf_.brake_minimum_action());// 定义临时变量calibration_value标定值初始化为0double calibration_value = 0.0;// 要用来查表加速度，若R档为加速度控制指令取反，非R档保持加速度控制指令double acceleration_lookup =(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)? -acceleration_cmd: acceleration_cmd;// 加速度查表限制 = 最大加速度 + 斜坡补偿double acceleration_lookup_limited =vehicle_param_.max_acceleration() +lon_based_pidcontroller_conf_.enable_slope_offset() *debug->slope_offset_compensation();double acceleration_lookup_limit = 0.0;// 使用加速度查表限制if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {acceleration_lookup_limit =(acceleration_lookup > acceleration_lookup_limited)? acceleration_lookup_limited: acceleration_lookup;}// 是否用预览点速度来查标定表(车速-加速度-控制指令百分数) falseif (FLAGS_use_preview_speed_for_table) {if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {calibration_value = control_interpolation_->Interpolate(std::make_pair(std::fabs(debug->preview_speed_reference()),acceleration_lookup_limit));} else { // 用速度加速度根据标定表线性插值得到控制量百分数calibration_valuecalibration_value = control_interpolation_->Interpolate(std::make_pair(std::fabs(debug->preview_speed_reference()), acceleration_lookup));}} else { if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {calibration_value = control_interpolation_->Interpolate(std::make_pair(std::fabs(injector_->vehicle_state()->linear_velocity()),acceleration_lookup_limit));} else {calibration_value = control_interpolation_->Interpolate(std::make_pair(std::fabs(injector_->vehicle_state()->linear_velocity()),acceleration_lookup));}}// 如果请求查表加速度>=0if (acceleration_lookup >= 0) {// 如果计算得到的控制百分数>=0if (calibration_value >= 0) {// 设置油门控制百分数，为油门下边界和查表得到的控制百分数之间的较大值throttle_cmd = std::max(calibration_value, throttle_lowerbound);} else {// 如果计算得到的控制百分数<0,但是加速度又大于0// 设置油门控制百分数为油门下边界throttle_cmd = throttle_lowerbound;}// 刹车指令为0，如果要加速brake_cmd = 0.0;} else {// 如果请求的查表加速度<0,油门指令置0throttle_cmd = 0.0;// 如果计算得到的控制百分数>=0if (calibration_value >= 0) {//刹车指令就取刹车下边界brake_cmd = brake_lowerbound;} else {// 如果计算得到的控制百分数<0 刹车指令就取标定值相反数和刹车下边界里较大值，其实就是取标定值进行限幅不能太小brake_cmd = std::max(-calibration_value, brake_lowerbound);}}// 如果启用了车辆电子驻车（EPB）if (FLAGS_use_vehicle_epb) {// 输出调试信息，表示进入使用车辆EPB的逻辑ADEBUG << "Into use vehicle epb.";// 如果加速度查找大于等于0if (acceleration_lookup >= 0) {// 如果坡道偏移补偿大于0if (debug->slope_offset_compensation() > 0) {// 如果加速度查找大于坡道偏移补偿if (acceleration_lookup > debug->slope_offset_compensation()) {// 设置EPB释放标志为trueparking_release_ = true;}} else {// 如果坡道偏移补偿小于等于0，则直接设置EPB释放标志为truparking_release_ = true;}// 如果当前处于停车制动状态且EPB释放标志为trueif (chassis->parking_brake() && parking_release_) {// 输出调试信息，表示进入停车制动释放逻辑ADEBUG << "Into park brake release.";// 设置制动指令中的停车制动为falsecmd->set_parking_brake(false);// 设置车辆停车制动SetParkingBrake(&lon_based_pidcontroller_conf_, cmd);}} else {// 如果加速度查找小于0  设置制动指令中的停车制动为falsecmd->set_parking_brake(false);// 如果当前处于完全停止状态且为长期停止if (debug->is_full_stop() && IsFullStopLongTerm(debug)) {// 输出调试信息，表示进入停车制动触发逻辑ADEBUG << "Into park brake trigger.";// 设置制动指令中的停车制动为truecmd->set_parking_brake(true);// 如果当前处于停车制动状态if (chassis->parking_brake()) {brake_cmd = 0.0; // 将制动命令设置为0.0}}}}// 将被限制的纵向位置误差设置到debug.station_error_limiteddebug->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_acceleration_lookup_limit(acceleration_lookup_limit);// 去查标定表的速度将chassis反馈值设置到debug.speed_lookupdebug->set_speed_lookup(injector_->vehicle_state()->linear_velocity());// 标定表中查到的控制百分数值debug->set_calibration_value(calibration_value);// 闭环反馈速度控制器计算得到的控制量加速度debug->set_acceleration_cmd_closeloop(acceleration_cmd_closeloop);// 如果打开csv日志记录默认falseif (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->acceleration_lookup_limit(), 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);// 使用加速度查找限制if (lon_based_pidcontroller_conf_.use_acceleration_lookup_limit()) {cmd->set_acceleration(acceleration_lookup_limit);} else {cmd->set_acceleration(acceleration_cmd);}// 纵向速度 <= 最大停车速度 || 空档 || 档位与轨迹消息档位一致if (std::fabs(injector_->vehicle_state()->linear_velocity()) <=vehicle_param_.max_abs_speed_when_stopped() ||chassis->gear_location() == trajectory_message_->gear() ||chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {// 若车辆处于停车或N档时下发规划发布的轨迹msg里的档位cmd->set_gear_location(trajectory_message_->gear());} else {// 若车辆不处于停车且不在N档时下发chassis反馈的车辆实际档位cmd->set_gear_location(chassis->gear_location());}return Status::OK();
}
// 重置速度、位置pid
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// 输入参数规划发布轨迹信息TrajectoryAnalyzer类指针对象trajectory_analyzer// 输入参数preview_time,预览时间，在控制配置文件里面设置// 输入参数ts，采样时间，在控制配置文件里设置// 输入参数debug，SimpleLongitudinalDebug类指针对象debug用来存放计算得到纵向误差信息// matched:匹配点，在参考轨迹上距离当前车辆距离最近的点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());// 轨迹信息将当前点x,y,theta,v以及匹配点信息输入，输出当前点的s,d,s',d'// 将大地坐标系转化为Frenet坐标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);// 所有的计算结果都是存到debug这个指针对象里//debug.matched_point.pathpoint.x=匹配点xdebug->mutable_current_matched_point()->mutable_path_point()->set_x(matched_point.x());//debug.matched_point.pathpoint.y=匹配点ydebug->mutable_current_matched_point()->mutable_path_point()->set_y(matched_point.y());// debug.reference_point.pathpoint.x=参考点xdebug->mutable_current_reference_point()->mutable_path_point()->set_x(reference_point.path_point().x());// debug.reference_point.pathpoint.y=参考点ydebug->mutable_current_reference_point()->mutable_path_point()->set_y(reference_point.path_point().y());// debug.preview_point.pathpoint.x=预览点xdebug->mutable_preview_reference_point()->mutable_path_point()->set_x(preview_point.path_point().x());// debug.preview_point.pathpoint.y=预览点ydebug->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();// 航向角误差 = 车辆当前状态航向角 - 匹配点的航向角// NormalizeAngle角度的规范化，就是将所有角度规范到-pi,pidouble heading_error = common::math::NormalizeAngle(vehicle_state->heading() -matched_point.theta());// 纵向速度 = 车辆速度 * cos(当前航向角 - 轨迹上距离最近点航向角)double lon_speed = vehicle_state->linear_velocity() * std::cos(heading_error);// 纵向加速度 = 车辆加速度 * cos(当前航向角 - 轨迹上距离最近点航向角)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里供类内其他函数调用// 参考的纵向位置debug.station_reference=参考点的累积弧长debug->set_station_reference(reference_point.path_point().s());// 当前的纵向位置debug.current_station=当前点的累积弧长debug->set_current_station(s_matched);// 纵向位置误差debug.station_error=参考点路径点的累积弧长-匹配点的累积弧长(匹配点就是路径最近点)debug->set_station_error(reference_point.path_point().s() - s_matched);// 速度参考值就是参考点的速度debug.speed_referencedebug->set_speed_reference(reference_point.v());// 当前速度debug->set_current_speed(lon_speed);// 速度误差就是参考点速度减匹配点速度 debug.speed_errordebug->set_speed_error(reference_point.v() - s_dot_matched);// 参考点加速度debug->set_acceleration_reference(reference_point.a());// 纵向加速度debug->set_current_acceleration(lon_acceleration);// 加速度误差=参考点加速度-纵向加速度/(1-kd)  1-kd由全局坐标转换到Frenet坐标引入，kappa就是曲率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;// 参考jerkdebug->set_jerk_reference(jerk_reference);// 设置当前jerkdebug->set_current_jerk(lon_jerk);// 加加速度误差=加加速度参考-纵向加加速度/(1-kd)存到debug里debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);// 更新previous_acceleration_reference_previous_acceleration_reference_ = debug->acceleration_reference();// 更新previous_acceleration_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
// 就是去轨迹点上找第一个符合条件的停止点，并将结果存在debug指针
// 然后快到停车点时，就给一个固定的standstill减速度，配置里默认设置-0.3
void LonController::GetPathRemain(SimpleLongitudinalDebug *debug) {// 首先定义初始化了停止点在规划发布轨迹点中的下标为0int stop_index = 0;// 定义了静态常量速度阈值，速度小于此阈值认为是停止条件之一static constexpr double kSpeedThreshold = 1e-3;// 定义了静态常量加速度常量 前进档时加速度>此阈值且<0认为是停止条件之一static constexpr double kForwardAccThreshold = -1e-2;// 定义了静态常量加速度常量 R档时加速度<此阈值且>0认为是停止条件之一static constexpr double kBackwardAccThreshold = 1e-1;// 定义了静态常量驻车速度，也是判断停车点的一个依据static constexpr double kParkingSpeed = 0.1;//若规划发布的轨迹信息trajectory_message_中档位为前进档if (trajectory_message_->gear() == canbus::Chassis::GEAR_DRIVE) {// 这里就是stop_index停车点下标从0开始遍历当前规划发布轨迹所有点while (stop_index < trajectory_message_->trajectory_point_size()) {// 定义当前此次循环遍历的轨迹点为轨迹msg中下标为stop_index的那个点auto &current_trajectory_point =trajectory_message_->trajectory_point(stop_index);// 若当前遍历的轨迹点速度绝对值 < 速度阈值 且 当前遍历的轨迹点加速度 > 前进加速度阈值 且 当前遍历的轨迹点加速度 < 0// 若符合这条件则找到了停车点直接break跳出while遍历循环if (fabs(current_trajectory_point.v()) < kSpeedThreshold &&current_trajectory_point.a() > kForwardAccThreshold &&current_trajectory_point.a() < 0.0) {break;}// 若此次循环不符合上述停车点的判定条件，则遍历规划发布的轨迹msg中的下一个点++stop_index;}} else {// 若为非前进档的停车点下标搜索while (stop_index < trajectory_message_->trajectory_point_size()) {auto &current_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;}}// 打印停止点ADEBUG << "stop_index is, " << stop_index;// 如果到了轨迹msg的最后一个点都没有符合上边符合要求的就将最后一个轨迹点为停止点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.path_remain里debug->set_path_remain(trajectory_message_->trajectory_point(stop_index).path_point().s() -debug->current_station());
}
// 判断是否通过目的地停止
bool LonController::IsStopByDestination(SimpleLongitudinalDebug *debug) {// 获取停止原因auto stop_reason = trajectory_message_->decision().main_decision().stop();// 输出当前停止原因的调试信息ADEBUG << "Current stop reason is \n" << stop_reason.DebugString();// 输出规划命令状态消息的调试信息ADEBUG << "Planning command status msg is \n"<< injector_->get_planning_command_status()->ShortDebugString();// 获取停止原因代码StopReasonCode stop_reason_code = stop_reason.reason_code();// 如果停止原因是信号、参考结束、预开放空间停止之一，或者规划命令状态为完成，或者决策中含有任务完成标志if (stop_reason_code == StopReasonCode::STOP_REASON_SIGNAL ||stop_reason_code == StopReasonCode::STOP_REASON_REFERENCE_END ||stop_reason_code == StopReasonCode::STOP_REASON_PRE_OPEN_SPACE_STOP ||injector_->get_planning_command_status()->status() ==CommandStatusType::FINISHED ||trajectory_message_->decision().main_decision().has_mission_complete()) {// 输出调试信息，表示当前停止原因在目的地范围内ADEBUG << "[IsStopByDestination]Current stop reason is in destination.";// 设置调试标志，表示停止原因是通过目的地停止的debug->set_is_stop_reason_by_destination(true);return true;}// 设置调试标志，表示停止原因不是通过目的地停止的debug->set_is_stop_reason_by_destination(false);return false;
}
// 判断是否存在长时间行人停止
bool LonController::IsPedestrianStopLongTerm(SimpleLongitudinalDebug *debug) {// 获取停止原因auto stop_reason = trajectory_message_->decision().main_decision().stop();// 输出当前停止原因的调试信息ADEBUG << "Current stop reason is \n" << stop_reason.DebugString();// 获取停止原因代码StopReasonCode stop_reason_code = stop_reason.reason_code();// 如果停止原因是行人或障碍物if (stop_reason_code == StopReasonCode::STOP_REASON_PEDESTRIAN ||stop_reason_code == StopReasonCode::STOP_REASON_OBSTACLE) {// 输出调试信息，表示停止原因是行人ADEBUG << "[IsPedestrianStopLongTerm]Stop reason for pedestrian.";is_stop_by_pedestrian_ = true;// 设置标志，表示通过行人停止} else {is_stop_by_pedestrian_ = false; // 设置标志，表示不是通过行人停止}// 输出当前行人停止标志和上次行人停止标志的状态信息ADEBUG << "Current is_stop_by_pedestrian: " << is_stop_by_pedestrian_<< ", is_stop_by_pedestrian_previous: "<< is_stop_by_pedestrian_previous_;// 如果当前是通过行人停止if (is_stop_by_pedestrian_) {// 如果当前停止不是由行人引起的，而上次停止是由行人引起的if (!(is_stop_by_pedestrian_ && is_stop_by_pedestrian_previous_)) {// 记录当前时间作为停止开始时间start_time_ = ::apollo::cyber::Clock::NowInSeconds();ADEBUG << "Stop reason for pedestrian, start time(s) is " << start_time_;} else {// 上次已经是行人停止，跳过开始时间初始化ADEBUG << "Last time stop is already pedestrian, skip start_time init.";}// 记录当前时间作为停止结束时间double end_time = ::apollo::cyber::Clock::NowInSeconds();ADEBUG << "Stop reason for pedestrian, current time(s) is " << end_time;// 计算停止持续时间wait_time_diff_ = end_time - start_time_;} else { // 如果不是通过行人停止，则开始时间和持续时间均为0start_time_ = 0.0;wait_time_diff_ = 0.0;}// 更新上次行人停止标志is_stop_by_pedestrian_previous_ = is_stop_by_pedestrian_;// 如果行人停止持续时间超过阈值if (wait_time_diff_ > lon_based_pidcontroller_conf_.pedestrian_stop_time()) {// 输出调试信息，表示行人停止持续时间超过阈值ADEBUG << "Current pedestrian stop lasting time(s) is " << wait_time_diff_<< ", larger than threshold: "<< lon_based_pidcontroller_conf_.pedestrian_stop_time();// 设置调试标志，表示停止原因是由行人引起的debug->set_is_stop_reason_by_prdestrian(true);return true;} else {// 输出调试信息，表示行人停止持续时间未达到阈值ADEBUG << "Current pedestrian stop lasting time(s) is " << wait_time_diff_<< ", not reach the threshold: "<< lon_based_pidcontroller_conf_.pedestrian_stop_time();// 设置调试标志，表示停止原因不是由行人引起的debug->set_is_stop_reason_by_prdestrian(false);return false;}
}
// 判断是否存在长时间完全停止
bool LonController::IsFullStopLongTerm(SimpleLongitudinalDebug *debug) {// 如果当前为完全停止状态if (debug->is_full_stop()) {// 如果当前停止不是由完全停止引起的，而上次停止是由完全停止引起的if (debug->is_full_stop() && !is_full_stop_previous_) {// 记录当前时间作为完全停止开始时间is_full_stop_start_time_ = ::apollo::cyber::Clock::NowInSeconds();ADEBUG << "Full stop long term start time(s) is "<< is_full_stop_start_time_;} else {// 上次已经是完全停止，跳过开始时间初始化ADEBUG << "Last time stop is already full stop, skip start_time init.";}// 记录当前时间作为完全停止结束时间double is_full_stop_start_end_time = ::apollo::cyber::Clock::NowInSeconds();// 计算完全停止持续时间is_full_stop_wait_time_diff_ =is_full_stop_start_end_time - is_full_stop_start_time_;} else {// 如果不是完全停止，则开始时间和持续时间均为0is_full_stop_start_time_ = 0.0;is_full_stop_wait_time_diff_ = 0.0;}// 更新上次完全停止标志is_full_stop_previous_ = debug->is_full_stop();// 如果完全停止持续时间超过阈值if (is_full_stop_wait_time_diff_ >lon_based_pidcontroller_conf_.full_stop_long_time()) {// 输出调试信息，表示完全停止持续时间超过阈值ADEBUG << "Current full stop lasting time(s) is "<< is_full_stop_wait_time_diff_ << ", larger than threshold: "<< lon_based_pidcontroller_conf_.full_stop_long_time();return true;} else {// 输出调试信息，表示完全停止持续时间未达到阈值ADEBUG << "Current full stop lasting time(s) is "<< is_full_stop_wait_time_diff_ << ", not reach the threshold: "<< lon_based_pidcontroller_conf_.full_stop_long_time();return false;}
}
// 设置驻车制动器
void LonController::SetParkingBrake(const LonBasedPidControllerConf *conf,control::ControlCommand *control_command) {// 如果控制指令中驻车制动器开启if (control_command->parking_brake()) {// epb on, parking brake: 0 -> 1// 如果驻车制动器状态切换为开启，并且为第一阶段if (epb_on_change_switch_) {ADEBUG << "Epb on, first set parking brake false.";// 设置控制指令中的驻车制动器状态为关闭control_command->set_parking_brake(false);// 驻车制动器状态切换计数加一++epb_change_count_;// 如果切换计数达到设定值if (epb_change_count_ >= conf->epb_change_count()) {// 关闭驻车制动器状态切换开关epb_on_change_switch_ = false;// 重置切换计数epb_change_count_ = 0;ADEBUG << "Epb on, first stage has been done.";}} else {ADEBUG << "Epb on, second set parking brake true.";// 设置控制指令中的驻车制动器状态为开启control_command->set_parking_brake(true);// 驻车制动器状态切换计数加一++epb_change_count_;// 如果切换计数达到设定值if (epb_change_count_ >= conf->epb_change_count()) {// 打开驻车制动器状态切换开关epb_on_change_switch_ = true;// 重置切换计数epb_change_count_ = 0;ADEBUG << "Epb on, second stage has been done.";}}} else {// epb off, parking brake: 1 -> 0// 如果控制指令中驻车制动器关闭// 如果驻车制动器状态切换为关闭，并且为第一阶段if (epb_off_change_switch_) {ADEBUG << "Epb off, first set praking brake true.";// 设置控制指令中的驻车制动器状态为开启control_command->set_parking_brake(true);// 驻车制动器状态切换计数加一++epb_change_count_;// 如果切换计数达到设定值if (epb_change_count_ >= conf->epb_change_count()) {epb_off_change_switch_ = false;// 关闭驻车制动器状态切换开关epb_change_count_ = 0;// 重置切换计数ADEBUG << "Epb off, first stage has been done.";}} else {ADEBUG << "Epb off, second set parking brake false.";// 设置控制指令中的驻车制动器状态为关闭control_command->set_parking_brake(false);// 驻车制动器状态切换计数加一++epb_change_count_;// 如果切换计数达到设定值if (epb_change_count_ >= conf->epb_change_count()) {// 打开驻车制动器状态切换开关epb_off_change_switch_ = true;// 重置切换计数epb_change_count_ = 0;ADEBUG << "Epb off, second stage has been done.";}}}
}}  // namespace control
}  // namespace apollo