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

牛仔很忙^

已于 2022-06-16 10:42:52 修改

阅读量1.4w

点赞数 14

分类专栏：决策规划文章标签：算法

于 2022-04-18 21:26:35 首次发布

本文链接：https://blog.csdn.net/nudt_zrs/article/details/124258999

版权

决策规划专栏收录该内容

17 篇文章

订阅专栏

二次规划（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 data
  c_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; // 非零元素总数为4
  c_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;

  // Exitflag
  c_int exitflag = 0;

  // Workspace structures
  OSQPWorkspace *work;
  OSQPSettings  *settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
  OSQPData      *data     = (OSQPData *)c_malloc(sizeof(OSQPData));

  // Populate data
  if (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 default
  if (settings) osqp_set_default_settings(settings);

  // Setup workspace
  exitflag = osqp_setup(&work, data, settings);

  // Solve Problem
  osqp_solve(work);

  // Clean workspace
  osqp_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 n
  c_int    m; ///< number of constraints m
  csc     *P; ///< the upper triangular part of the quadratic cost matrix P
  csc     *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 vectores
    Eigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::VectorXd gradient(2);                    //Q: n*1向量
    Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n矩阵,必须为稀疏矩阵SparseMatrix
    Eigen::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 solver
    OsqpEigen::Solver solver;

    // settings
    solver.settings()->setVerbosity(false);
    solver.settings()->setWarmStart(true);

    // set the initial data of the QP solver
    solver.data()->setNumberOfVariables(2);   //变量数n
    solver.data()->setNumberOfConstraints(2); //约束数m
    if (!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 solver
    if (!solver.initSolver())
        return 1;

    Eigen::VectorXd QPSolution;

    // solve the QP problem
    if (!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