[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$，那么
$$\begin{array}{rl}\dot{x}& =Ax+Bu=Ax-BKx=\left(A-BK\right)u\\ y& =Cx+DuCx-DKu=\left(C-DK\right)u\end{array}$$通过改变$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. 倒立摆的建模

这里不加证明地给出倒立摆的数学模型：
$$\begin{array}{r}(M+m)\ddot{x}+ml\ddot{\theta }\cos \theta -ml{\dot{\theta }}^2\sin \theta +b_1\dot{x}=F\\ (I+m{l}^2)\ddot{\theta }+m\ddot{x}l\cos \theta -mgl\sin \theta +b_2\dot{\theta }=0\end{array}\text{\hspace{1em}(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)$可以线性化为：
$$\begin{array}{r}(M+m)\ddot{x}+ml\ddot{\theta }+b_1\dot{x}=F\\ (I+m{l}^2)\ddot{\theta }+m\ddot{x}l-mgl\theta +b_2\dot{\theta }=0\end{array}\text{\hspace{1em}(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)$的两个方程可以解出：
$$\begin{array}{r}\ddot{x}=\frac{1}{(M+m)(I+m{l}^2)-m^2{l}^2}\left[-b_1(I+m{l}^2)x_2-m^{2}g{l}^2{x}_3+b_{2}ml{x}_4+F(I+m{l}^2)\right]\\ \ddot{\theta }=\frac{1}{m^2{l}^2-(M+m)(I+m{l}^2)}\left[-b_{1}ml{x}_2-(M+m)mgl{x}_3+b_2(M+m)x_4+mlF\right]\end{array}$$记
$$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)

即可得到反馈矩阵$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);

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

6. 优缺点

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

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

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