1.离散代数黎卡提方程

注：LQR算法中含N项

离散系统：
在这里插入图片描述

在matlab里有现成的函数dlqr()，但为了搞清楚其内核，编写matlab代码展示其求解过程

matlab帮助文件里的dlqr()说明
在这里插入图片描述对于离散代数黎卡提方程的求解，红圈3是关键，将其中的S单独拿出，即可转化为：

S0=A'*S*A-(A'*S*B+N)*inv(B'*S*B+R)*(B'*S*A+N')+Q

其中等号左边的S0认为是S(k+1)，右边的S认为是S(k)

此公式迭代即可，采用下文的迭代思想（仅仅参考迭代法的思想）：
在这里插入图片描述

2.matlab代码

clc
clear
close all
%% 1.参数
mb=240;
mt=30;
ks=16000;
kt=160000;
A0=[0 1 0 -1;-ks/mb 0 0 0;0 0 0 1;ks/mt 0 -kt/mt 0];
B0=[0;-1/mb;0;1/mt];
G0=[0;0;-1;0];
C0=[-ks/mb 0 0 0;1 0 0 0;0 0 1 0];
E0=[-1/mb;0;0];
%离散化
SimTime=200;
sim_step = 200;
[A_Dis,B_Dis]=c2d(A0,B0,SimTime/sim_step/4);%离散化
%% 2.LQR信息
q1=100;
q2=10000;
q3=0.01;
Q=[q1+q2*(ks/mb)^2 0 0 0;0 0 0 0;0 0 q3 0;0 0 0 0];
R=q2/mb/mb;
N=[q2*ks/mb/mb;0;0;0];
%% 3.迭代法解离散代数黎卡提方程
A=A_Dis;
B=B_Dis;
S = Q - N*inv(R)*N';
error0=10^-10;
for i=1:10000S0=A'*S*A-(A'*S*B+N)*inv(B'*S*B+R)*(B'*S*A+N')+Q;error=norm((S0-S),'Inf');max(max(abs(S0-S)))if error<error0breakelseS=S0;endi
end
K_cal=inv(B'*S*B+R)*(B'*S*A+N');
%% 4.对照组
[K_fun,P_fun]=dlqr(A_Dis,B_Dis,Q,R,N);%可以看出K_cal与K_fun是一样的，说明matlab的dlqr()的内核也是这样

运行结果：
K_cal（本代码运行结果）与K_fun（matlab自带的dlqr()函数计算结果）是一致的
在这里插入图片描述

代码在4638次循环结束，误差为5.6161e-11
计算得到的S与K:
在这里插入图片描述

3.python代码

import numpy as np
#原始离散数据
mb=240
mt=30
ks=16000
kt=160000
A_Dis=np.mat([[-0.243382598182876,0.108881140243305,-1.20976052488348,-0.00276338043649671],[-7.25874268288700,-0.364358650671223,-7.07451732045390,-0.0151220065610436],[-0.120976052488348,0.0106117759806808,0.830279867651214,0.00408985243408179],[1.47380289946490,-0.120976052488348,-21.8125463151029,0.951255920139562]])
B_Dis=np.mat([[-7.77114123864297e-05],[-0.000453671417680438],[-7.56100328052176e-06],[9.21126812165565e-05]])
#LQR数据
q1=100
q2=10000
q3=0.01
Q=np.mat([[q1+q2*(ks/mb)**2,0,0,0],[0,0,0,0],[0,0,q3,0],[0,0,0,0]])
R=q2/mb/mb
N=np.mat([[q2*ks/mb/mb],[0],[0],[0]])
#迭代法解黎卡提方程
A=A_Dis
B=B_Dis
S = Q - N / R @N.T
error0=10**-10for i in range(1,10000):S0=A.T @ S @ A-(A.T @ S @ B+N) @ np.linalg.inv(B.T @ S @ B+R) @ (B.T @ S @ A+N.T)+Qprint(abs(S0-S).max())#控制台输出误差if(abs(S0-S).max()<error0):breakelse:S=S0
print(i)
K_cal=np.linalg.inv(B.T @ S @ B+R) @ (B.T @ S @ A+N.T);