apollo::control::MracController类 参考

A mrac controller for actuation system (throttle/brake and steering) 更多...

#include <mrac_controller.h>

apollo::control::MracController 的协作图:

Public 成员函数
void	Init (const MracConf &mrac_conf, const common::LatencyParam &latency_param, const double dt)
	initialize mrac controller
common::Status	SetReferenceModel (const MracConf &mrac_conf)
	time constant, natural frequency and damping ratio
common::Status	SetAdaptionModel (const MracConf &mrac_conf)
	state adaptive gain, desired adaptive gain and nonlinear-component adaptive gain
common::Status	BuildReferenceModel ()
	build mrac (1st or 2nd) order reference model in the discrete-time form, with the bilinear transform (trapezoidal integration) method
common::Status	BuildAdaptionModel ()
	build mrac (1st or 2nd) order adaptive dynamic model in the discrete-time form
bool	CheckLyapunovPD (const Eigen::MatrixXd matrix_a, const Eigen::MatrixXd matrix_p) const
	check if the solution of the algebraic Lyapunov Equation is symmetric positive definite
void	EstimateInitialGains (const common::LatencyParam &latency_param)
	estimate the initial states of the adaptive gains via known actuation dynamics approximation
void	UpdateReference ()
	execute the reference state interation with respect to the designed inputs in discrete-time form, with the bilinear transform (trapezoidal integration) method
void	UpdateAdaption (Eigen::MatrixXd *law_adp, const Eigen::MatrixXd state_adp, const Eigen::MatrixXd gain_adp)
	execute the adaption interation with respect to the designed law in discrete-time form, with the bilinear transform (trapezoidal integration) method
void	AntiWindupCompensation (const double control_command, const double previous_command)
	calculate the anti-windup compensation with respect to the integral windup issue
void	Reset ()
	reset all the variables (including all the states, gains and externally-setting control parameters) for mrac controller
void	ResetStates ()
	reset internal states for mrac controller
void	ResetGains ()
	reset adaptive gains for mrac controller
virtual double	Control (const double command, const Eigen::MatrixXd state, const double input_limit, const double input_rate_limit)
	compute control value based on the original command
int	BoundOutput (const double output_unbounded, const double previous_output, double *output_bounded)
	bound the system output with the given bound and change-rate bound
void	SetInitialReferenceState (const Eigen::MatrixXd &state_reference_init)
	set initial values for state components in reference model dynamics
void	SetInitialActionState (const Eigen::MatrixXd &state_action_init)
	set initial values for state components in actual actuator dynamics
void	SetInitialCommand (const double command_init)
	set initial command (desired input)
void	SetInitialStateAdaptionGain (const Eigen::MatrixXd &gain_state_adaption_init)
	set initial values of state adaption gains for mrac control
void	SetInitialInputAdaptionGain (const double gain_input_adaption_init)
	set initial value of input adaption gain for mrac control
void	SetInitialNonlinearAdaptionGain (const double gain_nonlinear_adaption_init)
	set initial value of nonlinear adaption gain for mrac control
void	SetStateAdaptionRate (const double ratio_state)
	set convergence ratio for state components in adaptive dynamics
void	SetInputAdaptionRate (const double ratio_input)
	set convergence ratio for input components in adaptive dynamics
void	SetNonlinearAdaptionRate (const double ratio_nonlinear)
	set convergence ratio for nonlinear components in adaptive dynamics
double	StateAdaptionRate () const
	get convergence ratio for state components in adaptive dynamics
double	InputAdaptionRate () const
	get convergence ratio for input components in adaptive dynamics
double	NonlinearAdaptionRate () const
	get convergence ratio for nonlinear components in adaptive dynamics
int	ReferenceSaturationStatus () const
	get saturation status for reference system
int	ControlSaturationStatus () const
	get saturation status for control system
Eigen::MatrixXd	CurrentReferenceState () const
	get current state for reference system
Eigen::MatrixXd	CurrentStateAdaptionGain () const
	get current state adaptive gain for mrac control
Eigen::MatrixXd	CurrentInputAdaptionGain () const
	get current input adaptive gain for mrac control
Eigen::MatrixXd	CurrentNonlinearAdaptionGain () const
	get current nonlinear adaptive gain for mrac control

Protected 属性
bool	reference_model_enabled_ = false
bool	adaption_model_enabled_ = false
bool	adaption_clamping_enabled = false
int	model_order_ = 1
double	tau_reference_ = 0.0
double	tau_clamping_ = 0.0
double	wn_reference_ = 0.0
double	zeta_reference_ = 0.0
double	ts_ = 0.01
Eigen::MatrixXd	gamma_state_adaption_
Eigen::MatrixXd	gamma_input_adaption_
Eigen::MatrixXd	gamma_nonlinear_adaption_
double	gamma_ratio_state_ = 1.0
double	gamma_ratio_input_ = 1.0
double	gamma_ratio_nonlinear_ = 1.0
Eigen::MatrixXd	matrix_a_reference_
Eigen::MatrixXd	matrix_b_reference_
Eigen::MatrixXd	matrix_p_adaption_
Eigen::MatrixXd	matrix_b_adaption_
Eigen::MatrixXd	input_desired_
Eigen::MatrixXd	state_action_
Eigen::MatrixXd	state_reference_
Eigen::MatrixXd	gain_state_adaption_
Eigen::MatrixXd	gain_input_adaption_
Eigen::MatrixXd	gain_nonlinear_adaption_
Eigen::MatrixXd	gain_state_clamping_
Eigen::MatrixXd	gain_input_clamping_
Eigen::MatrixXd	gain_nonlinear_clamping_
Eigen::MatrixXd	gain_state_adaption_init_
Eigen::MatrixXd	gain_input_adaption_init_
Eigen::MatrixXd	gain_nonlinear_adaption_init_
double	control_previous_ = 0.0
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
Eigen::MatrixXd	gain_anti_windup_
Eigen::MatrixXd	compensation_anti_windup_

