在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)
此公式迭代即可,采用下文的迭代思想(仅仅参考迭代法的思想):
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:
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:
代码资源在这里