lat_plus_controller.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
#pragma once
 
#include <fstream>
#include <memory>
#include <string>
 
#include "Eigen/Core"
 
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/controllers/lat_based_lqr_plus_controller/proto/lat_based_lqr_plus_controller_conf.pb.h"
#include "modules/control/control_task_base_extend/proto/params_pipeline.pb.h"
#include "modules/control/control_component/proto/check_status.pb.h"
 
#include "cyber/plugin_manager/plugin_manager.h"
#include "modules/common/filters/digital_filter.h"
#include "modules/common/filters/digital_filter_coefficients.h"
#include "modules/common/filters/mean_filter.h"
#include "modules/control/control_component/controller_task_base/common/interpolation_1d.h"
#include "modules/control/control_component/controller_task_base/common/mrac_controller.h"
#include "modules/control/control_task_base_extend/common/control_checker.h"
#include "modules/control/control_task_base_extend/common/interpolation_plus_1d.h"
#include "modules/control/control_task_base_extend/common/leadlag_plus_controller.h"
#include "modules/control/control_task_base_extend/common/trajectory_analyzer_extend.h"
#include "modules/control/control_task_base_extend/control_task_extend.h"
 
namespace apollo {
namespace control {
 
class LatPlusController : public ControlTaskExtend {
public:
    LatPlusController();
 
    virtual ~LatPlusController();
 
    common::Status Init(std::shared_ptr<DependencyInjector> injector) override;
 
    common::Status ComputeControlCommand(
            const localization::LocalizationEstimate *localization,
            const canbus::Chassis *chassis,
            const planning::ADCTrajectory *trajectory,
            ControlCommand *cmd) override;
 
    common::Status Reset() override;
 
    void Stop() override;
 
    std::string Name() const override;
 
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);
 
    // logic for reverse driving mode
    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();
 
    const canbus::Chassis *chassis_ = nullptr;
 
    // vehicle
    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_ = "";
 
    // vehicle parameter
    common::VehicleParam vehicle_param_;
 
    // a proxy to analyze the planning trajectory
    TrajectoryAnalyzerExtend trajectory_analyzer_;
 
    // the following parameters are vehicle physics related.
    // control time interval
    double ts_ = 0.0;
    // corner stiffness; front
    double cf_ = 0.0;
    // corner stiffness; rear
    double cr_ = 0.0;
    // distance between front and rear wheel center
    double wheelbase_ = 0.0;
    // mass of the vehicle
    double mass_ = 0.0;
    // current mass of the vehicle
    double mass_current_ = 0.0;
    // distance from front wheel center to COM
    double lf_ = 0.0;
    // distance from rear wheel center to COM
    double lr_ = 0.0;
    // rotational inertia
    double iz_ = 0.0;
    // the ratio between the turn of the steering wheel and the turn of the wheels
    double steer_ratio_ = 0.0;
    // the maximum turn of steer
    double steer_single_direction_max_degree_ = 0.0;
 
    // limit steering to maximum theoretical lateral acceleration
    double max_lat_acc_ = 0.0;
 
    // number of control cycles look ahead (preview controller)
    int preview_window_ = 0;
 
    // longitudial length for look-ahead lateral error estimation during forward
    // driving and look-back lateral error estimation during backward driving
    // (look-ahead controller)
    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;
 
    // number of states without previews, includes
    // lateral error, lateral error rate, heading error, heading error rate
    const int basic_state_size_ = 4;
    // vehicle state matrix
    Eigen::MatrixXd matrix_a_;
    // vehicle state matrix (discrete-time)
    Eigen::MatrixXd matrix_ad_;
    // vehicle state matrix compound; related to preview
    Eigen::MatrixXd matrix_adc_;
    // control matrix
    Eigen::MatrixXd matrix_b_;
    // control matrix (discrete-time)
    Eigen::MatrixXd matrix_bd_;
    // control matrix compound
    Eigen::MatrixXd matrix_bdc_;
    // gain matrix
    Eigen::MatrixXd matrix_k_;
    // control authority weighting matrix
    Eigen::MatrixXd matrix_r_;
    // state weighting matrix
    Eigen::MatrixXd matrix_q_;
    // updated state weighting matrix
    Eigen::MatrixXd matrix_q_updated_;
    // vehicle state matrix coefficients
    Eigen::MatrixXd matrix_a_coeff_;
    // 4 by 1 matrix; state matrix
    Eigen::MatrixXd matrix_state_;
 
    // parameters for lqr solver; number of iterations
    int lqr_max_iteration_ = 0;
    // parameters for lqr solver; threshold for computation
    double lqr_eps_ = 0.0;
 
    common::DigitalFilter digital_filter_;
 
    std::unique_ptr<Interpolation1D> lat_err_interpolation_;
 
    std::unique_ptr<Interpolation1D> heading_err_interpolation_;
 
    // MeanFilter heading_rate_filter_;
    common::MeanFilter lateral_error_filter_;
    common::MeanFilter heading_error_filter_;
 
    // Lead/Lag controller
    bool enable_leadlag_ = false;
    LeadlagPlusController leadlag_controller_;
 
    // Mrac controller
    bool enable_mrac_ = false;
    MracController mrac_controller_;
 
    // Look-ahead controller
    bool enable_look_ahead_back_control_ = false;
    bool use_new_look_ahead_back_ = false;
 
    // for compute the differential valute to estimate acceleration/lon_jerk
    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;
 
    // for logging purpose
    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_;
 
    // AHC means auto heading compensation;
    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;
 
    // checker count
    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;
};
 
// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::LatPlusController, ControlTask)
 
}  // namespace control
}  // namespace apollo