[ToolsChains CPP] Osqp库的基础&&示例&&MPCdemo

2023-12-20 07:49:45

写在前面

本文参考官方教程，重复经典示例，然后自行搭建一个MPC控制器，并且接入我的闭环仿真器。

outline

osqp （c语言处理稀疏矩阵的表现形式太难受了 - 建议不用python matlab的话 入门即放弃，）
osqp eigen （于是学习使用了osqp-eigen版）
MPC 算法构建

安装

参考https://osqp.org/docs/get_started/sources.html进行安装，

关于安装的路径，如果是源代码安装的可以直接看编译文件cmake或者比编译时候的显示找到路径，如果是命令行安装的可以使用命令行来查找。
在这里插入图片描述我用来源码编译然后install安装、命令行安装、conda安装的方式。基本以上路径可以覆盖。

观察系统安装路径，安装的目标在include中嵌套了一层文件夹，所以带上了路径#include “osqp/osqp.h”
在这里插入图片描述

官网案例实现（CPP）：

https://osqp.org/docs/examples/index.html

源代码是基于c ，由于我的cmakelist设置等问题，我把部分c语言的东西换成立cpp来实现的
c语言处理稀疏矩阵的表现形式太难受了，于是我只实现了最简单的例子，而后使用了osqp-eigen版

osqp代码如下：

#include <stdlib.h>
#include "osqp/osqp.h"
#include <string>

int main(int argc, char **argv) {
    /* Load problem data */
    OSQPFloat P_x[3] = {4.0, 1.0, 2.0, };
    OSQPInt P_nnz = 3;
    OSQPInt P_i[3] = {0, 0, 1, };
    OSQPInt P_p[3] = {0, 1, 3, };
    OSQPFloat q[2] = {1.0, 1.0, };
    OSQPFloat A_x[4] = {1.0, 1.0, 1.0, 1.0, };
    OSQPInt A_nnz = 4;
    OSQPInt A_i[4] = {0, 1, 0, 2, };
    OSQPInt A_p[3] = {0, 2, 4, };
    OSQPFloat l[3] = {1.0, 0.0, 0.0, };
    OSQPFloat u[3] = {1.0, 0.7, 0.7, };
    OSQPInt n = 2;
    OSQPInt m = 3;



    // P_x[3]： 这是矩阵 P 的非零元素的数组。在这个例子中，P 是一个 2x2 的对称矩阵，由于是对称的，只需要存储上三角或下三角的元素。这里 {4.0, 1.0, 2.0} 表示的是上三角的元素。

    // P_nnz： 这是矩阵 P 的非零元素的数量。在这个例子中，P_nnz 等于 3。

    // P_i[3]： 这是矩阵 P 的非零元素的行索引数组。对应于 P_x 中的每个非零元素，P_i 存储其行索引。在这里，{0, 0, 1} 表示 {4.0, 1.0, 2.0} 对应的行索引。

    // P_p[3]： 这是矩阵 P 的列指针数组。它指示每一列的起始位置在 P_x 和 P_i 数组中的索引。在这个例子中，{0, 1, 3} 表示第一列从索引 0 开始，第二列从索引 1 开始，第三列从索引 3 开始。qkf：也就是还原行与列

    // q[2]： 这是目标函数的线性项。在这个例子中，目标函数是一个二次函数，q 中存储的是线性项的系数。

    // A_x[4]： 这是约束矩阵 A 的非零元素的数组。在这个例子中，A_x 存储了矩阵 A 的所有非零元素，即 {1.0, 1.0, 1.0, 1.0}。

    // A_nnz： 这是矩阵 A 的非零元素的数量。在这个例子中，A_nnz 等于 4。

    // A_i[4]： 这是矩阵 A 的非零元素的行索引数组。对应于 A_x 中的每个非零元素，A_i 存储其行索引。在这里，{0, 1, 0, 2} 表示 {1.0, 1.0, 1.0, 1.0} 对应的行索引。

    // A_p[3]： 这是矩阵 A 的列指针数组，指示每一列的起始位置在 A_x 和 A_i 数组中的索引。

    // l[3]： 这是约束的下界。在这个例子中，l 存储了每个约束的下界，即 {1.0, 0.0, 0.0}。

    // u[3]： 这是约束的上界。在这个例子中，u 存储了每个约束的上界，即 {1.0, 0.7, 0.7}。

    // n： 问题的变量数，也就是优化问题中的变量维度。在这个例子中，n 等于 2。

    // m： 问题的约束数。在这个例子中，m 等于 3。


    /* Exitflag */
    OSQPInt exitflag = 0;

    /* Solver, settings, matrices */
    OSQPSolver   *solver;
    OSQPSettings *settings;
    OSQPCscMatrix* P = static_cast<OSQPCscMatrix*>(std::malloc(sizeof(OSQPCscMatrix)));
    OSQPCscMatrix* A = static_cast<OSQPCscMatrix*>(std::malloc(sizeof(OSQPCscMatrix)));

    /* Populate matrices */
    csc_set_data(A, m, n, A_nnz, A_x, A_i, A_p);
    csc_set_data(P, n, n, P_nnz, P_x, P_i, P_p);

    /* Set default settings */
    settings = (OSQPSettings *)malloc(sizeof(OSQPSettings));
    if (settings) {
        osqp_set_default_settings(settings);
        settings->alpha = 1.0; /* Change alpha parameter */
    }

    /* Setup solver */
    exitflag = osqp_setup(&solver, P, q, A, l, u, m, n, settings);

    /* Solve problem */
    if (!exitflag) exitflag = osqp_solve(solver);

    /* Cleanup */
    osqp_cleanup(solver);
    if (A) std::free(A);
    if (P) std::free(P);
    if (settings) free(settings);

    return (int)exitflag;
};