详细描述

A mrac controller for actuation system (throttle/brake and steering)

在文件 mrac_controller.h 第 44 行定义.

成员函数说明

AntiWindupCompensation()

void apollo::control::MracController::AntiWindupCompensation	(	const double	control_command,
		const double	previous_command
	)

calculate the anti-windup compensation with respect to the integral windup issue

参数

control_command	desired control command for the actuator
previous_command	last control command for the actuator
dt	control sampling time

在文件 mrac_controller.cc 第 372 行定义.

                                                                           {
  Matrix offset_windup = Matrix::Zero(model_order_, 1);
  offset_windup(0, 0) =
      ((control_command > bound_command_) ? bound_command_ - control_command
                                          : 0.0) +
      ((control_command < -bound_command_) ? -bound_command_ - control_command
                                           : 0.0);
  if (model_order_ > 1) {
    offset_windup(1, 0) =
        ((control_command > previous_command + bound_command_rate_ * ts_)
             ? bound_command_rate_ - (control_command - previous_command) / ts_
             : 0.0) +
        ((control_command < previous_command - bound_command_rate_ * ts_)
             ? -bound_command_rate_ - (control_command - previous_command) / ts_
             : 0.0);
  }
  compensation_anti_windup_.col(1) = compensation_anti_windup_.col(0);
  compensation_anti_windup_.col(0) = gain_anti_windup_ * offset_windup;
}

BoundOutput()

int apollo::control::MracController::BoundOutput	(	const double	output_unbounded,
		const double	previous_output,
		double *	output_bounded
	)

bound the system output with the given bound and change-rate bound

参数

output_unbounded	original system output without bound
previous_output	system output in the last step
dt	sampling time interval
output_bounded	bounded system output

返回

saturation_status system saturation status indicator

在文件 mrac_controller.cc 第 393 行定义.

                                                                              {
  int status = 0;
  if (output_unbounded > bound_command_ ||
      output_unbounded > previous_output + bound_command_rate_ * ts_) {
    *output = (bound_command_ < previous_output + bound_command_rate_ * ts_)
                  ? bound_command_
                  : previous_output + bound_command_rate_ * ts_;
    // if output exceeds the upper bound, then status = 1; while if output
    // changing rate exceeds the upper rate bound, then status = 2
    status =
        (bound_command_ < previous_output + bound_command_rate_ * ts_) ? 1 : 2;
  } else if (output_unbounded < -bound_command_ ||
             output_unbounded < previous_output - bound_command_rate_ * ts_) {
    *output = (-bound_command_ > previous_output - bound_command_rate_ * ts_)
                  ? -bound_command_
                  : previous_output - bound_command_rate_ * ts_;
    // if output exceeds the lower bound, then status = -1; while if output
    // changing rate exceeds the lower rate bound, then status = -2
    status = (-bound_command_ > previous_output - bound_command_rate_ * ts_)
                 ? -1
                 : -2;
  } else {
    *output = output_unbounded;
    // if output does not violate neither bound nor rate bound, then status = 0
    status = 0;
  }
  return status;
}

BuildAdaptionModel()

Status apollo::control::MracController::BuildAdaptionModel

(

)

build mrac (1st or 2nd) order adaptive dynamic model in the discrete-time form

返回

Status adaption model initialization status

在文件 mrac_controller.cc 第 262 行定义.

                                          {
  if (model_order_ > 2) {
    const auto error_msg =
        absl::StrCat("mrac controller error: adaption model order ",
                     model_order_, " is beyond the designed range");
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }
  if (model_order_ == 1) {
    matrix_b_adaption_(0, 0) = 1.0;
  } else if (model_order_ == 2) {
    matrix_b_adaption_(1, 0) = wn_reference_ * wn_reference_;
  }
  if (!CheckLyapunovPD(matrix_a_reference_, matrix_p_adaption_)) {
    const std::string error_msg =
        "Solution of the algebraic Lyapunov equation is not symmetric positive "
        "definite";
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }
  return Status::OK();
}

BuildReferenceModel()

Status apollo::control::MracController::BuildReferenceModel

(

)

build mrac (1st or 2nd) order reference model in the discrete-time form, with the bilinear transform (trapezoidal integration) method

返回

Status reference model initialization status

