github仓库

线性指的是系统是线性的，典型的线性系统的状态方程

二次型是指代价函数 $J$ 是二次型的。

其中前一项中 $N$ 表示末端时刻， $x_d$ 是系统的参考，也就是目标，这个 $J$ 是末端代价， $S$ 为末端状态的权重矩阵，是一个对角阵， $Q$ 是运行过程中的权重矩阵， $R$ 是控制量的权重矩阵， $S$ 和 $Q$ 是 $N\times N$ 的，但是 $R$ 是 $P\times P$的，如果对某个元素要求大时可以对应的将 $R$ 增大，关键的是 $S$ 和 $Q$ 都是半正定阵， $R$ 为正定阵，只有这样系统才会有最小值，即 $s≥0,q≥0,r>0$

对于一个系统来说，当 $x_d$ 为零时，该系统就是一个调节系统，不为零就是一个跟踪系统。

LQR线性二次型调节器

对应的调节二次型的 LQR 的代价函数就是

采用逆向分级的方法来进行分析

系统的状态空间方程如下

其中 $u(n-1)$ 可以使 $x(n-1)$ 转化为 $x(n)$

那么根据上式也可以得出对应的调节二次型的雅各比矩阵

其中标 * 的表示最优解

根据贝尔曼最优理论：

当 $J_{N-2→N}$为最优时 $J_{N-1→N}$一定为最优的

观察上个式子，对于每一步的最优解都是取决于最后的一部分，因为前一部分的最优是根据上一步的最优来的，所以对于每一步我们只需要求解最后一部分，而且由于 $x(k)$ 是系统状态量，所以是已知的，根本不需要考虑，因此就是求出最优的 $u(k)$ 就可以了。所以对于$J_{k→N}$的最优解求解就是直接求导，导数为0就可以得到最优控制策略，证明二阶导是一个正定矩阵，所以得到的 $u(k)$一定是最优的解。

下面将展示如何求解 $u(N-1)$，因为其他的求解方式与之相同，不再赘述

由上述可知

所以对应的雅可比矩阵可以转化为

对上式求导

解得

令 $F(N-1)=(B^TP(0)B+R)^{-1}B^TP(0)A$，原式化简为

这就是一个全状态的一个反馈控制器。

由于一阶导为0只能证明是极值，所以求解二阶导来验证一下该结果是否为最小值

由于 $R$ 是一个正定矩阵，而 $B^TP(0)B$ 是一个半正定矩阵，所以结果是正定的，因此该结果是最小值

将最优的解带入到代价函数中去，可得到最优的代价

令

上式可为

所以最终可以得到

根据递归计算就可以得到所有的状态矩阵了，其中 $F[N-k]$是反馈增益， $u[N-k]$是N-k时的最优控制量，也会得到 $p[k]$进行下一步的计算，最后需要不断递归计算得到 $u[0]$也就是需要使用的输入量。

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
clear all;
close all;
clc;

%%%%%%%%%%系统定义
%定义系统矩阵A
A = [0 1; -1 -0.5];
n = size(A, 1);
%定义系统矩阵B
B = [0; 1];
p = size(B,2);

%%%%%%%%系统离散
%离散时间步长
Ts = 0.1;
%连续系统转为离散系统 不是必须的
%sys_d = c2d(ss(A, B), Ts);
%A = sys_d.a;
%B = sys_d.b;
%%%%%%%%%%系统初始化
%初始状态 可以有多个输入值，这个输入值是一个位移和速度
x0 = [1; 0];
x = x0;
%初始输入
u0 = 2;
u = u0;
%%%%%%%%%%初始化参数
%定义运行步数
k_steps = 100;
%定义存储系统状态结果的矩阵 维度为 n * k_step
x_history = zeros(n, k_steps);
%系统状态赋值存储
x_history(:, 1) = x;%对第一次进行赋值
%定义系统输入状态存储矩阵 p*k_step
u_history = zeros(p, k_steps);
%将系统输入初始赋值
u_history(:, 1) = u;
%%%%%%%%%%权重矩阵设计
%系统状态权重矩阵
Q = [1 0; 0 1];
%系统终值权重矩阵
S = [1 0; 0 1];
%系统输入权重矩阵(单输入)
R = 1;
%p矩阵初始化
p_k = S;
%F_N矩阵计算，可以提前计算好，之后使用，F_N的结果与系统的状态无关，与系统的输入输出也无关
N = k_steps;
for k = 1: N
    F = inv(R + B'* p_k * B)*B'* p_k * A;
    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];
    end
end
%%%%%%%%%%开始仿真
for k = 1: k_steps
    %计算系统输入
    u = - F_N((k-1)*p+1:k*p, :) * x;
    %系统输入带入系统方程，计算系统响应
    x = A * x + B * u;
    %保存系统状态到预先定义矩阵的相应位置
    x_history(:, k+1) = x;
    %保存系统输入到预先定义矩阵的相应位置
    u_history(:, k) = u;
end

%%%%%%%%%%结果显示
%系统状态视图
subplot(2, 1, 1);
for i = 1: n
    plot(x_history(i, :));
    hold;
