Apollo学习笔记（９）纵向控制代码详读

之前的大致说了一下纵向控制的原理，实际的代码中还有很多小细节需要注意的地方，这里就主要记录下来，阅读纵向控制代码中的一些体会。
这里就直接在代码中加入一些笔记，这样比较好理解。

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;
	// vrf表示车辆参考坐标系，右前上为正，会根据下面不同的情况获取车辆目前的速度，右为ｘ轴，前为ｙ轴，上为ｚ轴
	if(FLAGS_use_localization_vrf_linear_velocity && localization_->pose().has_linear_velocity_vrf())
	{
   
		linear_velocity_ = std::max(0.0,localization_->pose().linear_velocity_vrf().y());
	}
	else
	{
   
		linear_velocity_ = VehicleStateProvider::Instance()->linear_velocity();
	}
	// 对获取到的车速 MeanFilter
	if(FLAGS_enable_longitudinal_speed_filter)
	{
   
		linear_velocity_ = speed_filter_.Update(linear_velocity_);
	}
	// control_interpolation_　在类初始化时载入参数表中完成插值，主要是对车辆在不同速度下，油门或者刹车的开度值
	// 所对应的车身加速度，纵向控制归根到底还是加速度的控制
	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);	
	}
	
	// R 档，Ｄ档使用不同的PID参数
	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());			
		}
	} 
	// 这里只有两组PID参数，在不同速度时，纵向控制精度无法保证，后续需要改进
	else if (linear_velocity_ <= lon_controller_conf.switch_speed()) 
	{
   
		speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
	} 
	else 
	{
   
		speed_pid_controller_