在文件 mrac_controller.cc 第 242 行定义.

                                           {
  if (model_order_ > 2) {
    const auto error_msg =
        absl::StrCat("mrac controller error: reference model order ",
                     model_order_, " is beyond the designed range");
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }
  if (model_order_ == 1) {
    matrix_a_reference_(0, 0) = -1.0 / tau_reference_;
    matrix_b_reference_(0, 0) = 1.0 / tau_reference_;
  } else if (model_order_ == 2) {
    matrix_a_reference_(0, 1) = 1.0;
    matrix_a_reference_(1, 0) = -wn_reference_ * wn_reference_;
    matrix_a_reference_(1, 1) = -2 * zeta_reference_ * wn_reference_;
    matrix_b_reference_(1, 0) = wn_reference_ * wn_reference_;
  }
  return Status::OK();
}

CheckLyapunovPD()

bool apollo::control::MracController::CheckLyapunovPD	(	const Eigen::MatrixXd	matrix_a,
		const Eigen::MatrixXd	matrix_p
	)		const

check if the solution of the algebraic Lyapunov Equation is symmetric positive definite

参数

matrix_a	reference model matrix
matrix_p	Lyapunov function matrix

返回

indicator of the symmetric positive definite matrix

在文件 mrac_controller.cc 第 285 行定义.

                                                                  {
  Matrix matrix_q = -matrix_p * matrix_a - matrix_a.transpose() * matrix_p;
  Eigen::LLT<Matrix> llt_matrix_q(matrix_q);
  // if matrix Q is not symmetric or the Cholesky decomposition (LLT) failed
  // due to the matrix Q are not positive definite
  return (matrix_q.isApprox(matrix_q.transpose()) &&
          llt_matrix_q.info() != Eigen::NumericalIssue);
}

Control()

double apollo::control::MracController::Control ( const double  command, const Eigen::MatrixXd  state, const double  input_limit, const double  input_rate_limit  )

virtual

compute control value based on the original command

参数

command	original command as the input of the actuation system
state	actual output state of the actuation system
dt	sampling time interval
input_limit	physical or designed bound of the input
input_rate_limit	physical or designed bound of the input changing-rate

返回

control value based on mrac controller architecture

在文件 mrac_controller.cc 第 38 行定义.

                                                              {
  // check if the current sampling time is valid and the reference/adaption
  // model well set up during the initialization
  if (ts_ <= 0.0 || !reference_model_enabled_ || !adaption_model_enabled_) {
    AERROR << "MRAC: model build failed; will work as a unity compensator. The "
              "reference_model building status: "
           << reference_model_enabled_
           << "; The adaption_model building status: "
           << adaption_model_enabled_ << "; Current sampling time: " << ts_;
    return command;  // treat the mrac as a unity proportional controller
  }
 
  // update the state in the real actuation system
  state_action_.col(0) = state;
 
  // update the desired command in the real actuation system
  input_desired_(0, 0) = command;
 
  // update the command bounds for the real actuation system
  bound_command_ = input_limit * bound_ratio_;
  bound_command_rate_ = input_rate_limit * bound_ratio_;
 
  // update the state in the reference system
  UpdateReference();
 
  double state_bounded = 0.0;
  saturation_status_reference_ = BoundOutput(
      state_reference_(0, 0), state_reference_(0, 1), &state_bounded);
  state_reference_(0, 0) = state_bounded;
 
  // update the adaption laws including state adaption, command adaption and
  // nonlinear components adaption
  UpdateAdaption(&gain_state_adaption_, state_action_,
                 gamma_state_adaption_ * gamma_ratio_state_);
 
  UpdateAdaption(&gain_input_adaption_, input_desired_,
                 gamma_input_adaption_ * gamma_ratio_input_);
 
  // revert the adaption law updates if they are beyond the clamping values
  if (model_order_ == 1 && adaption_clamping_enabled &&
      (gain_state_adaption_(0, 0) > 0.0 ||
       gain_state_adaption_(0, 0) < gain_state_clamping_(0, 0) ||
       gain_input_adaption_(0, 0) > gain_input_clamping_(0, 0) ||
       gain_input_adaption_(0, 0) < 1.0)) {
    gain_state_adaption_.col(0) = gain_state_adaption_.col(1);
    gain_input_adaption_.col(0) = gain_input_adaption_.col(1);
  }
 
  // update the generated control based on the adaptive law
  double control_unbounded =
      gain_state_adaption_.col(0).transpose() * state_action_.col(0) +
      gain_input_adaption_(0, 0) * input_desired_(0, 0);
 
  double control = 0.0;
  saturation_status_control_ =
      BoundOutput(control_unbounded, control_previous_, &control);
 
  // update the anti-windup compensation if applied
  AntiWindupCompensation(control_unbounded, control_previous_);
 
  // update the previous value for next iteration
  gain_state_adaption_.col(1) = gain_state_adaption_.col(0);
  gain_input_adaption_.col(1) = gain_input_adaption_.col(0);
  state_reference_.col(1) = state_reference_.col(0);
  state_action_.col(1) = state_action_.col(0);
  input_desired_.col(1) = input_desired_.col(0);
  control_previous_ = control;
  return control;
}

ControlSaturationStatus()

int apollo::control::MracController::ControlSaturationStatus

