二次规划（QP）与OSQP求解器

二次规划（QP）

OSQP 求解器

OSQP-eigen接口

二次规划（QP）

优化在很多领域都发挥着重要应用，其中自动驾驶的运动规划可以看做一个优化问题，根据实际情况进行合理简化和建模。

一个优化问题包含：优化目标和约束条件（包含等式约束、不等式约束）。

常见的凸优化问题的标准形式如下：

如果约束条件或目标函数包含非线性，则为非线性优化。二次规划是一种特殊的非线性规划，也是标准的凸优化问题，能够快速求解。

在路径/轨迹优化中经常建模为二次优化问题进行求解，很多MPC的求解过程也是转化为序列二次规划进行求解。

二次优化的标准形式为：

$minimize\ \frac{1}{2} x^{T}Px + q^{T}x$

$subject \ to \ l\leq Ax \leq u$

关于二次项系数的正定性，以及优化理论的剖析，此处暂不做探讨，可以阅读参考文献，待了解更加深入后再进行补充。

OSQP 求解器

OSQP官网以及github

$minimize \frac{1}{2} x^{T}\begin{bmatrix} 4 & 1 \\ 1 & 2 \end{bmatrix} x+\begin{bmatrix} 1\\1 \end{bmatrix}^{T} x$

$subject to \begin{bmatrix} 1\\0 \\ 0 \end{bmatrix} \leq \begin{bmatrix} 1 & 1 \\ 1 &0 \\ 0&1 \end{bmatrix} \leq \begin{bmatrix} 1\\0.7 \\ 0.7 \end{bmatrix}$

c接口，采用csc矩阵

按列压缩CSC—Compressed sparse column
顾名思义将每一列出现的非零元素的个数统计后放好

#include "osqp.h"int main(int argc, char **argv) {// Load problem datac_float P_x[3] = { 4.0, 1.0, 2.0, };c_int   P_nnz  = 3;c_int   P_i[3] = { 0, 0, 1, };c_int   P_p[3] = { 0, 1, 3, };c_float q[2]   = { 1.0, 1.0, };c_float A_x[4] = { 1.0, 1.0, 1.0, 1.0, }; //A的数据，第1列（0，1行）对应的非零元素为1.0，1.0，第2列（0，2行）对应的非零元素为1.0，1.0。c_int   A_nnz  = 4; // 非零元素总数为4c_int   A_i[4] = { 0, 1, 0, 2, }; // 第1列对应的非零元素在0,1行，第2列对应的非零元素在0,2行c_int   A_p[3] = { 0, 2, 4, }; // 第i列的非零元素个数为A_p[i+1]-A_p[i], 如第0列非零元素个数为：2-0, 第1列非零元素个数为：4-2 c_float l[3]   = { 1.0, 0.0, 0.0, };c_float u[3]   = { 1.0, 0.7, 0.7, };c_int n = 2;c_int m = 3;// Exitflagc_int exitflag = 0;// Workspace structuresOSQPWorkspace *work;OSQPSettings  *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));OSQPData      *data     = (OSQPData *)c_malloc(sizeof(OSQPData));// Populate dataif (data) {data->n = n;data->m = m;data->P = csc_matrix(data->n, data->n, P_nnz, P_x, P_i, P_p);data->q = q;data->A = csc_matrix(data->m, data->n, A_nnz, A_x, A_i, A_p);data->l = l;data->u = u;}// Define solver settings as defaultif (settings) osqp_set_default_settings(settings);// Setup workspaceexitflag = osqp_setup(&work, data, settings);// Solve Problemosqp_solve(work);// Clean workspaceosqp_cleanup(work);if (data) {if (data->A) c_free(data->A);if (data->P) c_free(data->P);c_free(data);}if (settings)  c_free(settings);return exitflag;
}

在OSQP中, 对上述所数据用结构体OSQPData进行封装, 其定义如下. 其中 $P$ 和 $A$ 都以稀疏矩阵CSC的形式进行存储.

// the location of file: /usr/local/include/osqp/types.h
typedef struct {c_int    n; ///< number of variables nc_int    m; ///< number of constraints mcsc     *P; ///< the upper triangular part of the quadratic cost matrix Pcsc     *A; ///< linear constraints matrix A in csc format (size m x n)c_float *q; ///< dense array for linear part of cost function (size n)c_float *l; ///< dense array for lower bound (size m)c_float *u; ///< dense array for upper bound (size m)
} OSQPData;

