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

2401_87299053

已于 2025-02-11 23:55:32 修改

阅读量232

点赞数 1

分类专栏： matlab 文章标签： matlab 开发语言

于 2025-02-11 23:15:03 首次发布

本文链接：https://blog.youkuaiyun.com/2401_87299053/article/details/145580220

版权

matlab 专栏收录该内容

3 篇文章

订阅专栏

%纯看视频手打 TAT 最累的一集

初始框架（未补全计算系统输入）

clear
clc

%%%%%%%%%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%
%定义系统矩阵A
A=[0 1;-1 -0.5];
%计算系统矩阵A的维度
n=size(A,1);
%定义输入矩阵B
B=[0;1];
%计算输入矩阵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是离散后的动态系统的矩阵
%%%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%%
%状态初始化
x0=[1;0];
x=x0;
%输入初始化
u0=0;
u=u0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%定义系统运行步数
k_steps=100;
%定义x_history零矩阵 用于储存系统状态结果 维度n x k_step
x_history=zeros(n,k_steps);
%将系统状态初始值赋值到x_history矩阵第一个位置
x_history(:,1)=x;
%定义u_history零矩阵，用于储存系统输入结果 维度p x k_step
u_history=zeros(p,k_steps);
%将系统输入初始值赋值到u_history矩阵第一个位置
u_history(:,1)=u;

%%%%%%%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%
%设计系统状态权重矩阵Q 维度n x n
Q=[1 0;0 1];
%设计终值权重矩阵S 维度n x n
S=[1 0;0 1];
%设计输入权重矩阵R 维度p x p
R=0.1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%仿真开始 建立for循环
for k=1:k_steps
%计算系统输入

%系统输入代入系统方程，计算系统响应
x=A*x+B*u;
%保存系统状态到预先定义矩阵的相应位置
x_history(:,k+1)=x;
%保存系统输入到预先定义矩阵的相应位置
u_history(:,k)=u;

end

%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%
%结果视图：系统状态vs运行步数
subplot(2,1,1);
for i=1:n
    plot(x_history(i,:));
    hold;
