apollo::control::LatPlusController类 参考

LQR-Based lateral controller, to compute steering target. 更多...

#include <lat_plus_controller.h>

类 apollo::control::LatPlusController 继承关系图:

apollo::control::LatPlusController 的协作图:

Public 成员函数
	LatPlusController ()
	constructor
virtual	~LatPlusController ()
	destructor
common::Status	Init (std::shared_ptr< DependencyInjector > injector) override
	initialize Lateral Controller
common::Status	ComputeControlCommand (const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory, ControlCommand *cmd) override
	compute steering target based on current vehicle status and target trajectory
common::Status	Reset () override
	reset Lateral Controller
void	Stop () override
	stop Lateral controller
std::string	Name () const override
	Lateral controller name
Public 成员函数 继承自 apollo::control::ControlTaskExtend
	ControlTaskExtend ()
	constructor
virtual	~ControlTaskExtend ()
	destructor
Public 成员函数 继承自 apollo::control::ControlTask
	ControlTask ()=default
	constructor
virtual	~ControlTask ()=default
	destructor

Protected 成员函数
bool	LoadParams (ParamsPipeline *params_pipeline)
bool	SetCurrentParam (const std::string param_config_name, LatBaseLqrPlusControllerConf &current_lat_based_lqr_plus_controller_conf)
bool	ChooseCurrentParam (LatBaseLqrPlusControllerConf &current_lat_based_lqr_plus_controller_conf, const planning::ADCTrajectory *planning_published_trajectory)
void	UpdateState (SimpleLateralDebug *debug, SimpleLateralPlusDebug *debug_info, const canbus::Chassis *chassis)
void	UpdateDrivingOrientation ()
void	UpdateMatrix ()
void	UpdateMatrixCompound ()
double	ComputeFeedForward (double ref_curvature)
void	ComputeTrackLateralErrors (const double x, const double y, const double theta, const double linear_v, const TrajectoryAnalyzerExtend &trajectory_analyzer, const enum ChooseRefTrajectoryPoint &choose_track_point, SimpleLateralPlusDebug *lat_debug)
void	ComputeLateralErrors (const double x, const double y, const double theta, const double linear_v, const double angular_v, const double linear_a, const TrajectoryAnalyzerExtend &trajectory_analyzer, const canbus::Chassis *chassis, const enum ChooseRefTrajectoryPoint &choose_point, SimpleLateralDebug *debug, SimpleLateralPlusDebug *lat_debug)
bool	InitControlConf (const LatBaseLqrPlusControllerConf &current_lat_based_lqr_plus_controller_conf)
bool	UpdateControlConf (const LatBaseLqrPlusControllerConf &current_lat_based_lqr_plus_controller_conf)
void	InitializeFilters ()
void	LogInitParameters ()
void	ProcessLogs (const SimpleLateralDebug *debug, const canbus::Chassis *chassis)
void	CloseLogFile ()
bool	IsDrivingStraight (const canbus::Chassis *chassis, double ref_curvature)
void	UpdateAHCParam (const double &heading_err, const double &speed)
double	CalculateTempAHCValue (const double &speed)
double	AutoCompensatedHeadingValue (const double &speed)
void	AHCClear ()
void	LateralSafetyCheck (SimpleLateralDebug *debug, SimpleLateralPlusDebug *debug_info)
void	ResetLateralSafetyCheck ()
void	UpdateLookForwardPointError (const double lookahead_station, const double lookback_station, const double pose_x, const double pose_y, const double theta, const double linear_v, const common::TrajectoryPoint &ref_point, const TrajectoryAnalyzerExtend &trajectory_analyzer, SimpleLateralDebug *debug, SimpleLateralPlusDebug *debug_info)
void	EnhanceQParamsAlternate (const double lateral_error, const double heading_error, const planning::ADCTrajectory *planning_published_trajectory, const SimpleLongitudinalDebug *previous_lon_debug, SimpleLateralDebug *debug, SimpleLateralPlusDebug *debug_info)
bool	CheckContinousErrors (const uint8_t window_size, const double error, const double threshold, std::vector< double > *arr, int32_t *current_sign)
void	ResetComLength ()
void	ResetComLengthFromTurbulence ()
Protected 成员函数 继承自 apollo::control::ControlTaskExtend
template<typename T >
bool	LoadParamsPipelineConfig (T *pipeline_config)
bool	LoadCalibrationTable (const std::string &calibration_table_path, calibration_table *calibration_table_conf)
bool	VehicleStatusIdentificationUpdate (const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory)
bool	IsForwardModel ()
bool	IsEdgeFollow ()
bool	CheckInPit (double pit_replan_check_time, double pit_replan_check_count, double vehicle_speed, bool replan)
bool	IsLargeCurvature (const double ref_curvature, const double min_large_ref_curvature, bool *is_in_large_curvature)
bool	IsLeftCurvature (const double ref_curvature)
double	GetVehicleSpeed (std::shared_ptr< DependencyInjector > injector, const bool &use_filter, const double &filter_coeff)
double	ThrottleCmdFilter (const bool &use_filter, const double &filter_coeff, const double &throttle)
Protected 成员函数 继承自 apollo::control::ControlTask
template<typename T >
bool	LoadConfig (T *config)
bool	LoadCalibrationTable (calibration_table *calibration_table_conf)

Protected 属性
const canbus::Chassis *	chassis_ = nullptr
LatBaseLqrPlusControllerConf	lat_based_lqr_plus_controller_conf_
ParamsPipeline	params_pipeline_
std::vector< std::pair< std::string, LatBaseLqrPlusControllerConf > >	params_list_
LatBaseLqrPlusControllerConf	current_lat_based_lqr_plus_controller_conf_
std::string	current_param_config_name_ = ""
common::VehicleParam	vehicle_param_
TrajectoryAnalyzerExtend	trajectory_analyzer_
double	ts_ = 0.0
double	cf_ = 0.0
double	cr_ = 0.0
double	wheelbase_ = 0.0
double	mass_ = 0.0
double	mass_current_ = 0.0
double	lf_ = 0.0
double	lr_ = 0.0
double	iz_ = 0.0
double	steer_ratio_ = 0.0
double	steer_single_direction_max_degree_ = 0.0
double	max_lat_acc_ = 0.0
int	preview_window_ = 0
double	lookahead_station_low_speed_ = 0.0
double	lookback_station_low_speed_ = 0.0
double	lookahead_station_high_speed_ = 0.0
double	lookback_station_high_speed_ = 0.0
const int	basic_state_size_ = 4
Eigen::MatrixXd	matrix_a_
Eigen::MatrixXd	matrix_ad_
Eigen::MatrixXd	matrix_adc_
Eigen::MatrixXd	matrix_b_
Eigen::MatrixXd	matrix_bd_
Eigen::MatrixXd	matrix_bdc_
Eigen::MatrixXd	matrix_k_
Eigen::MatrixXd	matrix_r_
Eigen::MatrixXd	matrix_q_
Eigen::MatrixXd	matrix_q_updated_
Eigen::MatrixXd	matrix_a_coeff_
Eigen::MatrixXd	matrix_state_
int	lqr_max_iteration_ = 0
double	lqr_eps_ = 0.0
common::DigitalFilter	digital_filter_
std::unique_ptr< Interpolation1D >	lat_err_interpolation_
std::unique_ptr< Interpolation1D >	heading_err_interpolation_
common::MeanFilter	lateral_error_filter_
common::MeanFilter	heading_error_filter_
bool	enable_leadlag_ = false
LeadlagPlusController	leadlag_controller_
bool	enable_mrac_ = false
MracController	mrac_controller_
bool	enable_look_ahead_back_control_ = false
bool	use_new_look_ahead_back_ = false
double	previous_lateral_acceleration_ = 0.0
double	previous_heading_rate_ = 0.0
double	previous_ref_heading_rate_ = 0.0
double	previous_heading_acceleration_ = 0.0
double	previous_ref_heading_acceleration_ = 0.0
double	previous_lateral_error_lqr_input_ = 0.0
double	previous_heading_error_lqr_input_ = 0.0
std::ofstream	steer_log_file_
const std::string	name_
double	query_relative_time_
double	pre_steer_angle_ = 0.0
double	pre_steering_position_ = 0.0
double	minimum_speed_protection_ = 0.1
double	current_trajectory_timestamp_ = -1.0
double	init_vehicle_x_ = 0.0
double	init_vehicle_y_ = 0.0
double	init_vehicle_heading_ = 0.0
double	low_speed_bound_ = 0.0
double	low_speed_window_ = 0.0
double	driving_orientation_ = 0.0
std::shared_ptr< DependencyInjector >	injector_
bool	AHC_is_compensating_ = false
double	AHC_mean_ = 0.0
double	AHC_peak_ = 0.0
double	AHC_sum_ = 0.0
double	AHC_compensating_value_ = 0.0
double	AHC_compensated_value_ = 0.0
double	AHC_prev_compensated_value_ = 0.0
double	AHC_prev_output_value_ = 0.0
std::deque< double >	AHC_container_
int	reverse_handle_count_ = 0
ReverseLogicStage::ReverseStageType	now_reverse_stage_ = ReverseLogicStage::NO_REVERSE
double	efai_rate_ = 1.0
double	ed_date_ = 1.0
double	ref_curvature_last_ = 0.0
uint32_t	lateral_error_w_count_ = 0
uint32_t	lateral_error_e_count_ = 0
bool	is_in_large_curvature_ = false
bool	enable_enhance_qparam_ = false
enum ChooseRefTrajectoryPoint	choose_ref_point_ = USE_LAT_MATCH_COM
int32_t	current_sign_ = 0
uint8_t	check_continous_errors_window_ = 0
std::vector< double >	lateral_lr_errors_
int32_t	prev_sign_ = 0
int32_t	prev_abs_ = 0
double	com_change_ = 0.0
double	com_change_reset_ = 0.0
double	com_change_add_ = 0.0
double	com_turbulence_change_ = 0.0
double	com_change_turbulence_add_ = 0.0
double	com_length_previous_ = 0.0
double	com_length_turbulence_ = 0.0
bool	is_lat_turbulence_ = false
bool	is_lat_preview_in_straight_ = false
bool	is_need_cover_turbulence_com_length_ = false
bool	is_need_cover_turn_out_lane_com_length_ = false

详细描述

LQR-Based lateral controller, to compute steering target.

For more details, please refer to "Vehicle dynamics and control." Rajamani, Rajesh. Springer Science & Business Media, 2011.

在文件 lat_plus_controller.h 第 61 行定义.

构造及析构函数说明

LatPlusController()

apollo::control::LatPlusController::LatPlusController

(

)

constructor

在文件 lat_plus_controller.cc 第 70 行定义.

                                     : name_("LQR-based Lateral Plus Controller") {
    if (FLAGS_enable_csv_debug) {
        steer_log_file_.open(GetLogFileName());
        steer_log_file_ << std::fixed;
        steer_log_file_ << std::setprecision(6);
        WriteHeaders(steer_log_file_);
    }
    AINFO << "Using " << name_;
}

~LatPlusController()

apollo::control::LatPlusController::~LatPlusController ( )

virtual

destructor

在文件 lat_plus_controller.cc 第 80 行定义.

                                      {
    CloseLogFile();
}

成员函数说明

AHCClear()

void apollo::control::LatPlusController::AHCClear ( )

protected

在文件 lat_plus_controller.cc 第 1857 行定义.

                                 {
    AHC_container_.clear();
    AHC_sum_ = 0.0;
    AHC_peak_ = 0.0;
    AHC_mean_ = 0.0;
}

AutoCompensatedHeadingValue()

double apollo::control::LatPlusController::AutoCompensatedHeadingValue ( const double &  speed )

protected

在文件 lat_plus_controller.cc 第 1832 行定义.

                                                                         {
    if (AHC_mean_ > 0) {
        AHC_compensating_value_ += current_lat_based_lqr_plus_controller_conf_.ts()
                * InterpolationPlus1D::interpolation_1d(
                                           speed,
                                           current_lat_based_lqr_plus_controller_conf_.speed_segment(),
                                           current_lat_based_lqr_plus_controller_conf_.changing_rate());
    } else {
        AHC_compensating_value_ -= current_lat_based_lqr_plus_controller_conf_.ts()
                * InterpolationPlus1D::interpolation_1d(
                                           speed,
                                           current_lat_based_lqr_plus_controller_conf_.speed_segment(),
                                           current_lat_based_lqr_plus_controller_conf_.changing_rate());
    }
 
    // when finish compensating process, update compensated_value
    if (std::fabs(AHC_compensating_value_) > std::fabs(AHC_mean_)) {
        AHC_is_compensating_ = false;
        AHC_compensating_value_ = AHC_mean_;
        AHC_prev_compensated_value_ = AHC_compensated_value_;
    }
 
    return AHC_compensating_value_;
}

CalculateTempAHCValue()

double apollo::control::LatPlusController::CalculateTempAHCValue ( const double &  speed )

protected

在文件 lat_plus_controller.cc 第 1819 行定义.

                                                                   {
    double diff = AHC_compensated_value_ - AHC_prev_output_value_;
    double change_limit = current_lat_based_lqr_plus_controller_conf_.ts()
            * InterpolationPlus1D::interpolation_1d(
                                  speed,
                                  current_lat_based_lqr_plus_controller_conf_.speed_segment(),
                                  current_lat_based_lqr_plus_controller_conf_.changing_rate());
    diff = common::math::Clamp(diff, -change_limit, change_limit);
    double output_value = AHC_prev_output_value_ + diff;
    AHC_prev_output_value_ = output_value;
    return output_value;
}

CheckContinousErrors()

bool apollo::control::LatPlusController::CheckContinousErrors ( const uint8_t  window_size, const double  error, const double  threshold, std::vector< double > *  arr, int32_t *  current_sign  )

protected

在文件 lat_plus_controller.cc 第 1120 行定义.

                               {
    arr->push_back(error);
 
    if (arr->size() < window_size) {
        AERROR << "Not enough elements in the array.";
        return false;
    }
 
    if (arr->size() > window_size) {
        arr->erase(arr->begin(), arr->begin() + (arr->size() - window_size));
    }
 
    int8_t currentIndex = arr->size() - 1;
    int32_t current_direction = 0;
    for (int i = currentIndex; i > currentIndex - window_size; --i) {
        current_direction = ((*arr)[i] >= threshold) ? 1 : ((*arr)[i] <= -threshold) ? -1 : 0;
        ADEBUG << "current_direction is " << current_direction << " with: " << (*arr)[i] << " " << i;
        double current_error_abs = std::fabs((*arr)[i]);
        // &&(prev_abs_ >= current_error_abs && current_lat_based_lqr_plus_controller_conf.use_monotonicity_check())
        if (prev_sign_ != 0 && current_direction != 0 && prev_sign_ != current_direction) {
            ADEBUG << "Elements at positions " << i << " and " << i - 1 << " do not satisfy the condition.";
            current_direction = 0;
            *current_sign = current_direction;
            ADEBUG << "current_sign is " << *current_sign;
            return false;
        }
        prev_sign_ = current_direction;
        prev_abs_ = current_error_abs;
    }
    *current_sign = current_direction;
    ADEBUG << "current_sign is " << *current_sign;
    return true;
}

ChooseCurrentParam()

bool apollo::control::LatPlusController::ChooseCurrentParam ( LatBaseLqrPlusControllerConf &  current_lat_based_lqr_plus_controller_conf, const planning::ADCTrajectory *  planning_published_trajectory  )

