clear; clc; close all;
%质点质量
m1 = 200; %locomotive weight
m2 = 80*27; %wagon weight
m3 = m2;
m4 = m3;
m5 = m4;%四节车厢为一个单元，共有27个车厢
%重力加速度
g = 9.8;
%车钩系数
k1 = 10000; %%k*相对位移（m）
k2 = 10000;
k3 = 10000;
k4 = 10000;
%基本阻力系数
c11 = 0.0062/3.6g/1000;
c12 = 0.0048/3.6g/1000;
c13 = 0.0048/3.6g/1000;
c14 = 0.0048/3.6g/1000;
c15 = 0.0048/3.6g/1000;
c01 = 1.20g/1000;
c02 = 0.92g/1000;
c03 = 0.92g/1000;
c04 = 0.92g/1000;
c05 = 0.92g/1000;
%附加阻力
fs1 = 2;
fs2 = 4;
fs3 = 4;
fs4 = 4;
fs5 = 4;
%优化指标参数
Kf = 0.01;
Ke = 0.01;
Kv = 30000;
%机车+车辆数
n = 5;
%参考速度
vr = 0;
%采样时间
Ts = 0.5;
%迭代矩阵系数组成部分
B11 = [1/m1 0 0 0 0;
0 1/m2 0 0 0 ;
0 0 1/m3 0 0 ;
0 0 0 1/m4 0 ;
0 0 0 0 1/m5 ];%表示控制输入对列车各部分速度的影响。
A11 = [c11 0 0 0 0;
0 c12 0 0 0;
0 0 c13 0 0
0 0 0 c14 0;
0 0 0 0 c15];
A12 = [-k1/m1 0 0 0 ;
k1/m2 -k2/m2 0 0;
0 k2/m3 -k3/m3 0;
0 0 k3/m4 -k4/m4;
0 0 0 k4/m5];
A21 = [1 -1 0 0 0;
0 1 -1 0 0;
0 0 1 -1 0;
0 0 0 1 -1];
A22 = zeros(n-1,n-1);
%系统矩阵参数
Ap = [-1A11 A12;A21 A22];
Bp = [B11;zeros(n-1,n)];
C = [eye(n) zeros(n,n-1);zeros(n-1,2n-1)];
%系统离散化
[A,B]=c2d(Ap,Bp,Ts);
%优化目标函数的系数矩阵
R = Keeye(n); %控制输入的权重矩阵 R
lamda = -2Kvvr[ones(1,n) zeros(1,n-1)];%与速度相关的线性项 λ
epsior = 2Ke[m1c01+fs1 m2c02+fs2 m3c03+fs3 m4c04+fs4 m5c05+fs5];%与控制输入相关的线性项 ϵ
%R：控制输入的权重矩阵，用于惩罚控制输入的大小。
%λ：与速度跟踪误差相关的线性项，用于惩罚速度偏差。
%ϵ：与控制输入相关的线性项，用于惩罚车辆的阻力。
%这些参数在模型预测控制（MPC）中用于构建二次规划问题，平衡速度跟踪、控制输入大小和能量消耗之间的关系。
Q11 = Kveye(n);
Q12 = zeros(n,n-1);
Q21 = zeros(n-1,n);
Q22 = Kf*[k12 0 0 0;0 k22 2 0;0 0 k32 0;0 0 0 k42];
%Q22 = zeros(n-1,n-1);
Q = [Q11 Q12;Q21 Q22];
% 线性系统系数矩阵
%A=[1 1; 0 1]; B=[1 0;0 0.5];
% 初始状态量-如果不能在下一步回到约束范围内，则会造成无解
x0=[0;0;0;0;0;0;0;0;0]/6.6; %从km/h到m/s
%设置采样时间
Ts = 0.5;
% 预测步长
Np = 5;
Nc = 5;
% 优化目标参数，加权矩阵
%Q=eye(2); R=eye(2); %Q为22的单位矩阵
% 转化为用控制量ut表示的，关于状态量的推导方程的矩阵
At=[]; Bt=[]; temp=[];
% 转换后的加权矩阵
Qt=[]; Rt=[];
% 声明u来保存每一步采用的控制量
u=[];
%声明u1来保存当前步所采用的控制量
u1 = zeros(n,1);
% 初始化系统状态
x=x0;
xk=x0;
% 控制量ut的初始值
u0=zeros(nNc,1); %zero(x),x维的对角矩阵，有问题
%存储每个时刻的速度
v = [];
%存储每个时刻的相对位移
L = [];
%存储每个时刻的车钩力
P= [];
S = [k1,0,0,0;
0,k2,0,0;
0,0,k3,0;
0,0,0,k4];
G = [zeros(n-1,n) S];
P = G*xk;
%每时刻的时间存储
T = [];
T(:,1) = 0;
%当前时刻的速度
v1 = 0;
%下一时刻的速度
v2 = 0;
%列车位置信息的存储
s = [];
s(:,1) = 0;
%存储每个时刻的参考速度
Vr = [];
Vr(1,1) = 0;
%存储所有时刻的实际速度和参考速度
V = [];
%lamda = -2KvVr(:,1)*[ones(1,n) zeros(1,n-1)];
%添加
E1 = eye(n,n);
E2 = eye(n-1,n-1);
O1 = zeros(n,n);
O2 = zeros(n,n-1);
O3 = zeros(n-1,n);
O4 = zeros(n-1,n-1);
Z1 = [E1 O2 O1 O2 O1 O2 O1 O2 O1 O2;
O1 O2 E1 O2 O1 O2 O1 O2 O1 O2;
O1 O2 O1 O2 E1 O2 O1 O2 O1 O2;
O1 O2 O1 O2 O1 O2 E1 O2 O1 O2;
O1 O2 O1 O2 O1 O2 O1 O2 E1 O2];
Z2 = [O3 E2 O3 O4 O3 O4 O3 O4 O3 O4;
O3 O4 O3 E2 O3 O4 O3 O4 O3 O4;
O3 O4 O3 O4 O3 E2 O3 O4 O3 O4;
O3 O4 O3 O4 O3 O4 O3 E2 O3 O4;
O3 O4 O3 O4 O3 O4 O3 O4 O3 E2];
%这段代码是模型预测控制（MPC）算法的核心部分，用于在每个时间步长 k 中计算最优控制输入并更新系统状态
%循环次数为 1000，表示总仿真时间为 1000×T
for k=1:1000
%获取参考速度
%vr = v_r(k,Ts)
[F2,Phi]=mpcgain(A,B,Nc,Np);
At = F2;
Bt = Phi;
%调用 mpcgain 函数计算 MPC 控制律的增益矩阵 F2 和 Φ
%vr = target(Np,k,s(:,k))
%vr = 0ones(Np,1)/3.6;
%Vr(:,k+1) = vr(1,1)3.6;%调用 target 函数获取当前时刻 k 的参考速度 vr。
[vr] = target(Np,k,Ts);
%lamda = -2Kvvr(1,1)[ones(1,n) zeros(1,n-1)];
%lamda = -2Kvvr[ones(1,n) zeros(1,n-1)];
[Qt,Rt,Lamda,Epsior]=mpcgain2(Q,R,epsior,Nc,Np,vr,Kv); %加上vr传参计算优化目标函数的系数矩阵
Vr(k+1,1) = vr(1,1)*3.6;%将当前时刻的参考速度存储到数组 Vr 中，并将其从 m/s 转换为 km/h。
%约束系数矩阵
I = eye(nNc);
Umax = 3000[1;0;0;0;0;1;0;0;0;0;1;0;0;0;0;1;0;0;0;0;1;0;0;0;0];%ones(nNc,1);
Umin = -3000ones(nNc,1); %定义控制输入的上下限 U max 和 U min
[Fx,Phix]=mpcgain3(A,B,Nc,Np);%调用 mpcgain3 函数，计算与状态变量相关的矩阵 F x和 Φ x
Vmax = 803.6ones(nNc,1);
Vmin = zeros(nNc,1);%速度约束
Xmax = 0.2ones((n-1)Nc,1);
Xmin = -0.2ones((n-1)Nc,1);%位移约束
Ai = [Z1Phix;-Z1Phix;Z2Phix;-Z2Phix;I;-I];
bi = [Vmax-Z1Fxxk;-Vmin+Z1Fxxk;Xmax-Z2Fxxk;-Xmin+Z2Fx*xk;Umax;-Umin];
%构造约束系数矩阵 Ai和约束边界向量 b i，用于表示优化问题中的不等式约束。
%Ai = [I;-I];
%bi = [Umax;-Umin];
%{
加权矩阵的计算过程，以及推导方程矩阵的叠加过程
for i=1:Np
At=[At; A^i];
Bt=[Bt zeros(size(Bt,1), size(B,2));
A^(i-1)*B temp];
temp=[A^(i-1)*B temp];
Qt=[Qt zeros(size(Qt,1),size(Q,1));
zeros(size(Q,1),size(Qt,1)) Q];
Rt=[Rt zeros(size(Rt,1),size(R,1));
zeros(size(R,1),size(Rt,1)) R];
end
%}
%控制量ut的上下限
%lb = -1500*[1;0;0;0;0;1;0;0;0;0;1;0;0;0;0;1;0;0;0;0;1;0;0;0;0];%ones(n*Nc,1);
%ub = 1500*[1;0;0;0;0;1;0;0;0;0;1;0;0;0;0;1;0;0;0;0;1;0;0;0;0];
% 转换后的优化目标函数矩阵，循环优化函数中H后的表达式为优化目标的另一项
% H=2*(Bt'*Qt*Bt + Rt);
H = 2*(Bt'*Qt*Bt + Rt);
F = 2*(Bt'*Qt*At*xk+(Bt'*Lamda+Epsior)/2);
% 转换后的优化中的不等式约束左边系数矩阵，后面循环中的bi为不等式右边
%Ai=[Bt; -Bt];
% 关于ut的不等式约束，实际上约束的是状态量，常数4就是状态量约束的上下边界
%bi=[5-At*xk; 5+At*xk];
% 一切准备就绪，进行二次优化
[ut, fval, exitflag]=quadprog(H,F,Ai,bi,[],[],[],[],u0); %换成F 原F=(2*xk'*At'*Qt*Bt)'
% 显示求解结果是否正常
fprintf('%d\n', exitflag)
% 采用优化得到的控制量的第一个元素作为实际作用的控制量，代入到原系统中得到下一个时刻的状态量
%u1 = ut(1,1);
%u2 = ut(2,1); % u(1+Np)
for r = 1:n
u1(r,1) = ut(r,1);
end
%k时刻速度
v1 = xk(1,1);
x(:, k+1) = A*xk + B*u1;
xk = x(:, k+1);
%k+1时刻速度
v2 = xk(1,1);
s(:,k+1) = ((v1+v2)/2)*Ts + s(:,k);
% 对优化初始值进行修改，采用预测值的后段作为下一步的初始值
u0 = [ut(n:n*Nc); ut(n*(Nc-1):n*Nc)];
%将当前时刻的输出存储
u(:,k+1) = u1;
%将当前时刻的车钩力存储
P(:, k+1) = G*xk;
%当前时刻的时间存储
T(:,k+1) = k*Ts;
%{
采用优化得到的控制量的第一个元素作为实际作用的控制量，代入到原系统中得到下一个时刻的状态量
u1(k) = ut(1);
u2(k) = ut(1+Np);
x(:, k+1) = A*x(:, k) + B*[u1(k);u2(k)];
xk = x(:, k+1);
% 对优化初始值进行修改，采用预测值的后段作为下一步的初始值
u0 = [ut(2:Np); ut(Np); ut(Np+2:2*Np); ut(2*Np)];
u(:,k+1) = [u1(k);u2(k)];
%}
end
v = x(1:n,🙂3.6; %把m/s变成km/h
L = x(n+1:2n-1,🙂;
%将速度与参考速度整合
V = [v;Vr'];
%画图（速度、车钩力、相对位移、牵引/制动力、参考速度）
%{
figure();
plot(s',v');
title('Speed');
ylabel('Speed(km/h)');
xlabel('Distance(m)');
legend('locomotive','1st wagon','2nd wagon','3rd wagon','4th wagon');
%}
figure();
plot(s',V','LineWidth',1.5);
title('Speed-Distance');
ylabel('Speed(km/h)');
xlabel('Distance(m)');
axis([0,4000,0,80]);
legend('locomotive','1st wagon','2nd wagon','3rd wagon','4th wagon','Reference Speed');
%{
figure();
plot(T',Vr');
title('Reference Speed');
ylabel('Speed(km/h)');
xlabel('Time(s)');
legend('Reference Speed');
figure();
plot(T',v');
title('Speed');
ylabel('Speed(km/h)');
xlabel('Time(s)');
legend('locomotive','1st wagon','2nd wagon','3rd wagon','4th wagon');
%}
figure();
plot(T',V','LineWidth',1.5);
title('Speed-Time');
ylabel('Speed(km/h)');
xlabel('Time(s)');
axis([0,500,0,80]);
legend('locomotive','1st wagon','2nd wagon','3rd wagon','4th wagon','Reference Speed');%图例显示了机车和各车厢的速度，以及参考速度。
figure();
plot(T',L','LineWidth',1.5);
title('Relative displacement');
ylabel('Relative displacement(m)');
xlabel('Time(s)');
legend('x_1','x_2','x_3','x_4');%绘制相对位移-时间曲线
%figure();
%plot(x');
%legend('x_1','x_2','x_3','x_4','x_5','x_6','x_7','x_8','x_9');
figure();
plot(T',u','LineWidth',1.5);
title('Force');
ylabel('Force(kN)');
xlabel('Time(s)');
legend('locomotive','1st wagon','2nd wagon','3rd wagon','4th wagon'); %图例显示了机车和各车厢的牵引力。
figure();
plot(T',P','LineWidth',1.5);
title('Coupler Force');
ylabel('Coupler Force(kN)');
xlabel('Time(s)');
legend('f_1','f_2','f_3','f_4'); %绘制车钩力-时间曲线
能不能在这个代码的基础上增加一些内容，优化模型，实现列车的模型预测控制
