最优控制-LQR轨迹追踪-Drcan-4

一、目标误差控制

1.建模

image-20231204215846670

image-20231204215855697

image-20231204215901814

image-20231204215907148

2.编程实现

LQR_Test_tracking_E_offset_MSD.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
116
117
118
119
120
%% 将轨迹追踪问题转换为 LQR问题
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统参数
m_sys = 1;
% 定义阻尼系数
b_sys = 0.5;
% 定义弹簧弹性系数
k_sys = 1;

%% %%%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构建系统矩阵 A ,nxn
A = [0 1 ;-k_sys/m_sys -b_sys/m_sys];
% 计算A矩阵维度
n = size(A,1);
% 构建输入矩阵B ，nxp
B = [0;1/m_sys];
% 计算输入矩阵维度
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;


%% %%%%%%%%%%%%%%%%%%%%%% 权重设计 %%%%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵Q, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = 1;

%% %%%%%%%%%%%%%%%%%%%%%% 系统参考值 %%%%%%%%%%%%%%%%%%%%%%%%
% 系统目标状态参考值
xd = [1;0];
% 构建目标转移矩阵
AD = eye(n);  % 状态向量为n维

%% %%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 初始化系统状态
x0 = [1;0];
% 初始化状态赋值
x = x0;
% 构建初始化增广矩阵
[xa] = [x;xd]; 
% 系统输入初始化
u = 0;

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 100;
% 定义x_history零矩阵,用于存储系统状态结果,维度 n x k_step 
x_history = zeros(n,k_steps); % 状态变量维度为 nx1的
% 定义u_history零矩阵,用于存储系统输入结果,维度 p x k_step
u_history = zeros(p,k_steps);

% 构建增广矩阵Ca
Ca = [eye(n) -eye(n)];
% 构建增广矩阵Aa
Aa = [A zeros(n);zeros(n) AD];
% 构建增广矩阵Ba
Ba = [B;zeros(n,1)];
% 构建增广矩阵Sa
Sa = transpose(Ca) * S * Ca;
% 构建增广矩阵Qa
Qa = transpose(Ca) * Q * Ca;

%% %%%%%%%%%%%%%%%%%%%%%% 调用F1_LQR_Gain()函数 %%%%%%%%%%%%%%%%%%%%%%%%
[F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa);

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

%% %%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
% figure(1,'position',[150 150 500 500]);
% 状态变量结果图
subplot(2,1,1);
hold;
% 系统状态x1结果图,质量块位移
plot(0:length(x_history)-1,x_history(1,:));
% 系统状态x2结果图,质量块位移
plot(0:length(x_history)-1,x_history(2,:),'--');
grid on;
legend("x1","x2");

subplot(2,1,2);
for i  = 1:p
    stairs(u_history(i,:));
    hold;
end
grid on;
legend("u");

其中

F1_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
%% 将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

运行结果

系统在保证能耗小的情况下存在稳态误差，无法在满足性能指标足够好的情况下，同时保证目标的精确控制

image-20231204220016090

二、稳态非零参考值控制

在(一)中，存在一个问题（Q1），性能指标函数中与权重矩阵R有关的是系统的直接输入 u[k],若权重矩阵R过大的时候，u[k]则会躺平，甚至趋于0，无法在保证性能指标足够好的情况下，同时又对目标进行精确的跟踪

为了解决这个问题，引出如下的控制方法：

1.建模

image-20231206104151574

image-20231206104219856

image-20231206104228515

image-20231206104253378

2.编程实现

LQR_Test_tracking_SS_U_MSD.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
116
117
118
%% 将轨迹追踪问题转换为 LQR问题
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统参数
m_sys = 1;
% 定义阻尼系数
b_sys = 0.5;
% 定义弹簧弹性系数
k_sys = 1;

%% %%%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构建系统矩阵 A ,nxn
A = [0 1 ;-k_sys/m_sys -b_sys/m_sys];
% 计算A矩阵维度
n = size(A,1);
% 构建输入矩阵B ，nxp
B = [0;1/m_sys];
% 计算输入矩阵维度
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;


%% %%%%%%%%%%%%%%%%%%%%%% 权重设计 %%%%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵Q, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = 1;

