C++中求解二次规划问题（OSQP）的一个使用实例

悠以悠

已于 2023-12-22 11:09:37 修改

阅读量3.9k

点赞数 13

分类专栏：自动驾驶控制文章标签： c++ 线性代数算法

于 2023-04-19 00:49:37 首次发布

本文链接：https://blog.csdn.net/chaser30/article/details/130234454

版权

自动驾驶控制专栏收录该内容

3 篇文章 0 订阅

订阅专栏

文章目录

C++中求解二次规划问题（OSQP）的一个使用实例
- 参考
- 实例

C++中求解二次规划问题（OSQP）的一个使用实例

参考

库的安装（eigen/OSQP/OSQP-eigen）及其他实例可以参考这个链接
https://blog.csdn.net/nudt_zrs/article/details/124258999?spm=1001.2014.3001.5501

实例

以下面的一个优化问题求解为例
$\begin{equation} \begin{split} &min \hspace{0.5cm}(x_1 - 1)^2 + (x_2 -1)^2 \\ &s.t.\hspace{1cm} 0 \leq x_1 \leq 1.5 \\ &\hspace{1.5cm} 0 \leq x_2 \leq 1.5 \end{split} \end{equation}$

首先对目标方程进行简化，写成矩阵的形式：
$\begin{equation} \begin{split} obj &= (x_1^2 - 2x_1 + 1) + (x_2^2-2x_2+1) \\ &= x_1^2 + x_2^2 -2x_1 - 2x_2 + 2 \\ &= \begin{bmatrix} x_1 & x_2 \end{bmatrix} \begin{bmatrix} 1 & 0 \\0 & 1 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix}- 2\begin{bmatrix} 1 & 1 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix}+ 2 \end{split} \end{equation}$

由于二次规划问题求解器默认的求解形式为 $obj=\displaystyle \frac 12x^THx + f^Tx$ ，其中常数项对二次型求解没有影响，可以直接忽略。因此将上式写为
$\begin{equation} \begin{split} obj &= \frac 12\pmb x^T \begin{bmatrix} 2 & 0 \\0 & 2 \end{bmatrix} \pmb x + \begin{bmatrix} -2 \\ -2 \end{bmatrix}^T \pmb x \\ &= \frac 12 x^THx + f^Tx \end{split} \end{equation}$

而其不等式约束则可改写为如下形式
$\begin{equation} \begin{split} \begin{bmatrix} 0 \\ 0 \end{bmatrix} \leq &\begin{bmatrix} 1 & 0 \\ 0 &1 \end{bmatrix}\begin{bmatrix} x_1 \\ x_2 \end{bmatrix} \leq \begin{bmatrix} 1.5 \\ 1.5 \end{bmatrix} \\ \Rightarrow &lb \leq A x \leq ub \end{split} \end{equation}$

// 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)