[LQR简要快速入门]+[一级倒立摆的LQR控制]

1. 什么是LQR

LQR是一种最优控制算法，简要讲即为寻求一种算法，使得在满足系统稳定性能的同时，系统在达到稳定的过程中消耗的能量也最少（具有实际意义）。
利用最优控制理论的知识可以知道，既然要达到两个指标（1. 性能；2. 能量）的最优，可以很容易列出积分形式的最优指标：
$$J={\int }_0^{\infty }(x^{T}Qx+u^{T}Ru)dt\text{\hspace{1em}(1)}$$（有关于最优控制理论的部分，以后慢慢补齐博客，这里不影响理解）
一般地，最优指标$J$的选取有三种：拉格朗日型(a)，迈耶尔型(b)，波尔茨型(d)，三者形式如下：
$$J={\int }_{t_0}^{t_k}F\left(t,\vec{x},\dot{\vec{x}},\vec{u}\right)dt\text{\hspace{1em}(a)}$$ $$J=\Phi \left(\vec{x}(t_0),\vec{x}(t_k)\right)\text{\hspace{1em}(b)}$$ $$J={\int }_{t_0}^{t_k}F\left(t,\vec{x},\dot{\vec{x}},\vec{u}\right)dt+\Phi \left(\vec{x}(t_0),\vec{x}(t_k)\right)\text{\hspace{1em}(d)}$$ 这里选取拉格朗日型，原因很容易理解：在控制过程中，希望总的性能和能量最小，因此只有积分形式才能代表整个过程的总量。

在系统控制框图（上图）中，系统状态量$x$经过增益矩阵$K$返回到$u$处，即
$$u=-Kx+r$$
如果再令$r=0$，那么
x ˙ = A x + B u = A x − B K x = ( A − B K ) u y = C x + D u C x − D K u = ( C − D K ) u

$$\begin{aligned} \dot x&=Ax+Bu=Ax-BKx=\left( A-BK \right) u \\ y&=Cx+DuCx-DKu=\left( C-DK \right) u \end{aligned}$$ x˙y​=Ax+Bu=Ax−BKx=(A−BK)u=Cx+DuCx−DKu=(C−DK)u​通过改变$K$的值，从而达到控制系统性能的目的。

2. 公式含义

$x$ – 状态量；
$u$ – 控制量；
$Q,R$ – 权重矩阵（对角阵）。

公式$(1)$中$x^{T}Qx$可以粗略理解为$Q{x}^2$，同理第二项粗略理解为$R{u}^2$，这样一来
$$J={\int }_0^{\infty }\left(Q{x}^2+R{u}^2\right)dt\text{\hspace{1em}(1-1)}$$里的被积函数部分为非负值。在实际控制过程中，$x,u$都有可能取负值或正值，而要知道系统消耗的能量，势必要用绝对值来进行计算。因此，
$$J={\int }_0^{\infty }\left(\left|Qx\right|+\left|Ru\right|\right)dt$$显然，公式$(1-1)$和这种方法是等效的，且更加计算简便。

另一方面，$x$为状态量，$u$为控制量，则$x^{T}Qx$和$R{u}^2$分别间接代表了系统性能和所需能量。$J$为二者加和，那么$J$实则是同时综合考虑了性能和能量两方面指标。

式中还用到了两个权重系数$Q$和$R$。上面说到，$J$实则是同时综合考虑了性能和能量两方面指标，那么$Q$和$R$这两个权重矩阵取值的不同直接决定了$J$中性能和能量两部分各自所占“比例”（权重）的大小（例如，$Q$大些，表示考虑性能要更多些；$R$大些，表示考虑能量更多些），并进一步间接决定了系统控制过程的好坏。因此，LQR算法中最重要的一步也是不断调整$Q,R$的取值，使得系统达到较满意的状态。

3. 倒立摆的建模

这里不加证明地给出倒立摆的数学模型：
( M + m ) x ¨ + m l θ ¨ cos ⁡ θ − m l θ ˙ 2 sin ⁡ θ + b 1 x ˙ = F ( I + m l 2 ) θ ¨ + m x ¨ l cos ⁡ θ − m g l sin ⁡ θ + b 2 θ ˙ = 0 (2)

