osqp-eigen学习

Big David

于 2024-02-20 07:02:27 发布

阅读量985

点赞数 19

分类专栏：决策规划控制文章标签：自动驾驶 c++ osqp-eigen

本文链接：https://blog.csdn.net/BigDavid123/article/details/136171891

版权

决策规划控制专栏收录该内容

50 篇文章 18 订阅

订阅专栏

OSQP文档学习
参考博客：
（1）二次规划（QP）与OSQP求解器
（2）如何使用OSQP-Eigen

osqp-eigen

1 osqp-eigen接口

以下列问题的求解为例：
在这里插入图片描述 s.t. $1≤x_1≤1.5$ ， $1≤x_2≤1.5$

mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
catkin_create_pkg OSQP_Eigen roscpp std_msgs

main.cpp

// 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.txt

cmake_minimum_required(VERSION 3.0.2)
project(OSQP_Eigen)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

find_package(OsqpEigen)
find_package(Eigen3)

link_directories(
  /usr/local/lib 
)

include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES planning
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

add_executable(OSQP src/main.cpp)
target_link_libraries(OSQP OsqpEigen::OsqpEigen
                            libosqp.so  
                            libOsqpEigen.so 
                            libqdldl.so)

输出

QPSolution
1.0003
0.750274

2 如何使用OSQP-Eigen

上面主要介绍了用OSQP-Eigen的一般示例，下面给出几个典型不同约束的二次规划形式：
（1）具有线性约束的二次规划
在这里插入图片描述

#include "OsqpEigen/OsqpEigen.h"

#include <Eigen/Dense>
#include <iostream>

int main()
{
    Eigen::SparseMatrix<double> hessian(2, 2);
    Eigen::VectorXd gradient(2);
    Eigen::SparseMatrix<double> linearMatrix(3, 2);
    Eigen::VectorXd lowerBound(3);
    Eigen::VectorXd upperBound(3);
    // 具有线性约束的二次规划
    hessian.insert(0, 0) = 1;
    hessian.insert(1, 0) = -1;
    hessian.insert(0, 1) = -1;
    hessian.insert(1, 1) = 2;
    gradient << -2, -6;
    linearMatrix.insert(0, 0) = 1;
    linearMatrix.insert(0, 1) = 1;
    linearMatrix.insert(1, 0) = -1;
    linearMatrix.insert(1, 1) = 2;
    linearMatrix.insert(2, 0) = 2;
    linearMatrix.insert(2, 1) = 1;

    lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;
    upperBound << 2, 2, 3;

    OsqpEigen::Solver solver;

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

    solver.data()->setNumberOfVariables(2);//A矩阵的列数
    solver.data()->setNumberOfConstraints(3);//A矩阵的行数
		//设置P矩阵
    if (!solver.data()->setHessianMatrix(hessian)) return 1;
    //设置q or f矩阵
    if (!solver.data()->setGradient(gradient)) return 1;
    //设置线性约束的A矩阵
    if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
    //设置下边界
    if (!solver.data()->setLowerBound(lowerBound)) return 1;
    //设置上边界
    if (!solver.data()->setUpperBound(upperBound)) return 1;

    if (!solver.initSolver()) return 1;
    Eigen::VectorXd QPSolution;
    if (!solver.solve()) return 1;

    // get the controller input
    clock_t time_start = clock();
    clock_t time_end = clock();
    time_start = clock();

    QPSolution = solver.getSolution();
    time_end = clock();
    std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;
    
    std::cout << "QPSolution" << std::endl << QPSolution << std::endl;
    return 0;
}

输出：

time use:0.002ms
QPSolution
 0.6665
1.33322

（2）具有线性等式约束的二次规划
在这里插入图片描述

#include "OsqpEigen/OsqpEigen.h"

#include <Eigen/Dense>
#include <iostream>

