大家可以更改初始的状态量、参考轨迹点的数量以及采样周期的长短,可以试着去跟踪一些其他的直线。
大家可以先看一下这个代码,里面的注释不是很详细。推导过程后续会补上!要注意的点:
1.我们并没有对控制增量进行约束所以不需要构造新的状态空间;
2.本代码二次规划求解无松弛因子部分
点击文末卡片,加入会员全年无限制学习后台(MPC各矩阵的底层逻辑、MPC纵向控制、模型验证、MPC自适应巡航控制、非线性系统如何线性化及MPC动力学跟踪任何轨迹、约束添加及新求解器的求解、轨迹规划、纵向规划等80个系列)会员专享爆品课程及资源,同时获得分佣资格,可赚回自己的学费!
clc;
clear all;
%% 参考轨迹生成
N=100; %目标轨迹点的数量,首先设定目标轨迹上有100个预瞄点
T=0.05; %采样周期为0.05s
% 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)');
点击下方卡片,加入会员全年无限制学习后台(MPC各矩阵的底层逻辑、MPC纵向控制、模型验证、MPC自适应巡航控制、非线性系统如何线性化及MPC动力学跟踪任何轨迹、约束添加及新求解器的求解、轨迹规划、纵向规划等80个系列)会员专享爆品课程及资源,同时获得分佣资格,可赚回自己的学费!