end
legend(num2str((1: n)', 'x %d'));
xlim([1, k_steps]);
grid on;
%系统输入视图
subplot(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

LQR线性二次型跟踪器

跟踪二次型系统的LQR的代价函数为

定义系统误差

跟踪器就是对 $e(k)$ 的调节器，也就是 $e_d(k)=0$

可以定义矩阵的系统状态方程

一般来说跟踪的量是不会改变的，也就是 $A_0=I$

可以用一些符号代表矩阵来简化式子

由

其中

对应的代价函数为

带入上式为

令 $S_a=C_a^TSC_a$， $Q_a=C_a^TQC_a$，得

与 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
57
58
59
60
61
62
63
64
65
66
67
68
69
70
clear all;
close all;
clc;

A = [0 1; -1 -0.5];
n = size(A,1);
B = [0; 1];
p = size(B, 2);

Ts = 0.1;

[A, B]=c2d(A,B,Ts);

x0 = [0; 0];
x = x0;
xd = [1; 0];
xa = [x; xd];

u0 = 2;
u = u0;

k_steps = 100;

x_history = zeros(n, k_steps);
x_history(:, 1) = x;
u_history = zeros(p, k_steps);
u_history(:, 1) = u;

AD = eye(n);
Ca = [eye(n) -eye(n)];
Q = [1 0; 0 1];
S = [1 0; 0 1];
R = 0.01;
Sa = Ca' * S * Ca;
Qa = Ca' * Q * Ca;
Aa = [A zeros(n); zeros(n) AD];
Ba = [B; zeros(n, 1)];
p_k = Sa;

for k = 1: k_steps
    F = inv(R + Ba'* p_k * Ba) * Ba'* p_k * Aa;
    p_k = (Aa-Ba*F)'*p_k*(Aa-Ba*F)+ (F)'*R*(F)+Qa;

    %计算系统输入
    u = -F * xa;
    %系统输入带入系统方程，计算系统响应
    x = A * x + B * u;
    xa = [x; xd];
    %保存系统状态到预先定义矩阵的相应位置
    x_history(:, k+1) = x;
    %保存系统输入到预先定义矩阵的相应位置
    u_history(:, k) = u;
end
subplot(2, 1, 1);
for i = 1: n
    plot(x_history(i, :));
    hold;
end
legend(num2str((1: n)', 'x %d'));
xlim([1, k_steps]);
grid on;
%系统输入视图
subplot(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

LQR线性二次型跟踪器——稳态非零参考值控制

对于一个系统（这是系统的状态方程的递推形式）

由于 LQR线性二次型跟踪器 在应对 R 过大时，也就是太过于注重节能的话，系统的输入就会为 0，这将导致系统不运作，所以有了这个控制模式

对于有些系统，处于某个目标状态并且稳定时，它所需要的输入/输出是一个非 0 的值，这个输入 $u_d$ 将会使系统保持稳定在目标位置，也就是

所以需要定义稳态输入误差

带入到状态方程中就是

这里的话就可以得到一个增广矩阵，其中 $x_d$ 为常数，并且定义误差矩阵

列出代价函数，前面部分的计算都是与线性二次型跟踪器是一致的，只有第二部分是不一样的，这里的代价函数为

令 $S_a=C_a^TSC_a$， $Q_a=C_a^TQC_a$，得

其中第二项就表示系统的稳态输入误差，表示输入偏离目标位置的距离

与 LQR 调节器求解过程相同，得最终结果为

LQR线性二次型跟踪器——输入增量控制

上述中 $x_d$ 是一个常数，所以这里讨论 $x_d$ 为非常数的变化，并且设置

定义一个 $u$ 的增量

这个可以使得系统的输入更加平滑，输入的变化不那么剧烈。带入到系统状态方程中

此时设置增广矩阵

并且可以得到系统的递归函数

系统的代价函数可以设计为

令 $S_a=C_a^TSC_a$， $Q_a=C_a^TQC_a$，得

与 LQR 调节器求解过程相同，得最终结果为

LQR线性二次调节器——系统输入线性化

这一部分实际上就是工程上比较常用的，将 LQR 控制器的输入 u 定义为 $u=-Ke$ 的形式，一种线性反馈控制器，最后成为一个近似于 PD 控制器

连续系统

定义连续系统代价函数，由于 $t→\infty, e→0$

设计最优控制

可以得到

其中

对应的代价函数为

带入上式为

令 $S_a=C_a^TSC_a$， $Q_a=C_a^TQC_a$，得

将 $u=-Ke$ 带入到代价函数中去

第一种解法

可以假设存在一个向量 P，使得

假设系统是稳定的（实际上需要验证），带入代价函数中得到

将 1 式左侧微分展开，并且把 $x_a$ 微分形式替换

要想式子成立，括号里必须始终为 0

这就是 $Riccati$ 方程

第二种解法

由于函数第一项是恒定的数值，所以只考虑第二项的最小值就好，也就是

引入哈密顿函数

由于我们设定 $u=-Kx_a$，所以可以知道 $\lambda=Px_a$，并且 $-(R+R^T)^{-1}B_c^TP=K$

带入上述的方程得到

联立得到

所以得到

令 $Q_1=Q_a+Q_a^T,Q_2=R+R^T$，得到

最终得到一个 $Riccati$ 方程

总结

输入系统线性化相当于是得到了一个线性的输入， $u=-Kx$ 这个 $K$ 可以通过解算 $Riccati$ 方程得到解，并且将会是一个固定的常数，所以最终的控制有点像 PD 控制器

但是 $Riccati$ 方程是一个由许多解的方程，所以不容易手算出来，所以在 matlab 中直接调用 lqr 函数就可以得到对应的 $K$