int main()
{
    Eigen::SparseMatrix<double> hessian(2, 2);
    Eigen::VectorXd gradient(2);
    Eigen::SparseMatrix<double> linearMatrix(1, 2);
    Eigen::VectorXd lowerBound(1);
    Eigen::VectorXd upperBound(1);
    // 具有线性等式约束的二次规划
    hessian.insert(0, 0) = 1;
    hessian.insert(1, 0) = -1;
    hessian.insert(0, 1) = -1;
    hessian.insert(1, 1) = 2;
    gradient << -2, -6;
    linearMatrix.insert(0,0) = 1;
    linearMatrix.insert(0,1) = 1;

    lowerBound << 0;
    upperBound << 0;

    OsqpEigen::Solver solver;

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

    solver.data()->setNumberOfVariables(2);
    solver.data()->setNumberOfConstraints(1);

    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;

    if (!solver.initSolver()) return 1;
    Eigen::VectorXd QPSolution;
    if (!solver.solve()) return 1;

    // get the controller input
    clock_t time_start = clock();
    clock_t time_end = clock();
    time_start = clock();

    QPSolution = solver.getSolution();
    time_end = clock();
    std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;
    
    std::cout << "QPSolution" << std::endl << QPSolution << std::endl;
    return 0;
}

time use:0.002ms
QPSolution
-0.800002
 0.800003

（3）具有线性约束和边界的二次最小化
在这里插入图片描述

#include "OsqpEigen/OsqpEigen.h"

#include <Eigen/Dense>
#include <iostream>

int main()
{
    Eigen::SparseMatrix<double> hessian(3, 3);
    Eigen::VectorXd gradient(3);
    Eigen::SparseMatrix<double> linearMatrix(4, 3);
    Eigen::VectorXd lowerBound(4);
    Eigen::VectorXd upperBound(4);
    // 具有线性约束和边界的二次最小化
    hessian.insert(0,0) = 1;
    hessian.insert(1,0) = -1;
    hessian.insert(2,0) = 1;
    hessian.insert(0,1) = -1;
    hessian.insert(1,1) = 2;
    hessian.insert(2,1) = -2;
    hessian.insert(0,2) = 1;
    hessian.insert(1,2) = -2;
    hessian.insert(2,2) = 4;

    gradient << 2, -3, 1;
    linearMatrix.insert(0,0) = 1;
    linearMatrix.insert(1,0) = 0;
    linearMatrix.insert(2,0) = 0;
    linearMatrix.insert(3,0) = 1;

    linearMatrix.insert(0,1) = 0;
    linearMatrix.insert(1,1) = 1;
    linearMatrix.insert(2,1) = 0;
    linearMatrix.insert(3,1) = 1;

    linearMatrix.insert(0,2) = 0;
    linearMatrix.insert(1,2) = 0;
    linearMatrix.insert(2,2) = 1;
    linearMatrix.insert(3,2) = 1;

    lowerBound << 0, 0, 0, 0.5;
    upperBound << 1, 1, 1, 0.5;

    OsqpEigen::Solver solver;

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

    solver.data()->setNumberOfVariables(3);
    solver.data()->setNumberOfConstraints(4);

    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;

    if (!solver.initSolver()) return 1;
    Eigen::VectorXd QPSolution;
    if (!solver.solve()) return 1;

    // get the controller input
    clock_t time_start = clock();
    clock_t time_end = clock();
    time_start = clock();

    QPSolution = solver.getSolution();
    time_end = clock();
    std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;
    
    std::cout << "QPSolution" << std::endl << QPSolution << std::endl;
    return 0;
}

time use:0.003ms
QPSolution
7.42006e-12
        0.5
1.09987e-11

本文详解了osqp-eigen的具体用法，方便自己日后回顾总结

Big David

关注

19
点赞
踩
29

收藏

觉得还不错? 一键收藏
0
评论
osqp-eigen学习

本文详解了osqp-eigen库的具体用法🦐🦐
复制链接

扫一扫

专栏目录

osqp-eigen学习

1 osqp-eigen接口

2 如何使用OSQP-Eigen

“相关推荐”对你有帮助么？