mrac_controller.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2019 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 <vector>
 
#include "Eigen/Core"
 
#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
#include "modules/control/control_component/proto/mrac_conf.pb.h"
 
#include "modules/common/status/status.h"
 
namespace apollo {
namespace control {
 
class MracController {
 public:
  void Init(const MracConf &mrac_conf,
            const common::LatencyParam &latency_param, const double dt);
 
  common::Status SetReferenceModel(const MracConf &mrac_conf);
 
  common::Status SetAdaptionModel(const MracConf &mrac_conf);
 
  common::Status BuildReferenceModel();
 
  common::Status BuildAdaptionModel();
 
  bool CheckLyapunovPD(const Eigen::MatrixXd matrix_a,
                       const Eigen::MatrixXd matrix_p) const;
 
  void EstimateInitialGains(const common::LatencyParam &latency_param);
 
  void UpdateReference();
 
  void UpdateAdaption(Eigen::MatrixXd *law_adp, const Eigen::MatrixXd state_adp,
                      const Eigen::MatrixXd gain_adp);
 
  void AntiWindupCompensation(const double control_command,
                              const double previous_command);
 
  void Reset();
 
  void ResetStates();
 
  void ResetGains();
 
  virtual double Control(const double command, const Eigen::MatrixXd state,
                         const double input_limit,
                         const double input_rate_limit);
 
  int BoundOutput(const double output_unbounded, const double previous_output,
                  double *output_bounded);
 
  void SetInitialReferenceState(const Eigen::MatrixXd &state_reference_init);
 
  void SetInitialActionState(const Eigen::MatrixXd &state_action_init);
 
  void SetInitialCommand(const double command_init);
 
  void SetInitialStateAdaptionGain(
      const Eigen::MatrixXd &gain_state_adaption_init);
 
  void SetInitialInputAdaptionGain(const double gain_input_adaption_init);
 
  void SetInitialNonlinearAdaptionGain(
      const double gain_nonlinear_adaption_init);
 
  void SetStateAdaptionRate(const double ratio_state);
 
  void SetInputAdaptionRate(const double ratio_input);
 
  void SetNonlinearAdaptionRate(const double ratio_nonlinear);
 
  double StateAdaptionRate() const;
 
  double InputAdaptionRate() const;
 
  double NonlinearAdaptionRate() const;
 
  int ReferenceSaturationStatus() const;
 
  int ControlSaturationStatus() const;
 
  Eigen::MatrixXd CurrentReferenceState() const;
 
  Eigen::MatrixXd CurrentStateAdaptionGain() const;
 
  Eigen::MatrixXd CurrentInputAdaptionGain() const;
 
  Eigen::MatrixXd CurrentNonlinearAdaptionGain() const;
 
 protected:
  // indicator if the reference/adaption model is valid
  bool reference_model_enabled_ = false;
  bool adaption_model_enabled_ = false;
 
  // indicator if clamp the adaption laws
  bool adaption_clamping_enabled = false;
 
  // The order of the reference/adaption model
  int model_order_ = 1;
 
  // 1st-order Reference system coefficients in continuous-time domain
  double tau_reference_ = 0.0;
  double tau_clamping_ = 0.0;
  // 2nd-order Reference system coefficients in continuous-time domain
  double wn_reference_ = 0.0;
  double zeta_reference_ = 0.0;
 
  double ts_ = 0.01;  // By default, control sampling time is 0.01 sec
 
  // Adaption system coefficients
  // State adaption gain
  Eigen::MatrixXd gamma_state_adaption_;
  // Desired command adaption gain
  Eigen::MatrixXd gamma_input_adaption_;
  // Nonlinear dynamics adaption gain
  Eigen::MatrixXd gamma_nonlinear_adaption_;
  // Adjustable ratio of the state adaption gain
  double gamma_ratio_state_ = 1.0;
  // Adjustable ratio of the desired command adaption gain
  double gamma_ratio_input_ = 1.0;
  // Adjustable ratio of the nonlinear dynamics adaption gain
  double gamma_ratio_nonlinear_ = 1.0;
 
  // Reference system matrix in continuous-time domain
  Eigen::MatrixXd matrix_a_reference_;  // Matrix A in reference models
  Eigen::MatrixXd matrix_b_reference_;  // Matrix B in reference models
 
  // Adaption system matrix in discrete-time domain
  Eigen::MatrixXd matrix_p_adaption_;  // Matrix P in adaption models
  Eigen::MatrixXd matrix_b_adaption_;  // Matrix B in adaption models
 
  // Adaption system input variables in discrete-time domain
  Eigen::MatrixXd input_desired_;            // Updated desired command vector
  Eigen::MatrixXd state_action_;             // Updated actuation states vector
  Eigen::MatrixXd state_reference_;          // Reference states vector
  Eigen::MatrixXd gain_state_adaption_;      // State adaption vector
  Eigen::MatrixXd gain_input_adaption_;      // Desired command adaption vector
  Eigen::MatrixXd gain_nonlinear_adaption_;  // Nonlinear adaption vector
 
  Eigen::MatrixXd gain_state_clamping_;      // To clamp the state adaption
  Eigen::MatrixXd gain_input_clamping_;      // To clamp the command adaption
  Eigen::MatrixXd gain_nonlinear_clamping_;  // To clamp the nonlinear adaption
 
  Eigen::MatrixXd gain_state_adaption_init_;      // Initial state adaption
  Eigen::MatrixXd gain_input_adaption_init_;      // Initial input adaption
  Eigen::MatrixXd gain_nonlinear_adaption_init_;  // Initial nonlinear adaption
 
  // Mrac control output in the last step
  double control_previous_ = 0.0;
 
  // State saturation limits in discrete-time domain
  double bound_ratio_ = 0.0;
  double bound_command_ = 0.0;
  double bound_command_rate_ = 0.0;
  int saturation_status_reference_ = 0;
  int saturation_status_control_ = 0;
 
  // Anti-Windup compensation
  Eigen::MatrixXd gain_anti_windup_;
  Eigen::MatrixXd compensation_anti_windup_;
};
 
}  // namespace control
}  // namespace apollo