《无人驾驶车辆模型预测控制第一版》——跟踪直线轨迹

 参考轨迹点的数量我们取150个点，通过MPC算法去跟踪参考轨迹上的每一个点，跟踪完成一个点后判断是否为最后一个点，如果不是最后一个点，那么继续跟踪下一个点，如果是最后一个点，则跟踪结束。这里我们取采样周期为40ms。大家可以更改初始的状态量、参考轨迹点的数量以及采样周期的长短，可以试着去跟踪一些其他的直线。

模型预测控制的推导：

https://blog.csdn.net/m0_50888394/article/details/115556185

N=150;                                   %参考轨迹点的数量
T=0.04;                                  %采样周期
Xout=zeros(N,3);                         %生成N行3列的0矩阵，为参考点矩阵
Tout=zeros(N,1);                         %生成N行1列的0矩阵
for k=1:1:N
    Xout(k,1)=k*T;                       %第1列表示x值
    Xout(k,2)=2.5;                         %第2列表示y值
    Xout(k,3)=0;                         %第3列表示φ值
    Tout(k,1)=(k-1)*T;                   %为时刻矩阵
end
Nx=3;                                     %状态量个数
Nu =2;                                    %控制量个数
Tsim =20;                                 %文中注解仿真时间，我认为是预测时域Np
X0 = [0 0 pi/3];                          %初始位置的状态
[Nr,Nc] = size(Xout);                     %取Xout矩阵的行数和列数分别赋值给Nr和Nc
L = 1;                                    %车辆轴距
Rr = 1;                                   %轮胎半径
w = 1;                                    %车轮转速
vd1 = Rr*w;                               %参考系统的纵向速度
vd2 = 0;                                  %参考系统的角速度
x_real=zeros(Nr,Nc);                       %状态量矩阵   
x_piao=zeros(Nr,Nc);                       %状态量误差矩阵
u_real=zeros(Nr,2);                        %控制量矩阵 
u_piao=zeros(Nr,2);                        %控制量误差矩阵
x_real(1,:)=X0;                            %把初始状态赋值给状态量第一行
x_piao(1,:)=x_real(1,:)-Xout(1,:);         %计算第一个状态量误差
X_PIAO=zeros(Nr,Nx*Tsim);
XXX=zeros(Nr,Nx*Tsim);                     %用于保持每个时刻预测的所有状态值
q=[1 0 0;0 1 0;0 0 0.5];                   %状态量的权重系数
Q_cell=cell(Tsim,Tsim);
for i=1:1:Tsim
    for j=1:1:Tsim
        if i==j
            Q_cell{i,j}=q;
        else 
            Q_cell{i,j}=zeros(Nx,Nx);
        end 
    end
end
Q=cell2mat(Q_cell);                         %将多个矩阵合并成一个矩阵，得到权重系数矩阵Q
R=0.1*eye(Nu*Tsim,Nu*Tsim);                 %权重系数矩阵R
for i=1:1:Nr
    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;];                    %控制量系数矩阵   
    A_cell=cell(Tsim,1);
    B_cell=cell(Tsim,Tsim);
     for j=1:1:Tsim
        A_cell{j,1}=a^j;                    %A_cell为 ψ 矩阵的分量
        for k=1:1:Tsim
           if k<=j
                B_cell{j,k}=(a^(j-k))*b;    %B_cell为 θ 矩阵的分量
           else
                B_cell{j,k}=zeros(Nx,Nu);
           end
        end
    end
    A=cell2mat(A_cell);                     %合并A_cell生成 ψ 矩阵
    B=cell2mat(B_cell);                     %合并B_cell生成 θ 矩阵
    
    H=2*(B'*Q*B+R);                         %二次规划中的H
    f=(2*x_piao(i,:)*A'*Q*B)';              %二次规划中的f
    %% 约束条件
    A_cons=[];                              %不等式约束的系数矩阵
    b_cons=[];                              %不等式约束的值
    lb=[-1;-1];                             %自变量约束下界
    ub=[1;1];                               %自变量约束上界
    tic                                     %用来保存当前时间
    [X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);   %%二次规划函数求解
    toc                                     %用来记录程序完成时间
    X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';      %输出矩阵Y
      
      %% 输出每个时刻预测的所有的状态量
      %% i表示时刻k，预测时域值为20，则当时刻k=80开始后，预测到的时刻要超100了，所以之后都取100时刻的值。
    if i+j