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

OSQP介绍

Apollo 使用的二次规划求解使用的为 OSQP, 因此调查试用了一下.

OSQP官网介绍,特点如下:

• Efficient
  采用了ADMM-based first-order算法.

• Robust
  只需要问题本身为凸即可, 对问题数据无要求.

• Free
  Apache 2.0 license

• Embaddable
  生成嵌入式 C 代码, 不需要管理内存.

• Interface
  多语言,跨平台,支持 C/C++, Fortran, Matlab, Python, R, Julia, Rust.

• Library-Free
  无需安装依赖.

• 同类性能 bechmark 如下图
  

Ubuntu部署

参照官网说明即可.
由于 Binary 网页无法打开, 使用源码安装方式.
预先安装 gcc 与 cmake. 代码下载后,cmake-make-make install.
注意: ~qdldl/qdldl_sources does not contain a CMakeLists.txt file. cmake 报错的话, 需要下载完整版的源码, 也在 github 上.

C++接口

官方无 C++ 接口,有两个推荐的第三方维护的接口: google的osqp-cpp 与 Giulio Romualdi 的 osqp-eigen. osqp-cpp为osqp-eigen的类似. 区别如下:

1. license
   osqp-cpp 为 MIT(最宽松),osqp-eigen 为 LGPL(传染式开源). 关于开源证书可以参考知乎回答.
2. 两者除都依赖 Eigen 以外, osqp-cpp 依赖 google 自家的 abseil-cpp 库. osqp-eigen 没有其他依赖.

通过例子可以看出两者语法类似, 为方便, 本次实例采用了osqp-eigen. 如果公司不能开源代码, 可能需要使用osqp-cpp. 另外注意一点, osqp-cpp 不是 offically supprted.

osqp-eigen 部署与使用

按照官方说明安装即可, cmake-make-make install.
cmakelist 示例:

cmake_minimum_required(VERSION 3.0)
project(myproject)
find_package(OsqpEigen REQUIRED)
add_executable(example example.cpp)
target_link_libraries(example OsqpEigen::OsqpEigen)

标准问题构建

$$ \begin{aligned} \min \quad & \frac{1}{2}X^TPX+Q^TX \\ \text{ s.t. } \quad & L<=AX<=U \end{aligned} $$

$X$为 $n \times 1$ 向量
$A$为 $m \times n$ 约束矩阵, 约束条件个数为 $m$ 个
其中 $P$ 为 Hessian 正定矩阵($n\times n$ 对称矩阵)
$L$ 与 $U$ 分别为上下限线性约束条件.

如果有等式约束,如 $A_{eq}X=b_{eq}$,可化为不等式约束如下:
$$
b_{eq} <= A_{eq}X <= b_{eq}
$$

实际问题如下:

$$ \begin{aligned} \min \quad & (x_1-1)^2+(x_2-1)^2 \\ \text{ s.t. } \quad & 1<=x_1<=1.5 \\ & 1<=x_2<=1.5 \end{aligned} $$

图解下图

显而易见,最优解为$$ \begin{bmatrix} 1.0 \ 1.0\end{bmatrix}$$
转化成矩阵形式:

$$ \begin{aligned} \min \quad & \frac{1}{2}X^T \begin{bmatrix} 2 & 0 \\ 0 & 2\end{bmatrix} X + \begin{bmatrix} -2\\ -2\end{bmatrix} X \\ \text{ s.t. } \quad & \begin{bmatrix} 1\\ 1\end{bmatrix} <= \begin{bmatrix} 1&0\\ 0&1\end{bmatrix} X<= \begin{bmatrix} 1.5\\ 1.5\end{bmatrix} \end{aligned} $$

由此可得 $P,Q,A,L,U$.

求解示例代码

// 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;
}

完整工程请参考 github

运算输出结果为

QPSolution
1.0003
1.0003

与最优解误差很小.

• 注意点:
  1. Eigen::SparseMatrix 的初始化方式与一般 Matrix 不同,有两种方式:一是创建一个元素类型为 triplets 的列表,然后再将其转换为稀疏矩阵. 二是调用稀疏矩阵的成员函数.insert()直接插入数值. 稀疏矩阵的介绍可参考博文.
  2. 需要保证 NumberOfVariables 与 NumberOfConstraints 跟 Hessian 矩阵等维数对应上.