【LQR】离散代数黎卡提方程的求解，附Matlab/python代码（笔记）

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计算得到的一致

代码资源在这里