更多的关于Eigen的学习参考
更多的关于OSQP的学习参考
我们在github上找到了使用eigen库封装起来的OSQP库，比原来的OSQP写代码时更好用一些。
我们对照mathworks对quadprog的例子解释对库的使用
一个标准的QP问题可以以如下的标准形式进行描述：
注意这里面的写法不同的书本有些不同的表达：H , f 有时候也被写成 P, q.
接下来给出几个典型的不同约束的二次规划形式：
1，具有线性约束的二次规划
matlab的调用方法是：
H = [1 -1; -1 2];
f = [-2; -6];
A = [1 1; -1 2; 2 1];
b = [2; 2; 3];
%调用 quadprog。
[x,fval,exitflag,output,lambda] = ...
quadprog(H,f,A,b);
可以看到，这里的等式约束没有就可以不写，solver会，默认不去管它。
OSQP-Eigen库的c++调用是：
// 具有线性约束的二次规划
hessian.resize(2, 2);
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;
int NumberOfVariables = 2; //A矩阵的列数
int NumberOfConstraints = 3; //A矩阵的行数
我们可以看到c++代码里面是需要声明不等式约束的上下界的，如果没有就写无穷：
lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;
接下来，是调用c++ solver的通用部分：
OsqpEigen::Solver solver;
// settings
//solver.settings()->setVerbosity(false);
solver.settings()->setWarmStart(true);
// set the initial data of the QP solver
//矩阵A为m*n矩阵
solver.data()->setNumberOfVariables(NumberOfVariables); //设置A矩阵的列数，即n
solver.data()->setNumberOfConstraints(NumberOfConstraints); //设置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; //设置上边界
// instantiate the solver
if (!solver.initSolver())
return 1;
Eigen::VectorXd QPSolution;
// solve the QP problem
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;
具有线性等式约束的二次规划
matlab的求解方式写成：
[x,fval,exitflag,output,lambda] = ...
quadprog(H,f,[],[],Aeq,beq);
注意，这里没有不等式约束就写上[], [], 后面再接上等式约束。
/
具有线性等式约束的二次规划
hessian.resize(2,2);
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(1,2);
linearMatrix.insert(0,0) = 1;
linearMatrix.insert(0,1) = 1;
std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;
lowerBound.resize(1);
lowerBound << 0;
std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;
upperBound.resize(1);
upperBound << 0;
std::cout << "upperBound:" << std::endl << upperBound << std::endl;
int NumberOfVariables = 2; //A矩阵的列数
int NumberOfConstraints = 1; //A矩阵的行数
我们可以看到，c++对等式约束的实现就是把上下界都写成同一个数字：
并且不等式约束和等式约束都要放在A矩阵中。
lowerBound.resize(1);
lowerBound << 0;
upperBound.resize(1);
upperBound << 0;
具有线性约束和边界的二次最小化
matlab的求解是：
x = quadprog(H,f,[],[],Aeq,beq,lb,ub)
依旧是没有不等式约束，就写成 [ ], [ ].
c++ 的写法为：
hessian.resize(3,3);
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;
std::cout << "hessian:" << std::endl << hessian << std::endl;
gradient.resize(3);
gradient << 2, -3, 1;
std::cout << "gradient:" << std::endl << gradient << std::endl;
linearMatrix.resize(4,3);
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;
std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;
lowerBound.resize(4);
lowerBound << 0, 0, 0, 0.5;
std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;
upperBound.resize(4);
upperBound << 1, 1, 1, 0.5;
std::cout << "upperBound:" << std::endl << upperBound << std::endl;
int NumberOfVariables = 3; //A矩阵的列数
int NumberOfConstraints = 4; //A矩阵的行数
可以看到打印出来的 LinearMatrix A需要包含等式约束和不等式约束：
1 0 0
0 1 0
0 0 1
1 1 1
lowerBound:
0
0
0
0.5
upperBound:
1
1
1
0.5
这个里面就包含了前三个不等式约束，和最有一个相加为0.5的等式约束了。
最后我们验证一个比较有用的问题的解法：接一个带约束的五阶多项式问题：
这个问题的描述已经在之前的blog里面做了详细解释
最后文章也通过matlab quadprog得出了解，这里我们用c++再做一次实现：
hessian.resize(6, 6);
hessian1.resize(6, 6);
hessian2.resize(6, 6);
double Te = 5;
double d0 = 1;
double v0 = 0.2;
double a0 = 0;
double LB = 2.5;
double UB = 3.5;
for (int i = 0; i < 6; i++)
{
hessian1.insert(0, i) = pow(Te, (i + 1)) / (i + 1);
hessian1.insert(1, i) = pow(Te, (i + 2)) / (i + 2);
hessian1.insert(2, i) = pow(Te, (i + 3)) / (i + 3);
hessian1.insert(3, i) = pow(Te, (i + 4)) / (i + 4);
hessian1.insert(4, i) = pow(Te, (i + 5)) / (i + 5);
hessian1.insert(5, i) = pow(Te, (i + 6)) / (i + 6);
}
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 3; j++)
{
hessian2.insert(j, i) = 0;
}
}
for (int i = 3; i < 6; i++)
{
for (int j = 0; j < 3; j++)
{
hessian2.insert(i, j) = 0;
}
}
hessian2.insert(3, 3) = 36 * Te;
hessian2.insert(3, 4) = 72 * Te * Te;
hessian2.insert(3, 5) = 120 * Te * Te * Te;
hessian2.insert(4, 3) = 72 * Te * Te;
hessian2.insert(4, 4) = 192 * Te * Te * Te;
hessian2.insert(4, 5) = 360 * Te * Te * Te * Te;
hessian2.insert(5, 3) = 120 * Te * Te * Te;
hessian2.insert(5, 4) = 360 * Te * Te * Te * Te;
hessian2.insert(5, 5) = 720 * Te * Te * Te * Te * Te;
hessian = hessian1 + hessian2;
// 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(6);
gradient << 0, 0, 0, 0, 0, 0;
std::cout << "gradient:" << std::endl;
std::cout << gradient << std::endl;
linearMatrix.resize(6, 6);
// the first row
for (int i = 0; i < 6; i++)
{
linearMatrix.insert(0, i) = pow(Te, i);
}
//the second row
for (int i = 1; i < 6; i++)
{
linearMatrix.insert(1, i) = 0;
}
linearMatrix.insert(1, 0) = 1;
// the third row
for (int i = 2; i < 6; i++)
{
linearMatrix.insert(2, i) = 0;
}
linearMatrix.insert(2, 0) = 0;
linearMatrix.insert(2, 1) = 1;
// the fourth row
linearMatrix.insert(3, 0) = 0;
linearMatrix.insert(3, 1) = 1;
linearMatrix.insert(3, 2) = 2 * Te;
linearMatrix.insert(3, 3) = 3 * Te * Te;
linearMatrix.insert(3, 4) = 4 * Te * Te * Te;
linearMatrix.insert(3, 5) = 5 * Te * Te * Te * Te;
// the fifth row
linearMatrix.insert(4, 0) = 0;
linearMatrix.insert(4, 1) = 0;
linearMatrix.insert(4, 2) = 2;
linearMatrix.insert(4, 3) = 0;
linearMatrix.insert(4, 4) = 0;
linearMatrix.insert(4, 5) = 0;
// the sixth row
linearMatrix.insert(5, 0) = 0;
linearMatrix.insert(5, 1) = 0;
linearMatrix.insert(5, 2) = 2;
linearMatrix.insert(5, 3) = 6 * Te;
linearMatrix.insert(5, 4) = 12 * Te * Te;
linearMatrix.insert(5, 5) = 20 * Te * Te * Te;
std::cout << "linearMatrix:" << std::endl
<< linearMatrix << std::endl;
lowerBound.resize(6);
// lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;
lowerBound << (d0 + LB), d0, v0, 0, a0, 0;
std::cout << "lowerBound:" << std::endl;
std::cout << lowerBound << std::endl;
upperBound.resize(6);
upperBound << (d0 + UB), d0, v0, 0, a0, 0;
std::cout << "upperBound:" << std::endl;
std::cout << upperBound << std::endl;
int NumberOfVariables = 6; //A矩阵的列数
int NumberOfConstraints = 6; //A矩阵的行数
得出的解为：
iter objective pri res dua res rho time
1 0.0000e+00 3.50e+00 6.96e+05 1.00e-01 1.58e-04s
75 1.6280e+01 2.74e-10 6.54e-07 3.46e+01 2.68e-04s
status: solved
number of iterations: 75
optimal objective: 16.2803
run time: 2.84e-04s
optimal rho estimate: 4.40e+00
time use:0.008ms
QPSolution:
1
0.2
-1.71031e-17
0.152
-0.0472
0.00384
如果有兴趣可以关注下里面的数据：比如dual-residual 对偶间隙，看得出是通过对偶问题的方式求解的。
我们获得了和matlab一样的解，对比matlab和c++的求解效率，使用同一个硬件平台进行比较：
c++： time use:0.008ms
matlab: tiem use:80.8ms
最后是一些编译上的问题：
1 编译osqp-eigen库时报下面的错误：
CMake Error at cmake/OsqpEigenDependencies.cmake:12 (find_package):
Could not find a configuration file for package "Eigen3" that is compatible
with requested version "3.2.92".
The following configuration files were considered but not accepted:
/usr/lib/cmake/eigen3/Eigen3Config.cmake, version: unknown
Call Stack (most recent call first):
CMakeLists.txt:63 (include)
是Eigen库安装的问题，建议重装eigen:
step1：卸载eigen3（如果已经是最新的，就不要卸载了）
sudo rm -rf /usr/include/eigen3
sudo rm -rf /usr/lib/cmake/eigen3
step2: 安装最新eigen3
https://gitlab.com/libeigen/eigen/-/releases/3.3.9
下载tar.gz到任意位置，然后在保存文件的位置执行：
tar -zxvf eigen-3.3.9.tar.gz
cd eigen-3.3.9/ （这里是你的版本号，2021.3.21为止的最高版本为3.9）
mkdir build
cd build
cmake ..
sudo make
sudo make install
step2: 拷贝移动文件位置，需要让你的usr/local/include下面直接有eigen3， 一般安装完是不会有的，而我们调用
#include<Eigen/Dense>的时候，编译器会默认去这个地方找Eigen，除非你已经写好了Eigen的搜索位置
这一步很重要：复制Eigen库到 /usr/local/include 中 （这一步很重要，一定要执行，否则后面编译一些程序，会提示没有Eigen库）
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include
然后编译
cd osqp-eigen
mkdir build && cd build
cmake ..
make
[sudo] make install
最后提供一个测试版本，如果正常运行就说明安装成功了：
cd osqp-eigen/example
mkdir build && cd build
cmake ..
make
执行
./SimpleExample
283
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
