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

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

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

1.建模方法

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

本文采用差速小车模型进行建模：
e ( k + 1 ) = A e ( k ) + B u ( k ) e(k+1)=Ae(k)+Bu(k) e(k+1)=Ae(k)+Bu(k)
其中：
e ( k ) = [ e x e y e ϕ ] e(k)=\begin{bmatrix} e_x \\ e_y\\ e_\phi \end{bmatrix} e(k)=⎣ ⎡​ex​ey​eϕ​​⎦ ⎤​ A = [ 1 0 − T V r sin ⁡ ϕ r 0 1 T V r cos ⁡ ϕ r 0 0 1 ] A=\begin{bmatrix} 1 & 0 & -TV_r\sin\phi_r\\ 0 & 1 & TV_r\cos\phi_r\\ 0 & 0 &1 \end{bmatrix} A=⎣ ⎡​100​010​−TVr​sinϕr​TVr​cosϕr​1​⎦ ⎤​ B = [ T cos ⁡ ϕ r 0 T sin ⁡ ϕ r 0 0 T ] B=\begin{bmatrix} T\cos\phi_r & 0\\ T\sin\phi_r & 0\\ 0 & T \end{bmatrix} B=⎣ ⎡​Tcosϕr​Tsinϕr​0​00T​⎦ ⎤​
其中 e x e_x ex​、 e y e_y ey​、 e ϕ e_\phi eϕ​ 为横向、纵向、偏航角方向的误差 ， V r V_r Vr​为参考纵向速度， T T T为采样时间， v v 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;
        }
    }