lat_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_controller/proto/lat_based_lqr_controller_conf.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/leadlag_controller.h"
#include "modules/control/control_component/controller_task_base/common/mrac_controller.h"
#include "modules/control/control_component/controller_task_base/common/trajectory_analyzer.h"
#include "modules/control/control_component/controller_task_base/control_task.h"
 
namespace apollo {
namespace control {
 
class LatController : public ControlTask {
 public:
  LatController();
 
  virtual ~LatController();
 
  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:
  void UpdateState(SimpleLateralDebug *debug, const canbus::Chassis *chassis);
 
  // logic for reverse driving mode
  void UpdateDrivingOrientation();
 
  void UpdateMatrix();
 
  void UpdateMatrixCompound();
 
  double ComputeFeedForward(double ref_curvature) const;
 
  void ComputeLateralErrors(const double x, const double y, const double theta,
                            const double linear_v, const double angular_v,
                            const double linear_a,
                            const TrajectoryAnalyzer &trajectory_analyzer,
                            SimpleLateralDebug *debug,
                            const canbus::Chassis *chassis);
  bool LoadControlConf();
  void InitializeFilters();
  void LoadLatGainScheduler();
  void LogInitParameters();
  void ProcessLogs(const SimpleLateralDebug *debug,
                   const canbus::Chassis *chassis);
 
  void CloseLogFile();
 
  // vehicle
  LatBaseLqrControllerConf lat_based_lqr_controller_conf_;
 
  // vehicle parameter
  common::VehicleParam vehicle_param_;
 
  // a proxy to analyze the planning trajectory
  TrajectoryAnalyzer 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;
  // 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;
  LeadlagController leadlag_controller_;
 
  // Mrac controller
  bool enable_mrac_ = false;
  MracController mrac_controller_;
 
  // Look-ahead controller
  bool enable_look_ahead_back_control_ = 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;
 
  // 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_;
};
 
// 1.2 当前类声明为插件
CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::control::LatController,
                                     ControlTask)
 
}  // namespace control
}  // namespace apollo