Apollo MPC OSQP Solver
Apollo MPC OSQP Solver
Apollo MPC算法之前使用qpOASES Solver, 现在替换为OSQP, OSQP 开发者自己做个Benchmark, 测试Solver性能,结果现实OSQP的运行效率是比qpOASES要高的, 我们自己也做过测试,对于同样的MPC实时优化问题, OSQP的求解速度的确比qpOASES要快很多。
OSQP Benchmark
使用qpOASES Solver构造二次规划问题的形式与OSQP Solver构造二次规划问题的形式不相同,关于qpOASES Solver是如何构造二次规划问题的可以参考:
qpOASES QP Formulation
关于Apollo使用的基于道路进行线性化的车辆动力学公式以及离散化方式请参考:
Linearized Lateral Error Dynamics
现在假设在采样时刻 我们已经得到如下车辆Error Dynamics 线性系统状态方程
同时, 我们状态量和输入量的上下限, , , 已经规定好, 和为时刻的状态惩罚矩阵和输入惩罚矩阵。, 和分别为时刻 Linearized Error Dynamics的系统矩阵, 输入矩阵和干扰矩阵。
在时刻,将这些量带入初始化OSQP Solver:
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)
: 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_;
}
参数对应表格:
参数 | 代码变量名称 |
---|---|
matrix_a | |
matrix_b | |
matrix_q | |
matrix_r | |
matrix_initial_x | |
matrix_x_upper | |
matrix_x_lower | |
matrix_u_upper | |
matrix_u_lower |
优化目标函数和约束
Apollo MPC使用如下优化目标函数,
其中为prediction horizon。
OSQP 二次规划标准形式
OSQP优化问题的标准形式如下:
上述二次规划问题只包括不等式约束不包括等式约束,对于等式约束,应该对其进行如下变换,将其化为不等式约束:
对于Apollo模型预测控制问题,等式约束来自于系统的状态方程。 目前开源出的代码中, OSQP等式约束中是忽略了这一项的, 只用了约束。
针对于OSQP求解器的该种接口形式,需要对MPC优化问题进行重新构造,从而适配接口,也就是说要求出对应于上节提到优化问题的Hessian Matrix , Gradient Vector , Equality Constraint Matrix 和Equality Constraint Vectors ,。
OSQP Solver中的矩阵数据采取CSC格式,参考博客如下:
CSC Format
为了防止大家混淆变量,我们重新梳理一下变量和的含义, 上一节中提到的车辆Linearized Error Dynamics中的包括横向和纵向误差变量一共六个状态变量, 为当前采样时刻。
输入包括方向盘转角和纵向加速度
OSQP solver中为二次规划问题的决策变量,由当前时刻的状态变量,当前时刻的输入,预测的状态变量和输入构成。
其中为预测步长。
Hessian矩阵形式如下:
void MpcOsqp::CalculateKernel(std::vector<c_float> *P_data,
std::vector<c_int> *P_indices,
std::vector<c_int> *P_indptr) {
// col1:(row,val),...; col2:(row,val),....; ...
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(num_param_);
int value_index = 0;
// state and terminal state
for (size_t i = 0; i <= horizon_; ++i) {
for (size_t j = 0; j < state_dim_; ++j) {
// (row, val)
columns[i * state_dim_ + j].emplace_back(i * state_dim_ + j,
matrix_q_(j, j));
++value_index;
}
}
// control
const size_t state_total_dim = state_dim_ * (horizon_ + 1);
for (size_t i = 0; i < horizon_; ++i) {
for (size_t j = 0; j < control_dim_; ++j) {
// (row, val)
columns[i * control_dim_ + j + state_total_dim].emplace_back(
state_total_dim + i * control_dim_ + j, matrix_r_(j, j));
++value_index;
}
}
CHECK_EQ(value_index, num_param_);
int ind_p = 0;
for (size_t i = 0; i < num_param_; ++i) {
// TODO(SHU) Check this
P_indptr->emplace_back(ind_p);
for (const auto &row_data_pair : columns[i]) {
P_data->emplace_back(row_data_pair.second); // val
P_indices->emplace_back(row_data_pair.first); // row
++ind_p;
}
}
P_indptr->emplace_back(ind_p);
}
Gradient Vector 构造形式入下:
// reference is always zero
void MpcOsqp::CalculateGradient() {
// populate the gradient vector
gradient_ = Eigen::VectorXd::Zero(
state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
for (size_t i = 0; i < horizon_ + 1; i++) {
gradient_.block(i * state_dim_, 0, state_dim_, 1) =
-1.0 * matrix_q_ * matrix_x_ref_;
}
ADEBUG << "Gradient_mat";
ADEBUG << gradient_;
}
Equality Constraint Matrix构造如下, 包括上下两个部分,上部分对应等式约束,下部分对应不等式约束, 每个部分又分为左右两部分, 左部分对应于决策变量中的状态量部分,右部分对应决策变量中输入量部分。
// equality constraints x(k+1) = A*x(k)
void MpcOsqp::CalculateEqualityConstraint(std::vector<c_float> *A_data,
std::vector<c_int> *A_indices,
std::vector<c_int> *A_indptr) {
constexpr double kEpsilon = 1e-6;
// block matrix
Eigen::MatrixXd matrix_constraint = Eigen::MatrixXd::Zero(
state_dim_ * (horizon_ + 1) + state_dim_ * (horizon_ + 1) +
control_dim_ * horizon_,
state_dim_ * (horizon_ + 1) + control_dim_ * horizon_);
Eigen::MatrixXd state_identity_mat = Eigen::MatrixXd::Identity(
state_dim_ * (horizon_ + 1), state_dim_ * (horizon_ + 1));
ADEBUG << "state_identity_mat" << state_identity_mat;
matrix_constraint.block(0, 0, state_dim_ * (horizon_ + 1),
state_dim_ * (horizon_ + 1)) =
-1 * state_identity_mat;
ADEBUG << "matrix_constraint";
ADEBUG << matrix_constraint;
Eigen::MatrixXd control_identity_mat =
Eigen::MatrixXd::Identity(control_dim_, control_dim_);
for (size_t i = 0; i < horizon_; i++) {
matrix_constraint.block((i + 1) * state_dim_, i * state_dim_, state_dim_,
state_dim_) = matrix_a_;
}
ADEBUG << "matrix_constraint with A";
ADEBUG << matrix_constraint;
for (size_t i = 0; i < horizon_; i++) {
matrix_constraint.block((i + 1) * state_dim_,
i * control_dim_ + (horizon_ + 1) * state_dim_,
state_dim_, control_dim_) = matrix_b_;
}
ADEBUG << "matrix_constraint with B";
ADEBUG << matrix_constraint;
Eigen::MatrixXd all_identity_mat =
Eigen::MatrixXd::Identity(num_param_, num_param_);
matrix_constraint.block(state_dim_ * (horizon_ + 1), 0, num_param_,
num_param_) = all_identity_mat;
ADEBUG << "matrix_constraint with I";
ADEBUG << matrix_constraint;
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(num_param_ + 1);
int value_index = 0;
// state and terminal state
for (size_t i = 0; i < num_param_; ++i) { // col
for (size_t j = 0; j < num_param_ + state_dim_ * (horizon_ + 1);
++j) // row
if (std::fabs(matrix_constraint(j, i)) > kEpsilon) {
// (row, val)
columns[i].emplace_back(j, matrix_constraint(j, i));
++value_index;
}
}
ADEBUG << "value_index";
ADEBUG << value_index;
int ind_A = 0;
for (size_t i = 0; i < num_param_; ++i) {
A_indptr->emplace_back(ind_A);
for (const auto &row_data_pair : columns[i]) {
A_data->emplace_back(row_data_pair.second); // value
A_indices->emplace_back(row_data_pair.first); // row
++ind_A;
}
}
A_indptr->emplace_back(ind_A);
}
Constraint Vector形式如下:
void MpcOsqp::CalculateConstraintVectors() {
// evaluate the lower and the upper inequality vectors
Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(
state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(
state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
for (size_t i = 0; i < horizon_; i++) {
lowerInequality.block(control_dim_ * i + state_dim_ * (horizon_ + 1), 0,
control_dim_, 1) = matrix_u_lower_;
upperInequality.block(control_dim_ * i + state_dim_ * (horizon_ + 1), 0,
control_dim_, 1) = matrix_u_upper_;
}
ADEBUG << " matrix_u_lower_";
for (size_t i = 0; i < horizon_ + 1; i++) {
lowerInequality.block(state_dim_ * i, 0, state_dim_, 1) = matrix_x_lower_;
upperInequality.block(state_dim_ * i, 0, state_dim_, 1) = matrix_x_upper_;
}
ADEBUG << " matrix_x_lower_";
// evaluate the lower and the upper equality vectors
Eigen::VectorXd lowerEquality =
Eigen::MatrixXd::Zero(state_dim_ * (horizon_ + 1), 1);
Eigen::VectorXd upperEquality;
lowerEquality.block(0, 0, state_dim_, 1) = -1 * matrix_initial_x_;
upperEquality = lowerEquality;
lowerEquality = lowerEquality;
ADEBUG << " matrix_initial_x_";
// merge inequality and equality vectors
lowerBound_ = Eigen::MatrixXd::Zero(
2 * state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
lowerBound_ << lowerEquality, lowerInequality;
ADEBUG << " lowerBound_ ";
upperBound_ = Eigen::MatrixXd::Zero(
2 * state_dim_ * (horizon_ + 1) + control_dim_ * horizon_, 1);
upperBound_ << upperEquality, upperInequality;
ADEBUG << " upperBound_";
}
求解器主函数部分如下
bool MpcOsqp::Solve(std::vector<double> *control_cmd) {
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 = 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;
}
针对上述二次规划问题构造形式我觉得可以做出如下改进:
- 如果我们改变目标函数形式,变为如下形式的话
我们需要变换 Gradient Vector
- 干扰矩阵也应该考虑, 此时应该修改Constraint Vector:
下一篇: Web应用中路径的一些记录