基于ROS的无人车模型预测控制（MPC）C++实现

基于ROS的无人车模型预测控制（MPC）C++实现

最近在做毕业设计的控制器部分，网上关于cpp实现模型预测控制的资料很少，基本都是Matlab/Simulink实现，顺手写一下学习过程。

1.建模方法

模型预测控制的基本原理本文不作赘述，可以参考：
DandD的模型预测控制轨迹跟踪的教学视频
Dr. CAN的模型预测控制原理教学视频

本文采用差速小车模型进行建模：
$e(k+1)=Ae(k)+Bu(k)$
其中：
$e(k)=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\varphi }\end{array}\right]$ $A=\left[\begin{array}{ccc}1& 0& -T{V}_r\sin {\varphi }_r\\ 0& 1& T{V}_r\cos {\varphi }_r\\ 0& 0& 1\end{array}\right]$ $B=\left[\begin{array}{cc}T\cos {\varphi }_r& 0\\ T\sin {\varphi }_r& 0\\ 0& T\end{array}\right]$
其中$e_x$、$e_y$、$e_{\varphi }$ 为横向、纵向、偏航角方向的误差 ，$V_r$为参考纵向速度，$T$为采样时间，$v$为小车纵向速度,$\omega$为小车的横摆角速度。（阿克曼模型的车辆可以参考前面两个视频的建模方法，差别不大）

2.求解器选择及配置

这次选用的是Gurobi作为MPC中二次规划问题的求解器，优异的求解速度完全可以胜任MPC的底层求解任务，学生和老师还可以在官网申请免费的许可证。Gurobi的安装和配置以及C++接口配置请参考我的前两篇博客。

3.实现过程

try{
        GRBEnv env = GRBEnv(true);  //初始化求解器
        env.start();    //激活求解器
        GRBModel model = GRBModel(env);   //建立求解模型
        GRBVar V[N_P+1],OMEGA[N_P+1];     //创建变量纵向速度 角速度
        for(int i=0; i<=N_P; i++){        //在模型中添加变量并设置约束
            std::ostringstream vname1, vname2;
            vname1<<"v"<<i;
            vname2<<"omega"<<i;
            V[i] = model.addVar(0,max_velocity,0.0,GRB_CONTINUOUS,vname1.str());
            OMEGA[i] = model.addVar(-max_palstance,max_palstance,0.0,GRB_CONTINUOUS,vname2.str());
        }
        std::vector<double> phi_r(N_P);
        phi_r = get_phi_reference();			//计算未来N_P个周期的参考偏航角
        GRBLinExpr X_ERROR[N_P+1];      //创建状态变量
        GRBLinExpr Y_ERROR[N_P+1];
        GRBLinExpr PHI_ERROR[N_P+1];
        X_ERROR[0] = error.x_e;
        Y_ERROR[0] = error.y_e;
        PHI_ERROR[0] = error.phi_e;
        for(int i=0; i<N_P ;i++){      //创建状态迭代方程
            X_ERROR[i+1]=X_ERROR[i]-T*V_R*sin(phi_r[i])*PHI_ERROR[i]+T*cos(phi_r[i])*V[i];
            Y_ERROR[i+1]=Y_ERROR[i]+T*V_R*cos(phi_r[i])*PHI_ERROR[i]+T*sin(phi_r[i])*V[i];
            PHI_ERROR[i+1]=PHI_ERROR[i]+T*OMEGA[i];
        }
        GRBQuadExpr obj = 0;    //创建优化目标值
        for(int i=0; i<=N_P; i++){
            obj+=K[0]*X_ERROR[i]*X_ERROR[i]+K[1]*Y_ERROR[i]*Y_ERROR[i]+K[2]*PHI_ERROR[i]*PHI_ERROR[i];  //优化误差目标值
            obj+=K[3]*(V[i]-V_R)*(V[i]-V_R)+K[4]*OMEGA[i]*OMEGA[i];
        }
        model.setObjective(obj,GRB_MINIMIZE);   //将优化目标值添加进模型
        model.optimize();   //求解
        result.velocity = V[0].get(GRB_DoubleAttr_X);   //MPC仅使用第一个输出量
        result.palstance = OMEGA[0].get(GRB_DoubleAttr_X);
    }   catch(GRBException e){
            std::cout<<"Error code:"<<e.getErrorCode()<<std::endl;
            std::cout<<e.getMessage()<<std::endl;
    }
        catch (...){
            std::cout << "Exception during optimization" << std::endl;
        }
    }