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

2024-07-15 23:51| 来源: 网络整理| 查看: 265

二次规划（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 #include int main() { // allocate QP problem matrices and vectores Eigen::SparseMatrix hessian(2, 2); //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrix Eigen::VectorXd gradient(2); //Q: n*1向量 Eigen::SparseMatrix 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; //注意稀疏矩阵的初始化方式,无法使用

【本文地址】

公司简介

联系我们