matplot画图控制marker点的个数_笔记|模型预测控制的原理和实例

本文是出自《无人驾驶车辆模型预测控制》的一些笔记，方便自己的查看和加深对原理、程序的理解(实际上这个工程实例我也没完全理解，总觉得有些公式和程序对不上的感觉)

MPC的原理图

必要的文字描述

一些公式推导

• 一些变量定义的意思

• 模型的建立

工程实例

• 题目：车辆从原点(0,0)出发，以期望纵向速度v = 1 m/s跟踪一条直线轨迹 y = 2，采样时间50ms，仿真设定总时间为20s。
• 预测模型的建立：

由于上面没有说明控制变量是啥，所以翻看第二章，控制变量应该是车速v和前轮偏角。但是这个A、B是咋推导出来的我不太明白。

• 代码记录
• 参考轨迹生成：

%% 参考轨迹生成N=100;%100个点T=0.05;%采样时间% Xout=zeros(2*N,3);% Tout=zeros(2*N,1);Xout=zeros(N,3);Tout=zeros(N,1);for k=1:1:N    Xout(k,1)=k*T;%x坐标，每个采样时间x前进一个点    Xout(k,2)=2;%y坐标，保持为2    Xout(k,3)=0;%航向角保持为0    Tout(k,1)=(k-1)*T;end

• 控制系统的参数初始化部分

%% Tracking a constant reference trajectoryNx=3;%状态量个数Nu =2;%控制量个数Tsim =20;%仿真时间题目说20s,我觉得这里可以理解为这里的控制时域，预测时域，都是20X0 = [0 0 pi/3];%初始的状态量，原点0,0，航向角为60度[Nr,Nc] = size(Xout); % Nr is the number of rows of Xout% Mobile Robot Parametersc = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];%好像没啥用，先不管L = 1;Rr = 1;w = 1;% Mobile Robot variable Modelvd1 = Rr*w; % For circular trajectory参考系统的纵向速度vd2 = 0;%参考系统的前轮偏角x_real=zeros(Nr,Nc);%Nr行数，参考点的个数100，Nc列数，3,所以是100*3x_piao=zeros(Nr,Nc);%定义偏差矩阵x_piao，100*3u_real=zeros(Nr,2);%控制量的真实矩阵u_piao=zeros(Nr,2);%控制量的偏差矩阵x_real(1,:)=X0;%初试化状态，起点是0，0,前轮偏角是60度x_piao(1,:)=x_real(1,:)-Xout(1,:);%初始化状态偏差X_PIAO=zeros(Nr,Nx*Tsim);%X_PIAO矩阵，囊括每一个时间步的预测增量，因为Tsim=20,所以大小为100,3*20XXX=zeros(Nr,Nx*Tsim);%XXX矩阵，用于保持每个时刻预测的所有状态值，每一个状态都会有Tsim个预测值，所以每一个状态一共有20个预测值q=[1 0 0;0 1 0;0 0 0.5];%权重矩阵Q_cell=cell(Tsim,Tsim);%20*20个元胞数组

• 初始化大Q和大R，对应下面公式的Qe、Re

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);%初始化Q_cell矩阵        end     endendQ=cell2mat(Q_cell);R=0.1*eye(Nu*Tsim,Nu*Tsim);%大小为2*20,2*20

• 关键部分的迭代计算

B对应下面这个矩阵，C应该就是1

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);%20*1，20行1列个元胞矩阵    B_cell=cell(Tsim,Tsim);%20*20，20行20列个元胞矩阵         for j=1:1:Tsim        A_cell{j,1}=a^j;%先给个A^j，每个cell是3*3，一共20个，A是60*3        for k=1:1:Tsim           if k<=j                B_cell{j,k}=(a^(j-k))*b;           else                B_cell{j,k}=zeros(Nx,Nu);%如果行号小于列号，就是0           end        end    end    A=cell2mat(A_cell);%60*3    B=cell2mat(B_cell);%B是大θt        H=2*(B'*Q*B+R);%这里乘2不知何意，应该没有加松弛因子，跟书本有出入    f=2*B'*Q*A*x_piao(i,:)';%这里着实不太理解为何多了一个A    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);%X是一个100*21的预测矩阵，每一个行对应每一个状态点-在每一个时刻的控制序列    toc    X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';%A*偏差+B*控制序列，更新预测增量大矩阵    if i+j

• 最后画图

%% 以下为绘图部分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('状态量-heta对比');xlabel('采样时间T');ylabel('heta')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(heta)');

总结一下

这段代码的核心部分实在是太迷了，还是我自己太菜了，唉，我再多看看资料，加油~~~