osqp轨迹优化

osqp基本用法

安装
osqp.org为原生的osqp库，教程非常详细
(Official版本中并未提供c++接口，可用Community版本中提供的c++接口
github.com/google/osqp-cpp或者github.com/robotology/osqp-eigen，按照文档安装即可，这里采用后者osqp-eigen)，本质上osqp-eigen是对osqp接口进行了c++和Eigen的再封装

用法
标准的qp问题可写成以下公式:
m i n 1 2 x T P x + Q x s . t . L ≤ A x ≤ U min \frac{1}{2} x^{T}Px+Qx \ s.t. \quad L \leq Ax \leq U min21​xTPx+Qxs.t.L≤Ax≤U
特别的，若出现部分等式约束部分不等式约束可有:
L e q = U e q L_{eq}=U_{eq} Leq​=Ueq​
问题举例：
P = [ 1 − 1 1 − 1 2 − 2 1 − 2 4 ] Q = [ 2 − 3 1 ] P=\begin{bmatrix} 1&-1&1\ -1&2&-2\ 1&-2&4 \end{bmatrix} \quad Q=\begin{bmatrix} 2&-3&1 \end{bmatrix} P=⎣⎡​1−11​−12−2​1−24​⎦⎤​Q=[2​−3​1​]
0 ≤ x 1 , x 2 , x 3 ≤ 1 x 1 + x 2 + x 3 = 1 / 2 0\leq x_1,x_2,x_3 \leq1\quad x_1+x_2+x_3=1/{2} 0≤x1​,x2​,x3​≤1x1​+x2​+x3​=1/2
式中既含有不等式约束，也含有等式约束，可整理为标准式如下:
[ 0 0 0 1 / 2 ] ≤ [ 1 0 0 0 1 0 0 0 1 1 1 1 ] [ x 1 x 2 x 3 ] ≤ [ 1 1 1 1 / 2 ] \begin{bmatrix} 0\ 0\ 0\ 1/{2}\ \end{bmatrix} \leq \begin{bmatrix} 1&0&0\ 0&1&0\ 0&0&1\ 1&1&1\end{bmatrix} \begin{bmatrix} x_1\ x_2\x_3\end{bmatrix} \leq \begin{bmatrix} 1\ 1\ 1\ 1/{2}\ \end{bmatrix} ⎣⎢⎢⎡​0001/2​⎦⎥⎥⎤​≤⎣⎢⎢⎡​1001​0101​0011​⎦⎥⎥⎤​⎣⎡​x1​x2​x3​​⎦⎤​≤⎣⎢⎢⎡​1111/2​⎦⎥⎥⎤​

求解的具体代码如下:

// c++ 库函数
#include "bits/stdc++.h"
using namespace std;

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"

// eigen
#include 

int main(int argc, char **argv)
{

    Eigen::SparseMatrix P;
    Eigen::VectorXd Q;
    Eigen::SparseMatrix A;
    Eigen::VectorXd L;
    Eigen::VectorXd U;

    //设置P矩阵
    P.resize(3, 3);
    P.insert(0, 0) = 1;     P.insert(0, 1) = -1;    P.insert(0, 2) = 1;
    P.insert(1, 0) = -1;    P.insert(1, 1) = 2;     P.insert(1, 2) =-2;
    P.insert(2, 0) = 1;     P.insert(2, 1) = -2;    P.insert(2, 2) = 4;
  

    //设置Q矩阵
    Q.resize(3);
    Q << 2,-3,1;

    //设置A矩阵
    A.resize(4, 3);
    A.insert(0, 0) = 1;     A.insert(0, 1) = 0;     A.insert(0, 2) = 0;
    A.insert(1, 0) = 0;     A.insert(1, 1) = 1;     A.insert(1, 2) = 0;
    A.insert(2, 0) = 0;     A.insert(2, 1) = 0;     A.insert(2, 2) = 1;
    A.insert(3, 0) = 1;     A.insert(3, 1) = 1;     A.insert(3, 2) = 1;

    //设置变量上限
    L.resize(4);
    L << 0,0,0,0.5;

    //设置变量下限
    U.resize(4);
    U << 1,1,1,0.5;

    //配置求解器
    OsqpEigen::Solver solver;

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

    solver.data()->setNumberOfVariables(3);   //变量数n
    solver.data()->setNumberOfConstraints(4); //约束数m

    if (!solver.data()->setHessianMatrix(P))
        return 1;
    if (!solver.data()->setGradient(Q))
        return 1;
    if (!solver.data()->setLinearConstraintsMatrix(A))
        return 1;
    if (!solver.data()->setLowerBound(L))
        return 1;
    if (!solver.data()->setUpperBound(U))
        return 1;

    // 初始化求解器
    if (!solver.initSolver())
        return 1;

    Eigen::VectorXd QPSolution;

    //求解qp问题
    if (!solver.solve())
    {
        return 1;
    }

    QPSolution = solver.getSolution();
    cout << "QPsolution " << endl
         << QPSolution << endl;

    return 0;
}

CMakeLists.txt配置文件写法如下：

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

运动规划中典型的二次规划问题

• OBVP[3]
  单维度下多项式轨迹问题可有:
  x ( t ) = c 5 t 5 + c 4 t 4 + c 3 t 3 + c 2 t 2 + c 1 t 1 + c 0 x(t)=c_5t^5+c_4t^4+c_3t^3+c_2t^2+c_1t^1+c_0 x(t)=c5​t5+c4​t4+c3​t3+c2​t2+c1​t1+c0​
  分别求导至V和A：
  x ( t ) ( 1 ) = 5 c 5 t 4 + 4 c 4 t 3 + 3 c 3 t 2 + 2 c 2 t 1 + c 1 x ( t ) ( 2 ) = 20 c 5 t 3 + 12 c 4 t 2 + 6 c 3 t 1 + 2 c 2 x(t)^{(1)}=5c_5t^4+4c_4t^3+3c_3t^2+2c_2t^1+c_1 \ x(t)^{(2)}=20c_5t^3+12c_4t^2+6c_3t^1+2c_2 x(t)(1)=5c5​t4+4c4​t3+3c3​t2+2c2​t1+c1​x(t)(2)=20c5​t3+12c4​t2+6c3​t1+2c2​
  且已知起点终点的PVA数据，假设给定初始状态t=0时为(a,0,0),末状态t=T时为(b,0,0)
  则有约束公式:
  [ a b 0 0 0 ] \begin{bmatrix}a\ b\ 0\ 0\ 0 \ \end{bmatrix} ⎣⎢⎢⎢⎢⎡​ab000​⎦⎥⎥⎥⎥⎤​

• Minimum Snap

• MPC

参考

[1]
[2]
[3]