protected

在文件 lat_plus_controller.cc 第 362 行定义.

                                                                    {
    std::string param_config_name = "public_road_forward_param";
 
    if (!IsForwardModel()) {
        param_config_name = "public_road_backward_param";
    }
 
    if (IsLargeCurvature(
                std::fabs(ref_curvature_last_),
                current_lat_based_lqr_plus_controller_conf_.min_large_ref_cur(),
                &is_in_large_curvature_)
        && IsForwardModel()) {
        param_config_name = "public_road_forward_s_turn_param";
    }
 
    if (planning_published_trajectory->trajectory_type() == ADCTrajectory::OPEN_SPACE) {
        if (IsForwardModel()) {
            param_config_name = "open_space_forward_param";
        } else {
            param_config_name = "open_space_backward_param";
        }
    }
 
    if (IsEdgeFollow()) {
        if (!IsLargeCurvature(
                    std::fabs(ref_curvature_last_),
                    current_lat_based_lqr_plus_controller_conf_.min_large_ref_cur(),
                    &is_in_large_curvature_)) {
            param_config_name = "edge_follow_public_road_forward_param";
        } else {
            if (IsLeftCurvature(ref_curvature_last_)) {
                param_config_name = "edge_follow_public_road_forward_sl_turn_param";
            } else {
                param_config_name = "edge_follow_public_road_forward_sr_turn_param";
            }
        }
    }
 
    if (planning_published_trajectory->debug().planning_data().scenario().scenario_plugin_type() == "ZONE_COVER") {
        if (!IsForwardModel()) {
            param_config_name = "zone_cover_backward_param";
        } else {
            param_config_name = "zone_cover_forward_param";
        }
    }
 
    if (!SetCurrentParam(param_config_name, current_lat_based_lqr_plus_controller_conf)) {
        AERROR << "Failed to set current lat param config: " << param_config_name;
        return false;
    } else {
        if (param_config_name != current_param_config_name_) {
            AINFO << "Update the current lat param config name is " << param_config_name;
            current_param_config_name_ = param_config_name;
        }
        return true;
    }
}

CloseLogFile()

void apollo::control::LatPlusController::CloseLogFile ( )

protected

在文件 lat_plus_controller.cc 第 339 行定义.

                                     {
    if (FLAGS_enable_csv_debug && steer_log_file_.is_open()) {
        steer_log_file_.close();
    }
}

ComputeControlCommand()

Status apollo::control::LatPlusController::ComputeControlCommand ( const localization::LocalizationEstimate *  localization, const canbus::Chassis *  chassis, const planning::ADCTrajectory *  trajectory, ControlCommand *  cmd  )

overridevirtual

compute steering target based on current vehicle status and target trajectory

参数

localization	vehicle location
chassis	vehicle status e.g., speed, acceleration
trajectory	trajectory generated by planning
cmd	control command

返回

Status computation status

实现了 apollo::control::ControlTask.

在文件 lat_plus_controller.cc 第 430 行定义.

                             {
    chassis_ = chassis;
 
    if (!VehicleStatusIdentificationUpdate(localization, chassis, planning_published_trajectory)) {
        AERROR << "Fail to update the vehicle status identification!";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to update the vehicle status identification!");
    }
 
    if (!ChooseCurrentParam(current_lat_based_lqr_plus_controller_conf_, planning_published_trajectory)) {
        AERROR << "Fail to load the current lon control param!";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "Fail to load the current lon control param!");
    }
 
    if (!UpdateControlConf(current_lat_based_lqr_plus_controller_conf_)) {
        AERROR << "failed to load control conf";
        return Status(ErrorCode::CONTROL_COMPUTE_ERROR, "failed to load control_conf");
    }
 
    LogInitParameters();
 
    leadlag_controller_.SetLeadlagConf(current_lat_based_lqr_plus_controller_conf_.reverse_leadlag_conf());
 
    auto vehicle_state = injector_->vehicle_state();
    auto previous_lon_debug = injector_->Get_previous_lon_debug_info();
    auto target_tracking_trajectory = *planning_published_trajectory;
 
    if (FLAGS_use_navigation_mode
        && current_lat_based_lqr_plus_controller_conf_.enable_navigation_mode_position_update()) {
        auto time_stamp_diff = planning_published_trajectory->header().timestamp_sec() - current_trajectory_timestamp_;
 
        auto curr_vehicle_x = localization->pose().position().x();
        auto curr_vehicle_y = localization->pose().position().y();
 
        double curr_vehicle_heading = 0.0;
        const auto &orientation = localization->pose().orientation();
        if (localization->pose().has_heading()) {
            curr_vehicle_heading = localization->pose().heading();
        } else {
            curr_vehicle_heading = common::math::QuaternionToHeading(
                    orientation.qw(), orientation.qx(), orientation.qy(), orientation.qz());
        }
 
        // new planning trajectory
        if (time_stamp_diff > 1.0e-6) {
            init_vehicle_x_ = curr_vehicle_x;
            init_vehicle_y_ = curr_vehicle_y;
            init_vehicle_heading_ = curr_vehicle_heading;
 
            current_trajectory_timestamp_ = planning_published_trajectory->header().timestamp_sec();
        } else {
            auto x_diff_map = curr_vehicle_x - init_vehicle_x_;
            auto y_diff_map = curr_vehicle_y - init_vehicle_y_;
            auto theta_diff = curr_vehicle_heading - init_vehicle_heading_;
 
            auto cos_map_veh = std::cos(init_vehicle_heading_);
            auto sin_map_veh = std::sin(init_vehicle_heading_);
 
            auto x_diff_veh = cos_map_veh * x_diff_map + sin_map_veh * y_diff_map;
            auto y_diff_veh = -sin_map_veh * x_diff_map + cos_map_veh * y_diff_map;
 
            auto cos_theta_diff = std::cos(-theta_diff);
            auto sin_theta_diff = std::sin(-theta_diff);
 
            auto tx = -(cos_theta_diff * x_diff_veh - sin_theta_diff * y_diff_veh);
            auto ty = -(sin_theta_diff * x_diff_veh + cos_theta_diff * y_diff_veh);
 
            auto ptr_trajectory_points = target_tracking_trajectory.mutable_trajectory_point();
            std::for_each(
                    ptr_trajectory_points->begin(),
                    ptr_trajectory_points->end(),
                    [&cos_theta_diff, &sin_theta_diff, &tx, &ty, &theta_diff](common::TrajectoryPoint &p) {
                        auto x = p.path_point().x();
                        auto y = p.path_point().y();
                        auto theta = p.path_point().theta();
 
                        auto x_new = cos_theta_diff * x - sin_theta_diff * y + tx;
                        auto y_new = sin_theta_diff * x + cos_theta_diff * y + ty;
                        auto theta_new = common::math::NormalizeAngle(theta - theta_diff);
 
                        p.mutable_path_point()->set_x(x_new);
                        p.mutable_path_point()->set_y(y_new);
                        p.mutable_path_point()->set_theta(theta_new);
                    });
        }
    }
 
    trajectory_analyzer_ = std::move(TrajectoryAnalyzerExtend(&target_tracking_trajectory));
 
    // Transform the coordinate of the planning trajectory from the center of the
    // rear-axis to the center of mass, if conditions matched
    if (((current_lat_based_lqr_plus_controller_conf_.trajectory_transform_to_com_reverse() && (!IsForwardModel()))
         || (current_lat_based_lqr_plus_controller_conf_.trajectory_transform_to_com_drive() && IsForwardModel()))
        && enable_look_ahead_back_control_) {
        trajectory_analyzer_.TrajectoryTransformToCOM(lr_);
    }
 
    // Re-build the vehicle dynamic models at reverse driving (in particular,
    // replace the lateral translational motion dynamics with the corresponding
    // kinematic models)
    if (!IsForwardModel()) {
        /*
        A matrix (Gear Reverse)
        [0.0, 0.0, 1.0 * v 0.0;
         0.0, (-(c_f + c_r) / m) / v, (c_f + c_r) / m,
         (l_r * c_r - l_f * c_f) / m / v;
         0.0, 0.0, 0.0, 1.0;
         0.0, ((lr * cr - lf * cf) / i_z) / v, (l_f * c_f - l_r * c_r) / i_z,
         (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
        */
        cf_ = -current_lat_based_lqr_plus_controller_conf_.cf();
        cr_ = -current_lat_based_lqr_plus_controller_conf_.cr();
        matrix_a_(0, 1) = 0.0;
        matrix_a_coeff_(0, 2) = 1.0;
    } else {
        /*
        A matrix (Gear Drive)
        [0.0, 1.0, 0.0, 0.0;
         0.0, (-(c_f + c_r) / m) / v, (c_f + c_r) / m,
         (l_r * c_r - l_f * c_f) / m / v;
         0.0, 0.0, 0.0, 1.0;
         0.0, ((lr * cr - lf * cf) / i_z) / v, (l_f * c_f - l_r * c_r) / i_z,
         (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
        */
        cf_ = current_lat_based_lqr_plus_controller_conf_.cf();
        cr_ = current_lat_based_lqr_plus_controller_conf_.cr();
        matrix_a_(0, 1) = 1.0;
        matrix_a_coeff_(0, 2) = 0.0;
    }
    matrix_a_(1, 2) = (cf_ + cr_) / mass_;
    matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
    matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
    matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
    matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
    matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;
 
    /*
    b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
    */
    matrix_b_(1, 0) = cf_ / mass_;
    matrix_b_(3, 0) = lf_ * cf_ / iz_;
    matrix_bd_ = matrix_b_ * ts_;
 
    UpdateDrivingOrientation();
 
    SimpleLateralDebug *debug = cmd->mutable_debug()->mutable_simple_lat_debug();
    debug->Clear();
    auto lat_debug_info = injector_->mutable_control_debug_info()->mutable_simple_lat_debug();
    lat_debug_info->Clear();
 
    // Update state = [Lateral Error, Lateral Error Rate, Heading Error, Heading
    // Error Rate, preview lateral error1 , preview lateral error2, ...]
    UpdateState(debug, lat_debug_info, chassis);
 
    UpdateMatrix();
 
    // Compound discrete matrix with road preview model
    UpdateMatrixCompound();
 
    // Adjust matrix_q_updated when in reverse gear
    int q_param_size = current_lat_based_lqr_plus_controller_conf_.matrix_q_size();
    for (int i = 0; i < q_param_size; ++i) {
        matrix_q_(i, i) = current_lat_based_lqr_plus_controller_conf_.matrix_q(i);
    }
 
    if (enable_enhance_qparam_ && (!previous_lon_debug->is_full_stop()) && (!previous_lon_debug->is_full_stop_soft())) {
        EnhanceQParamsAlternate(
                lat_debug_info->lateral_error_match_local(),
                lat_debug_info->heading_error_match_local(),
                planning_published_trajectory,
                previous_lon_debug,
                debug,
                lat_debug_info);
    }
 
    uint num_iteration;
    double result_diff;
    // Add gain scheduler for higher speed steering
    if (FLAGS_enable_gain_scheduler) {
        double curvature = std::fabs(debug->curvature());
        double speed = std::fabs(vehicle_state->linear_velocity());
 
        matrix_q_updated_(0, 0) = ed_date_ * matrix_q_(0, 0)
                * InterpolationPlus1D::interpolation_1d(
                                          speed,
                                          current_lat_based_lqr_plus_controller_conf_.lat_speed_input(),
                                          current_lat_based_lqr_plus_controller_conf_.lat_err_speed_gain_output())
                * InterpolationPlus1D::interpolation_1d(
                                          curvature,
                                          current_lat_based_lqr_plus_controller_conf_.lat_curvature_input(),
                                          current_lat_based_lqr_plus_controller_conf_.lat_err_curvature_gain_output());
 
        matrix_q_updated_(1, 1)
                = matrix_q_(1, 1)
                * InterpolationPlus1D::interpolation_1d(
                          speed,
                          current_lat_based_lqr_plus_controller_conf_.lat_speed_input(),
                          current_lat_based_lqr_plus_controller_conf_.lat_err_delta_speed_gain_output())
                * InterpolationPlus1D::interpolation_1d(
                          curvature,
                          current_lat_based_lqr_plus_controller_conf_.lat_curvature_input(),
                          current_lat_based_lqr_plus_controller_conf_.lat_err_delta_curvature_gain_output());
 
        matrix_q_updated_(2, 2)
                = efai_rate_ * matrix_q_(2, 2)
                * InterpolationPlus1D::interpolation_1d(
                          speed,
                          current_lat_based_lqr_plus_controller_conf_.lat_speed_input(),
                          current_lat_based_lqr_plus_controller_conf_.heading_err_speed_gain_output())
                * InterpolationPlus1D::interpolation_1d(
                          curvature,
                          current_lat_based_lqr_plus_controller_conf_.lat_curvature_input(),
                          current_lat_based_lqr_plus_controller_conf_.heading_err_curvature_gain_output());
 
        matrix_q_updated_(3, 3)
                = matrix_q_(3, 3)
                * InterpolationPlus1D::interpolation_1d(
                          speed,
                          current_lat_based_lqr_plus_controller_conf_.lat_speed_input(),
                          current_lat_based_lqr_plus_controller_conf_.heading_err_delta_speed_gain_output())
                * InterpolationPlus1D::interpolation_1d(
                          curvature,
                          current_lat_based_lqr_plus_controller_conf_.lat_curvature_input(),
                          current_lat_based_lqr_plus_controller_conf_.heading_err_delta_curvature_gain_output());
        ADEBUG << "current vehicle is " << speed << ", curvature is " << curvature;
        ADEBUG << "matrix_q_updated_(0, 0) is " << matrix_q_updated_(0, 0);
        ADEBUG << "matrix_q_updated_(1, 1) is " << matrix_q_updated_(1, 1);
        ADEBUG << "matrix_q_updated_(2, 2) is " << matrix_q_updated_(2, 2);
        ADEBUG << "matrix_q_updated_(3, 3) is " << matrix_q_updated_(3, 3);
 
        common::math::SolveLQRProblem(
                matrix_adc_,
                matrix_bdc_,
                matrix_q_updated_,
                matrix_r_,
                lqr_eps_,
                lqr_max_iteration_,
                &matrix_k_,
                &num_iteration,
                &result_diff);
    } else {
        common::math::SolveLQRProblem(
                matrix_adc_,
                matrix_bdc_,
                matrix_q_,
                matrix_r_,
                lqr_eps_,
                lqr_max_iteration_,
                &matrix_k_,
                &num_iteration,
                &result_diff);
    }
 
    ADEBUG << "LQR num_iteration is " << num_iteration << ", max iteration threshold is " << lqr_max_iteration_
           << "; result_diff is " << result_diff;
 
    lat_debug_info->set_lqr_iteration_num(num_iteration);
    lat_debug_info->set_lqr_result_diff(result_diff);
 
    // feedback = - K * state
    // Convert vehicle steer angle from rad to degree and then to steer degree
    // then to 100% ratio
    const double steer_angle_feedback
            = -(matrix_k_ * matrix_state_)(0, 0) * 180 / M_PI * steer_ratio_ / steer_single_direction_max_degree_ * 100;
 
    double curvature = 0.0;
    if (enable_look_ahead_back_control_) {
        if (use_new_look_ahead_back_) {
            if (!IsForwardModel()) {
                curvature = lat_debug_info->lookback_curvature_new();
            } else {
                curvature = lat_debug_info->lookahead_curvature_new();
            }
        } else {
            curvature = lat_debug_info->lookahead_curvature();
        }
    } else {
        curvature = debug->curvature();
    }
 
    const double steer_angle_feedforward = ComputeFeedForward(curvature);
 
    double steer_angle = 0.0;
    double steer_angle_feedback_augment = 0.0;
    // Augment the feedback control on lateral error at the desired speed domain
    if (enable_leadlag_) {
        if (current_lat_based_lqr_plus_controller_conf_.enable_feedback_augment_on_high_speed()
            || std::fabs(vehicle_state->linear_velocity()) < low_speed_bound_) {
            steer_angle_feedback_augment = leadlag_controller_.Control(-matrix_state_(0, 0), ts_) * 180 / M_PI
                    * steer_ratio_ / steer_single_direction_max_degree_ * 100;
            if (std::fabs(vehicle_state->linear_velocity()) > low_speed_bound_ - low_speed_window_) {
                // Within the low-high speed transition window, linerly interplolate the
                // augment control gain for "soft" control switch
                steer_angle_feedback_augment = common::math::lerp(
                        steer_angle_feedback_augment,
                        low_speed_bound_ - low_speed_window_,
                        0.0,
                        low_speed_bound_,
                        std::fabs(vehicle_state->linear_velocity()));
            }
        }
    }
    steer_angle = steer_angle_feedback + steer_angle_feedforward + steer_angle_feedback_augment;
 
    // Compute the steering command limit with the given maximum lateral
    // acceleration
    const double steer_limit = FLAGS_set_steer_limit
            ? std::atan(
                      max_lat_acc_ * wheelbase_ / (vehicle_state->linear_velocity() * vehicle_state->linear_velocity()))
                    * steer_ratio_ * 180 / M_PI / steer_single_direction_max_degree_ * 100
            : 100.0;
 
    const double steer_diff_with_max_rate
            = current_lat_based_lqr_plus_controller_conf_.enable_maximum_steer_rate_limit()
            ? vehicle_param_.max_steer_angle_rate() * ts_ * 180 / M_PI / steer_single_direction_max_degree_ * 100
            : 100.0;
 
    const double steering_position = chassis->steering_percentage();
 
    // Re-compute the steering command if the MRAC control is enabled, with steer
    // angle limitation and steer rate limitation
    if (enable_mrac_) {
        const int mrac_model_order = current_lat_based_lqr_plus_controller_conf_.steer_mrac_conf().mrac_model_order();
        Matrix steer_state = Matrix::Zero(mrac_model_order, 1);
        steer_state(0, 0) = chassis->steering_percentage();
        if (mrac_model_order > 1) {
            steer_state(1, 0) = (steering_position - pre_steering_position_) / ts_;
        }
        if (std::fabs(vehicle_state->linear_velocity()) > FLAGS_minimum_speed_resolution) {
            mrac_controller_.SetStateAdaptionRate(1.0);
            mrac_controller_.SetInputAdaptionRate(1.0);
        } else {
            mrac_controller_.SetStateAdaptionRate(0.0);
            mrac_controller_.SetInputAdaptionRate(0.0);
        }
        steer_angle = mrac_controller_.Control(steer_angle, steer_state, steer_limit, steer_diff_with_max_rate / ts_);
        // Set the steer mrac debug message
        MracDebug *mracdebug = debug->mutable_steer_mrac_debug();
        Matrix steer_reference = mrac_controller_.CurrentReferenceState();
        mracdebug->set_mrac_model_order(mrac_model_order);
        for (int i = 0; i < mrac_model_order; ++i) {
            mracdebug->add_mrac_reference_state(steer_reference(i, 0));
            mracdebug->add_mrac_state_error(steer_state(i, 0) - steer_reference(i, 0));
            mracdebug->mutable_mrac_adaptive_gain()->add_state_adaptive_gain(
                    mrac_controller_.CurrentStateAdaptionGain()(i, 0));
        }
        mracdebug->mutable_mrac_adaptive_gain()->add_input_adaptive_gain(
                mrac_controller_.CurrentInputAdaptionGain()(0, 0));
        mracdebug->set_mrac_reference_saturation_status(mrac_controller_.ReferenceSaturationStatus());
        mracdebug->set_mrac_control_saturation_status(mrac_controller_.ControlSaturationStatus());
    }
    pre_steering_position_ = steering_position;
    debug->set_steer_mrac_enable_status(enable_mrac_);
 
    // Clamp the steer angle with steer limitations at current speed
    double steer_angle_limited = common::math::Clamp(steer_angle, -steer_limit, steer_limit);
    steer_angle = steer_angle_limited;
    debug->set_steer_angle_limited(steer_angle_limited);
    // Limit the steering command with the designed digital filter
    steer_angle = digital_filter_.Filter(steer_angle);
    steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);
 
    // Check if the steer is locked and hence the previous steer angle should be
    // executed
    if (injector_->vehicle_state()->gear() != canbus::Chassis::GEAR_REVERSE) {
        if ((std::abs(vehicle_state->linear_velocity()) < current_lat_based_lqr_plus_controller_conf_.lock_steer_speed()
             || previous_lon_debug->path_remain() <= 0)
            && vehicle_state->gear() == canbus::Chassis::GEAR_DRIVE
            && chassis->driving_mode() == canbus::Chassis::COMPLETE_AUTO_DRIVE) {
            ADEBUG << "Into lock steer, path_remain is " << previous_lon_debug->path_remain() << "linear_velocity is "
                   << vehicle_state->linear_velocity();
            lat_debug_info->set_is_lock_steer(true);
            steer_angle = pre_steer_angle_;
        } else {
            lat_debug_info->set_is_lock_steer(false);
        }
    }
 
    // Set the steer commands
    cmd->set_steering_target(common::math::Clamp(
            steer_angle, pre_steer_angle_ - steer_diff_with_max_rate, pre_steer_angle_ + steer_diff_with_max_rate));
    cmd->set_steering_rate(FLAGS_steer_angle_rate);
 
    pre_steer_angle_ = cmd->steering_target();
 
    // compute extra information for logging and debugging
    const double steer_angle_lateral_contribution = -matrix_k_(0, 0) * matrix_state_(0, 0) * 180 / M_PI * steer_ratio_
            / steer_single_direction_max_degree_ * 100;
 
    const double steer_angle_lateral_rate_contribution = -matrix_k_(0, 1) * matrix_state_(1, 0) * 180 / M_PI
            * steer_ratio_ / steer_single_direction_max_degree_ * 100;
 
    const double steer_angle_heading_contribution = -matrix_k_(0, 2) * matrix_state_(2, 0) * 180 / M_PI * steer_ratio_
            / steer_single_direction_max_degree_ * 100;
 
    const double steer_angle_heading_rate_contribution = -matrix_k_(0, 3) * matrix_state_(3, 0) * 180 / M_PI
            * steer_ratio_ / steer_single_direction_max_degree_ * 100;
 
    debug->set_heading(driving_orientation_);
    debug->set_steer_angle(steer_angle);
    debug->set_steer_angle_feedforward(steer_angle_feedforward);
    debug->set_steer_angle_lateral_contribution(steer_angle_lateral_contribution);
    debug->set_steer_angle_lateral_rate_contribution(steer_angle_lateral_rate_contribution);
    debug->set_steer_angle_heading_contribution(steer_angle_heading_contribution);
    debug->set_steer_angle_heading_rate_contribution(steer_angle_heading_rate_contribution);
    debug->set_steer_angle_feedback(steer_angle_feedback);
    debug->set_steer_angle_feedback_augment(steer_angle_feedback_augment);
    debug->set_steering_position(steering_position);
    debug->set_ref_speed(vehicle_state->linear_velocity());
 
    lat_debug_info->set_efai_rate(efai_rate_);
    lat_debug_info->set_ed_rate(ed_date_);
    lat_debug_info->set_enable_look_ahead_back_control(enable_look_ahead_back_control_);
    lat_debug_info->set_is_in_large_curvature(is_in_large_curvature_);
 
    ProcessLogs(debug, chassis);
    return Status::OK();
}

