最优控制-LQR-drcan-3

一、理论知识

视频教程链接：drcan老师

LQR的全称是线性二次型调节器（Linear Quadratic Regulator）
Linear线性，指的是LQR需要一个线性模型，这个模型通常用一组动态方程和输出方程描述系统。如果一个系统满足线性定理，那么它所遵循的动态可以表示为线性方程。其中，“线性”一词指的是，当每个输入变量与某个参数按比例变化时，输出变量也会相应按比例变化

Quadratic二次，是指二次代价函数的意思。在LQR控制的过程中，通过设计一个二次型的代价函数来描述控制系统的性能指标，并通过最小化该二次型代价函数来实现系统的优化调节。

LQR不是一种控制器？
PID（比例-积分-微分）控制器是一种经典的反馈控制器，它本身就是一种控制器。而LQR（线性二次型调节）是进行最优控制设计的一种方法，在其中，我们需要使用数学工具帮助我们计算出一个反馈矩阵来实现最优的状态反馈控制

龙胆也老师知乎

image-20231204191730374

image-20231204191742390

image-20231204191753402

image-20231204191759465

image-20231204191810897

二、编程实现

1.Code 1–迭代方法

通过对模型进行建模，得到反馈增益矩阵，编程实现控制过程

LQR_Gain.m

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
%% 通过迭代的方法实现LQR控制---当迭代次数非常大的时候，得到的系统反馈增益矩阵会趋于常数矩阵
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;
%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统矩阵A
A = [0 1;-1 -0.5];
% 计算系统矩阵A 的维度(1-->获得行数)
n = size(A,1);
% 定义输入矩阵B
B = [0;1];
% 计算输入矩阵B 的维度（2-->获得列数）
p = size(B,2);
%%%%%%%%%%%%%%%%%%%%%%%% 系统离散 %%%%%%%%%%%%%%%%%%%%%%%%
%% 离散时间步长
Ts = 0.1;
%% 连续系统转离散系统
% ss 函数用于创建状态空间模型,一般需要四个矩阵 A系统 B输入 C输出 D直通;   C矩阵需要和A矩阵的列数一致
C = zeros(1,size(A,2));
D = 0;
sys = ss(A,B,C,D);
% c2d 函数用于将连续时间系统的状态空间模型、传递函数或零极点增益模型转换为离散时间模型
sys_d = c2d(sys,Ts);
%% 提取离散系统A矩阵
A = sys_d.a;
%% 提取离散系统B矩阵
B = sys_d.b;
%%%%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 状态初始化
x0 = [1;0];
x = x0;
% 输入初始化
u0 = 0;
u = u0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵,用于存储系统状态结果,维度 n x k_step 
x_history = zeros(n,k_steps); % 状态变量维度为 nx1的
% 将系统状态初始值赋值到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, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = 0.1;

% 定义末端时刻
N = k_steps;
P_k = S;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算最优反馈增益
% 计算是从k=1-->k=n顺着计算---但是使用F[N-k]是是逆着使用（使用逆向分级解析求解---J(N-1-->N)代价中的F[N-1]最先求得）
for k = 1:N
    % 计算F[N-k]---F的维度是 pxn 的
    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];   % 将每次求解得到的F[N-k]逆着存储为一个矩阵,后面就可以顺着使用 ';' 表示矩阵中新增加一行
    end
end



% 仿真开始，建立for循环
for k = 1 : k_steps
    % 计算系统输入--所以每次提取 F_N 矩阵中的 (k-1)*p+1:k*p 行中的矩阵块 作为每一次的反馈增益
    u = -F_N((k-1)*p+1:k*p,:)* x;
    % 系统输入带入系统方程,计算系统响应
    x = A * x + B * u;
    % 保存系统状态到预先定义矩阵的相应位置
    x_history(:,k+1) = x;
    % 保存系统输入到预先定义矩阵的相应位置
    u_history(:,k+1) = u;
end

%%%%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
%% 结果视图：系统状态vs.运行步数
% subplot 函数用于在单个图窗中创建多个子图。这允许你在同一窗口中并排显示多个不同的图表
% subplot(m, n, p)---- m 和 n 分别指定了子图网格的行数和列数 ---- p 指定了当前激活的子图的位置。
subplot(2,1,1)  % 单个图窗显示2*1个子图 该图为第1个子图
for i  = 1:n
    plot(x_history(i,:));
    hold;
