LQR的核心是设计QRN，并求解对应的黎卡提方程
对于连续状态空间方程系统，先求连续LQR后离散 和 先离散后求离散LQR方程 的结果 是不一样的
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:10000
S0=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<error0
break
else
S=S0;
end
i
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**-10
for 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)+Q
print(abs(S0-S).max())#控制台输出误差
if(abs(S0-S).max()<error0):
break
else:
S=S0
print(i)
K_cal=np.linalg.inv(B.T @ S @ B+R) @ (B.T @ S @ A+N.T);
python运行结果：
代码在第9999次循环结束
控制台输出abs(S0-S).max()均在e-08大小
最后计算得到的S与K:
与Matlab计算得到的一致