ComputeFeedForward()

double apollo::control::LatPlusController::ComputeFeedForward ( double  ref_curvature )

protected

在文件 lat_plus_controller.cc 第 1191 行定义.

                                                                 {
    const double kv = lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
 
    // Calculate the feedforward term of the lateral controller; then change it
    // from rad to %
    const double v = injector_->vehicle_state()->linear_velocity();
    double steer_angle_feedforwardterm = 0.0;
    if (!IsForwardModel() && !current_lat_based_lqr_plus_controller_conf_.reverse_use_dynamic_model()) {
        steer_angle_feedforwardterm = current_lat_based_lqr_plus_controller_conf_.reverse_feedforward_ratio()
                * wheelbase_ * ref_curvature * 180 / M_PI * steer_ratio_ / steer_single_direction_max_degree_ * 100;
    } else {
        steer_angle_feedforwardterm
                = (wheelbase_ * ref_curvature + kv * v * v * ref_curvature
                   - matrix_k_(0, 2)
                           * (lr_ * ref_curvature - lf_ * mass_ * v * v * ref_curvature / 2 / cr_ / wheelbase_))
                * 180 / M_PI * steer_ratio_ / steer_single_direction_max_degree_ * 100;
    }
 
    return steer_angle_feedforwardterm;
}

ComputeLateralErrors()

void apollo::control::LatPlusController::ComputeLateralErrors ( const double  x, const double  y, const double  theta, const double  linear_v, const double  angular_v, const double  linear_a, const TrajectoryAnalyzerExtend &  trajectory_analyzer, const canbus::Chassis *  chassis, const enum ChooseRefTrajectoryPoint &  choose_point, SimpleLateralDebug *  debug, SimpleLateralPlusDebug *  lat_debug  )

protected