%% %%%%%%%%%%%%%%%%%%%%%% 系统参考值 %%%%%%%%%%%%%%%%%%%%%%%%
% 系统目标状态参考值
xd = [1;0];
% 构建目标转移矩阵
AD = eye(n);  % 状态向量为n维

%% %%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 初始化系统状态
x0 = [0;0];
% 初始化状态赋值
x = x0;
% 构建初始化增广矩阵
[xa] = [x;xd]; 
% 系统输入初始化
u0 = 1;
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(:,1) = u;

% 调用模块[F2],计算系统增广矩阵Aa,Ba,Qa,Sa,R以及目标输入ud
[Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd);

%% %%%%%%%%%%%%%%%%%%%%%% 调用F1_LQR_Gain()函数 %%%%%%%%%%%%%%%%%%%%%%%%
[F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa);

%% %%%%%%%%%%%%%%%%%%%%%% 仿真开始 %%%%%%%%%%%%%%%%%%%%%%%%
% 仿真开始，建立for循环
for k = 1 : k_steps
    % 计算系统输入
    u = -F * xa + ud;
    % 系统输入带入系统方程,计算系统响应--将得到的u作用到原系统中
    x = A * x + B * u;
    % 更新增广矩阵xa
    xa = [x;xd];
    % 保存系统状态到预先定义矩阵的相应位置
    x_history(:,k+1) = x;
    % 保存系统输入到预先定义矩阵的相应位置
    u_history(:,k+1) = u;
end

%% %%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
% 通过 figure 函数的参数来自定义窗口的属性，例如窗口大小、位置、标题等。position表示位置 后面的数组表示窗口大小
figure('position',[150 150 1000 1000]);
% 状态变量结果图
subplot(2,1,1);
hold;
% 系统状态x1结果图,质量块位移
plot(0:length(x_history)-1,x_history(1,:));
% 系统状态x2结果图,质量块位移
plot(0:length(x_history)-1,x_history(2,:),'--');
grid on;
legend("x1","x2");

subplot(2,1,2);
for i  = 1:p
    stairs(u_history(i,:));
    hold;
end
grid on;
legend("u");

其中

F2_InputAugmentMatrix_SS_U.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

%% 输入: 系统矩阵 A,B 权重矩阵:Q,R,S;  稳态目标状态: xd
%% 输出: 增广矩阵 Aa,Ba,Sa,Qa.  权重矩阵 R ，稳态控制输入 ud
function [Aa,Ba,Qa,Sa,R,ud] = F2_InputAugmentMatrix_SS_U(A,B,Q,R,S,xd);
    % 计算系统矩阵维度,n
    n = size(A,1);
    % 计算输入矩阵维度
    p = size(B,2);
    % 构建增广矩阵Ca
    Ca = [eye(n) -eye(n)];
    % 构建增广矩阵Aa
    Aa = [A eye(n)-A;zeros(n) eye(n)];
    % 构建增广矩阵Ba
    Ba = [B;zeros(n,p)];
    % 构建增广矩阵Sa
    Sa = transpose(Ca) * S * Ca;
    % 构建增广矩阵Qa
    Qa = transpose(Ca) * Q * Ca;
    % 设计权重矩阵R。此处R不变化
    R = R;
    % 计算稳态控制输入ud
    % mldivide 函数(求解线性方程组)，通常通过反斜杠 \ 表示，是用于解决线性方程组的一种非常有效的方法。它被用于计算线性方程组 Ax = B 的解，其中 A 是一个矩阵，x 是一个未知向量，而 B 是一个已知向量或矩阵
    ud = mldivide(B,(eye(n) - A)*xd);
end
（1）运行结果

对于测试1，R=0.1,对于输入的功耗关注比重不大，因此系统超调较小

image-20231206105216822

对于测试2，R=1,对于输入的功耗关注比重较大，因此系统超调较大，典型的欠阻尼系统

image-20231206105317664

三、输入增量控制

在(二、稳态非零参考值控制)中，目标状态为一个常量定值Xd,但是若Xd非定值呢？如何解决

1.建模

image-20231206162629971

image-20231206162658016

image-20231206162706765

image-20231206162713324

image-20231206162720531

image-20231206162727433

image-20231206162735728

image-20231206162743488

2.编程实现

F3_InputAugmentMatrix_Delta_U.m

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

