qpoases解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;


MatrixXd fei(P,1); //AK


MatrixXd theta(P,P); //BK


MatrixXd E(P,1); //E


MatrixXd H(P,P); //H


MatrixXd f(P,1); //f

MatrixXd g(1,P);


MatrixXd xk(1,1);

MatrixXd lb(P,1);

MatrixXd ub(P,1);


float pos = 0;

float vmax = 1,vmin = -1;

const int num_of_elements = H.rows() * H.cols(); //元素数

double H_matrix[num_of_elements];

const auto k_num_of_rows = f.rows();

double g_matrix[k_num_of_rows]; // NOLINT ， 与offset一般大

const int num_of_lb = lb.rows();

double lb_matrix[num_of_lb];

double ub_matrix[num_of_lb];

for(int i=0;i<num_of_lb;i++)

{

lb_matrix[i] = vmin;

ub_matrix[i] = vmax;

}


QProblemB v_pro(P);

Options options;

//options.enableFlippingBounds = BT_FALSE;

options.initialStatusBounds = ST_INACTIVE;

options.numRefinementSteps = 1;

options.enableCholeskyRefactorisation = 1;

v_pro.setOptions( options );


while (robot->step(timeStep) != -1) {

// pos+=0.1;

xk(0,0) = pos;

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

fei(i,0) = pow(A_,i+1);

}

// cout <<"fei "<<fei <<"\n";


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

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

theta(i,j)= pow(A_,i-j)*B_;

}

}

//cout <<"theta"<<theta <<"\n";


E = fei*xk-Rk;

H = 2.0*(theta.transpose()*Q*theta+W); //求矩阵的转置

f = (2.0*E.transpose()*Q*theta).transpose();

g = f.transpose(); //g为f的转置

// cout <<E <<"\n" << H <<"\n" <<f <<"\n";

//转为qpOASES格式

for(int i =0;i<H.rows();i++)

{

for(int j=0;j<H.cols();j++)

{

H_matrix[(i)*H.cols()+j] = H(i,j);

}

g_matrix[i] = g(0,i);

}

int_t nWSR = 10;

v_pro.init(H_matrix, g_matrix, lb_matrix, ub_matrix, nWSR);

real_t xOpt[2];

v_pro.getPrimalSolution( xOpt );

printf( "\nxOpt = [ %e, %e ]; objVal = %e\n\n", xOpt[0],xOpt[1],v_pro.getObjVal() );


setV(xOpt[0],timeStep);

// setV(0.5,timeStep);

//pos = pos + xOpt[0]*T ;

pos = robot->getSelf()->getPosition()[0];

printf("pos:%f\n",pos);


};


// Enter here exit cleanup code.


delete robot;

return 0;

}



int testQP()

{

/* Setup data of first QP. */

real_t H[2 * 2] = { 1.0, 0.0, 0.0, 0.5 };

real_t A[1 * 2] = { 1.0, 1.0 };

real_t g[2] = { 1.5, 1.0 };

real_t lb[2] = { 0.5, -2.0 };

real_t ub[2] = { 5.0, 2.0 };

real_t lbA[1] = { -1.0 };

real_t ubA[1] = { 2.0 };

/* Setup data of second QP. */

real_t g_new[2] = { 1.0, 1.5 };

real_t lb_new[2] = { 0.0, -1.0 };

real_t ub_new[2] = { 5.0, -0.5 };

real_t lbA_new[1] = { -2.0 };

real_t ubA_new[1] = { 1.0 };

/* Setting up QProblem object. */

QProblem example(2, 1);

/* Solve first QP. */

int_t nWSR = 10;

example.init(H, g, A, lb, ub, lbA, ubA, nWSR);


real_t xOpt[2];

real_t yOpt[2+1];

example.getPrimalSolution( xOpt );

example.getDualSolution( yOpt );

printf( "\nxOpt = [ %e, %e ]; yOpt = [ %e, %e, %e ]; objVal = %e\n\n",

xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );


/* Solve second QP. */

nWSR = 10;

example.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR);

/* Get and print solution of second QP. */

// real_t xOpt[2];

example.getPrimalSolution(xOpt);

printf("\n xOpt = [ %e, %e ]; objVal = %e\n\n",

xOpt[0], xOpt[1], example.getObjVal());

return 0;

}


float wheel_p=0;

void setV(double _v,float _time_step)

{

double r = 0.05;

_v = -_v;

wheel_p += _time_step/1000*_v/r;

motor_FR->setPosition(wheel_p);

motor_FL->setPosition(wheel_p);

motor_BR->setPosition(wheel_p);

motor_BL->setPosition(wheel_p);


}

#include "MPC.hpp"
#include "math.h"
#include "iostream"

#define PI 3.1415926
#define T 0.03
#define w0 0.5
#define w1 5.0
#define Ku 1
#define Kl 1


using namespace Eigen;
using namespace std;
USING_NAMESPACE_QPOASES