CMakeList.txt

cmake_minimum_required(VERSION 3.10)# set the project name
project(OSQP)# add the executable
add_executable(OSQP osqp_example.c)# Find OSQP library and headers
find_package(osqp REQUIRED)# Link the OSQP shared library
target_link_libraries(OSQP PRIVATE osqp::osqp)

运行结果如下：

OSQP-eigen接口

除此之外，有两个推荐的第三方维护的接口:google的osqp-cpp与Giulio Romualdi的osqp-eigen.

以osqp-eigen为例（通常有固定的代码书写流程）

以下列问题的求解为例：

$min \bigl(\begin{smallmatrix} x_{1} -1 \end{smallmatrix}\bigr)^{2} +\bigl(\begin{smallmatrix} x_{2} -1 \end{smallmatrix}\bigr)^{2}$

$min \frac{1}{2} x^{T}\begin{bmatrix} 2 & 0 \\ 0 & 2 \end{bmatrix} x + \begin{bmatrix} -2\\-2 \end{bmatrix}^{T} x$

$s.t. \0 \leq x_{1} \leq 1.5, \0 \leq x_{2} \leq 1.5$

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"// eigen
#include <Eigen/Dense>
#include <iostream>int main()
{// allocate QP problem matrices and vectoresEigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrixEigen::VectorXd gradient(2);                    //Q: n*1向量Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n矩阵,必须为稀疏矩阵SparseMatrixEigen::VectorXd lowerBound(2);                  //L: m*1下限向量Eigen::VectorXd upperBound(2);                  //U: m*1上限向量hessian.insert(0, 0) = 2.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化hessian.insert(1, 1) = 2.0;// std::cout << "hessian:" << std::endl//           << hessian << std::endl;gradient << -2, -2;linearMatrix.insert(0, 0) = 1.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化linearMatrix.insert(1, 1) = 1.0;// std::cout << "linearMatrix:" << std::endl//           << linearMatrix << std::endl;lowerBound << 1, 1;upperBound << 1.5, 1.5;// instantiate the solverOsqpEigen::Solver solver;// settingssolver.settings()->setVerbosity(false);solver.settings()->setWarmStart(true);// set the initial data of the QP solversolver.data()->setNumberOfVariables(2);   //变量数nsolver.data()->setNumberOfConstraints(2); //约束数mif (!solver.data()->setHessianMatrix(hessian))return 1;if (!solver.data()->setGradient(gradient))return 1;if (!solver.data()->setLinearConstraintsMatrix(linearMatrix))return 1;if (!solver.data()->setLowerBound(lowerBound))return 1;if (!solver.data()->setUpperBound(upperBound))return 1;// instantiate the solverif (!solver.initSolver())return 1;Eigen::VectorXd QPSolution;// solve the QP problemif (!solver.solve()){return 1;}QPSolution = solver.getSolution();std::cout << "QPSolution" << std::endl<< QPSolution << std::endl; //输出为m*1的向量return 0;
}

CMakeList文件内容：

cmake_minimum_required(VERSION 3.1)set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)project(OSQP_test LANGUAGES CXX)find_package(OsqpEigen)
find_package(Eigen3)include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})#MPCExample
add_executable(OSQP_test main.cpp)target_link_libraries(OSQP_test OsqpEigen::OsqpEigen)

这篇文章只是简单介绍了二次规划的思想和OSQP的代码流程，希望后面能结合实际的路径/轨迹优化问题，进行实践验证。

参考链接：

自动驾驶规划中使用OSQP进行二次规划 - 知乎

OSQP使用说明 - 知乎

使用OSQP解决二次凸优化(QP)问题_首飞爱玩机器人的博客-CSDN博客_osqp库

Apollo 6.0 QP（二次规划）算法解析 - 知乎

Apollo MPC OSQP Solver_cyytum的博客-CSDN博客_mpc osqp求解器

OSQP solver documentation — OSQP documentation

凸优化笔记(3)Quadratic Programming简介 - 知乎

二次规划 | 机器之心

osqp-eigen使用记录_Oxalate-c的博客-CSDN博客_osqp-eigen

利用OSQP库计算标准二次规划(QP)问题的实例 - Challenging-eXtraordinary