(

)

const

get saturation status for control system

返回

saturation status

在文件 mrac_controller.cc 第 518 行定义.

                                                  {
  return saturation_status_control_;
}

CurrentInputAdaptionGain()

Matrix apollo::control::MracController::CurrentInputAdaptionGain

(

)

const

get current input adaptive gain for mrac control

返回

current input adaptive gain

在文件 mrac_controller.cc 第 530 行定义.

                                                      {
  return gain_input_adaption_;
}

CurrentNonlinearAdaptionGain()

Matrix apollo::control::MracController::CurrentNonlinearAdaptionGain

(

)

const

get current nonlinear adaptive gain for mrac control

返回

current nonlinear adaptive gain

在文件 mrac_controller.cc 第 534 行定义.

                                                          {
  return gain_nonlinear_adaption_;
}

CurrentReferenceState()

Matrix apollo::control::MracController::CurrentReferenceState

(

)

const

get current state for reference system

返回

current state

在文件 mrac_controller.cc 第 522 行定义.

                                                   {
  return state_reference_;
}

CurrentStateAdaptionGain()

Matrix apollo::control::MracController::CurrentStateAdaptionGain

(

)

const

get current state adaptive gain for mrac control

返回

current state adaptive gain

在文件 mrac_controller.cc 第 526 行定义.

                                                      {
  return gain_state_adaption_;
}

EstimateInitialGains()

void apollo::control::MracController::EstimateInitialGains

(

const common::LatencyParam &

latency_param

)

estimate the initial states of the adaptive gains via known actuation dynamics approximation

参数

latency_param

configuration for actuation latency parameters

在文件 mrac_controller.cc 第 295 行定义.

                                                                           {
  const double rise_time_estimate =
      latency_param.dead_time() + latency_param.rise_time();
  const double settling_time_estimate =
      latency_param.dead_time() + latency_param.settling_time();
  Matrix matrix_a_estimate = Matrix::Zero(model_order_, model_order_);
  Matrix matrix_b_estimate = Matrix::Zero(model_order_, 1);
  Matrix matrix_a_clamping = Matrix::Zero(model_order_, model_order_);
  Matrix matrix_b_clamping = Matrix::Zero(model_order_, 1);
  if (model_order_ == 1 &&
      (rise_time_estimate >= ts_ || settling_time_estimate >= ts_)) {
    const double tau_estimate = (rise_time_estimate >= ts_)
                                    ? rise_time_estimate / 2.2
                                    : settling_time_estimate / 4.0;
    // generate the initial adaptive gains
    matrix_a_estimate(0, 0) = -1.0 / tau_estimate;
    matrix_b_estimate(0, 0) = 1.0 / tau_estimate;
    gain_state_adaption_init_(0, 0) =
        (matrix_a_reference_(0, 0) - matrix_a_estimate(0, 0)) /
        matrix_b_estimate(0, 0);
    gain_input_adaption_init_(0, 0) =
        matrix_b_reference_(0, 0) / matrix_b_estimate(0, 0);
    // generate the clamping bounds for adaptive gains
    if (adaption_clamping_enabled) {
      matrix_a_clamping(0, 0) = -1.0 / tau_clamping_;
      matrix_b_clamping(0, 0) = 1.0 / tau_clamping_;
      gain_state_clamping_(0, 0) =
          (matrix_a_clamping(0, 0) - matrix_a_estimate(0, 0)) /
          matrix_b_estimate(0, 0);
      gain_input_clamping_(0, 0) =
          matrix_b_clamping(0, 0) / matrix_b_estimate(0, 0);
    }
  } else if (model_order_ == 2 &&
             (rise_time_estimate >= ts_ && settling_time_estimate >= ts_)) {
    const double wn_estimate = 1.8 / rise_time_estimate;
    const double zeta_estimate =
        4.6 / (rise_time_estimate * settling_time_estimate);
    matrix_a_estimate(0, 1) = 1.0;
    matrix_a_estimate(1, 0) = -wn_estimate * wn_estimate;
    matrix_a_estimate(1, 1) = -2 * zeta_estimate * wn_estimate;
    matrix_b_estimate(1, 0) = wn_estimate * wn_estimate;
    gain_state_adaption_init_.col(0) =
        (common::math::PseudoInverse<double, 2, 1>(matrix_b_estimate) *
         (matrix_a_reference_ - matrix_a_estimate))
            .transpose();
    gain_input_adaption_init_.col(0) =
        (common::math::PseudoInverse<double, 2, 1>(matrix_b_estimate) *
         matrix_b_reference_)
            .transpose();
  } else {
    AWARN << "No pre-known actuation dynamics; the initial states of the "
             "adaptive gains are set as zeros";
  }
}

Init()

void apollo::control::MracController::Init	(	const MracConf &	mrac_conf,
		const common::LatencyParam &	latency_param,
		const double	dt
	)

initialize mrac controller

参数

mrac_conf	configuration for mrac controller
latency_param	configuration for latency parameter
dt	sampling time interval

