Self-Driving-Car-MPC-01-MPC-Basic

《无人驾驶车辆模型预测控制》- 龚建伟 - 第三章 - 模型预测控制算法基础与仿真分析

chapter3-3-3.m

• 参考轨迹生成

  • Xout
    • 第一列为每个时刻T, 2T, ..., nT
    • 第二列为参考轨迹y = 2

• 控制系统基本情况

  • 3个状态量:x速度，y速度，角速度
  • 2个控制量:线速度、角速度

• 矩阵定义

  • x_real 记录车辆的观测坐标
    • x_real第1行填初始位置 x_real(1,:)=X0;（X0 = [0 0 pi/3];）
  • x_piao 记录车辆当前坐标与参考坐标的差
    • x_piao(1,:)=x_real(1,:)-Xout(1,:);
  • u_real=zeros(Nr,2); u_piao=zeros(Nr,2)

• MPC主体

  • 每一个周期for i=1:1:Nr
    • 获取系统当前状态量、更新状态空间方程
      • 用系统当前状态（朝向）更新矩阵a、b

        t_d =Xout(i,3);
        a=[1    0   -vd1*sin(t_d)*T;
           0    1   vd1*cos(t_d)*T;
           0    0   1;];
        b=[cos(t_d)*T   0;
           sin(t_d)*T   0;
           0            T;];    

    • 求解一个标准二次规划问题
      • Reference:
        • quadprog
        • Model Predictive Control of a Mobile Robot Using Linearization
      • 解二次规划问题 x = quadprog(H,f,A,b,Aeq,beq,lb,ub)

        H=2*(B'*Q*B+R);
        f=2*B'*Q*A*x_piao(i,:)';
        [X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);

      • XXX保存每一时刻预测的所有状态值

        X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';
        if i+j<Nr
           for j=1:1:Tsim
               XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);
               XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);
               XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);
           end
        else
           for j=1:1:Tsim
               XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);
               XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);
               XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);
           end
        end

    • 将第一个解施加在系统上
      • u_piao保存解出的第一个u

        u_piao(i,1)=X(1,1);
        u_piao(i,2)=X(2,1);

      • 将第一个解施加在系统上

        u_real(i,1)=vd1+u_piao(i,1);
        u_real(i,2)=vd2+u_piao(i,2);

      • 用解得的控制量更新下一时刻状态

        XOUT=dsolve('Dx-vd11*cos(z)=0','Dy-vd11*sin(z)=0','Dz-vd22=0','x(0)=X00(1)','y(0)=X00(2)','z(0)=X00(3)');
        t=T; 
        x_real(i+1,1)=eval(XOUT.x);
        x_real(i+1,2)=eval(XOUT.y);
        x_real(i+1,3)=eval(XOUT.z);
        if(i<Nr)
         x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:);
        end