%% 输入: 系统矩阵 A,B 权重矩阵:Q,R,S;  目标转移矩阵:AD
%% 输出: 增广矩阵 Aa,Ba,Sa,Qa.  权重矩阵 R 
function [Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD);
    % 计算系统矩阵维度,n
    n = size(A,1);
    % 计算输入矩阵维度
    p = size(B,2);
    % 构建增广矩阵Ca
    Ca = [eye(n) -eye(n) zeros(n,p)];
    % 构建增广矩阵Aa
    Aa = [A zeros(n) B;zeros(n) AD zeros(n,p); zeros(p,n) zeros(p,n) eye(p,p)];
    % 构建增广矩阵Ba
    Ba = [B;zeros(n,p);eye(p)];
    % 构建增广矩阵Sa
    Sa = transpose(Ca) * S * Ca;
    % 构建增广矩阵Qa
    Qa = transpose(Ca) * Q * Ca;
    % 设计的权重矩阵R，不发生改变此处
    R = R;
end
（1）目标状态xd仍为常量

LQR_Test_tracking_Delta_U_MSD_1.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
116
117
118
119
120
121
122
123
%% 将轨迹追踪问题转换为 LQR问题
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统参数
m_sys = 1;
% 定义阻尼系数
b_sys = 0.5;
% 定义弹簧弹性系数
k_sys = 1;

%% %%%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构建系统矩阵 A ,nxn
A = [0 1 ;-k_sys/m_sys -b_sys/m_sys];
% 计算A矩阵维度
n = size(A,1);
% 构建输入矩阵B ，nxp
B = [0;1/m_sys];
% 计算输入矩阵维度
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;


%% %%%%%%%%%%%%%%%%%%%%%% 权重设计 %%%%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵Q, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = 2;

%% %%%%%%%%%%%%%%%%%%%%%% 系统参考值 %%%%%%%%%%%%%%%%%%%%%%%%
% 常数目标状态
% 系统目标状态参考值
xd = [1;0];
% 构建目标转移矩阵
AD = eye(n);  % 状态向量为n维

%% %%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 初始化系统状态
x0 = [0;0];
% 初始化状态赋值
x = x0;
% 系统输入初始化
u0 = 0;
u = u0;
% 构建初始化增广矩阵
[xa] = [x;xd;u]; 

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
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(:,1) = u;

% 调用模块[F3],计算系统增广矩阵Aa,Ba,Qa,Sa,R
[Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD);

%% %%%%%%%%%%%%%%%%%%%%%% 调用F1_LQR_Gain()函数 %%%%%%%%%%%%%%%%%%%%%%%%
% 计算系统反馈增益
[F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa);

%% %%%%%%%%%%%%%%%%%%%%%% 仿真开始 %%%%%%%%%%%%%%%%%%%%%%%%
% 仿真开始，建立for循环
for k = 1 : k_steps
    % 计算系统输入增量
    Delta_u = -F * xa;
    % 计算系统输入
    u = Delta_u + u;
    % 系统输入带入系统方程,计算系统响应--将得到的u作用到原系统中
    x = A * x + B * u;
    % 更新系统增广状态
    xd = AD * xd;
    xa = [x;xd;u]; % x xd 均是k+1时刻的值 而 u是k时刻的值
    % 保存系统状态到预先定义矩阵的相应位置
    x_history(:,k+1) = x;
    % 保存系统输入到预先定义矩阵的相应位置
    u_history(:,k) = u;
end

%% %%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
% 通过 figure 函数的参数来自定义窗口的属性，例如窗口大小、位置、标题等。position表示位置 后面的数组表示窗口大小
figure('position',[150 150 1000 1000]);
% 状态变量结果图
subplot(2,1,1);
hold;
% 系统状态x1结果图,质量块位移
plot(0:length(x_history)-1,x_history(1,:));
% 系统状态x2结果图,质量块位移
plot(0:length(x_history)-1,x_history(2,:),'--');
grid on;
legend("x1","x2");

subplot(2,1,2);
for i  = 1:p
    stairs(u_history(i,:));
    hold;
end
grid on;
legend("u");

运行结果：

对于测试1，R = 0.1时，输入u[k]的变化幅度较大

image-20231206163309803

对于测试2，R =1时，输入u[k]的变化幅度较小

image-20231206163344033

（2）若目标状态非常数参考值，系统状态非匀速变化