end
legend(num2str((1:n)','x %d'));
xlim([1,k_steps]);
grid on
%结果视图：系统状态vs运行步数
subplot(2,1,2);
for i=1:p
    stairs(u_history(i,:));
    hold;
end
legend(num2str((1:p)','u %d'));
xlim([1,k_steps]);
grid on

补充全之后：

clear
clc

%%%%%%%%%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%
%定义系统矩阵A
A=[0 1;-1 -0.5];
%计算系统矩阵A的维度
n=size(A,1);
%定义输入矩阵B
B=[0;1];
%计算输入矩阵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是离散后的动态系统的矩阵
%%%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%%
%状态初始化
x0=[1;0];
x=x0;
%输入初始化
u0=0;
u=u0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%定义系统运行步数
k_steps=100;
%定义x_history零矩阵 用于储存系统状态结果 维度n x k_step
x_history=zeros(n,k_steps);
%将系统状态初始值赋值到x_history矩阵第一个位置
x_history(:,1)=x;
%定义u_history零矩阵，用于储存系统输入结果 维度p x k_step
u_history=zeros(p,k_steps);
%将系统输入初始值赋值到u_history矩阵第一个位置
u_history(:,1)=u;

%%%%%%%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%
%设计系统状态权重矩阵Q 维度n x n
Q=[1 0;0 1];
%设计终值权重矩阵S 维度n x n
S=[1 0;0 1];
%设计输入权重矩阵R 维度p x p
R=0.1;

%定义末端时刻
N=k_steps;
P_k=S;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算最优反馈增益
for k=1:N
    %计算F[N-k]
    F=inv(R+B'*P_k*B)*B'*P_k*A;
    %计算P[k]
    P_k=(A-B*F)'*P_k*(A-B*F)+(F)'*R*(F)+Q;
    if k==1
        F_N=F;
    else
        F_N=[F;F_N];
    end
end


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

end

%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%
%结果视图：系统状态vs运行步数
subplot(2,1,1);
for i=1:n
    plot(x_history(i,:));
    hold;
end
legend(num2str((1:n)','x %d'));
xlim([1,k_steps]);
grid on
%结果视图：系统状态vs运行步数
subplot(2,1,2);
for i=1:p
    stairs(u_history(i,:));
    hold;
end
legend(num2str((1:p)','u %d'));
xlim([1,k_steps]);
grid on

N→inf 的时候 F[N-k]→常数，定义在代码中直接求这个常数的函数：

%输入：系统矩阵A，B 权重矩阵Q，R，S
%输出：反馈增益矩阵F
function [F]=F1_LQR_Gain(A,B,Q,R,S);
%计算系统矩阵A的维度
n=size(A,1);
%计算输入矩阵B的维度
p=size(B,2);
%系统终值代价权重矩阵 定义为P0
P0=S;
%定义最大迭代次数 用于限制程序运行时间
max_iter=200;
%初始化矩阵P为0矩阵 后续用于存放计算得到的一系列矩阵p[k]
P=zeros(n,n*max_iter);
%初始化矩阵P的第一个位置为P0
P(:,1:n)=P0;
%定义P[k-1]的初值为P0，即当k=1时，参考式（4.4.23）与（4.4.24）
P_k_min_1=P0;
%定义系统稳态误差阈值，用于判断系统是否达到稳态
tol=1e-3;
%初始化系统误差为无穷
diff=Inf;
%初始化系统反馈增益为无穷
F_N_min_k=Inf;
%初始化系统迭代步
k=1;
%判断系统是否达到稳态 即相邻步的增益差是否小于预设阈值 如达到稳态则跳出while循环
while diff>tol
    %将系统增益F[N-k]赋值给Fpre[N-k]，此步骤用于判断系统是否达到稳态
    F_N_min_k_pre=F_N_min_k;
    %计算F[N-k] 参考式（4.4.23b)
    F_N_min_k=inv(R+B'*P_k_min_1*B)*B'*P_k_min_1*A;

    %计算P[k],参考式（4.4.24b)
    P_k=(A-B*F_N_min_k)'*P_k_min_1*(A-B*F_N_min_k)+(F_N_min_k)'*R*(F_N_min_k)+Q;
    %将P[k]矩阵存入P矩阵的相应位置
    P(:,n*k-n+1:n*k)=P_k;
    %更新P[k-1]用于下一次迭代
    P_k_min_1=P_k;
    %计算系统相邻步增益差值
    diff=abs(max(F_N_min_k-F_N_min_k_pre));
        %迭代步＋1
        k=k+1;
        %如果程序超过预设最大迭代步则报错
        if k>max_iter
            error('Maximun Number of Iterations Exceeded');
        end
end
%输出系统迭代步
fprintf('No.of Interation is %d\n',k);
%模块输出：系统增益F
F=F_N_min_k;
end

此时对应的主程序代码为

clear
clc

%%%%%%%%%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%
%定义系统矩阵A
A=[0 1;-1 -0.5];
%计算系统矩阵A的维度
n=size(A,1);
%定义输入矩阵B
B=[0;1];
%计算输入矩阵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是离散后的动态系统的矩阵
%%%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%%
%状态初始化
x0=[1;0];
x=x0;
%输入初始化
u0=0;
u=u0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%定义系统运行步数
k_steps=100;
%定义x_history零矩阵 用于储存系统状态结果 维度n x k_step
x_history=zeros(n,k_steps);
%将系统状态初始值赋值到x_history矩阵第一个位置
x_history(:,1)=x;
%定义u_history零矩阵，用于储存系统输入结果 维度p x k_step
u_history=zeros(p,k_steps);
%将系统输入初始值赋值到u_history矩阵第一个位置
u_history(:,1)=u;

%%%%%%%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%
%设计系统状态权重矩阵Q 维度n x n
Q=[1 0;0 1];
%设计终值权重矩阵S 维度n x n
S=[1 0;0 1];
%设计输入权重矩阵R 维度p x p
R=0.01;

%定义末端时刻
N=k_steps;
P_k=S;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%计算最优反馈增益
for k=1:N
    %计算F[N-k]
    F=inv(R+B'*P_k*B)*B'*P_k*A;
    %计算P[k]
    P_k=(A-B*F)'*P_k*(A-B*F)+(F)'*R*(F)+Q;
    if k==1
        F_N=F;
    else
        F_N=[F;F_N];
    end
end
[F]=F1_LQR_Gain(A,B,Q,R,S);


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

end

%%%%%%%%%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%
%结果视图：系统状态vs运行步数
subplot(2,1,1);
for i=1:n
    plot(x_history(i,:));
    hold;
end
legend(num2str((1:n)','x %d'));
xlim([1,k_steps]);
grid on
%结果视图：系统状态vs运行步数
subplot(2,1,2);
for i=1:p
    stairs(u_history(i,:));
    hold;
end
legend(num2str((1:p)','u %d'));
xlim([1,k_steps]);
grid on