qpoases解MPC控制-Condensed Format(仅仅与输出量有关)

 障碍物表示形式

凸多面体

凸多面体可以根据问题形式的不同采用不同的定义形式，可以定义为空间中的一个凸集，半空间的交集(H-representation)和一系列点构成的凸包(V-representation)。

具体MPC控制算法推导建下面链接：

LQR、MPC以及osqp库_osqp mpc-优快云博客

MPC控制算法推导 - 知乎

一个模型预测控制（MPC）的简单实现 - 知乎

#include <webots/Robot.hpp>

#include <webots/Motor.hpp>

#include <webots/Supervisor.hpp>

#include <iostream>

#include <Eigen/Dense>

// #include "Array.hh" //使用了MIT中的文件 其修改了向量与矩阵的名字 不然会与Eigen冲突

// #include "QuadProg++.hh"

#include <qpOASES.hpp>

#include <fstream>


// All the webots classes are defined in the "webots" namespace

using namespace webots;

using namespace std;

using namespace Eigen;

using namespace qpOASES;

Motor *motor_FR,*motor_FL,*motor_BR,*motor_BL;

Supervisor* robot;

int testQP();

void setV(double _v,float _time_step);



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

//testQP();

//Robot *robot = new Robot();

robot = new Supervisor();

motor_FR = robot->getMotor("motor_FR");

motor_FL = robot->getMotor("motor_FL");

motor_BR = robot->getMotor("motor_BR");

motor_BL = robot->getMotor("motor_BL");

// get the time step of the current world.

int timeStep = (int)robot->getBasicTimeStep();

// double T = timeStep/1000;

double T = 0.05;

int P = 5;//预测长度


MatrixXd Q(P,P); //状态误差权重

for(int i=0;i<P;i++){

Q(i,i) = 5*1;

}

//std::cout << "Q =\n" << Q << std::endl;


MatrixXd W(P,P); //控制输出权重

for(int i=0;i<P;i++){

W(i,i) = 1;

}


MatrixXd Rk(P,1); //参考值序列

for(int i=0;i<P;i++){

Rk(i,0) = 2;

}


double A_ = 1;

double B_ = T;