在文件 mrac_controller.cc 第 141 行定义.

                                                                              {
  control_previous_ = 0.0;
  saturation_status_control_ = 0;
  saturation_status_reference_ = 0;
  ts_ = dt;
  // Initialize the common model parameters
  model_order_ = mrac_conf.mrac_model_order();
  // Initialize the saturation limits
  bound_ratio_ = mrac_conf.mrac_saturation_level();
  // Initialize the system states
  input_desired_ = Matrix::Zero(1, 2);
  state_action_ = Matrix::Zero(model_order_, 2);
  state_reference_ = Matrix::Zero(model_order_, 2);
  // Initialize the adaptive control gains
  gain_state_adaption_ = Matrix::Zero(model_order_, 2);
  gain_input_adaption_ = Matrix::Ones(1, 2);
  gain_nonlinear_adaption_ = Matrix::Zero(1, 2);
  gain_state_clamping_ = Matrix::Zero(model_order_, 1);
  gain_input_clamping_ = Matrix::Ones(1, 1);
  gain_nonlinear_clamping_ = Matrix::Zero(1, 1);
  gain_state_adaption_init_ = Matrix::Zero(model_order_, 1);
  gain_input_adaption_init_ = Matrix::Ones(1, 1);
  gain_nonlinear_adaption_init_ = Matrix::Zero(1, 1);
  // Initialize the adaptive convergence gains and anti-windup gains
  gamma_state_adaption_ = Matrix::Zero(model_order_, model_order_);
  gamma_input_adaption_ = Matrix::Zero(1, 1);
  gamma_nonlinear_adaption_ = Matrix::Zero(1, 1);
  gain_anti_windup_ = Matrix::Zero(model_order_, model_order_);
  compensation_anti_windup_ = Matrix::Zero(model_order_, 2);
  // Initialize the reference model parameters
  matrix_a_reference_ = Matrix::Zero(model_order_, model_order_);
  matrix_b_reference_ = Matrix::Zero(model_order_, 1);
  reference_model_enabled_ =
      (SetReferenceModel(mrac_conf).ok() && BuildReferenceModel().ok());
  // Initialize the adaption model parameters
  matrix_p_adaption_ = Matrix::Zero(model_order_, model_order_);
  matrix_b_adaption_ = Matrix::Zero(model_order_, 1);
  adaption_model_enabled_ =
      (SetAdaptionModel(mrac_conf).ok() && BuildAdaptionModel().ok());
  EstimateInitialGains(latency_param);
  gain_state_adaption_.col(1) = gain_state_adaption_init_;
  gain_input_adaption_.col(1) = gain_input_adaption_init_;
}

InputAdaptionRate()

double apollo::control::MracController::InputAdaptionRate

(

)

const

get convergence ratio for input components in adaptive dynamics

返回

input adaption ratio

在文件 mrac_controller.cc 第 508 行定义.

{ return gamma_ratio_input_; }

NonlinearAdaptionRate()

double apollo::control::MracController::NonlinearAdaptionRate

(

)

const

get convergence ratio for nonlinear components in adaptive dynamics

返回

nonlinear adaption ratio

在文件 mrac_controller.cc 第 510 行定义.

                                                   {
  return gamma_ratio_nonlinear_;
}

ReferenceSaturationStatus()

int apollo::control::MracController::ReferenceSaturationStatus

(

)

const

get saturation status for reference system

返回

saturation status

在文件 mrac_controller.cc 第 514 行定义.

                                                    {
  return saturation_status_reference_;
}

Reset()

void apollo::control::MracController::Reset

(

)

reset all the variables (including all the states, gains and externally-setting control parameters) for mrac controller

在文件 mrac_controller.cc 第 110 行定义.

                           {
  // reset the overall states
  ResetStates();
  // reset the adaptive gains
  ResetGains();
  // reset all the externally-setting, non-conf control parameters
  gamma_ratio_state_ = 1.0;
  gamma_ratio_input_ = 1.0;
  gamma_ratio_nonlinear_ = 1.0;
}

ResetGains()

void apollo::control::MracController::ResetGains

(

)

reset adaptive gains for mrac controller

在文件 mrac_controller.cc 第 133 行定义.

                                {
  gain_state_adaption_.setZero(model_order_, 2);
  gain_input_adaption_ = Matrix::Ones(1, 2);
  gain_nonlinear_adaption_.setZero(1, 2);
  gain_state_adaption_.col(1) = gain_state_adaption_init_;
  gain_input_adaption_.col(1) = gain_input_adaption_init_;
}

ResetStates()

void apollo::control::MracController::ResetStates

(

)

reset internal states for mrac controller

在文件 mrac_controller.cc 第 121 行定义.

                                 {
  // reset the inputs and outputs of the closed-loop MRAC controller
  control_previous_ = 0.0;
  input_desired_.setZero(1, 2);
  // reset the internal states, anti-windup compensations and status
  state_action_.setZero(model_order_, 2);
  state_reference_.setZero(model_order_, 2);
  compensation_anti_windup_.setZero(model_order_, 2);
  saturation_status_reference_ = 0;
  saturation_status_control_ = 0;
}

SetAdaptionModel()

Status apollo::control::MracController::SetAdaptionModel

(

const MracConf &

mrac_conf

)