$$\begin{aligned} (M+m) \ddot x + ml \ddot \theta \cos{\theta}-ml \dot \theta ^2 \sin{\theta}+b_1 \dot x = F \\ (I+ml^2) \ddot \theta +m \ddot x l \cos{\theta} -mgl \sin{\theta} +b_2 \dot \theta = 0 \end{aligned}$$ \tag{2} (M+m)x¨+mlθ¨cosθ−mlθ˙2sinθ+b1​x˙=F(I+ml2)θ¨+mx¨lcosθ−mglsinθ+b2​θ˙=0​(2)其中：
$M$ – 小车质量；
$m$ – 摆杆质量；
$x$ – 小车位移坐标；
$l$ – 杆长一半；
$\theta$ – 摆杆与竖直向上夹角（顺时针为正）；
$b_1$ – 小车与地面摩擦系数，与速度成正比；
$b_2$ – 摆杆与小车连接处摩擦系数，与角速度成正比；
$F$ – 施加在小车上的外力；
$g$ – 重力加速度；
$I$ – 摆杆的转动惯量。

3.1 线性化

假设倒立摆初始状态为竖直向上（即稳定态），初始时刻有一个脉冲信号作为干扰。
假设角度变化$\Delta \theta$极小，则$\cos \theta \approx 1,\sin \theta \approx \theta ,{\dot{\theta }}^2\approx 0$，公式$(2)$可以线性化为：
( M + m ) x ¨ + m l θ ¨ + b 1 x ˙ = F ( I + m l 2 ) θ ¨ + m x ¨ l − m g l θ + b 2 θ ˙ = 0 (3)

$$\begin{aligned} (M+m) \ddot x + ml \ddot \theta+b_1 \dot x = F \\ (I+ml^2) \ddot \theta +m \ddot x l -mgl \theta +b_2 \dot \theta = 0 \end{aligned}$$ \tag{3} (M+m)x¨+mlθ¨+b1​x˙=F(I+ml2)θ¨+mx¨l−mglθ+b2​θ˙=0​(3)

3.2 状态空间建立

设状态向量$x=[x_1{x}_2{x}_3{x}_4{]}^T=[x\dot{x}\theta \dot{\theta }{]}^T$，那么显然需要利用$(3)$式求出$\ddot{x}$和$\ddot{\theta }$。
联立$(3)$的两个方程可以解出：
x ¨ = 1 ( M + m ) ( I + m l 2 ) − m 2 l 2 [ − b 1 ( I + m l 2 ) x 2 − m 2 g l 2 x 3 + b 2 m l x 4 + F ( I + m l 2 ) ] θ ¨ = 1 m 2 l 2 − ( M + m ) ( I + m l 2 ) [ − b 1 m l x 2 − ( M + m ) m g l x 3 + b 2 ( M + m ) x 4 + m l F ]

$$\begin{aligned} \ddot x = \frac{1}{(M+m)(I+ml^2)-m^2l^2} \left[ -b_1(I+ml^2)x_2-m^2gl^2x_3+b_2mlx_4+F(I+ml^2) \right]\\ \ddot \theta = \frac{1}{m^2l^2-(M+m)(I+ml^2)} \left[ -b_1mlx_2-(M+m)mglx_3+b_2 (M+m)x_4+mlF \right] \end{aligned}$$ x¨=(M+m)(I+ml2)−m2l21​[−b1​(I+ml2)x2​−m2gl2x3​+b2​mlx4​+F(I+ml2)]θ¨=m2l2−(M+m)(I+ml2)1​[−b1​mlx2​−(M+m)mglx3​+b2​(M+m)x4​+mlF]​记
$$N=(M+m)(I+m{l}^2)-m^2{l}^2$$那么可以建立状态空间表达式：
$$\begin{array}{rl}\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ {\dot{x}}_3\\ {\dot{x}}_4\end{array}\right]& =\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{b_1(I+m{l}^2)}{N}& -\frac{m^{2}g{l}^2}{N}& \frac{b_{2}ml}{N}\\ 0& 0& 0& 1\\ 0& \frac{b_{1}ml}{N}& \frac{(M+mmgl}{N}& -\frac{b_2(M+m)}{N}\end{array}\right]\cdot \left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]+\left[\begin{array}{c}0\\ \frac{I+m{l}^2}{N}\\ 0\\ -\frac{ml}{N}\end{array}\right]F\\ y& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\cdot \left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_3\end{array}\right]\end{array}$$其中$A,B,C,D$分别为
$$\begin{array}{rl}A& =\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{b_1(I+m{l}^2)}{N}& -\frac{m^{2}g{l}^2}{N}& \frac{b_{2}ml}{N}\\ 0& 0& 0& 1\\ 0& \frac{b_{1}ml}{N}& \frac{(M+mmgl}{N}& -\frac{b_2(M+m)}{N}\end{array}\right]\\ B& =\left[\begin{array}{c}0\\ \frac{I+m{l}^2}{N}\\ 0\\ -\frac{ml}{N}\end{array}\right]\\ C& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\\ D& =\left[\begin{array}{c}0\\ 0\end{array}\right]\end{array}$$

