qpoases解MPC控制

 障碍物表示形式

凸多面体

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

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

LQR、MPC以及osqp库_osqp mpc-CSDN博客

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;
 
}

其中使用osqp与eigen联合求解为(Non-Condensed Format)，它不仅仅与输出控制量有关，与状态量也有关系。