state adaptive gain, desired adaptive gain and nonlinear-component adaptive gain

参数

mrac_conf

configuration for adaption model

返回

Status parameter initialization status

在文件 mrac_controller.cc 第 210 行定义.

                                                                 {
  const int p_size = mrac_conf.adaption_matrix_p_size();
  const int x_size = mrac_conf.adaption_state_gain_size();
  const int aw_size = mrac_conf.anti_windup_compensation_gain_size();
  if (p_size != model_order_ * model_order_ || x_size > model_order_ ||
      aw_size > model_order_) {
    const auto error_msg = absl::StrCat(
        "mrac controller error: adaption matrix p element number: ", p_size,
        ", state gain number: ", x_size,
        ", and anti-windup compensation gain number: ", aw_size,
        " in configuration file do not match the model number: ", model_order_);
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }
  for (int i = 0; i < model_order_; ++i) {
    gamma_state_adaption_(i, i) =
        (i <= x_size - 1) ? mrac_conf.adaption_state_gain(i)
                          : mrac_conf.adaption_state_gain(x_size - 1);
    gain_anti_windup_(i, i) =
        (i <= aw_size - 1)
            ? mrac_conf.anti_windup_compensation_gain(i)
            : mrac_conf.anti_windup_compensation_gain(aw_size - 1);
    for (int j = 0; j < model_order_; ++j) {
      matrix_p_adaption_(i, j) =
          mrac_conf.adaption_matrix_p(i * model_order_ + j);
    }
  }
  gamma_input_adaption_(0, 0) = mrac_conf.adaption_desired_gain();
  gamma_nonlinear_adaption_(0, 0) = mrac_conf.adaption_nonlinear_gain();
  return Status::OK();
}

SetInitialActionState()

void apollo::control::MracController::SetInitialActionState

(

const Eigen::MatrixXd &

state_action_init

)

set initial values for state components in actual actuator dynamics

参数

state_reference_init

initial action states

在文件 mrac_controller.cc 第 436 行定义.

                                                                          {
  if (state_action_init.rows() != model_order_ ||
      state_action_init.cols() != 1) {
    AWARN << "failed to set the initial action states, due to the given "
             "state size: "
          << state_action_init.rows() << " x " << state_action_init.cols()
          << " doesn't match the model order: " << model_order_;
  } else {
    state_action_.col(1) = state_action_init;
  }
}

SetInitialCommand()

void apollo::control::MracController::SetInitialCommand

(

const double

command_init

)

set initial command (desired input)

参数

command_init

initial desired input

在文件 mrac_controller.cc 第 448 行定义.

                                                                {
  input_desired_(0, 1) = command_init;
}

SetInitialInputAdaptionGain()

void apollo::control::MracController::SetInitialInputAdaptionGain

(

const double

gain_input_adaption_init

)

set initial value of input adaption gain for mrac control

参数

gain_input_adaption_init

initial input adaption gain

在文件 mrac_controller.cc 第 466 行定义.

                                           {
  gain_input_adaption_(0, 1) = gain_input_adaption_init;
}

SetInitialNonlinearAdaptionGain()

void apollo::control::MracController::SetInitialNonlinearAdaptionGain

(

const double

gain_nonlinear_adaption_init

)

set initial value of nonlinear adaption gain for mrac control

参数

gain_nonlinear_adaption_init

initial nonlinear adaption gain

在文件 mrac_controller.cc 第 471 行定义.

                                               {
  gain_nonlinear_adaption_(0, 1) = gain_nonlinear_adaption_init;
}

SetInitialReferenceState()

void apollo::control::MracController::SetInitialReferenceState

(

const Eigen::MatrixXd &

state_reference_init

)

set initial values for state components in reference model dynamics

参数

state_reference_init

initial reference states

在文件 mrac_controller.cc 第 423 行定义.

                                        {
  if (state_reference_init.rows() != model_order_ ||
      state_reference_init.cols() != 1) {
    AWARN << "failed to set the initial reference states, due to the given "
             "state size: "
          << state_reference_init.rows() << " x " << state_reference_init.cols()
          << " doesn't match the model order: " << model_order_;
  } else {
    state_reference_.col(1) = state_reference_init;
  }
}

SetInitialStateAdaptionGain()

void apollo::control::MracController::SetInitialStateAdaptionGain

(

const Eigen::MatrixXd &

gain_state_adaption_init

)

set initial values of state adaption gains for mrac control

参数

gain_state_adaption_init

initial state adaption gains

在文件 mrac_controller.cc 第 452 行定义.

                                            {
  if (gain_state_adaption_init.rows() != model_order_ ||
      gain_state_adaption_init.cols() != 1) {
    AWARN << "failed to set the initial state adaption gains, due to the given "
             "state size: "
          << gain_state_adaption_init.rows() << " x "
          << gain_state_adaption_init.cols()
          << " doesn't match the model order: " << model_order_;
  } else {
    gain_state_adaption_.col(1) = gain_state_adaption_init;
  }
}

SetInputAdaptionRate()

void apollo::control::MracController::SetInputAdaptionRate

(

const double

ratio_input

)

set convergence ratio for input components in adaptive dynamics

参数

ratio_input