运行结果
在这里插入图片描述关于输出结果的解释我询问了gpt：

GPT A：
这是OSQP求解一个凸二次规划问题的输出结果。以下是对输出的解读：
问题规模和结构

变量数目：n = 2
约束数目：m = 3
非零元素数目：nnz(P) + nnz(A) = 7

OSQP设置

代数法：Built-in
线性系统求解器：QDLDL v0.1.6
终止准则：
    相对绝对误差：eps_abs = 1.0e-03
    相对相对误差：eps_rel = 1.0e-03
    主变量无穷范数误差：eps_prim_inf = 1.0e-04
    对偶变量无穷范数误差：eps_dual_inf = 1.0e-04
内点方法参数：
    rho (亲和力参数)：1.00e-01（自适应）
    sigma：1.00e-06
    alpha：1.00
    最大迭代次数：max_iter = 4000
    终止检查：每25次迭代检查一次
    时间限制：1.00e+10秒（相当于没有时间限制）
    缩放：开启
    缩放终止检查：关闭
其他设置：
    热启动：开启
    精炼：关闭

求解过程

第一次迭代：
    目标函数值：-4.9384e-03
    主变量（原始变量）相对误差：1.00e+00
    对偶变量相对误差：2.00e+02
    亲和力参数：1.00e-01（自适应）
    时间：1.55e-04秒

第50次迭代：
    目标函数值：1.8800e+00
    主变量相对误差：1.91e-07
    对偶变量相对误差：7.50e-07
    亲和力参数：1.38e+00
    时间：2.36e-04秒

最终结果

状态：已解决
迭代次数：50
最优目标值：1.8800
运行时间：2.53e-04秒
最优亲和力参数估计：1.36e+00

我继续追问：

Q：osqp不是应该求解最小值吗，为什么目标函数值还变大了

GPT A：
你提出的疑问是非常合理的。在一般情况下，确实期望优化算法（尤其是求解凸优化问题的算法）的目标函数值随着迭代逐渐减小，因为算法的目标是找到一个能够使目标函数值最小化的解。

然而，有时在实际应用中，目标函数值的变化可能受到问题本身的特性、算法的内部细节或者算法参数的影响。在你提供的输出中，可能是由于算法初始化的原因，首次迭代的目标函数值为负值，而之后的迭代过程中逐渐增加并最终趋于正值。