LQR_Test_tracking_Delta_U_MSD_2.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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
%% 将轨迹追踪问题转换为 LQR问题
%% 程序初始化，清空工作空间，缓存
clear all;
close all;
clc;

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统参数
m_sys = 1;
% 定义阻尼系数
b_sys = 0.5;
% 定义弹簧弹性系数
k_sys = 1;

%% %%%%%%%%%%%%%%%%%%%%%%%%%% 系统定义 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构建系统矩阵 A ,nxn
A = [0 1 ;-k_sys/m_sys -b_sys/m_sys];
% 计算A矩阵维度
n = size(A,1);
% 构建输入矩阵B ，nxp
B = [0;1/m_sys];
% 计算输入矩阵维度
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;


%% %%%%%%%%%%%%%%%%%%%%%% 权重设计 %%%%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵Q, 维度 nxn
Q = [1 0;0 1];
% 设计系统终值权重矩阵S，维度 nxn
S = [1 0;0 1];
% 设计系统输入权重矩阵S，维度 pxp
R = 1;

%% %%%%%%%%%%%%%%%%%%%%%% 系统参考值 %%%%%%%%%%%%%%%%%%%%%%%%
% 非常数 目标状态
xd = [0;0.2];
% 构建目标转移矩阵
AD = c2d(ss([0 1;0 0],[0;0],[0 0],[0]),0.1).a;  % 状态向量为n维

%% %%%%%%%%%%%%%%%%%%%%%% 系统初始化 %%%%%%%%%%%%%%%%%%%%%%%%
% 初始化系统状态
x0 = [0;0];
% 初始化状态赋值
x = x0;
% 系统输入初始化
u0 = 0;
u = u0;
% 构建初始化增广矩阵
[xa] = [x;xd;u]; 

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 200;
% 定义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(:,1) = u;

% 调用模块[F3],计算系统增广矩阵Aa,Ba,Qa,Sa,R
[Aa,Ba,Qa,Sa,R] = F3_InputAugmentMatrix_Delta_U(A,B,Q,R,S,AD);

%% %%%%%%%%%%%%%%%%%%%%%% 调用F1_LQR_Gain()函数 %%%%%%%%%%%%%%%%%%%%%%%%
% 计算系统反馈增益
[F] = F1_LQR_Gain(Aa,Ba,Qa,R,Sa);

%% %%%%%%%%%%%%%%%%%%%%%% 仿真开始 %%%%%%%%%%%%%%%%%%%%%%%%
% 仿真开始，建立for循环
for k = 1 : k_steps
    % 系统目标状态不再是常数，if else 语句定义不同阶段的系统目标状态
    if(k==50)
        xd = [xd(1);-0.2];
    elseif(k==100)
        xd = [xd(1);0.2];
    elseif(k==150)
        xd = [xd(1);-0.2];
    elseif(k==200)
        xd = [xd(1);0.2];
    end
    % 计算系统输入增量
    Delta_u = -F * xa;
    % 计算系统输入
    u = Delta_u + u;
    % 系统输入带入系统方程,计算系统响应--将得到的u作用到原系统中
    x = A * x + B * u;
    % 更新系统增广状态
    xd = AD * xd;
    xa = [x;xd;u]; % x xd 均是k+1时刻的值 而 u是k时刻的值
    % 保存系统状态到预先定义矩阵的相应位置
    x_history(:,k+1) = x;
    % 保存系统输入到预先定义矩阵的相应位置
    u_history(:,k) = u;
end

%% %%%%%%%%%%%%%%%%%%%%%% 结果 %%%%%%%%%%%%%%%%%%%%%%%%
% 通过 figure 函数的参数来自定义窗口的属性，例如窗口大小、位置、标题等。position表示位置 后面的数组表示窗口大小
figure('position',[150 150 1000 1000]);
% 状态变量结果图
subplot(2,1,1);
hold;
% 系统状态x1结果图,质量块位移
plot(0:length(x_history)-1,x_history(1,:));
% 系统状态x2结果图,质量块位移
plot(0:length(x_history)-1,x_history(2,:),'--');
grid on;
legend("x1","x2");

subplot(2,1,2);
for i  = 1:p
    stairs(u_history(i,:));
    hold;
end
grid on;
legend("u");

运行结果

可以看到x2在0.2 -0.2 0.2 -0.2 之间变化 控制输入也在变化使得系统对状态轨迹进行跟踪

image-20231206172201918