convergence ratio for input adaption

在文件 mrac_controller.cc 第 486 行定义.

                                                                  {
  if (ratio_input < 0.0) {
    AWARN << "failed to set the input adaption rate, due to new ratio < 0; the "
             "current ratio is still: "
          << gamma_ratio_input_;
  } else {
    gamma_ratio_input_ = ratio_input;
  }
}

SetNonlinearAdaptionRate()

void apollo::control::MracController::SetNonlinearAdaptionRate

(

const double

ratio_nonlinear

)

set convergence ratio for nonlinear components in adaptive dynamics

参数

ratio_nonlinear

convergence ratio for additional nonlinear adaption

在文件 mrac_controller.cc 第 496 行定义.

                                                                          {
  if (ratio_nonlinear < 0.0) {
    AWARN << "failed to set the nonlinear adaption rate, due to new ratio < 0; "
             "the current ratio is still: "
          << gamma_ratio_nonlinear_;
  } else {
    gamma_ratio_nonlinear_ = ratio_nonlinear;
  }
}

SetReferenceModel()

Status apollo::control::MracController::SetReferenceModel

(

const MracConf &

mrac_conf

)

time constant, natural frequency and damping ratio

参数

mrac_conf

configuration for reference model

返回

Status parameter initialization status

在文件 mrac_controller.cc 第 186 行定义.

                                                                  {
  const double Epsilon = 0.000001;
  if (((mrac_conf.reference_time_constant() < Epsilon && model_order_ == 1)) ||
      ((mrac_conf.reference_natural_frequency() < Epsilon &&
        model_order_ == 2))) {
    const auto error_msg = absl::StrCat(
        "mrac controller error: reference model time-constant parameter: ",
        mrac_conf.reference_time_constant(),
        "and natural frequency parameter: ",
        mrac_conf.reference_natural_frequency(),
        " in configuration file are not reasonable with respect to the "
        "reference model order: ",
        model_order_);
    AERROR << error_msg;
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }
  tau_reference_ = mrac_conf.reference_time_constant();
  wn_reference_ = mrac_conf.reference_natural_frequency();
  zeta_reference_ = mrac_conf.reference_damping_ratio();
  tau_clamping_ = mrac_conf.clamping_time_constant();
  adaption_clamping_enabled = (tau_clamping_ >= Epsilon);
  return Status::OK();
}

SetStateAdaptionRate()

void apollo::control::MracController::SetStateAdaptionRate

(

const double

ratio_state

)

set convergence ratio for state components in adaptive dynamics

参数

ratio_state

convergence ratio for state adaption

在文件 mrac_controller.cc 第 476 行定义.

                                                                  {
  if (ratio_state < 0.0) {
    AWARN << "failed to set the state adaption rate, due to new ratio < 0; the "
             "current ratio is still: "
          << gamma_ratio_state_;
  } else {
    gamma_ratio_state_ = ratio_state;
  }
}

StateAdaptionRate()

double apollo::control::MracController::StateAdaptionRate

(

)

const

get convergence ratio for state components in adaptive dynamics

返回

state adaption ratio

在文件 mrac_controller.cc 第 506 行定义.

{ return gamma_ratio_state_; }

UpdateAdaption()

void apollo::control::MracController::UpdateAdaption	(	Eigen::MatrixXd *	law_adp,
		const Eigen::MatrixXd	state_adp,
		const Eigen::MatrixXd	gain_adp
	)

execute the adaption interation with respect to the designed law in discrete-time form, with the bilinear transform (trapezoidal integration) method

参数

law_adp	adaptive law at k and k-1 steps
state_adp	state used in the adaptive law at k and k-1 steps
gain_adp	adaptive gain for the given adaptive law

在文件 mrac_controller.cc 第 359 行定义.

                                                           {
  Matrix state_error = state_action_ - state_reference_;
  law_adp->col(0) =
      law_adp->col(1) -
      0.5 * ts_ * gain_adp *
          (state_adp.col(0) * (state_error.col(0).transpose() +
                               compensation_anti_windup_.col(0).transpose()) +
           state_adp.col(1) * (state_error.col(1).transpose() +
                               compensation_anti_windup_.col(1).transpose())) *
          matrix_p_adaption_ * matrix_b_adaption_;
}

UpdateReference()

void apollo::control::MracController::UpdateReference

(

)

execute the reference state interation with respect to the designed inputs in discrete-time form, with the bilinear transform (trapezoidal integration) method

在文件 mrac_controller.cc 第 350 行定义.

                                     {
  Matrix matrix_i = Matrix::Identity(model_order_, model_order_);
  state_reference_.col(0) =
      (matrix_i - ts_ * 0.5 * matrix_a_reference_).inverse() *
      ((matrix_i + ts_ * 0.5 * matrix_a_reference_) * state_reference_.col(1) +
       ts_ * 0.5 * matrix_b_reference_ *
           (input_desired_(0, 0) + input_desired_(0, 1)));
}

类成员变量说明

adaption_clamping_enabled

bool apollo::control::MracController::adaption_clamping_enabled = false

protected

在文件 mrac_controller.h 第 286 行定义.

adaption_model_enabled_