MatrixXd MPC_controller::MPC_Solve_qp(Eigen::Vector3d X_k,std::vector<Eigen::Vector3d >X_r,std::vector<Eigen::Vector2d >U_r,const int N)
{
    //cout<<"current w : "<<w_max<<endl;
    根据参考输入计算出的系数矩阵
    vector<MatrixXd> A_r(N),B_r(N),A_multiply1(N);
    MatrixXd O_r(3*N,1);
    MatrixXd A_bar(3*N,3);
    MatrixXd X_ref(3*N,1);
    MatrixXd A_multiply2;
    MatrixXd B_bar = MatrixXd::Zero(3*N,2*N);
    MatrixXd C_bar = MatrixXd::Identity(3*N,3*N);
    MatrixXd A_r_init = MatrixXd::Zero(3,3);
    MatrixXd B_r_init = MatrixXd::Zero(3,2);
    MatrixXd eye_3 = MatrixXd::Identity(3,3);
    MatrixXd Q = MatrixXd::Identity(3*N,3*N)*w0;
    MatrixXd R = MatrixXd::Identity(3*N,3*N)*w1;
//    cout<<"Ur 1 : "<<endl<<U_r[0]<<endl;
//    cout<<"Xr 1 : "<<endl<<X_r[0]<<endl;
//    cout<<"Xk : "<<endl<<X_k<<endl;
    for(int k=0;k<N;k++)
    {
        A_r[k] = A_r_init;
        B_r[k] = B_r_init;
        A_r[k](0,2) = -U_r[k](0)*sin(X_r[k](2));
        A_r[k](1,2) = U_r[k](0)*cos(X_r[k](2));
        Vector3d temp_vec = -T*A_r[k]*X_r[k];
        O_r.block<3,1>(k*3,0) = temp_vec;
        A_r[k] = eye_3+T*A_r[k];
        B_r[k](0,0) = cos(X_r[k](2))*T;
        B_r[k](1,0) = sin(X_r[k](2))*T;
        B_r[k](2,1) = T;
        X_ref.block<3,1>(k*3,0) = X_r[k];
//        cout<<"B_r "<<k+1<<" : "<<endl<<B_r[k]<<endl;

        if(k==0) A_multiply1[k] = A_r[k];
        else A_multiply1[k] = A_multiply1[k-1]*A_r[k];
        A_bar.block<3,3>(3*k,0) = A_multiply1[k];
    }
//    cout<<"Ar 1 : "<<endl<<A_r[0]<<endl;
//    cout<<"Br 1 : "<<endl<<B_r[0]<<endl;
//    cout<<"Or 1 : "<<endl<<O_r.block<3,1>(0,0)<<endl;
//    cout<<"O_r : "<<endl<<O_r<<endl;

    for(int k=0;k<N;k++)
    {
        B_bar.block<3,2>(3*k,2*k) = B_r[k];
        A_multiply2 = eye_3;
        for(int i=0;i<k;i++)
        {
            A_multiply2 = A_multiply2*A_r[k-i];
            C_bar.block<3,3>(3*k,3*(k-1-i)) = A_multiply2;
            B_bar.block<3,2>(3*k,2*(k-1-i)) = A_multiply2*B_r[k-1-i];
        }
    }

    //cout<<"C bar : "<<endl<<C_bar<<endl;

    MatrixXd E =A_bar*X_k+C_bar*O_r-X_ref;
    MatrixXd Hesse = 2*B_bar.transpose()*(Q+R)*B_bar;      Hesse矩阵
    VectorXd gradient = 2*B_bar.transpose()*Q*E;       一次项系数


    real_t H[2*N*2*N],g[2*N],A[2*N],lb[2*N],ub[2*N],lbA[1],ubA[1];
    lbA[0] = N*(v_min+w_min)/Ku;
    ubA[0] = N*(v_max+w_max)/Kl;
    for(int i=0;i<2*N;i++)
    {
        g[i] = gradient(i);
        A[i] = 1;
        if(i%2==0)
        {
            lb[i] = v_min;
            ub[i] = v_max;
        }
        else
        {
            lb[i] = w_min;
            ub[i] = w_max;
        }
        for(int j=0;j<2*N;j++)
        {
            H[i*2*N+j] = Hesse(i,j);
        }
    }

    int_t nWSR = 800;

    QProblem mpc_qp_solver(2*N,1);
    mpc_qp_solver.init(H,g,A,lb,ub,lbA,ubA,nWSR);

    real_t x_solution[2*N];
    mpc_qp_solver.getPrimalSolution(x_solution);


    Vector2d u_k;
    MatrixXd U_result = MatrixXd::Zero(2,N);
    for(int i=0;i<N;i++)
    {
        u_k(0) = x_solution[2*i];
        u_k(1) = x_solution[2*i+1];
        U_result.col(i) = u_k;
//        std::cout<<"U "<<i+1<<" : "<<endl<<u_k<<endl;
    }
    //cout<<"N : "<<N<<endl;
    return U_result;
}

void MPC_controller::MPC_init(ros::NodeHandle &nh)
{
    nh.getParam("/ego_planner_node/MPC/v_max", v_max);
    nh.getParam("/ego_planner_node/MPC/w_max", w_max);

    // nh.param("/ego_planner_node/MPC/v_max",v_max,0.7);
    // nh.param("/ego_planner_node/MPC/w_max",w_max,0.3);
    ROS_INFO("v_max : %f , w_max : %f",v_max,w_max);
    w_min = -w_max;
    v_min=0;

}