4. LQR算法实现

选取权重矩阵$Q=diag(1000,1,100,1),R=1$。
在MATLAB中有很方便的函数

K = lqr(A, B, Q, R)
1

即可得到反馈矩阵$K$。

5. MATLAB代码仿真

这里贴出代码，如果使用，请点个赞或收藏，谢谢！

clc;
clear variables;

% ---------倒立摆基本参数---------
M = 2;
m = 0.1;
l = 0.5;
b1 = 0.1;
b2 = 0.1;
g = 9.8;

L = 2*l;
J = 1/3*m*L^2;

JJ = J + m*l^2;

N = (M+m)*JJ-m^2*l^2;

% ---------状态空间建立----------
A = [0 1 0 0;
    0 -b1*JJ/N -m^2*g*l^2/N b2*m*l/N;
    0 0 0 1;
    0 b1*m*l/N (M+m)*m*g*l/N -b2*(M+m)/N];

B = [0;
    JJ/N;
    0;
    -m*l/N];

C = [1 0 0 0;
    0 0 1 0];
D = [0;
    0];

%% 设置Q R
q = [1000 1 100 1];
Q = diag(q);

R = 1;

%% 计算K
K = lqr(A, B, Q, R); 

%% 进行LQR计算
Ac = A - B*K;

%% LQR仿真，脉冲信号激发
t = 0 : 0.01 : 15;

ssold = ss(A, B, C, D);
ssnew = ss(Ac, B, C, D);


imold = impulse(ssold, t);
imnew = step(ssnew, t);

xold = imold(:, 1);
theold = imold(:, 2);

xnew = imnew(:, 1);
thenew = imnew(:, 2);


%% 画图
figure(1);
clf;

plot(t, xnew, 'linewidth', 2);
grid on;
grid minor;
xlabel('Time, s');
ylabel('$x$/m', 'interpreter', 'latex');
title('Time – Position');
set(gca, 'fontname', 'times new roman', 'fontsize', 25);



figure(2);
clf;

plot(t, thenew / 3.14 * 180, 'linewidth', 2);
grid on;
grid minor;
xlabel('Time, s');
ylabel('$\theta$/m', 'interpreter', 'latex');
title('Time – Angle');
set(gca, 'fontname', 'times new roman', 'fontsize', 25);
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

仿真结果如下（角度单位为$°$）：

6. 优缺点

LQR的优点：
1.不需要大量计算，只需要进行线性化即可；
2.存在现成函数lqr可以使用，计算方便快捷；
3.仿真速度快，鲁棒性强。

LQR缺点：
1.需要将系统线性化，但在有些位置处不可进行线性化；
2.状态空间$\dot{x}=Ax+Bu$默认为零初始状态，因此$x_0=0$，如果要研究非零初始状态的系统，需要进行变换。

博主刚接触LQR控制，很多地方理解还很浅，此篇博客权当记录学习笔记和一次小小仿真实践，如果有观点欢迎在评论区评论，也欢迎有所收获的小伙伴点赞收藏！