有几个可能的原因：

算法初始化： 初始点的选择可能导致了首次迭代的目标函数值为负。这通常不影响最终的优化结果，因为目标是在迭代中找到更好的解。

数值精度： 在计算中，特别是在数值优化中，数值精度的影响可能导致目标函数值的轻微波动。这些波动通常在优化过程中逐渐减小。

亲和力参数的调整： 亲和力参数（rho）的自适应调整可能导致目标函数值的变化。这是因为亲和力参数的调整可能会影响算法在原始问题和对偶问题之间的权衡。

虽然首次迭代的目标函数值为负可能看起来有点奇怪，但关键是最终算法是否收敛到了满足准确性要求的解。在你提供的输出中，算法的最终状态是 “已解决”，迭代次数也是有限的，最终目标函数值是正的，这可能表示算法在问题上取得了满意的结果。

osqp eigen

官网教学：https://robotology.github.io/osqp-eigen/
案例教学：https://blog.csdn.net/shoufei403/article/details/108672152
案例教学2：https://zhuanlan.zhihu.com/p/376122376

推荐用conda方式安装
在这里插入图片描述

示例代码

以下代码是根据链接教程中我补全的代码，教程中仅为片段

#include <stdlib.h>
#include "osqp/osqp.h"
#include "OsqpEigen/OsqpEigen.h"
#include <Eigen/Dense>
#include <iostream>

int main(int argc, char **argv) {
    Eigen::SparseMatrix<double> hessian;
    Eigen::VectorXd gradient;
    Eigen::SparseMatrix<double> linearMatrix;
    Eigen::VectorXd lowerBound;
    Eigen::VectorXd upperBound;

    short int num_var=2,num_contrains=3;
    hessian.resize(num_var,num_var);
    hessian.insert(0,0) = 1;
    hessian.insert(1,0) = -1;
    hessian.insert(0,1) = -1;
    hessian.insert(1,1) = 2;
    std::cout << "hessian:" << std::endl << hessian << std::endl;
    gradient.resize(2);
    gradient << -2, -6;
    std::cout << "gradient:" << std::endl << gradient << std::endl;
    linearMatrix.resize(3,2);
    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;
    std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;

    lowerBound.resize(3);
    lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;
    std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;

    upperBound.resize(3);
    upperBound << 2, 2, 3;
    std::cout << "upperBound:" << std::endl << upperBound << std::endl;

    OsqpEigen::Solver solver;

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

    solver.data()->setNumberOfVariables(num_var); //设置A矩阵的列数，即n
    solver.data()->setNumberOfConstraints(num_contrains); //设置A矩阵的行数，即m
    if(!solver.data()->setHessianMatrix(hessian)) return 1;//设置P矩阵
    if(!solver.data()->setGradient(gradient)) return 1; //设置q or f矩阵。当没有时设置为全0向量
    if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;//设置线性约束的A矩阵
    if(!solver.data()->setLowerBound(lowerBound)) return 1;//设置下边界
    if(!solver.data()->setUpperBound(upperBound)) return 1;//设置上边界

    if (!solver.initSolver()) return 1;

    if (!solver.solve()) return 1;

    auto QPSolution=solver.getSolution();
    std::cout <<"size of QP solution： " << QPSolution.size()<<std::endl;
    std::cout << "Solution for x[0]: " << QPSolution(0) << std::endl;
    std::cout << "Solution for x[1]: " << QPSolution(1) << std::endl;




};

在这里插入图片描述

以下为答案
在这里插入图片描述
关于例子中的其他两种问题：等式、边界、甚至比如不等式约束，在这个框架下改变很小，不再详细写出。

MPC 算法构建下一次更新

文章来源:https://blog.csdn.net/weixin_46479223/article/details/135070489
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若内容造成侵权/违法违规/事实不符，请联系我的编程经验分享网邮箱：veading@qq.com进行投诉反馈，一经查实，立即删除！

[ToolsChains CPP] Osqp库的基础&amp;&amp;示例&amp;&amp;MPCdemo