mrac_controller.h

Go to the documentation of this file.

/******************************************************************************
 * 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/configs/proto/vehicle_config.pb.h"
#include "modules/common/status/status.h"
#include "modules/control/proto/mrac_conf.pb.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