apollo::common::math::MpcOsqp类 参考

#include <mpc_osqp.h>

apollo::common::math::MpcOsqp 的协作图:

Public 成员函数
	MpcOsqp (const Eigen::MatrixXd &matrix_a, const Eigen::MatrixXd &matrix_b, const Eigen::MatrixXd &matrix_q, const Eigen::MatrixXd &matrix_r, const Eigen::MatrixXd &matrix_initial_x, const Eigen::MatrixXd &matrix_u_lower, const Eigen::MatrixXd &matrix_u_upper, const Eigen::MatrixXd &matrix_x_lower, const Eigen::MatrixXd &matrix_x_upper, const Eigen::MatrixXd &matrix_x_ref, const int max_iter, const int horizon, const double eps_abs)
	Solver for discrete-time model predictive control problem.
bool	Solve (std::vector< double > *control_cmd)

详细描述

在文件 mpc_osqp.h 第 33 行定义.

构造及析构函数说明

MpcOsqp()

apollo::common::math::MpcOsqp::MpcOsqp	(	const Eigen::MatrixXd &	matrix_a,
		const Eigen::MatrixXd &	matrix_b,
		const Eigen::MatrixXd &	matrix_q,
		const Eigen::MatrixXd &	matrix_r,
		const Eigen::MatrixXd &	matrix_initial_x,
		const Eigen::MatrixXd &	matrix_u_lower,
		const Eigen::MatrixXd &	matrix_u_upper,
		const Eigen::MatrixXd &	matrix_x_lower,
		const Eigen::MatrixXd &	matrix_x_upper,
		const Eigen::MatrixXd &	matrix_x_ref,
		const int	max_iter,
		const int	horizon,
		const double	eps_abs
	)

Solver for discrete-time model predictive control problem.

参数

matrix_a	The system dynamic matrix
matrix_b	The control matrix
matrix_q	The cost matrix for control state
matrix_lower	The lower bound control constrain matrix
matrix_upper	The upper bound control constrain matrix
matrix_initial_state	The initial state matrix
max_iter	The maximum iterations

在文件 mpc_osqp.cc 第 22 行定义.

    : matrix_a_(matrix_a),
      matrix_b_(matrix_b),
      matrix_q_(matrix_q),
      matrix_r_(matrix_r),
      matrix_initial_x_(matrix_initial_x),
      matrix_u_lower_(matrix_u_lower),
      matrix_u_upper_(matrix_u_upper),
      matrix_x_lower_(matrix_x_lower),
      matrix_x_upper_(matrix_x_upper),
      matrix_x_ref_(matrix_x_ref),
      max_iteration_(max_iter),
      horizon_(horizon),
      eps_abs_(eps_abs) {
  state_dim_ = matrix_b.rows();
  control_dim_ = matrix_b.cols();
  ADEBUG << "state_dim" << state_dim_;
  ADEBUG << "control_dim_" << control_dim_;
  num_param_ = state_dim_ * (horizon_ + 1) + control_dim_ * horizon_;
}

成员函数说明

Solve()

bool apollo::common::math::MpcOsqp::Solve

(

std::vector< double > *

control_cmd

)

在文件 mpc_osqp.cc 第 281 行定义.

                                                  {
  ADEBUG << "Before Calc Gradient";
  CalculateGradient();
  ADEBUG << "After Calc Gradient";
  CalculateConstraintVectors();
  ADEBUG << "MPC2Matrix";
 
  OSQPData *data = Data();
  ADEBUG << "OSQP data done";
  ADEBUG << "OSQP data n" << data->n;
  ADEBUG << "OSQP data m" << data->m;
  for (int i = 0; i < data->n; ++i) {
    ADEBUG << "OSQP data q" << i << ":" << (data->q)[i];
  }
  ADEBUG << "OSQP data l" << data->l;
  for (int i = 0; i < data->m; ++i) {
    ADEBUG << "OSQP data l" << i << ":" << (data->l)[i];
  }
  ADEBUG << "OSQP data u" << data->u;
  for (int i = 0; i < data->m; ++i) {
    ADEBUG << "OSQP data u" << i << ":" << (data->u)[i];
  }
 
  OSQPSettings *settings = Settings();
  ADEBUG << "OSQP setting done";
  OSQPWorkspace *osqp_workspace = nullptr;
  // osqp_setup(&osqp_workspace, data, settings);
  osqp_workspace = osqp_setup(data, settings);
  ADEBUG << "OSQP workspace ready";
  osqp_solve(osqp_workspace);
 
  auto status = osqp_workspace->info->status_val;
  ADEBUG << "status:" << status;
  // check status
  if (status < 0 || (status != 1 && status != 2)) {
    AERROR << "failed optimization status:\t" << osqp_workspace->info->status;
    osqp_cleanup(osqp_workspace);
    FreeData(data);
    c_free(settings);
    return false;
  } else if (osqp_workspace->solution == nullptr) {
    AERROR << "The solution from OSQP is nullptr";
    osqp_cleanup(osqp_workspace);
    FreeData(data);
    c_free(settings);
    return false;
  }
 
  size_t first_control = state_dim_ * (horizon_ + 1);
  for (size_t i = 0; i < control_dim_; ++i) {
    control_cmd->at(i) = osqp_workspace->solution->x[i + first_control];
    ADEBUG << "control_cmd:" << i << ":" << control_cmd->at(i);
  }
 
  // Cleanup
  osqp_cleanup(osqp_workspace);
  FreeData(data);
  c_free(settings);
  return true;
}

---

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

• modules/common/math/mpc_osqp.h
• modules/common/math/mpc_osqp.cc