《無人駕駛車輛模型預測控制第一版》——跟蹤直線軌迹

大家可以更改初始的狀态量、參考軌迹點的數量以及采樣周期的長短，可以試着去跟蹤一些其他的直線。

大家可以先看一下這個代碼，裡面的注釋不是很詳細。推導過程後續會補上！要注意的點：

1.我們并沒有對控制增量進行限制是以不需要構造新的狀态空間；

2.本代碼二次規劃求解無松弛因子部分

需要全套資源和課程學習（5套simulink模型+10份代碼+5篇參考論文）的朋友加我微信（Jeossirey-LSJ）擷取。

clc;
clear all;
%% 參考軌迹生成
N=100;   %目标軌迹點的數量，首先設定目标軌迹上有100個預瞄點
T=0.05;  %采樣周期為0.05ｓ
% Xout=zeros(2*N,3);
% Tout=zeros(2*N,1);
Xout=zeros(N,3); % Xout用于存儲目标軌迹點上的車輛狀态；車輛共有3個狀态變量：X軸坐标、Y軸坐标、航向角；
Tout=zeros(N,1); % Tout用于存儲離散的時間序列，每一行代表一個離散的時間點．
for k=1:1:N    %生成期望軌迹以及時間序列
    Xout(k,1)=k*T;
    Xout(k,2)=2;
    Xout(k,3)=0;
    Tout(k,1)=(k-1)*T;
end

%仿真系統基本情況
Nx=3; %狀态量3個
Nu =2; %控制量2個
Tsim =20; %仿真時間20
X0 = [0 0 pi/3];%初始的位姿
[Nr,Nc] = size(Xout); % 參考狀态量的行數和列數給了Nr和Nc,即Nr=100,Nc=3
% Mobile Robot Parameters
c = [1 0 0 ;0 1 0 ;0 0 1 ;]; %輸出矩陣，我們将3個狀态量全部輸出。
L = 1; %車輛軸距
% Mobile Robot variable Model
vd1 = 1; % 車輛的參考速度
vd2 = 0; % 車輛參考前輪轉角

%矩陣定義
x_real=zeros(Nr,Nc); %存儲車輛的實際位置，100*3
x_piao=zeros(Nr,Nc); %狀态量誤差，100*3
u_real=zeros(Nr,2); %真正的控制量，100*2
u_piao=zeros(Nr,2); %控制量誤差，100*2
x_real(1,:)=X0; %将初始位置放到實際位置矩陣的第一行
x_piao(1,:)=x_real(1,:)-Xout(1,:); %狀态量誤差的第一行為實際狀态量-參考狀态量
X_PIAO=zeros(Nr,Nx*Tsim); %X_PIAO的每一行代表一個仿真時刻（對應一個軌迹點），都需要對未來20個時刻的狀态量誤差進行預測，是以X_PIAO每一行的資料可以分為20（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); %狀态量權重矩陣
R=0.1*eye(Nu*Tsim,Nu*Tsim); %控制量權重矩陣

% MPC主體
for i=1:1:Nr %從第1到100個點
    t_d =Xout(i,3); %擷取參考的橫擺角，當跟蹤第一個點的時候擷取第一個點的參考橫擺角
    a=[1    0   -vd1*sin(t_d)*T;  %離散線性化之後的a
       0    1   vd1*cos(t_d)*T;
       0    0   1;];
    b=[cos(t_d)*T   0;  %離散線性化之後的b
       sin(t_d)*T   0;
       tan(vd2)*T/L vd1*T/(cos(vd2)^2);];   
    A_cell=cell(Tsim,1); %A為20*1的元胞數組，預測時域是20，是以20行
    B_cell=cell(Tsim,Tsim); %B為20*20的元胞數組，預測時域和控制時域都是20，是以20行20列。
     for j=1:1:Tsim
        A_cell{j,1}=a^j; %構造A矩陣，通過找規律得到
        for k=1:1:Tsim
           if k<=j  %B矩陣為下三角矩陣，是以當列小于行的時候有值
                B_cell{j,k}=(a^(j-k))*b; %通過找規律得到
           else
                B_cell{j,k}=zeros(Nx,Nu);
           end
        end
    end
    A=cell2mat(A_cell); %将元胞資料轉化為矩陣
    B=cell2mat(B_cell);
    
    %二次規劃矩陣
    H=2*(B'*Q*B+R); %二次規劃的H矩陣
    f=2*B'*Q*A*x_piao(i,:)'; %二次規劃的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)';
    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(i,1)=X(1,1);
    u_piao(i,2)=X(2,1);
    Tvec=[0:0.05:4];
    X00=x_real(i,:);
    vd11=vd1+u_piao(i,1);
    vd22=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
    u_real(i,1)=vd1+u_piao(i,1);
    u_real(i,2)=vd2+u_piao(i,2);
    
    figure(1);
    plot(Xout(1:Nr,1),Xout(1:Nr,2));
    hold on;
    plot(x_real(i,1),x_real(i,2),'r*');
    title('跟蹤結果對比');
    xlabel('橫向位置X');
    axis([-1 5 -1 3]);
    ylabel('縱向位置Y');
    hold on;
    for k=1:1:Tsim
         X(i,k+1)=XXX(i,1+3*(k-1));
         Y(i,k+1)=XXX(i,2+3*(k-1));
    end
    X(i,1)=x_real(i,1);
    Y(i,1)=x_real(i,2);
    plot(X(i,:),Y(i,:),'y')
    hold on;
    
end

%% 進一步了解系統運作過程中其他的情況，以下繪圖部分
%系統狀态量随時間的變化
figure(2)
subplot(3,1,1);
plot(Tout(1:Nr),Xout(1:Nr,1),'k--');
hold on;
plot(Tout(1:Nr),x_real(1:Nr,1),'k');
%grid on;
%title('狀态量-橫向坐标X對比');
xlabel('采樣時間T');
ylabel('橫向位置X')
subplot(3,1,2);
plot(Tout(1:Nr),Xout(1:Nr,2),'k--');
hold on;
plot(Tout(1:Nr),x_real(1:Nr,2),'k');
%grid on;
%title('狀态量-橫向坐标Y對比');
xlabel('采樣時間T');
ylabel('縱向位置Y')
subplot(3,1,3);
plot(Tout(1:Nr),Xout(1:Nr,3),'k--');
hold on;
plot(Tout(1:Nr),x_real(1:Nr,3),'k');
%grid on;
hold on;
%title('狀态量-\theta對比');
xlabel('采樣時間T');
ylabel('\theta')
%控制量随時間的變化
figure(3)
subplot(2,1,1);
plot(Tout(1:Nr),u_real(1:Nr,1),'k');
%grid on;
%title('控制量-縱向速度v對比');
xlabel('采樣時間T');
ylabel('縱向速度')
subplot(2,1,2)
plot(Tout(1:Nr),u_real(1:Nr,2),'k');
%grid on;
%title('控制量-角加速度對比');
xlabel('采樣時間T');
ylabel('角加速度')
%狀态量偏差随時間的變化
figure(4)
subplot(3,1,1);
plot(Tout(1:Nr),x_piao(1:Nr,1),'k');
%grid on;
xlabel('采樣時間T');
ylabel('e(x)');
subplot(3,1,2);
plot(Tout(1:Nr),x_piao(1:Nr,2),'k');
%grid on;
xlabel('采樣時間T');
ylabel('e(y)');
subplot(3,1,3);
plot(Tout(1:Nr),x_piao(1:Nr,3),'k');
%grid on;
xlabel('采樣時間T');
ylabel('e(\theta)');