bool apollo::control::MracController::adaption_model_enabled_ = false

protected

在文件 mrac_controller.h 第 283 行定义.

bound_command_

double apollo::control::MracController::bound_command_ = 0.0

protected

在文件 mrac_controller.h 第 343 行定义.

bound_command_rate_

double apollo::control::MracController::bound_command_rate_ = 0.0

protected

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

bound_ratio_

double apollo::control::MracController::bound_ratio_ = 0.0

protected

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

compensation_anti_windup_

Eigen::MatrixXd apollo::control::MracController::compensation_anti_windup_

protected

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

control_previous_

double apollo::control::MracController::control_previous_ = 0.0

protected

在文件 mrac_controller.h 第 339 行定义.

gain_anti_windup_

Eigen::MatrixXd apollo::control::MracController::gain_anti_windup_

protected

在文件 mrac_controller.h 第 349 行定义.

gain_input_adaption_

Eigen::MatrixXd apollo::control::MracController::gain_input_adaption_

protected

在文件 mrac_controller.h 第 327 行定义.

gain_input_adaption_init_

Eigen::MatrixXd apollo::control::MracController::gain_input_adaption_init_

protected

在文件 mrac_controller.h 第 335 行定义.

gain_input_clamping_

Eigen::MatrixXd apollo::control::MracController::gain_input_clamping_

protected

在文件 mrac_controller.h 第 331 行定义.

gain_nonlinear_adaption_

Eigen::MatrixXd apollo::control::MracController::gain_nonlinear_adaption_

protected

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

gain_nonlinear_adaption_init_

Eigen::MatrixXd apollo::control::MracController::gain_nonlinear_adaption_init_

protected

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

gain_nonlinear_clamping_

Eigen::MatrixXd apollo::control::MracController::gain_nonlinear_clamping_

protected

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

gain_state_adaption_

Eigen::MatrixXd apollo::control::MracController::gain_state_adaption_

protected

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

gain_state_adaption_init_

Eigen::MatrixXd apollo::control::MracController::gain_state_adaption_init_

protected

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

gain_state_clamping_

Eigen::MatrixXd apollo::control::MracController::gain_state_clamping_

protected

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

gamma_input_adaption_

Eigen::MatrixXd apollo::control::MracController::gamma_input_adaption_

protected

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

gamma_nonlinear_adaption_

Eigen::MatrixXd apollo::control::MracController::gamma_nonlinear_adaption_

protected

在文件 mrac_controller.h 第 306 行定义.

gamma_ratio_input_

double apollo::control::MracController::gamma_ratio_input_ = 1.0

protected

在文件 mrac_controller.h 第 310 行定义.

gamma_ratio_nonlinear_

double apollo::control::MracController::gamma_ratio_nonlinear_ = 1.0

protected

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

gamma_ratio_state_

double apollo::control::MracController::gamma_ratio_state_ = 1.0

protected

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

gamma_state_adaption_

Eigen::MatrixXd apollo::control::MracController::gamma_state_adaption_

protected

在文件 mrac_controller.h 第 302 行定义.

input_desired_

Eigen::MatrixXd apollo::control::MracController::input_desired_

protected

在文件 mrac_controller.h 第 323 行定义.

matrix_a_reference_

Eigen::MatrixXd apollo::control::MracController::matrix_a_reference_

protected

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

matrix_b_adaption_

Eigen::MatrixXd apollo::control::MracController::matrix_b_adaption_

protected

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

matrix_b_reference_

Eigen::MatrixXd apollo::control::MracController::matrix_b_reference_

protected

在文件 mrac_controller.h 第 316 行定义.

matrix_p_adaption_

Eigen::MatrixXd apollo::control::MracController::matrix_p_adaption_

protected

在文件 mrac_controller.h 第 319 行定义.

model_order_

int apollo::control::MracController::model_order_ = 1

protected

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

reference_model_enabled_

bool apollo::control::MracController::reference_model_enabled_ = false

protected

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

saturation_status_control_

int apollo::control::MracController::saturation_status_control_ = 0

protected

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

saturation_status_reference_

int apollo::control::MracController::saturation_status_reference_ = 0

protected

在文件 mrac_controller.h 第 345 行定义.

state_action_

Eigen::MatrixXd apollo::control::MracController::state_action_

protected

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

state_reference_

Eigen::MatrixXd apollo::control::MracController::state_reference_

protected

在文件 mrac_controller.h 第 325 行定义.

tau_clamping_

double apollo::control::MracController::tau_clamping_ = 0.0

protected

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

tau_reference_

double apollo::control::MracController::tau_reference_ = 0.0

protected

在文件 mrac_controller.h 第 292 行定义.

ts_

double apollo::control::MracController::ts_ = 0.01

protected

在文件 mrac_controller.h 第 298 行定义.

wn_reference_

double apollo::control::MracController::wn_reference_ = 0.0

protected

在文件 mrac_controller.h 第 295 行定义.

zeta_reference_

double apollo::control::MracController::zeta_reference_ = 0.0

protected

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

---

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

• modules/control/control_component/controller_task_base/common/mrac_controller.h
• modules/control/control_component/controller_task_base/common/mrac_controller.cc