【dr.can】最优控制第七节代码

clear
clc
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%定义系统参数
%定义质量块质量
m_sys=1;
%定义阻尼系数
b_sys=0.5;
%定义弹簧弹性系数
k_sys=1;
%%%%%%%%%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%
%定义系统矩阵A,n x n
A=[0 1;-k_sys/m_sys -b_sys/m_sys];
%计算系统矩阵A的维度
n=size(A,1);
%定义输入矩阵B,n x p
B=[0 ;1/m_sys];
%计算输入矩阵B的维度
p=size(B,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%系统离散%%%%%%%%%
%离散系统步长
Ts=0.1;
%连续系统转离散系统
C=[1 0];
D=0;% C D 矩阵是根据系统输出和状态变量、输入之间的关系来的，y=Cx+Du，这个系统里y=x(t)，
% %所以C矩阵应该是【1 0】，D是0矩阵
sys_d=c2d(ss(A,B,C,D),Ts);
%提取离散系统A、B矩阵
A=sys_d.a;
B=sys_d.b;
%[A_d,B_d]=c2d(A,B,Ts);%A_d和B_d是离散后的动态系统的矩阵
%%%%%%%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%
%设计系统状态权重矩阵Q 维度n x n
Q=[1 0;0 1];
%设计终值权重矩阵S 维度n x n
S=[1 0;0 1];
%设计输入权重矩阵R 维度p x p
R=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%
%系统状态参考值
xd=[1;0];
%构建目标转移矩阵 参考式（4.5.4)
AD=eye(n);

%%%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%%
%状态初始化
x0=[0;0];
x=x0;
%构建初始化增广矩阵
xa=[x;xd];
%系统输入初始化
u=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%定义系统运行步数
k_steps=100;
%定义x_history零矩阵 用于储存系统状态结果 维度n x k_step
x_history=zeros(n,k_steps);
%定义u_history零矩阵，用于储存系统输入结果 维度p x k_step
u_history=zeros(p,k_steps);

%构建增广矩阵ca
Ca=[eye(n) -eye(n)];
%构建增广矩阵Aa
Aa=[A zeros(n);zeros(n) AD]
%构建增广矩阵Ba
Ba=[B;zeros(n,1)];
%构建增广矩阵Sa
Sa=transpose(Ca)*S*Ca;
%构建增广矩阵Qa
Qa=transpose(Ca)*Q*Ca;

%读取模块[F1]计算系统反馈增益F
[F]=F1_LQR_Gain(Aa,Ba,Qa,R,Sa);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%仿真开始 建立for循环
for k=1:k_steps
%计算系统输入
u=-F*(xa);
%系统输入代入系统方程，计算系统响应
x=A*x+B*u;
%更新增广状态xa
xa=[x;xd];
%保存系统状态到预先定义矩阵的相应位置
x_history(:,k+1)=x;
%保存系统输入到预先定义矩阵的相应位置
u_history(:,k)=u;

end

%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%
% 通过 figure 函数的参数来自定义窗口的属性，例如窗口大小、位置、标题等。
% position表示位置 后面的数组表示窗口大小
figure('Position',[150 150 500 500]);
%状态变量结果图
subplot(2,1,1);
hold;
%系统状态x1结果图 质量块位移
plot(0:length(x_history)-1,x_history(1,:));
%%系统状态x2结果图 质量块速度
plot(0:length(x_history)-1,x_history(2,:),'--');
grid on
legend("x1","x2")

subplot(2,1,2);
for i  = 1:p
    stairs(u_history(i,:));
    hold;
end
grid on;
legend("u");

参考网站

GitHub - Teddy-Liao/Beauty-of-Control-using-Matlab: 复制了《控制之美卷2》书籍提供的Octave源代码，并用Matlab进行部分复现。

最优控制 - 动态规划 - drcan-4 | Maxswordsman