end
% legend 函数用于向图表添加图例，以帮助识别图表中的各个数据系列
% num2str 函数用于将数字转换为字符串  --- 'x %d' 格式化字符串
legend(num2str((1:n)','x %d'));
% xlim 函数用于设置或查询当前坐标轴的 x 轴的界限。这个函数非常有用，当你需要调整坐标轴的显示范围或者了解当前的显示范围时
% 你可以直接指定 x 轴的最小值和最大值来设置界限 --- xlim([xmin xmax])
xlim([1,k_steps]);
grid on;

%% 结果视图：系统输入vs.运行步数
subplot(2,1,2)  % 单个图窗显示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;

运行结果

模型的目标状态为0，最后使得系统从初始状态趋于0状态

image-20231204192408227

2.Code 2–LQR封装函数

F1_LQR_Gain().m

迭代次数非常大的时候，可以得到LQR实现过程中的反馈增益矩阵，会趋近一个常数矩阵，将LQR控制方法封装为一个函数，得到最后的常数矩阵

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
%% 将LQR控制封装为函数，最后迭代得到常数矩阵
%% 输入：系统矩阵 A，B； 权重矩阵 Q R S
%% 输出：反馈增益矩阵
function [F] = F1_LQR_Gain(A,B,Q,R,S);

% 计算系统矩阵维度 n
n = size(A,1); % 行
% 计算输入矩阵维度 p
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时
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]
    F_N_min_k = inv(R+B'*P_k_min_1*B)*B'*P_k_min_1*A;
    % 计算P[k]
    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("Maximum Number of Iterations Exceeded");
    end
end

% 输出系统迭代步
fprintf('No. of Interation is %d \n',k);
% 模块输出：系统增益F
F = F_N_min_k;
end

3.Code 3 – 函数测试（单个输入）

LQR_Test_Function.m

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
%% 测试反馈增益矩阵函数--单输入
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;
%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统矩阵A
A = [0 1;-1 -0.5];
% 计算系统矩阵A 的维度(1-->获得行数)
n = size(A,1);
% 定义输入矩阵B
B = [0;1];
% 计算输入矩阵B 的维度（2-->获得列数）
p = size(B,2);
%%%%%%%%%%%%%%%%%%%%%%%% 系统离散 %%%%%%%%%%%%%%%%%%%%%%%%
%% 离散时间步长
Ts = 0.1;
%% 连续系统转离散系统
% ss 函数用于创建状态空间模型,一般需要四个矩阵 A系统 B输入 C输出 D直通;   C矩阵需要和A矩阵的列数一致
C = zeros(1,size(A,2));
D = 0;
sys = ss(A,B,C,D);
% c2d 函数用于将连续时间系统的状态空间模型、传递函数或零极点增益模型转换为离散时间模型
sys_d = c2d(sys,Ts);
%% 提取离散系统A矩阵
A = sys_d.a;
%% 提取离散系统B矩阵
B = sys_d.b;
%%%%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 状态初始化
x0 = [1;0];
x = x0;
% 输入初始化
u0 = 0;
u = u0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵,用于存储系统状态结果,维度 n x k_step 
x_history = zeros(n,k_steps); % 状态变量维度为 nx1的
% 将系统状态初始值赋值到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, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = 0.1;

% 定义末端时刻
N = k_steps;
P_k = S;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算最优反馈增益
% 调用函数得到常数反馈增益矩阵
[F] = F1_LQR_Gain(A,B,Q,R,S);
% 仿真开始，建立for循环
for k = 1 : k_steps
    % 计算系统输入--所以每次提取 F_N 矩阵中的 (k-1)*p+1:k*p 行中的矩阵块 作为每一次的反馈增益
    u = - F * x;
    % 系统输入带入系统方程,计算系统响应
    x = A * x + B * u;
    % 保存系统状态到预先定义矩阵的相应位置
    x_history(:,k+1) = x;
    % 保存系统输入到预先定义矩阵的相应位置
    u_history(:,k+1) = u;
end

%%%%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
%% 结果视图：系统状态vs.运行步数
% subplot 函数用于在单个图窗中创建多个子图。这允许你在同一窗口中并排显示多个不同的图表
% subplot(m, n, p)---- m 和 n 分别指定了子图网格的行数和列数 ---- p 指定了当前激活的子图的位置。
subplot(2,1,1)  % 单个图窗显示2*1个子图 该图为第1个子图
for i  = 1:n
    plot(x_history(i,:));
    hold;
end
% legend 函数用于向图表添加图例，以帮助识别图表中的各个数据系列
% num2str 函数用于将数字转换为字符串  --- 'x %d' 格式化字符串
legend(num2str((1:n)','x %d'));
% xlim 函数用于设置或查询当前坐标轴的 x 轴的界限。这个函数非常有用，当你需要调整坐标轴的显示范围或者了解当前的显示范围时
% 你可以直接指定 x 轴的最小值和最大值来设置界限 --- xlim([xmin xmax])
xlim([1,k_steps]);
grid on;

%% 结果视图：系统输入vs.运行步数
subplot(2,1,2)  % 单个图窗显示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;

运行结果

image-20231204192846600

4.Code 4 – 函数测试（多个输入）

LQR_Test_Function_M_Input.m

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
%% 测试反馈增益矩阵函数--多输入
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;
%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统矩阵A
A = [0 1;-1 -0.5];
% 计算系统矩阵A 的维度(1-->获得行数)
n = size(A,1);
% 定义输入矩阵B
B = [0 0.2;-0.1 0.5];
% 计算输入矩阵B 的维度（2-->获得列数）
p = size(B,2);
%%%%%%%%%%%%%%%%%%%%%%%% 系统离散 %%%%%%%%%%%%%%%%%%%%%%%%
%% 离散时间步长
Ts = 0.1;
%% 连续系统转离散系统
% ss 函数用于创建状态空间模型,一般需要四个矩阵 A系统 B输入 C输出 D直通;   C矩阵需要和A矩阵的列数一致
C = zeros(1,size(A,2));
D = 0;
sys = ss(A,B,C,D);
% c2d 函数用于将连续时间系统的状态空间模型、传递函数或零极点增益模型转换为离散时间模型
sys_d = c2d(sys,Ts);
%% 提取离散系统A矩阵
A = sys_d.a;
%% 提取离散系统B矩阵
B = sys_d.b;
%%%%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 状态初始化
x0 = [1;0];
x = x0;
% 输入初始化
u0 = 0;
u = u0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵,用于存储系统状态结果,维度 n x k_step 
x_history = zeros(n,k_steps); % 状态变量维度为 nx1的
% 将系统状态初始值赋值到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, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = [0.1 0;0,0.1];

% 定义末端时刻
N = k_steps;
P_k = S;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算最优反馈增益
% 调用函数得到常数反馈增益矩阵
[F] = F1_LQR_Gain(A,B,Q,R,S);
% 仿真开始，建立for循环
for k = 1 : k_steps
    % 计算系统输入--所以每次提取 F_N 矩阵中的 (k-1)*p+1:k*p 行中的矩阵块 作为每一次的反馈增益
    u = - F * x;
    % 系统输入带入系统方程,计算系统响应
    x = A * x + B * u;
    % 保存系统状态到预先定义矩阵的相应位置
    x_history(:,k+1) = x;
    % 保存系统输入到预先定义矩阵的相应位置
    u_history(:,k+1) = u;
end

%%%%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
%% 结果视图：系统状态vs.运行步数
% subplot 函数用于在单个图窗中创建多个子图。这允许你在同一窗口中并排显示多个不同的图表
% subplot(m, n, p)---- m 和 n 分别指定了子图网格的行数和列数 ---- p 指定了当前激活的子图的位置。
subplot(2,1,1)  % 单个图窗显示2*1个子图 该图为第1个子图
for i  = 1:n
    plot(x_history(i,:));
    hold;
end
% legend 函数用于向图表添加图例，以帮助识别图表中的各个数据系列
% num2str 函数用于将数字转换为字符串  --- 'x %d' 格式化字符串
legend(num2str((1:n)','x %d'));
% xlim 函数用于设置或查询当前坐标轴的 x 轴的界限。这个函数非常有用，当你需要调整坐标轴的显示范围或者了解当前的显示范围时
% 你可以直接指定 x 轴的最小值和最大值来设置界限 --- xlim([xmin xmax])
xlim([1,k_steps]);
grid on;

%% 结果视图：系统输入vs.运行步数
subplot(2,1,2)  % 单个图窗显示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;

运行结果

image-20231204192943154