在文件 lat_plus_controller.cc 第 1291 行定义.

                                           {
    TrajectoryPoint target_point;
 
    if (current_lat_based_lqr_plus_controller_conf_.query_time_nearest_point_only()) {
        target_point
                = trajectory_analyzer.QueryNearestPointByAbsoluteTime(Clock::NowInSeconds() + query_relative_time_);
    } else {
        if (FLAGS_use_navigation_mode
            && !current_lat_based_lqr_plus_controller_conf_.enable_navigation_mode_position_update()) {
            target_point
                    = trajectory_analyzer.QueryNearestPointByAbsoluteTime(Clock::NowInSeconds() + query_relative_time_);
        } else {
            target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
        }
    }
    ADEBUG << "x point: " << x << " y point: " << y;
    ADEBUG << "match trajectory point information : " << target_point.ShortDebugString();
 
    auto vehicle_state = injector_->vehicle_state();
 
    // target_lr
    const auto &com_lr = vehicle_state->ComputeCOMPosition(lr_);
    lat_debug->set_lr(lr_);
    lat_debug->mutable_lat_lr_position()->set_x(com_lr.x());
    lat_debug->mutable_lat_lr_position()->set_y(com_lr.y());
    auto target_point_lr = trajectory_analyzer.QueryNearestPointByPosition(com_lr.x(), com_lr.y());
    lat_debug->mutable_target_point_lr()->mutable_path_point()->set_x(target_point_lr.path_point().x());
    lat_debug->mutable_target_point_lr()->mutable_path_point()->set_y(target_point_lr.path_point().y());
    double s_target_lr, s_dot_target_lr, d_target_lr, d_dot_target_lr;
    trajectory_analyzer.ToTrajectoryFrame(
            com_lr.x(),
            com_lr.y(),
            vehicle_state->heading(),
            linear_v,
            target_point.path_point(),
            &s_target_lr,
            &s_dot_target_lr,
            &d_target_lr,
            &d_dot_target_lr);
    debug->set_ref_heading(target_point_lr.path_point().theta());
    lat_debug->set_curvature_target_lr(target_point_lr.path_point().kappa());
    lat_debug->set_lateral_error_target_lr(d_target_lr);
    lat_debug->set_heading_error_target_lr(common::math::NormalizeAngle(theta - target_point_lr.path_point().theta()));
 
    // target_com
    target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
    debug->mutable_current_target_point()->mutable_path_point()->set_x(target_point.path_point().x());
    debug->mutable_current_target_point()->mutable_path_point()->set_y(target_point.path_point().y());
    double s_target_com, s_dot_target_com, d_target_com, d_dot_target_com;
    trajectory_analyzer.ToTrajectoryFrame(
            x,
            y,
            vehicle_state->heading(),
            linear_v,
            target_point.path_point(),
            &s_target_com,
            &s_dot_target_com,
            &d_target_com,
            &d_dot_target_com);
    debug->set_ref_heading(target_point.path_point().theta());
    lat_debug->set_curvature_target_com(target_point.path_point().kappa());
    lat_debug->set_lateral_error_target_com(d_target_com);
    lat_debug->set_heading_error_target_com(common::math::NormalizeAngle(theta - target_point.path_point().theta()));
 
    // match_com
    auto matched_point_com = trajectory_analyzer.QueryMatchedPathPoint(x, y);
    lat_debug->mutable_matched_point_com()->mutable_path_point()->set_x(matched_point_com.x());
    lat_debug->mutable_matched_point_com()->mutable_path_point()->set_y(matched_point_com.y());
    double s_match_com, s_dot_match_com, d_match_com, d_dot_match_com;
    trajectory_analyzer.ToTrajectoryFrame(
            x, y, theta, linear_v, matched_point_com, &s_match_com, &s_dot_match_com, &d_match_com, &d_dot_match_com);
    debug->set_ref_heading(matched_point_com.theta());
    lat_debug->set_curvature_match_com(matched_point_com.kappa());
    lat_debug->set_lateral_error_match_com(d_match_com);
    lat_debug->set_heading_error_match_com(common::math::NormalizeAngle(theta - matched_point_com.theta()));
 
    // match_local
    auto matched_point_local = trajectory_analyzer.QueryMatchedPathPoint(vehicle_state->x(), vehicle_state->y());
    lat_debug->mutable_matched_point_local()->mutable_path_point()->set_x(matched_point_local.x());
    lat_debug->mutable_matched_point_local()->mutable_path_point()->set_y(matched_point_local.y());
    double s_match_local, s_dot_match_local, d_match_local, d_dot_match_local;
    trajectory_analyzer.ToTrajectoryFrame(
            vehicle_state->x(),
            vehicle_state->y(),
            theta,
            linear_v,
            matched_point_local,
            &s_match_local,
            &s_dot_match_local,
            &d_match_local,
            &d_dot_match_local);
    debug->set_ref_heading(matched_point_local.theta());
    lat_debug->set_curvature_match_local(matched_point_local.kappa());
    lat_debug->set_lateral_error_match_local(d_match_local);
    lat_debug->set_heading_error_match_local(common::math::NormalizeAngle(theta - matched_point_local.theta()));
 
    // absolute_local
    double current_absolute_time = FLAGS_sim_by_record
            ? (chassis_->header().timestamp_sec() + query_relative_time_)
            : (::apollo::cyber::Clock::NowInSeconds() + query_relative_time_);
    auto absolute_point_local = trajectory_analyzer.QueryNearestPointByAbsoluteTime(current_absolute_time);
    lat_debug->mutable_absolute_point_local()->mutable_path_point()->set_x(absolute_point_local.path_point().x());
    lat_debug->mutable_absolute_point_local()->mutable_path_point()->set_y(absolute_point_local.path_point().y());
    double s_absolute_local, s_dot_absolute_local, d_absolute_local, d_dot_absolute_local;
    trajectory_analyzer.ToTrajectoryFrame(
            vehicle_state->x(),
            vehicle_state->y(),
            theta,
            linear_v,
            absolute_point_local.path_point(),
            &s_absolute_local,
            &s_dot_absolute_local,
            &d_absolute_local,
            &d_dot_absolute_local);
    debug->set_ref_heading(absolute_point_local.path_point().theta());
    lat_debug->set_curvature_absolute_local(absolute_point_local.path_point().kappa());
    lat_debug->set_lateral_error_absolute_local(d_absolute_local);
    lat_debug->set_heading_error_absolute_local(
            common::math::NormalizeAngle(theta - absolute_point_local.path_point().theta()));
 
    double lateral_error = 0.0;
    double heading_error = 0.0;
    double curvature = 0.0;
 
    switch (choose_point) {
    case control::ChooseRefTrajectoryPoint::USE_LAT_TARGET_COM: {
        lateral_error = lat_debug->lateral_error_target_com();
        heading_error = lat_debug->heading_error_target_com();
        curvature = lat_debug->curvature_target_com();
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_COM: {
        lateral_error = lat_debug->lateral_error_match_com();
        heading_error = lat_debug->heading_error_match_com();
        curvature = lat_debug->curvature_match_com();
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_LOCAL: {
        lateral_error = lat_debug->lateral_error_match_local();
        heading_error = lat_debug->heading_error_match_local();
        curvature = lat_debug->curvature_match_local();
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_ABSOLUTE_LOCAL: {
        lateral_error = lat_debug->lateral_error_absolute_local();
        heading_error = lat_debug->heading_error_absolute_local();
        curvature = lat_debug->curvature_absolute_local();
        break;
    }
    default: {
        const double dx = com_lr.x() - target_point.path_point().x();
        const double dy = com_lr.y() - target_point.path_point().y();
        const double cos_target_heading = std::cos(target_point.path_point().theta());
        const double sin_target_heading = std::sin(target_point.path_point().theta());
 
        double lateral_error = cos_target_heading * dy - sin_target_heading * dx;
        if (current_lat_based_lqr_plus_controller_conf_.enable_navigation_mode_error_filter()) {
            lateral_error = lateral_error_filter_.Update(lateral_error);
        }
        debug->set_ref_heading(target_point.path_point().theta());
        heading_error = common::math::NormalizeAngle(theta - debug->ref_heading());
        curvature = target_point.path_point().kappa();
        break;
    }
    }
 
    // Set curvature
    debug->set_curvature(curvature);
    lat_debug->set_curvature_change(debug->curvature() - ref_curvature_last_);
    ref_curvature_last_ = debug->curvature();
 
    // Set lateral error and heading error origin
    if (std::fabs(lat_debug->lateral_error_match_com()) <= std::fabs(lat_debug->lateral_error_match_local())) {
        debug->set_lateral_error(lat_debug->lateral_error_match_com());
        lat_debug->set_current_min_error_point(control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_COM);
    } else {
        debug->set_lateral_error(lat_debug->lateral_error_match_local());
        lat_debug->set_current_min_error_point(control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_LOCAL);
    }
    if (std::fabs(lat_debug->heading_error_match_com()) <= std::fabs(lat_debug->heading_error_match_local())) {
        debug->set_heading_error(lat_debug->heading_error_match_com());
    } else {
        debug->set_heading_error(lat_debug->heading_error_match_local());
    }
 
    // Set the lateral error lqr input
    lat_debug->set_lateral_error_lqr_input(lateral_error);
    lat_debug->set_lateral_error_lqr_input_change(
            lat_debug->lateral_error_lqr_input() - previous_lateral_error_lqr_input_);
    previous_lateral_error_lqr_input_ = lat_debug->lateral_error_lqr_input();
 
    // set heading error
    lat_debug->set_heading_error_by_ref(heading_error);
    if (current_lat_based_lqr_plus_controller_conf_.enable_navigation_mode_error_filter()) {
        heading_error = heading_error_filter_.Update(heading_error);
    }
    // the car may have inherent steer error, the method below can handle this
    // error
    if (current_lat_based_lqr_plus_controller_conf_.use_heading_auto_compensation()) {
        double compensation_output_value = CalculateTempAHCValue(linear_v);
        heading_error -= compensation_output_value;
        lat_debug->set_heading_error_by_auto_compensation(heading_error);
        injector_->mutable_control_debug_info()
                ->mutable_simple_lat_debug()
                ->mutable_ahc_value()
                ->set_compensation_output_value(compensation_output_value);
        if (IsDrivingStraight(chassis, target_point_lr.path_point().kappa())) {
            UpdateAHCParam(heading_error, injector_->vehicle_state()->linear_velocity());
        } else {
            AHCClear();
        }
    }
    // Set the heading error lqr input
    lat_debug->set_heading_error_lqr_input(heading_error);
    lat_debug->set_heading_error_lqr_input_change(
            lat_debug->heading_error_lqr_input() - previous_heading_error_lqr_input_);
    previous_heading_error_lqr_input_ = lat_debug->heading_error_lqr_input();
 
    // Within the low-high speed transition window, linerly interplolate the
    // lookahead/lookback station for "soft" prediction window switch
    double lookahead_station = 0.0;
    double lookback_station = 0.0;
    if (std::fabs(linear_v) >= low_speed_bound_) {
        lookahead_station = lookahead_station_high_speed_;
        lookback_station = lookback_station_high_speed_;
    } else if (std::fabs(linear_v) < low_speed_bound_ - low_speed_window_) {
        lookahead_station = lookahead_station_low_speed_;
        lookback_station = lookback_station_low_speed_;
    } else {
        lookahead_station = common::math::lerp(
                lookahead_station_low_speed_,
                low_speed_bound_ - low_speed_window_,
                lookahead_station_high_speed_,
                low_speed_bound_,
                std::fabs(linear_v));
        lookback_station = common::math::lerp(
                lookback_station_low_speed_,
                low_speed_bound_ - low_speed_window_,
                lookback_station_high_speed_,
                low_speed_bound_,
                std::fabs(linear_v));
    }
    lat_debug->set_lookahead_station(lookahead_station);
    lat_debug->set_lookback_station(lookback_station);
 
    // update lookforward point error
    switch (choose_point) {
    case control::ChooseRefTrajectoryPoint::USE_LAT_TARGET_COM: {
        UpdateLookForwardPointError(
                lookahead_station,
                lookback_station,
                x,
                y,
                theta,
                linear_v,
                target_point,
                trajectory_analyzer,
                debug,
                lat_debug);
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_COM: {
        UpdateLookForwardPointError(
                lookahead_station,
                lookback_station,
                x,
                y,
                theta,
                linear_v,
                target_point,
                trajectory_analyzer,
                debug,
                lat_debug);
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_LOCAL: {
        UpdateLookForwardPointError(
                lookahead_station,
                lookback_station,
                vehicle_state->x(),
                vehicle_state->y(),
                theta,
                linear_v,
                absolute_point_local,
                trajectory_analyzer,
                debug,
                lat_debug);
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_ABSOLUTE_LOCAL: {
        UpdateLookForwardPointError(
                lookahead_station,
                lookback_station,
                vehicle_state->x(),
                vehicle_state->y(),
                theta,
                linear_v,
                absolute_point_local,
                trajectory_analyzer,
                debug,
                lat_debug);
        break;
    }
    default: {
        UpdateLookForwardPointError(
                lookahead_station,
                lookback_station,
                com_lr.x(),
                com_lr.y(),
                theta,
                linear_v,
                target_point_lr,
                trajectory_analyzer,
                debug,
                lat_debug);
        break;
    }
    }
 
    LateralSafetyCheck(debug, lat_debug);
 
    auto lateral_error_dot = linear_v * std::sin(heading_error);
    auto lateral_error_dot_dot = linear_a * std::sin(heading_error);
    if (FLAGS_reverse_heading_control) {
        if (!IsForwardModel()) {
            lateral_error_dot = -lateral_error_dot;
            lateral_error_dot_dot = -lateral_error_dot_dot;
        }
    }
    auto centripetal_acceleration = linear_v * linear_v / wheelbase_
            * std::tan(chassis->steering_percentage() / 100 * vehicle_param_.max_steer_angle() / steer_ratio_);
    debug->set_lateral_error_rate(lateral_error_dot);
    debug->set_lateral_acceleration(lateral_error_dot_dot);
    debug->set_lateral_centripetal_acceleration(centripetal_acceleration);
    debug->set_lateral_jerk((debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
    previous_lateral_acceleration_ = debug->lateral_acceleration();
 
    if (!IsForwardModel()) {
        debug->set_heading_rate(-angular_v);
    } else {
        debug->set_heading_rate(angular_v);
    }
    debug->set_ref_heading_rate(target_point.path_point().kappa() * target_point.v());
    debug->set_heading_error_rate(debug->heading_rate() - debug->ref_heading_rate());
 
    debug->set_heading_acceleration((debug->heading_rate() - previous_heading_rate_) / ts_);
    debug->set_ref_heading_acceleration((debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);
    debug->set_heading_error_acceleration(debug->heading_acceleration() - debug->ref_heading_acceleration());
    previous_heading_rate_ = debug->heading_rate();
    previous_ref_heading_rate_ = debug->ref_heading_rate();
 
    debug->set_heading_jerk((debug->heading_acceleration() - previous_heading_acceleration_) / ts_);
    debug->set_ref_heading_jerk((debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) / ts_);
    debug->set_heading_error_jerk(debug->heading_jerk() - debug->ref_heading_jerk());
    previous_heading_acceleration_ = debug->heading_acceleration();
    previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
}

ComputeTrackLateralErrors()

void apollo::control::LatPlusController::ComputeTrackLateralErrors ( const double  x, const double  y, const double  theta, const double  linear_v, const TrajectoryAnalyzerExtend &  trajectory_analyzer, const enum ChooseRefTrajectoryPoint &  choose_track_point, SimpleLateralPlusDebug *  lat_debug  )

protected

在文件 lat_plus_controller.cc 第 1212 行定义.

                                           {
    double lateral_error = 0.0;
    double heading_error = 0.0;
    double curvature = 0.0;
    PathPoint matched_track_point_com;
    switch (choose_point) {
    case control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_LR: {
        matched_track_point_com.set_x(lat_debug->target_point_lr().path_point().x());
        matched_track_point_com.set_y(lat_debug->target_point_lr().path_point().y());
        lateral_error = lat_debug->lateral_error_target_lr();
        heading_error = lat_debug->heading_error_target_lr();
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_LOCAL: {
        matched_track_point_com.set_x(lat_debug->matched_point_local().path_point().x());
        matched_track_point_com.set_y(lat_debug->matched_point_local().path_point().y());
        lateral_error = lat_debug->lateral_error_match_local();
        heading_error = lat_debug->heading_error_match_local();
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_ABSOLUTE_LOCAL: {
        matched_track_point_com.set_x(lat_debug->absolute_point_local().path_point().x());
        matched_track_point_com.set_y(lat_debug->absolute_point_local().path_point().y());
        lateral_error = lat_debug->lateral_error_absolute_local();
        heading_error = lat_debug->heading_error_absolute_local();
        break;
    }
    case control::ChooseRefTrajectoryPoint::USE_LAT_TRACK_COM: {
        // match_track_com
        matched_track_point_com = trajectory_analyzer.QueryMatchedPathPoint(x, y);
        double s_track_com, s_dot_track_com, d_track_com, d_dot_track_com;
        trajectory_analyzer.ToTrajectoryFrame(
                x,
                y,
                theta,
                linear_v,
                matched_track_point_com,
                &s_track_com,
                &s_dot_track_com,
                &d_track_com,
                &d_dot_track_com);
 
        lateral_error = d_track_com;
        heading_error = common::math::NormalizeAngle(theta - matched_track_point_com.theta());
        break;
    }
    default: {
        // match_track_com
        matched_track_point_com = trajectory_analyzer.QueryMatchedPathPoint(x, y);
        double s_track_com, s_dot_track_com, d_track_com, d_dot_track_com;
        trajectory_analyzer.ToTrajectoryFrame(
                x,
                y,
                theta,
                linear_v,
                matched_track_point_com,
                &s_track_com,
                &s_dot_track_com,
                &d_track_com,
                &d_dot_track_com);
 
        lateral_error = d_track_com;
        heading_error = common::math::NormalizeAngle(theta - matched_track_point_com.theta());
        break;
    }
    }
    lat_debug->mutable_track_point_com()->mutable_path_point()->set_x(matched_track_point_com.x());
    lat_debug->mutable_track_point_com()->mutable_path_point()->set_y(matched_track_point_com.y());
    lat_debug->set_lateral_error_track_com(lateral_error);
    lat_debug->set_heading_error_track_com(heading_error);
}

EnhanceQParamsAlternate()

void apollo::control::LatPlusController::EnhanceQParamsAlternate ( const double  lateral_error, const double  heading_error, const planning::ADCTrajectory *  planning_published_trajectory, const SimpleLongitudinalDebug *  previous_lon_debug, SimpleLateralDebug *  debug, SimpleLateralPlusDebug *  debug_info  )

protected

在文件 lat_plus_controller.cc 第 1864 行定义.

                                            {
    if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
        ADEBUG << "[AL new reverse]" << "count_ = " << reverse_handle_count_;
        ADEBUG << "[AL new reverse]" << "heading err = " << debug_info->heading_error_lqr_input();
        ADEBUG << "[AL new reverse]" << "lat err = " << debug_info->lateral_error_lqr_input();
        if (now_reverse_stage_ == ReverseLogicStage::NO_REVERSE) {
            ADEBUG << "[AL new reverse]" << "first in reverse";
            now_reverse_stage_ = ReverseLogicStage::FIRST_HEADING_HANDLE_STAGE;
        } else if (now_reverse_stage_ == ReverseLogicStage::FIRST_HEADING_HANDLE_STAGE) {
            reverse_handle_count_++;
            ADEBUG << "[AL new reverse]" << "in 000000000000 stage damagely change heading ";
            // quit satuation
            if (reverse_handle_count_ >= current_lat_based_lqr_plus_controller_conf_.first_handle_heading_window()
                || debug_info->heading_error_lqr_input()
                        < current_lat_based_lqr_plus_controller_conf_.heading_err_threshold()) {
                ADEBUG << "[AL new reverse]" << "quit 00000000000 stage==============";
                now_reverse_stage_ = ReverseLogicStage::QUIT_FIRST_HEADING_HANDLE_STAGE;
                reverse_handle_count_ = 0;
                efai_rate_ = 1.0;
                ed_date_ = 1.0;
            } else {
                efai_rate_ = current_lat_based_lqr_plus_controller_conf_.first_handle_heading_stage_prarm();
                ed_date_ = 1.0;
            }
 
        } else if (now_reverse_stage_ == ReverseLogicStage::QUIT_FIRST_HEADING_HANDLE_STAGE) {
            ADEBUG << "[AL new reverse]" << "in 1111111111111 clear stage";
            reverse_handle_count_++;
            if (reverse_handle_count_
                >= current_lat_based_lqr_plus_controller_conf_.out_first_handle_heading_window()) {
                ADEBUG << "[AL new reverse]" << "quit 111111111 stage==============";
                now_reverse_stage_ = ReverseLogicStage::HANDLE_LAT_ERR_STAGE;
                reverse_handle_count_ = 0;
                efai_rate_ = 1.0;
                ed_date_ = 1.0;
            }
 
        } else if (now_reverse_stage_ == ReverseLogicStage::HANDLE_LAT_ERR_STAGE) {
            ADEBUG << "[AL new reverse]" << "in 222222222 lat err stage";
            reverse_handle_count_++;
            if ((reverse_handle_count_ >= current_lat_based_lqr_plus_controller_conf_.handle_lat_err_window())
                || (std::fabs(debug_info->lateral_error_lqr_input())
                    < current_lat_based_lqr_plus_controller_conf_.lat_err_threshold())
                || (std::fabs(debug->curvature())
                    < current_lat_based_lqr_plus_controller_conf_.openspace_enhance_curv())
                || (std::fabs(previous_lon_debug->path_remain())
                    < current_lat_based_lqr_plus_controller_conf_.reverse_final_heading_check_path())) {
                ADEBUG << "[AL new reverse]" << "quit 222222222 stage==============";
                now_reverse_stage_ = ReverseLogicStage::HANDLE_HEADING_ERR_STAGE;
                reverse_handle_count_ = 0;
                efai_rate_ = 1.0;
                ed_date_ = 1.0;
            } else if (
                    (pre_steer_angle_ <= (-1) * current_lat_based_lqr_plus_controller_conf_.reverse_dead_steer_left())
                    && (debug_info->heading_error_lqr_input()
                        > current_lat_based_lqr_plus_controller_conf_.reverse_too_big_heading_err())
                    && (debug_info->lateral_error_lqr_input()
                        > current_lat_based_lqr_plus_controller_conf_.reverse_too_big_lat_err())) {
                // right behind steer
                AINFO << "[STEERDEAD quit ,right behind ]" << "quit lat stage=======";
                now_reverse_stage_ = ReverseLogicStage::HANDLE_HEADING_ERR_STAGE;
                reverse_handle_count_ = 0;
                efai_rate_ = 1.0;
                ed_date_ = 1.0;
            } else if (
                    (pre_steer_angle_ >= current_lat_based_lqr_plus_controller_conf_.reverse_dead_steer_left())
                    && (debug_info->heading_error_lqr_input()
                        < (-1) * current_lat_based_lqr_plus_controller_conf_.reverse_too_big_heading_err())
                    && (debug_info->lateral_error_lqr_input()
                        < (-1) * current_lat_based_lqr_plus_controller_conf_.reverse_too_big_lat_err())) {
                // left behind steer
                AINFO << "[STEERDEAD quit ,left behind ]" << "quit lat stage=======";
                now_reverse_stage_ = ReverseLogicStage::HANDLE_HEADING_ERR_STAGE;
                reverse_handle_count_ = 0;
                efai_rate_ = 1.0;
                ed_date_ = 1.0;
            } else {
                efai_rate_ = current_lat_based_lqr_plus_controller_conf_.handle_lat_err_prarm_ignore();
                ed_date_ = current_lat_based_lqr_plus_controller_conf_.handle_lat_err_prarm_domain();
            }
 
        } else if (now_reverse_stage_ == ReverseLogicStage::HANDLE_HEADING_ERR_STAGE) {
            ADEBUG << "[AL new reverse]" << "in 33333333333 heading err stage";
            reverse_handle_count_++;
            if (reverse_handle_count_ >= current_lat_based_lqr_plus_controller_conf_.handle_heading_err_window()
                || std::fabs(debug_info->heading_error_lqr_input())
                        < current_lat_based_lqr_plus_controller_conf_.heading_err_threshold()
                || std::fabs(debug->curvature())
                        < current_lat_based_lqr_plus_controller_conf_.openspace_enhance_curv()) {
                ADEBUG << "[AL new reverse]" << "quit 333333333 stage==============";
                now_reverse_stage_ = ReverseLogicStage::HANDLE_LAT_ERR_STAGE;
                reverse_handle_count_ = 0;
                efai_rate_ = 1.0;
                ed_date_ = 1.0;
            } else {
                efai_rate_ = current_lat_based_lqr_plus_controller_conf_.handle_heading_err_parm_domain();
                ed_date_ = current_lat_based_lqr_plus_controller_conf_.handle_heading_err_parm_ignore();
            }
        } else {
            ADEBUG << "[AL new reverse]" << "wrong!!!!! don't change any param";
        }
    }
    if (injector_->vehicle_state()->gear() != canbus::Chassis::GEAR_REVERSE) {
        if (now_reverse_stage_ != ReverseLogicStage::NO_REVERSE) {
            ADEBUG << "[AL new reverse]" << "quit reverse ,clear all marks";
            now_reverse_stage_ = ReverseLogicStage::NO_REVERSE;
            reverse_handle_count_ = 0;
            efai_rate_ = 1.0;
            ed_date_ = 1.0;
        }
    }
 
    if ((injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE)
        && (std::fabs(previous_lon_debug->path_remain())
            < current_lat_based_lqr_plus_controller_conf_.reverse_final_heading_check_path())
        && (std::fabs(debug_info->heading_error_lqr_input())
            > current_lat_based_lqr_plus_controller_conf_.reverse_final_heading_check_heading())
        && (planning_published_trajectory->trajectory_type() == ADCTrajectory::OPEN_SPACE)) {
        ADEBUG << "[FINALCHECK]final check in reverse park";
        efai_rate_ = current_lat_based_lqr_plus_controller_conf_.handle_heading_err_parm_domain();
        ed_date_ = current_lat_based_lqr_plus_controller_conf_.handle_heading_err_parm_ignore();
    }
 
    debug_info->set_now_reverse_stage(now_reverse_stage_);
}

Init()

Status apollo::control::LatPlusController::Init ( std::shared_ptr< DependencyInjector >  injector )

overridevirtual

initialize Lateral Controller

参数

control_conf

control configurations

返回

Status initialization status

实现了 apollo::control::ControlTask.

在文件 lat_plus_controller.cc 第 276 行定义.

                                                                         {
    if (!ControlTask::LoadConfig<LatBaseLqrPlusControllerConf>(&lat_based_lqr_plus_controller_conf_)) {
        AERROR << "failed to load control conf in init.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lat control_conf init");
    }
 
    if (!InitControlConf(lat_based_lqr_plus_controller_conf_)) {
        AERROR << "failed to load control conf in init.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load control_conf init");
    }
 
    if (!UpdateControlConf(lat_based_lqr_plus_controller_conf_)) {
        AERROR << "failed to update control conf in init.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to update control_conf init");
    }
 
    if (!ControlTaskExtend::LoadParamsPipelineConfig<ParamsPipeline>(&params_pipeline_)) {
        AERROR << "failed to load lon pid params pipeline config in init.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lon pid params pipeline config");
    }
 
    if (!LoadParams(&params_pipeline_)) {
        AERROR << "failed to load lat lqr controller params.";
        return Status(ErrorCode::CONTROL_INIT_ERROR, "failed to load lon pid params pipeline config");
    }
 
    AINFO << name_ << " begin.";
    LogInitParameters();
    InitializeFilters();
 
    injector_ = injector;
    leadlag_controller_.InitLeadlag();
 
    enable_mrac_ = lat_based_lqr_plus_controller_conf_.enable_steer_mrac_control();
    if (enable_mrac_) {
        mrac_controller_.Init(
                lat_based_lqr_plus_controller_conf_.steer_mrac_conf(), vehicle_param_.steering_latency_param(), ts_);
    }
 
    return Status::OK();
}

InitControlConf()

bool apollo::control::LatPlusController::InitControlConf ( const LatBaseLqrPlusControllerConf &  current_lat_based_lqr_plus_controller_conf )

protected

在文件 lat_plus_controller.cc 第 84 行定义.

                                                                                        {
    vehicle_param_ = common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
 
    ts_ = current_lat_based_lqr_plus_controller_conf.ts();
    if (ts_ <= 0.0) {
        AERROR << "[LatPlusController] Invalid control update interval.";
        return false;
    }
    preview_window_ = current_lat_based_lqr_plus_controller_conf.preview_window();
    wheelbase_ = vehicle_param_.wheel_base();
    steer_ratio_ = vehicle_param_.steer_ratio();
    steer_single_direction_max_degree_ = vehicle_param_.max_steer_angle() / M_PI * 180;
 
    AHC_container_.resize(current_lat_based_lqr_plus_controller_conf.ahc_container_size());
    AHC_compensated_value_ = 0.0;
    AHC_prev_compensated_value_ = 0.0;
 
    // Matrix init operations.
    const int matrix_size = basic_state_size_ + preview_window_;
    matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
    matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
    matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);
 
    matrix_a_coeff_ = Matrix::Zero(matrix_size, matrix_size);
 
    /*
    b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
    */
    matrix_b_ = Matrix::Zero(basic_state_size_, 1);
    matrix_bd_ = Matrix::Zero(basic_state_size_, 1);
    matrix_bdc_ = Matrix::Zero(matrix_size, 1);
 
    matrix_state_ = Matrix::Zero(matrix_size, 1);
    matrix_k_ = Matrix::Zero(1, matrix_size);
    matrix_r_ = Matrix::Identity(1, 1);
    matrix_q_ = Matrix::Zero(matrix_size, matrix_size);
 
    return true;
}

InitializeFilters()

void apollo::control::LatPlusController::InitializeFilters ( )

protected

在文件 lat_plus_controller.cc 第 264 行定义.

                                          {
    // Low pass filter
    std::vector<double> den(3, 0.0);
    std::vector<double> num(3, 0.0);
    common::LpfCoefficients(ts_, lat_based_lqr_plus_controller_conf_.cutoff_freq(), &den, &num);
    digital_filter_.set_coefficients(den, num);
    lateral_error_filter_ = common::MeanFilter(
            static_cast<std::uint_fast8_t>(lat_based_lqr_plus_controller_conf_.mean_filter_window_size()));
    heading_error_filter_ = common::MeanFilter(
            static_cast<std::uint_fast8_t>(lat_based_lqr_plus_controller_conf_.mean_filter_window_size()));
}

IsDrivingStraight()

bool apollo::control::LatPlusController::IsDrivingStraight ( const canbus::Chassis *  chassis, double  ref_curvature  )

protected

在文件 lat_plus_controller.cc 第 1761 行定义.

                                                                                                  {
    // steer < 1.0, v > 0.5, curvature < 0.01
    if (fabs(chassis->steering_percentage())
                < current_lat_based_lqr_plus_controller_conf_.straight_steering_percentage()
        && fabs(injector_->vehicle_state()->linear_velocity())
                > current_lat_based_lqr_plus_controller_conf_.straight_min_velocity()
        && fabs(ref_curvature) < current_lat_based_lqr_plus_controller_conf_.straight_max_ref_cur()) {
        return true;
    } else {
        return false;
    }
}

LateralSafetyCheck()

void apollo::control::LatPlusController::LateralSafetyCheck ( SimpleLateralDebug *  debug, SimpleLateralPlusDebug *  debug_info  )

protected

在文件 lat_plus_controller.cc 第 1996 行定义.

                                                                                                        {
    u_int32_t max_uint32 = std::numeric_limits<uint32_t>::max();
    auto check_conf = current_lat_based_lqr_plus_controller_conf_.lat_safety_check_conf();
    auto lat_check_debug = debug_info->mutable_lat_control_check_debug();
    // lateral error check
    double lateral_error
            = enable_look_ahead_back_control_ ? debug->lateral_error_feedback() : debug_info->lateral_error_lqr_input();
    ControlCheckStatus lateral_error_failed = ControlChecker::check_lateral_error(check_conf, &lateral_error);
    if (lateral_error_failed == WARNING) {
        lateral_error_w_count_ = lateral_error_w_count_ % max_uint32;
        lateral_error_w_count_++;
        lat_check_debug->set_lateral_error_w(lateral_error);
    } else {
        lat_check_debug->set_lateral_error_w(0.0);
    }
 
    if (lateral_error_failed == ERROR) {
        lateral_error_e_count_ = lateral_error_e_count_ % max_uint32;
        lateral_error_e_count_++;
        lat_check_debug->set_lateral_error_e(lateral_error);
    } else {
        lat_check_debug->set_lateral_error_e(0.0);
    }
    lat_check_debug->set_lateral_error_check_result(lateral_error_failed);
    lat_check_debug->set_lateral_error_check_e_count(lateral_error_e_count_);
    lat_check_debug->set_lateral_error_check_w_count(lateral_error_w_count_);
 
    if (current_lat_based_lqr_plus_controller_conf_.use_safety_check_output()) {
        if (enable_look_ahead_back_control_) {
            debug->set_lateral_error_feedback(lateral_error);
        } else {
            debug_info->set_lateral_error_lqr_input(lateral_error);
        }
    }
}

LoadParams()

bool apollo::control::LatPlusController::LoadParams ( ParamsPipeline *  params_pipeline )

protected

在文件 lat_plus_controller.cc 第 318 行定义.

                                                                  {
    AINFO << "Into the lat load params.";
    for (int i = 0; i < params_pipeline->params_declare_size(); i++) {
        LatBaseLqrPlusControllerConf param_config;
        auto param_config_path = params_pipeline->params_declare(i).config_path();
        std::string param_config_path_relative = absl::StrCat("conf", "/", param_config_path);
        AINFO << "Param config_path_relative is " << param_config_path_relative;
        std::string param_config_path_absolute
                = apollo::cyber::plugin_manager::PluginManager::Instance()->GetPluginConfPath<ControlTask>(
                        "apollo::control::LatPlusController", param_config_path_relative);
        AINFO << "Param config_path_absolute is " << param_config_path_absolute;
        if (!apollo::cyber::common::GetProtoFromFile(param_config_path_absolute, &param_config)) {
            AERROR << "Load config: " << params_pipeline->params_declare(i).config_name() << " param failed!";
            return false;
        }
        AINFO << "Load the lat config file successfully, file path: " << param_config_path;
        params_list_.push_back(std::make_pair(params_pipeline->params_declare(i).config_name(), param_config));
    }
    return true;
}

LogInitParameters()

void apollo::control::LatPlusController::LogInitParameters ( )

protected

在文件 lat_plus_controller.cc 第 255 行定义.

                                          {
    if (mass_current_ != mass_) {
        AINFO << "Update the vehicle mass.";
        AINFO << "[LatPlusController parameters]" << " mass_: " << mass_ << "," << " iz_: " << iz_ << ","
              << " lf_: " << lf_ << "," << " lr_: " << lr_;
        mass_current_ = mass_;
    }
}

Name()

std::string apollo::control::LatPlusController::Name ( ) const

overridevirtual

Lateral controller name

返回

string controller name in string

重载 apollo::control::ControlTaskExtend .

在文件 lat_plus_controller.cc 第 426 行定义.

                                        {
    return name_;
}

ProcessLogs()

void apollo::control::LatPlusController::ProcessLogs ( const SimpleLateralDebug *  debug, const canbus::Chassis *  chassis  )

protected

在文件 lat_plus_controller.cc 第 216 行定义.

                                                                                                 {
    const std::string log_str = absl::StrCat(
            debug->lateral_error(),
            ",",
            debug->ref_heading(),
            ",",
            debug->heading(),
            ",",
            debug->heading_error(),
            ",",
            debug->heading_error_rate(),
            ",",
            debug->lateral_error_rate(),
            ",",
            debug->curvature(),
            ",",
            debug->steer_angle(),
            ",",
            debug->steer_angle_feedforward(),
            ",",
            debug->steer_angle_lateral_contribution(),
            ",",
            debug->steer_angle_lateral_rate_contribution(),
            ",",
            debug->steer_angle_heading_contribution(),
            ",",
            debug->steer_angle_heading_rate_contribution(),
            ",",
            debug->steer_angle_feedback(),
            ",",
            chassis->steering_percentage(),
            ",",
            injector_->vehicle_state()->linear_velocity());
    if (FLAGS_enable_csv_debug) {
        steer_log_file_ << log_str << std::endl;
    }
    ADEBUG << "Steer_Control_Detail: " << log_str;
}

Reset()

Status apollo::control::LatPlusController::Reset ( )

overridevirtual

reset Lateral Controller

返回

Status reset status

实现了 apollo::control::ControlTask.

在文件 lat_plus_controller.cc 第 852 行定义.

                                {
    matrix_state_.setZero();
    if (enable_mrac_) {
        mrac_controller_.Reset();
    }
    ResetLateralSafetyCheck();
    com_change_add_ = 0.0;
    com_change_turbulence_add_ = 0.0;
    previous_lateral_error_lqr_input_ = 0.0;
    previous_heading_error_lqr_input_ = 0.0;
    ref_curvature_last_ = 0.0;
    is_lat_turbulence_ = false;
    is_lat_preview_in_straight_ = false;
    is_need_cover_turbulence_com_length_ = false;
    is_need_cover_turn_out_lane_com_length_ = false;
    return Status::OK();
}

ResetComLength()

void apollo::control::LatPlusController::ResetComLength ( )

protected

在文件 lat_plus_controller.cc 第 1094 行定义.

                                       {
    auto vehicle_state = injector_->vehicle_state();
    if (com_change_add_ < -1e-3) {
        com_change_reset_ = vehicle_state->linear_velocity() * ts_
                * current_lat_based_lqr_plus_controller_conf_.com_change_rate();
    } else if (com_change_add_ > 1e-3) {
        com_change_reset_ = -std::fabs(vehicle_state->linear_velocity()) * ts_
                * current_lat_based_lqr_plus_controller_conf_.com_change_rate();
    } else {
        com_change_reset_ = 0;
    }
    com_change_add_ = com_change_add_ + com_change_reset_;
    ADEBUG << "com_change_add_ is " << com_change_add_;
    ADEBUG << "com_change_reset_ is " << com_change_reset_;
}

ResetComLengthFromTurbulence()

void apollo::control::LatPlusController::ResetComLengthFromTurbulence ( )

protected

在文件 lat_plus_controller.cc 第 1110 行定义.

                                                     {
    ADEBUG << "Into ResetComLengthFromTurbulence";
    auto vehicle_state = injector_->vehicle_state();
    com_turbulence_change_ = vehicle_state->linear_velocity() * ts_
            * current_lat_based_lqr_plus_controller_conf_.com_quit_turbulence_change_rate();
    com_change_turbulence_add_ = com_change_turbulence_add_ + com_turbulence_change_;
    com_change_turbulence_add_ = common::math::Clamp(
            com_change_turbulence_add_, 0.0, lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add());
}

ResetLateralSafetyCheck()

void apollo::control::LatPlusController::ResetLateralSafetyCheck ( )

protected

在文件 lat_plus_controller.cc 第 2032 行定义.

                                                {
    lateral_error_w_count_ = 0;
    lateral_error_e_count_ = 0;
}

SetCurrentParam()

bool apollo::control::LatPlusController::SetCurrentParam ( const std::string  param_config_name, LatBaseLqrPlusControllerConf &  current_lat_based_lqr_plus_controller_conf  )

protected

在文件 lat_plus_controller.cc 第 345 行定义.

                                                                                  {
    bool set_success = false;
    for (const auto &param : params_list_) {
        if (param.first == param_config_name) {
            current_lat_based_lqr_plus_controller_conf.CopyFrom(param.second);
            set_success = true;
            break;
        }
    }
    if (!set_success) {
        AERROR << "Fail to find lat current param[" << param_config_name << "]";
    }
    return set_success;
}

Stop()

void apollo::control::LatPlusController::Stop ( )

overridevirtual

stop Lateral controller

实现了 apollo::control::ControlTask.

在文件 lat_plus_controller.cc 第 422 行定义.

                             {
    CloseLogFile();
}

UpdateAHCParam()

void apollo::control::LatPlusController::UpdateAHCParam ( const double &  heading_err, const double &  speed  )

protected

在文件 lat_plus_controller.cc 第 1774 行定义.

                                                                                     {
    if (AHC_is_compensating_) {
        AHC_container_.clear();
        AHC_sum_ = 0.0;
        AHC_compensated_value_ = AHC_prev_compensated_value_ + AutoCompensatedHeadingValue(speed);
        AHC_compensated_value_ = common::math::Clamp(
                AHC_compensated_value_,
                current_lat_based_lqr_plus_controller_conf_.ahc_mean_value_low_threshold(),
                current_lat_based_lqr_plus_controller_conf_.ahc_mean_value_high_threshold());
        return;
    }
 
    if (AHC_container_.size() < current_lat_based_lqr_plus_controller_conf_.ahc_container_size()) {
        AHC_container_.push_back(heading_err);
        AHC_sum_ += heading_err;
    }
 
    AHC_sum_ += heading_err - AHC_container_.front();
    AHC_mean_ = AHC_sum_ / current_lat_based_lqr_plus_controller_conf_.ahc_container_size();
    AHC_peak_ = *std::max_element(AHC_container_.begin(), AHC_container_.end())
            - *std::min_element(AHC_container_.begin(), AHC_container_.end());
    AHC_container_.pop_front();
    AHC_container_.push_back(heading_err);
 
    if (std::fabs(AHC_mean_) > std::fabs(current_lat_based_lqr_plus_controller_conf_.ahc_mean_value_low_threshold())
        && std::fabs(AHC_mean_) < std::fabs(current_lat_based_lqr_plus_controller_conf_.ahc_mean_value_high_threshold())
        && std::fabs(AHC_peak_) < std::fabs(current_lat_based_lqr_plus_controller_conf_.ahc_peak_value_threshold())) {
        AHC_is_compensating_ = true;
        AHC_compensating_value_ = 0.0;
    }
 
    // set debug info
    injector_->mutable_control_debug_info()->mutable_simple_lat_debug()->mutable_ahc_value()->set_ahc_sum(AHC_sum_);
    injector_->mutable_control_debug_info()->mutable_simple_lat_debug()->mutable_ahc_value()->set_ahc_mean(AHC_mean_);
    injector_->mutable_control_debug_info()->mutable_simple_lat_debug()->mutable_ahc_value()->set_ahc_peak(AHC_peak_);
    injector_->mutable_control_debug_info()
            ->mutable_simple_lat_debug()
            ->mutable_ahc_value()
            ->set_ahc_compensating_value(AHC_compensating_value_);
    injector_->mutable_control_debug_info()->mutable_simple_lat_debug()->mutable_ahc_value()->set_ahc_compensated_value(
            AHC_compensated_value_);
    injector_->mutable_control_debug_info()->mutable_simple_lat_debug()->mutable_ahc_value()->set_ahc_is_compensating(
            AHC_is_compensating_);
}

UpdateControlConf()

bool apollo::control::LatPlusController::UpdateControlConf ( const LatBaseLqrPlusControllerConf &  current_lat_based_lqr_plus_controller_conf )

protected

在文件 lat_plus_controller.cc 第 125 行定义.

                                                                                        {
    cf_ = current_lat_based_lqr_plus_controller_conf.cf();
    cr_ = current_lat_based_lqr_plus_controller_conf.cr();
    preview_window_ = current_lat_based_lqr_plus_controller_conf.preview_window();
    lookahead_station_low_speed_ = current_lat_based_lqr_plus_controller_conf.lookahead_station();
    lookback_station_low_speed_ = current_lat_based_lqr_plus_controller_conf.lookback_station();
    lookahead_station_high_speed_ = current_lat_based_lqr_plus_controller_conf.lookahead_station_high_speed();
    lookback_station_high_speed_ = current_lat_based_lqr_plus_controller_conf.lookback_station_high_speed();
    max_lat_acc_ = current_lat_based_lqr_plus_controller_conf.max_lateral_acceleration();
    low_speed_bound_ = current_lat_based_lqr_plus_controller_conf.switch_speed();
    low_speed_window_ = current_lat_based_lqr_plus_controller_conf.switch_speed_window();
 
    const double mass_fl = current_lat_based_lqr_plus_controller_conf.mass_fl();
    const double mass_fr = current_lat_based_lqr_plus_controller_conf.mass_fr();
    const double mass_rl = current_lat_based_lqr_plus_controller_conf.mass_rl();
    const double mass_rr = current_lat_based_lqr_plus_controller_conf.mass_rr();
    const double mass_front = mass_fl + mass_fr;
    const double mass_rear = mass_rl + mass_rr;
    mass_ = mass_front + mass_rear;
 
    lf_ = wheelbase_ * (1.0 - mass_front / mass_);
    lr_ = wheelbase_ * (1.0 - mass_rear / mass_);
 
    // moment of inertia
    iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;
 
    lqr_eps_ = current_lat_based_lqr_plus_controller_conf.eps();
    lqr_max_iteration_ = current_lat_based_lqr_plus_controller_conf.max_iteration();
 
    query_relative_time_ = current_lat_based_lqr_plus_controller_conf.query_relative_time();
 
    minimum_speed_protection_ = FLAGS_minimum_speed_protection;
 
    // Matrix init operations.
    /*
    A matrix (Gear Drive)
    [0.0, 1.0, 0.0, 0.0;
     0.0, (-(c_f + c_r) / m) / v, (c_f + c_r) / m,
     (l_r * c_r - l_f * c_f) / m / v;
     0.0, 0.0, 0.0, 1.0;
     0.0, ((lr * cr - lf * cf) / i_z) / v, (l_f * c_f - l_r * c_r) / i_z,
     (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
    */
    matrix_a_(0, 1) = 1.0;
    matrix_a_(1, 2) = (cf_ + cr_) / mass_;
    matrix_a_(2, 3) = 1.0;
    matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
 
    // matrix_a_coeff_ = Matrix::Zero(matrix_size, matrix_size);
    matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
    matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
    matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
    matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;
 
    /*
    b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
    */
    matrix_b_(1, 0) = cf_ / mass_;
    matrix_b_(3, 0) = lf_ * cf_ / iz_;
    matrix_bd_ = matrix_b_ * ts_;
 
    const int matrix_size = basic_state_size_ + preview_window_;
    int q_param_size = current_lat_based_lqr_plus_controller_conf.matrix_q_size();
    if (matrix_size != q_param_size) {
        const auto error_msg = absl::StrCat(
                "lateral controller error: matrix_q size: ",
                q_param_size,
                " in parameter file not equal to matrix_size: ",
                matrix_size);
        AERROR << error_msg;
        return false;
    }
 
    for (int i = 0; i < q_param_size; ++i) {
        matrix_q_(i, i) = current_lat_based_lqr_plus_controller_conf.matrix_q(i);
    }
 
    matrix_q_updated_ = matrix_q_;
    enable_leadlag_ = current_lat_based_lqr_plus_controller_conf.enable_reverse_leadlag_compensation();
    enable_look_ahead_back_control_ = current_lat_based_lqr_plus_controller_conf.enable_look_ahead_back_control();
    use_new_look_ahead_back_ = current_lat_based_lqr_plus_controller_conf.use_new_look_ahead_back();
    enable_enhance_qparam_ = current_lat_based_lqr_plus_controller_conf.enable_enhance_qparam();
    check_continous_errors_window_ = current_lat_based_lqr_plus_controller_conf.check_continous_errors_window();
    if (!is_lat_turbulence_) {
        com_length_turbulence_ = lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add();
    }
 
    return true;
}

UpdateDrivingOrientation()

void apollo::control::LatPlusController::UpdateDrivingOrientation ( )

protected

在文件 lat_plus_controller.cc 第 1746 行定义.

                                                 {
    auto vehicle_state = injector_->vehicle_state();
    driving_orientation_ = vehicle_state->heading();
    matrix_bd_ = matrix_b_ * ts_;
    // Reverse the driving direction if the vehicle is in reverse mode
    if (FLAGS_reverse_heading_control) {
        if (!IsForwardModel()) {
            driving_orientation_ = common::math::NormalizeAngle(driving_orientation_ + M_PI);
            // Update Matrix_b for reverse mode
            matrix_bd_ = -matrix_b_ * ts_;
            ADEBUG << "Matrix_b changed due to gear direction";
        }
    }
}

UpdateLookForwardPointError()

void apollo::control::LatPlusController::UpdateLookForwardPointError ( const double  lookahead_station, const double  lookback_station, const double  pose_x, const double  pose_y, const double  theta, const double  linear_v, const common::TrajectoryPoint &  ref_point, const TrajectoryAnalyzerExtend &  trajectory_analyzer, SimpleLateralDebug *  debug, SimpleLateralPlusDebug *  debug_info  )

protected

在文件 lat_plus_controller.cc 第 1660 行定义.

                                            {
    auto lookahead_point_new = trajectory_analyzer.QueryNearestPointByRelativeStation(ref_point, lookahead_station);
    debug_info->mutable_lookahead_point_new()->CopyFrom(lookahead_point_new);
 
    auto lookback_point_new = trajectory_analyzer.QueryNearestPointByRelativeStation(ref_point, -lookback_station);
    debug_info->mutable_lookback_point_new()->CopyFrom(lookback_point_new);
 
    auto lookahead_point = trajectory_analyzer.QueryNearestPointByRelativeTime(
            ref_point.relative_time()
            + lookahead_station / (std::max(std::fabs(linear_v), 0.1) * std::cos(debug->heading())));
 
    debug_info->set_lookahead_time(ref_point.relative_time());
    debug_info->set_lookahead_station_s_diff(lookahead_station);
 
    // Estimate the heading error with look-ahead/look-back windows as feedback
    // signal for special driving scenarios
    // Calculate heading error feedback
    double heading_error_feedback = 0.0;
    double heading_error_feedback_new = 0.0;
    double lookahead_time_new = 0.0;
    double lookahead_station_s_diff_new = 0.0;
    double lookback_time_new = 0.0;
    double lookback_station_s_diff_new = 0.0;
 
    if (!IsForwardModel()) {
        heading_error_feedback = debug_info->heading_error_lqr_input();
        heading_error_feedback_new = common::math::NormalizeAngle(theta - lookback_point_new.path_point().theta());
        lookback_time_new = lookahead_point_new.relative_time() - ref_point.relative_time();
        lookback_station_s_diff_new = lookahead_point_new.path_point().s() - ref_point.path_point().s();
    } else {
        heading_error_feedback = common::math::NormalizeAngle(
                debug_info->heading_error_lqr_input() + ref_point.path_point().theta()
                - lookahead_point.path_point().theta());
        heading_error_feedback_new = common::math::NormalizeAngle(theta - lookahead_point_new.path_point().theta());
        lookahead_time_new = lookahead_point_new.relative_time() - ref_point.relative_time();
        lookahead_station_s_diff_new = lookahead_point_new.path_point().s() - ref_point.path_point().s();
    }
    debug_info->set_lookahead_time_new(lookahead_time_new);
    debug_info->set_lookahead_station_s_diff_new(lookahead_station_s_diff_new);
    debug_info->set_lookback_time_new(lookback_time_new);
    debug_info->set_lookback_station_s_diff_new(lookback_station_s_diff_new);
 
    debug_info->set_lookahead_curvature(lookahead_point.path_point().kappa());
    debug_info->set_lookahead_curvature_new(lookahead_point_new.path_point().kappa());
    debug_info->set_lookback_curvature_new(lookback_point_new.path_point().kappa());
 
    debug->set_heading_error_feedback(heading_error_feedback);
    debug_info->set_heading_error_feedback_new(heading_error_feedback_new);
 
    // Estimate the lateral error with look-ahead/look-back windows as feedback
    // signal for special driving scenarios
    // Calculate lateral error feedback
    double lateral_error_feedback = 0.0;
    double lateral_error_feedback_new = 0.0;
    if (!IsForwardModel()) {
        lateral_error_feedback = debug_info->lateral_error_lqr_input() - lookback_station * std::sin(debug->heading());
        const double dx = pose_x - lookback_point_new.path_point().x();
        const double dy = pose_y - lookback_point_new.path_point().y();
        const double cos_target_heading = std::cos(lookback_point_new.path_point().theta());
        const double sin_target_heading = std::sin(lookback_point_new.path_point().theta());
        lateral_error_feedback_new = cos_target_heading * dy - sin_target_heading * dx;
    } else {
        lateral_error_feedback = debug_info->lateral_error_lqr_input() + lookahead_station * std::sin(debug->heading());
        const double dx = pose_x - lookahead_point_new.path_point().x();
        const double dy = pose_y - lookahead_point_new.path_point().y();
        const double cos_target_heading = std::cos(lookahead_point_new.path_point().theta());
        const double sin_target_heading = std::sin(lookahead_point_new.path_point().theta());
        lateral_error_feedback_new = cos_target_heading * dy - sin_target_heading * dx;
    }
    debug->set_lateral_error_feedback(lateral_error_feedback);
    debug_info->set_lateral_error_feedback_new(lateral_error_feedback_new);
 
    ADEBUG << "lateral_error_feedback: " << lateral_error_feedback
           << ", lateral_error_feedback_new: " << lateral_error_feedback_new;
}

UpdateMatrix()

void apollo::control::LatPlusController::UpdateMatrix ( )

protected

在文件 lat_plus_controller.cc 第 1159 行定义.

                                     {
    double v = 0.01;
    // At reverse driving, replace the lateral translational motion dynamics with
    // the corresponding kinematic models
    if (!IsForwardModel() && !current_lat_based_lqr_plus_controller_conf_.reverse_use_dynamic_model()) {
        v = std::min(injector_->vehicle_state()->linear_velocity(), -minimum_speed_protection_);
        matrix_a_(0, 2) = matrix_a_coeff_(0, 2) * v;
    } else {
        v = std::max(injector_->vehicle_state()->linear_velocity(), minimum_speed_protection_);
        matrix_a_(0, 2) = 0.0;
    }
    matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
    matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
    matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
    matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;
    Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
    matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() * (matrix_i + ts_ * 0.5 * matrix_a_);
}

UpdateMatrixCompound()

void apollo::control::LatPlusController::UpdateMatrixCompound ( )

protected

在文件 lat_plus_controller.cc 第 1178 行定义.

                                             {
    // Initialize preview matrix
    matrix_adc_.block(0, 0, basic_state_size_, basic_state_size_) = matrix_ad_;
    matrix_bdc_.block(0, 0, basic_state_size_, 1) = matrix_bd_;
    if (preview_window_ > 0) {
        matrix_bdc_(matrix_bdc_.rows() - 1, 0) = 1;
        // Update A matrix;
        for (int i = 0; i < preview_window_ - 1; ++i) {
            matrix_adc_(basic_state_size_ + i, basic_state_size_ + 1 + i) = 1;
        }
    }
}

UpdateState()

void apollo::control::LatPlusController::UpdateState ( SimpleLateralDebug *  debug, SimpleLateralPlusDebug *  debug_info, const canbus::Chassis *  chassis  )

protected

在文件 lat_plus_controller.cc 第 870 行定义.

                                      {
    auto vehicle_state = injector_->vehicle_state();
    if (FLAGS_use_navigation_mode) {
        ComputeLateralErrors(
                0.0,
                0.0,
                driving_orientation_,
                vehicle_state->linear_velocity(),
                vehicle_state->angular_velocity(),
                vehicle_state->linear_acceleration(),
                trajectory_analyzer_,
                chassis,
                choose_ref_point_,
                debug,
                debug_info);
    } else {
        // Transform the coordinate of the vehicle states from the center of the
        // rear-axis to the center of mass, if conditions matched
        double com_length = 0.0;
        if (is_lat_turbulence_) {
            com_length = com_length_turbulence_;
            is_need_cover_turbulence_com_length_ = true;
        } else if (!is_lat_turbulence_ && is_need_cover_turbulence_com_length_) {
            com_length = com_length_turbulence_ + com_change_turbulence_add_;
            ResetComLengthFromTurbulence();
            if (com_length >= lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add()) {
                is_need_cover_turbulence_com_length_ = false;
                com_change_turbulence_add_ = 0.0;
            }
        } else if (current_lat_based_lqr_plus_controller_conf_.is_need_turn_out_lane()) {
            com_length = current_lat_based_lqr_plus_controller_conf_.turn_out_lane_com_length();
            is_need_cover_turn_out_lane_com_length_ = true;
        } else if (
                !current_lat_based_lqr_plus_controller_conf_.is_need_turn_out_lane()
                && is_need_cover_turn_out_lane_com_length_) {
            com_length = current_lat_based_lqr_plus_controller_conf_.turn_out_lane_com_length()
                    + com_change_turbulence_add_;
            ResetComLengthFromTurbulence();
            if (com_length >= lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add()) {
                is_need_cover_turn_out_lane_com_length_ = false;
                com_change_turbulence_add_ = 0.0;
            }
        } else {
            com_length = lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add();
            if (current_lat_based_lqr_plus_controller_conf_.use_continous_errors_check()) {
                com_length = lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add() + com_change_add_;
            } else {
                com_length = lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add() + com_change_add_;
                ResetComLength();
            }
        }
 
        com_length = common::math::Clamp(com_length, -2 * lr_, 2 * lr_);
        debug_info->set_com_length(com_length);
        const auto &com = vehicle_state->ComputeCOMPosition(com_length);
        debug_info->mutable_lat_com_position()->set_x(com.x());
        debug_info->mutable_lat_com_position()->set_y(com.y());
        ComputeLateralErrors(
                com.x(),
                com.y(),
                driving_orientation_,
                vehicle_state->linear_velocity(),
                vehicle_state->angular_velocity(),
                vehicle_state->linear_acceleration(),
                trajectory_analyzer_,
                chassis,
                choose_ref_point_,
                debug,
                debug_info);
    }
    // State matrix update;
    // First four elements are fixed;
    if (enable_look_ahead_back_control_) {
        if (use_new_look_ahead_back_) {
            matrix_state_(0, 0) = debug_info->lateral_error_feedback_new();
            matrix_state_(2, 0) = debug_info->heading_error_feedback_new();
        } else {
            matrix_state_(0, 0) = debug->lateral_error_feedback();
            matrix_state_(2, 0) = debug->heading_error_feedback();
        }
    } else {
        matrix_state_(0, 0) = debug_info->lateral_error_lqr_input();
        matrix_state_(2, 0) = debug_info->heading_error_lqr_input();
    }
    matrix_state_(1, 0) = debug->lateral_error_rate();
    matrix_state_(3, 0) = debug->heading_error_rate();
 
    // Next elements are depending on preview window size;
    for (int i = 0; i < preview_window_; ++i) {
        const double preview_time = ts_ * (i + 1);
        const auto preview_point = trajectory_analyzer_.QueryNearestPointByRelativeTime(preview_time);
 
        const auto matched_point = trajectory_analyzer_.QueryNearestPointByPosition(
                preview_point.path_point().x(), preview_point.path_point().y());
 
        const double dx = preview_point.path_point().x() - matched_point.path_point().x();
        const double dy = preview_point.path_point().y() - matched_point.path_point().y();
 
        const double cos_matched_theta = std::cos(matched_point.path_point().theta());
        const double sin_matched_theta = std::sin(matched_point.path_point().theta());
        const double preview_d_error = cos_matched_theta * dy - sin_matched_theta * dx;
 
        matrix_state_(basic_state_size_ + i, 0) = preview_d_error;
    }
 
    const auto &track_com
            = vehicle_state->ComputeCOMPosition(current_lat_based_lqr_plus_controller_conf_.track_com_length());
    debug_info->mutable_lat_track_com_position()->set_x(track_com.x());
    debug_info->mutable_lat_track_com_position()->set_y(track_com.y());
 
    if (is_lat_turbulence_) {
        ComputeTrackLateralErrors(
                track_com.x(),
                track_com.y(),
                driving_orientation_,
                vehicle_state->linear_velocity(),
                trajectory_analyzer_,
                control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_LOCAL,
                debug_info);
    } else {
        ComputeTrackLateralErrors(
                track_com.x(),
                track_com.y(),
                driving_orientation_,
                vehicle_state->linear_velocity(),
                trajectory_analyzer_,
                current_lat_based_lqr_plus_controller_conf_.choose_track_point(),
                debug_info);
    }
 
    if (current_lat_based_lqr_plus_controller_conf_.use_continous_errors_check()
        && CheckContinousErrors(
                check_continous_errors_window_,
                debug_info->lateral_error_track_com(),
                current_lat_based_lqr_plus_controller_conf_.lat_continous_err_threshold(),
                &lateral_lr_errors_,
                &current_sign_)) {
        if (current_sign_ > 0) {
            ADEBUG << "Current vehicle is left of trajectory";
            // idenfy turn left
            if (chassis->steering_percentage()
                >= current_lat_based_lqr_plus_controller_conf_.straight_steering_percentage()) {
                // reduce the com if turn left
                com_change_ = -vehicle_state->linear_velocity() * ts_
                        * current_lat_based_lqr_plus_controller_conf_.com_change_rate();
                ADEBUG << "reduce the com: " << com_change_ << " in turn left";
            } else if (
                    chassis->steering_percentage()
                    <= -current_lat_based_lqr_plus_controller_conf_.straight_steering_percentage()) {
                // idenfy turn right
                // increase the com if turn right
                com_change_ = vehicle_state->linear_velocity() * ts_
                        * current_lat_based_lqr_plus_controller_conf_.com_change_rate();
                ADEBUG << "increase the com: " << com_change_ << " in turn right";
            } else {
                ResetComLength();
            }
            ADEBUG << "Current com_length is " << debug_info->com_length();
            choose_ref_point_ = control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_COM;
        } else if (current_sign_ < 0) {
            ADEBUG << "Current vehicle is right of trajectory";
            // idenfy turn left
            if (chassis->steering_percentage()
                >= current_lat_based_lqr_plus_controller_conf_.straight_steering_percentage()) {
                // increase the com if turn left
                com_change_ = vehicle_state->linear_velocity() * ts_
                        * current_lat_based_lqr_plus_controller_conf_.com_change_rate();
                ADEBUG << "increase the com: " << com_change_ << " in turn left";
            } else if (
                    chassis->steering_percentage()
                    <= -current_lat_based_lqr_plus_controller_conf_.straight_steering_percentage()) {
                // idenfy turn right
                // reduce the com if turn right
                com_change_ = -vehicle_state->linear_velocity() * ts_
                        * current_lat_based_lqr_plus_controller_conf_.com_change_rate();
                ADEBUG << "reduce the com: " << com_change_ << " in turn right";
            } else {
                ResetComLength();
            }
            ADEBUG << "Current com_length is " << debug_info->com_length();
            choose_ref_point_ = control::ChooseRefTrajectoryPoint::USE_LAT_MATCH_COM;
        } else {
            com_change_ = 0;
        }
        com_change_add_ = com_change_add_ + com_change_;
        com_change_add_ = common::math::Clamp(
                com_change_add_,
                -(lr_ + current_lat_based_lqr_plus_controller_conf_.lat_err_position_add()),
                (lr_ - current_lat_based_lqr_plus_controller_conf_.lat_err_position_add()));
        ADEBUG << "com_change_add_ is " << com_change_add_;
        ADEBUG << "com_change_ is " << com_change_;
        ADEBUG << "current_sign_ is " << current_sign_;
    } else {
        choose_ref_point_ = current_lat_based_lqr_plus_controller_conf_.choose_ref_point();
    }
 
    // check cuvature, lat_error, heading_error changing turbulence
    if (current_lat_based_lqr_plus_controller_conf_.use_turbulence_check()
        && (std::fabs(debug_info->lateral_error_lqr_input_change())
                    > current_lat_based_lqr_plus_controller_conf_.lateral_error_turbulence_threshold()
            && std::fabs(debug_info->heading_error_lqr_input_change())
                    > current_lat_based_lqr_plus_controller_conf_.heading_error_turbulence_threshold())) {
        AINFO << "Into turbulence condition";
        is_lat_turbulence_ = true;
        choose_ref_point_ = control::ChooseRefTrajectoryPoint::USE_LAT_ABSOLUTE_LOCAL;
        com_length_turbulence_ = 0;
    } else {
        ADEBUG << "Quit turbulence condition";
        is_lat_turbulence_ = false;
    }
 
    // check quit turbulence and into long straight condition
 
    for (size_t i = 0; i < lateral_lr_errors_.size(); i++) {
        debug_info->add_continous_lateral_errors(lateral_lr_errors_[i]);
    }
    debug_info->set_current_sign(current_sign_);
    debug_info->set_com_change(com_change_);
    debug_info->set_current_choose_point(choose_ref_point_);
}

类成员变量说明

AHC_compensated_value_

double apollo::control::LatPlusController::AHC_compensated_value_ = 0.0

protected

在文件 lat_plus_controller.h 第 358 行定义.

AHC_compensating_value_

double apollo::control::LatPlusController::AHC_compensating_value_ = 0.0

protected

在文件 lat_plus_controller.h 第 357 行定义.

AHC_container_

std::deque<double> apollo::control::LatPlusController::AHC_container_

protected

在文件 lat_plus_controller.h 第 361 行定义.

AHC_is_compensating_

bool apollo::control::LatPlusController::AHC_is_compensating_ = false

protected

在文件 lat_plus_controller.h 第 353 行定义.

AHC_mean_

double apollo::control::LatPlusController::AHC_mean_ = 0.0

protected

在文件 lat_plus_controller.h 第 354 行定义.

AHC_peak_

double apollo::control::LatPlusController::AHC_peak_ = 0.0

protected

在文件 lat_plus_controller.h 第 355 行定义.

AHC_prev_compensated_value_

double apollo::control::LatPlusController::AHC_prev_compensated_value_ = 0.0

protected

在文件 lat_plus_controller.h 第 359 行定义.

AHC_prev_output_value_

double apollo::control::LatPlusController::AHC_prev_output_value_ = 0.0

protected

在文件 lat_plus_controller.h 第 360 行定义.

AHC_sum_

double apollo::control::LatPlusController::AHC_sum_ = 0.0

protected

在文件 lat_plus_controller.h 第 356 行定义.

basic_state_size_

const int apollo::control::LatPlusController::basic_state_size_ = 4

protected

在文件 lat_plus_controller.h 第 258 行定义.

cf_

double apollo::control::LatPlusController::cf_ = 0.0

protected

在文件 lat_plus_controller.h 第 222 行定义.

chassis_

const canbus::Chassis* apollo::control::LatPlusController::chassis_ = nullptr

protected

在文件 lat_plus_controller.h 第 202 行定义.

check_continous_errors_window_

uint8_t apollo::control::LatPlusController::check_continous_errors_window_ = 0

protected

在文件 lat_plus_controller.h 第 380 行定义.

choose_ref_point_

enum ChooseRefTrajectoryPoint apollo::control::LatPlusController::choose_ref_point_ = USE_LAT_MATCH_COM

protected

在文件 lat_plus_controller.h 第 378 行定义.

com_change_

double apollo::control::LatPlusController::com_change_ = 0.0

protected

在文件 lat_plus_controller.h 第 384 行定义.

com_change_add_

double apollo::control::LatPlusController::com_change_add_ = 0.0

protected

在文件 lat_plus_controller.h 第 386 行定义.

com_change_reset_

double apollo::control::LatPlusController::com_change_reset_ = 0.0

protected

在文件 lat_plus_controller.h 第 385 行定义.

com_change_turbulence_add_

double apollo::control::LatPlusController::com_change_turbulence_add_ = 0.0

protected

在文件 lat_plus_controller.h 第 388 行定义.

com_length_previous_

double apollo::control::LatPlusController::com_length_previous_ = 0.0

protected

在文件 lat_plus_controller.h 第 389 行定义.

com_length_turbulence_

double apollo::control::LatPlusController::com_length_turbulence_ = 0.0

protected

在文件 lat_plus_controller.h 第 390 行定义.

com_turbulence_change_

double apollo::control::LatPlusController::com_turbulence_change_ = 0.0

protected

在文件 lat_plus_controller.h 第 387 行定义.

cr_

double apollo::control::LatPlusController::cr_ = 0.0

protected

在文件 lat_plus_controller.h 第 224 行定义.

current_lat_based_lqr_plus_controller_conf_

LatBaseLqrPlusControllerConf apollo::control::LatPlusController::current_lat_based_lqr_plus_controller_conf_

protected

在文件 lat_plus_controller.h 第 209 行定义.

current_param_config_name_

std::string apollo::control::LatPlusController::current_param_config_name_ = ""

protected

在文件 lat_plus_controller.h 第 210 行定义.

current_sign_

int32_t apollo::control::LatPlusController::current_sign_ = 0

protected

在文件 lat_plus_controller.h 第 379 行定义.

current_trajectory_timestamp_

double apollo::control::LatPlusController::current_trajectory_timestamp_ = -1.0

protected

在文件 lat_plus_controller.h 第 336 行定义.

digital_filter_

common::DigitalFilter apollo::control::LatPlusController::digital_filter_

protected

在文件 lat_plus_controller.h 第 289 行定义.

driving_orientation_

double apollo::control::LatPlusController::driving_orientation_ = 0.0

protected

在文件 lat_plus_controller.h 第 348 行定义.

ed_date_

double apollo::control::LatPlusController::ed_date_ = 1.0

protected

在文件 lat_plus_controller.h 第 367 行定义.

efai_rate_

double apollo::control::LatPlusController::efai_rate_ = 1.0

protected

在文件 lat_plus_controller.h 第 366 行定义.

enable_enhance_qparam_

bool apollo::control::LatPlusController::enable_enhance_qparam_ = false

protected

在文件 lat_plus_controller.h 第 376 行定义.

enable_leadlag_

bool apollo::control::LatPlusController::enable_leadlag_ = false

protected

在文件 lat_plus_controller.h 第 300 行定义.

enable_look_ahead_back_control_

bool apollo::control::LatPlusController::enable_look_ahead_back_control_ = false

protected

在文件 lat_plus_controller.h 第 308 行定义.

enable_mrac_

bool apollo::control::LatPlusController::enable_mrac_ = false

protected

在文件 lat_plus_controller.h 第 304 行定义.

heading_err_interpolation_

std::unique_ptr<Interpolation1D> apollo::control::LatPlusController::heading_err_interpolation_

protected

在文件 lat_plus_controller.h 第 293 行定义.

heading_error_filter_

common::MeanFilter apollo::control::LatPlusController::heading_error_filter_

protected

在文件 lat_plus_controller.h 第 297 行定义.

init_vehicle_heading_

double apollo::control::LatPlusController::init_vehicle_heading_ = 0.0

protected

在文件 lat_plus_controller.h 第 342 行定义.

init_vehicle_x_

double apollo::control::LatPlusController::init_vehicle_x_ = 0.0

protected

在文件 lat_plus_controller.h 第 338 行定义.

init_vehicle_y_

double apollo::control::LatPlusController::init_vehicle_y_ = 0.0

protected

在文件 lat_plus_controller.h 第 340 行定义.

injector_

std::shared_ptr<DependencyInjector> apollo::control::LatPlusController::injector_

protected

在文件 lat_plus_controller.h 第 350 行定义.

is_in_large_curvature_

bool apollo::control::LatPlusController::is_in_large_curvature_ = false

protected

在文件 lat_plus_controller.h 第 375 行定义.

is_lat_preview_in_straight_

bool apollo::control::LatPlusController::is_lat_preview_in_straight_ = false

protected

在文件 lat_plus_controller.h 第 392 行定义.

is_lat_turbulence_

bool apollo::control::LatPlusController::is_lat_turbulence_ = false

protected

在文件 lat_plus_controller.h 第 391 行定义.

is_need_cover_turbulence_com_length_

bool apollo::control::LatPlusController::is_need_cover_turbulence_com_length_ = false

protected

在文件 lat_plus_controller.h 第 393 行定义.

is_need_cover_turn_out_lane_com_length_

bool apollo::control::LatPlusController::is_need_cover_turn_out_lane_com_length_ = false

protected

在文件 lat_plus_controller.h 第 394 行定义.

iz_

double apollo::control::LatPlusController::iz_ = 0.0

protected

在文件 lat_plus_controller.h 第 236 行定义.

lat_based_lqr_plus_controller_conf_

LatBaseLqrPlusControllerConf apollo::control::LatPlusController::lat_based_lqr_plus_controller_conf_

protected

在文件 lat_plus_controller.h 第 205 行定义.

lat_err_interpolation_

std::unique_ptr<Interpolation1D> apollo::control::LatPlusController::lat_err_interpolation_

protected

在文件 lat_plus_controller.h 第 291 行定义.

lateral_error_e_count_

uint32_t apollo::control::LatPlusController::lateral_error_e_count_ = 0

protected

在文件 lat_plus_controller.h 第 373 行定义.

lateral_error_filter_

common::MeanFilter apollo::control::LatPlusController::lateral_error_filter_

protected

在文件 lat_plus_controller.h 第 296 行定义.

lateral_error_w_count_

uint32_t apollo::control::LatPlusController::lateral_error_w_count_ = 0

protected

在文件 lat_plus_controller.h 第 372 行定义.

lateral_lr_errors_

std::vector<double> apollo::control::LatPlusController::lateral_lr_errors_

protected

在文件 lat_plus_controller.h 第 381 行定义.

leadlag_controller_

LeadlagPlusController apollo::control::LatPlusController::leadlag_controller_

protected

在文件 lat_plus_controller.h 第 301 行定义.

lf_

double apollo::control::LatPlusController::lf_ = 0.0

protected

在文件 lat_plus_controller.h 第 232 行定义.

lookahead_station_high_speed_

double apollo::control::LatPlusController::lookahead_station_high_speed_ = 0.0

protected

在文件 lat_plus_controller.h 第 253 行定义.

lookahead_station_low_speed_

double apollo::control::LatPlusController::lookahead_station_low_speed_ = 0.0

protected

在文件 lat_plus_controller.h 第 251 行定义.

lookback_station_high_speed_

double apollo::control::LatPlusController::lookback_station_high_speed_ = 0.0

protected

在文件 lat_plus_controller.h 第 254 行定义.

lookback_station_low_speed_

double apollo::control::LatPlusController::lookback_station_low_speed_ = 0.0

protected

在文件 lat_plus_controller.h 第 252 行定义.

low_speed_bound_

double apollo::control::LatPlusController::low_speed_bound_ = 0.0

protected

在文件 lat_plus_controller.h 第 344 行定义.

low_speed_window_

double apollo::control::LatPlusController::low_speed_window_ = 0.0

protected

在文件 lat_plus_controller.h 第 346 行定义.

lqr_eps_

double apollo::control::LatPlusController::lqr_eps_ = 0.0

protected

在文件 lat_plus_controller.h 第 287 行定义.

lqr_max_iteration_

int apollo::control::LatPlusController::lqr_max_iteration_ = 0

protected

在文件 lat_plus_controller.h 第 285 行定义.

lr_

double apollo::control::LatPlusController::lr_ = 0.0

protected

在文件 lat_plus_controller.h 第 234 行定义.

mass_

double apollo::control::LatPlusController::mass_ = 0.0

protected

在文件 lat_plus_controller.h 第 228 行定义.

mass_current_

double apollo::control::LatPlusController::mass_current_ = 0.0

protected

在文件 lat_plus_controller.h 第 230 行定义.

matrix_a_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_a_

protected

在文件 lat_plus_controller.h 第 260 行定义.

matrix_a_coeff_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_a_coeff_

protected

在文件 lat_plus_controller.h 第 280 行定义.

matrix_ad_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_ad_

protected

在文件 lat_plus_controller.h 第 262 行定义.

matrix_adc_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_adc_

protected

在文件 lat_plus_controller.h 第 264 行定义.

matrix_b_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_b_

protected

在文件 lat_plus_controller.h 第 266 行定义.

matrix_bd_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_bd_

protected

在文件 lat_plus_controller.h 第 268 行定义.

matrix_bdc_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_bdc_

protected

在文件 lat_plus_controller.h 第 270 行定义.

matrix_k_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_k_

protected

在文件 lat_plus_controller.h 第 272 行定义.

matrix_q_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_q_

protected

在文件 lat_plus_controller.h 第 276 行定义.

matrix_q_updated_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_q_updated_

protected

在文件 lat_plus_controller.h 第 278 行定义.

matrix_r_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_r_

protected

在文件 lat_plus_controller.h 第 274 行定义.

matrix_state_

Eigen::MatrixXd apollo::control::LatPlusController::matrix_state_

protected

在文件 lat_plus_controller.h 第 282 行定义.

max_lat_acc_

double apollo::control::LatPlusController::max_lat_acc_ = 0.0

protected

在文件 lat_plus_controller.h 第 243 行定义.

minimum_speed_protection_

double apollo::control::LatPlusController::minimum_speed_protection_ = 0.1

protected

在文件 lat_plus_controller.h 第 334 行定义.

mrac_controller_

MracController apollo::control::LatPlusController::mrac_controller_

protected

在文件 lat_plus_controller.h 第 305 行定义.

name_

const std::string apollo::control::LatPlusController::name_

protected

在文件 lat_plus_controller.h 第 326 行定义.

now_reverse_stage_

ReverseLogicStage::ReverseStageType apollo::control::LatPlusController::now_reverse_stage_ = ReverseLogicStage::NO_REVERSE

protected

在文件 lat_plus_controller.h 第 365 行定义.

params_list_

std::vector<std::pair<std::string, LatBaseLqrPlusControllerConf> > apollo::control::LatPlusController::params_list_

protected

在文件 lat_plus_controller.h 第 208 行定义.

params_pipeline_

ParamsPipeline apollo::control::LatPlusController::params_pipeline_

protected

在文件 lat_plus_controller.h 第 207 行定义.

pre_steer_angle_

double apollo::control::LatPlusController::pre_steer_angle_ = 0.0

protected

在文件 lat_plus_controller.h 第 330 行定义.

pre_steering_position_

double apollo::control::LatPlusController::pre_steering_position_ = 0.0

protected

在文件 lat_plus_controller.h 第 332 行定义.

prev_abs_

int32_t apollo::control::LatPlusController::prev_abs_ = 0

protected

在文件 lat_plus_controller.h 第 383 行定义.

prev_sign_

int32_t apollo::control::LatPlusController::prev_sign_ = 0

protected

在文件 lat_plus_controller.h 第 382 行定义.

preview_window_

int apollo::control::LatPlusController::preview_window_ = 0

protected

在文件 lat_plus_controller.h 第 246 行定义.

previous_heading_acceleration_

double apollo::control::LatPlusController::previous_heading_acceleration_ = 0.0

protected

在文件 lat_plus_controller.h 第 317 行定义.

previous_heading_error_lqr_input_

double apollo::control::LatPlusController::previous_heading_error_lqr_input_ = 0.0

protected

在文件 lat_plus_controller.h 第 321 行定义.

previous_heading_rate_

double apollo::control::LatPlusController::previous_heading_rate_ = 0.0

protected

在文件 lat_plus_controller.h 第 314 行定义.

previous_lateral_acceleration_

double apollo::control::LatPlusController::previous_lateral_acceleration_ = 0.0

protected

在文件 lat_plus_controller.h 第 312 行定义.

previous_lateral_error_lqr_input_

double apollo::control::LatPlusController::previous_lateral_error_lqr_input_ = 0.0

protected

在文件 lat_plus_controller.h 第 320 行定义.

previous_ref_heading_acceleration_

double apollo::control::LatPlusController::previous_ref_heading_acceleration_ = 0.0

protected

在文件 lat_plus_controller.h 第 318 行定义.

previous_ref_heading_rate_

double apollo::control::LatPlusController::previous_ref_heading_rate_ = 0.0

protected

在文件 lat_plus_controller.h 第 315 行定义.

query_relative_time_

double apollo::control::LatPlusController::query_relative_time_

protected

在文件 lat_plus_controller.h 第 328 行定义.

ref_curvature_last_

double apollo::control::LatPlusController::ref_curvature_last_ = 0.0

protected

在文件 lat_plus_controller.h 第 369 行定义.

reverse_handle_count_

int apollo::control::LatPlusController::reverse_handle_count_ = 0

protected

在文件 lat_plus_controller.h 第 363 行定义.

steer_log_file_

std::ofstream apollo::control::LatPlusController::steer_log_file_

protected

在文件 lat_plus_controller.h 第 324 行定义.

steer_ratio_

double apollo::control::LatPlusController::steer_ratio_ = 0.0

protected

在文件 lat_plus_controller.h 第 238 行定义.

steer_single_direction_max_degree_

double apollo::control::LatPlusController::steer_single_direction_max_degree_ = 0.0

protected

在文件 lat_plus_controller.h 第 240 行定义.

trajectory_analyzer_

TrajectoryAnalyzerExtend apollo::control::LatPlusController::trajectory_analyzer_

protected

在文件 lat_plus_controller.h 第 216 行定义.

ts_

double apollo::control::LatPlusController::ts_ = 0.0

protected

在文件 lat_plus_controller.h 第 220 行定义.

use_new_look_ahead_back_

bool apollo::control::LatPlusController::use_new_look_ahead_back_ = false

protected

在文件 lat_plus_controller.h 第 309 行定义.

vehicle_param_

common::VehicleParam apollo::control::LatPlusController::vehicle_param_

protected

在文件 lat_plus_controller.h 第 213 行定义.

wheelbase_

double apollo::control::LatPlusController::wheelbase_ = 0.0

protected

在文件 lat_plus_controller.h 第 226 行定义.

---

该类的文档由以下文件生成:

• modules/control/controllers/lat_based_lqr_plus_controller/lat_plus_controller.h
• modules/control/controllers/lat_based_lqr_plus_controller/lat_plus_controller.cc