0.本系列目的

理解与运用LQR

理解

六个问题

解释什么是LQR，并梳理LQR求解过程。
大家可以思考这么几个问题：

1. LQR控制的是什么（什么是LQR）?
2. LQR的适用场景是（使用条件）？
3. LQR的变量是什么，输入是什么，输出是什么？
4. LQR是怎么解决这些问题的？
5. LQR中有没有为了优化问题设置的假设条件？

---

6. 在实际控制中，怎么改变状态变量的初值，如果我想让LQR控制器控制的某个状态变量存在稳态误差，该怎么设置参数？

• 可能需要一点自控理论知识，不过对聪明人来说，学过矩阵乘法就行^ ^

如果看不懂就看下面的参考文章，保证好懂
本文部分片段搬运自以下文章，并根据学校课件和个人理解进行修改，侵删！
(个人感觉最好✨→) 线性二次型调节器(LQR)原理详解
https://blog.csdn.net/qq_36133747/article/details/123413115
LQR最优控制方法小结
https://zhuanlan.zhihu.com/p/363033191
RoboMaster平衡步兵机器人控制系统设计
https://zhuanlan.zhihu.com/p/563048952

运用

一阶倒立摆

matlab+simscape multibody实现

1. 对一阶倒立摆模型进行建模，设计LQR参数
2. 在matlab上，实现模型的框图设计
3. 通过simscape multibody将这一过程可视化

见下一期LQR的理解与运用 第二期——一阶倒立摆在matlab上的LQR实现

简单的轮足模型（二阶倒立摆）

参考RoboMaster平衡步兵机器人控制系统设计
构造以下模型

1 理解LQR

写在前面

本文讨论LQR基本原理时，被控对象都是线性定常系统，即系统状态不随时间变化的系统。状态空间表达如下：
$$\begin{array}{l}\dot{x}=Ax+Bu\\ y=Cx+Du\end{array}\text{\hspace{1em}(1)}$$

符号	意义
$A$	状态转移矩阵，描述了系统状态的演化规律。
$B$	输入矩阵，描述了外部输入对系统状态的影响。
$C$	输出矩阵，描述了状态变量如何映射到输出信号。
$D$	直接传递矩阵，描述了外部输入直接传递到输出信号的影响。
$x$	状态变量，系统的内部状态，可以描述系统的动态特性。
$u$	输入变量，系统的外部输入，可以是一个或者多个变量。
$Q$	控制效果矩阵，表示控制器对状态变量的重视程度。
$R$	状态效果矩阵，表示控制器对控制输入的重视程度。

Q1:LQR控制的是什么

结论：LQR控制的是线性时不变系统的状态变量$x$的变化。
具体而言，LQR通过对系统状态变量的反馈控制，使系统状态向着期望的状态稳定，并且能够实现一定的性能指标要求，如响应速度、稳态误差等。

---

我们设定一个线性反馈控制器$u=-Kx$,用以得到输入参数$u$ 与状态变量$x$的关系（求解矩阵$K$的方法会在后面提到），此时第一行可以写为
$$\dot{x}=Ax-BKx=\underset{A_{cl}}{\underbrace{(A-BK)}}x\text{\hspace{1em}(2)}$$
让系统稳定的条件是矩阵$A_{cl}$ 的特征值的实部均为负数（？），我们当然可以手动选择几个满足上述条件的特征值，然后反解$K$，从而得到控制器。

而LQR的出现，就是为了让几个参数的选择更为合理，从而使得控制器控制效果更好。其实现方式正是通过设计代价函数$J$实现的。

本文讨论无限时间的LQR问题（有限时间的LQR问题属于状态时变的问题，这里暂时不考虑），无限时间的LQR问题设计的成本代价泛函$J$为：

$$J={\int }_0^{\infty }\left(x^{T}Qx+u^{T}Ru\right)dt,Q=Q^T,R=R^T,Q\ge 0,R>0\text{\hspace{1em}(3)}$$

一般来说，Q阵和R阵为对角阵，分别确定了状态变量$x$和输入参数$u$的权重。对角阵上的值越大说明我们设计时对于该量的重视程度越大，即希望这个量在变化过程中保持较小的值，换种说法就是对于该量的“惩罚”越大。我们的设计目标就是得到一系列的控制序列使代价累积的最小。

代价函数的解释，举例：

图片内容及部分文字来自线性二次型调节器(LQR)原理详解

因此，问题转变为了选择合适的反馈矩阵$K$使得代价函数$J$最小。(见Q3)

Q2:LQR的适用场景与形式

• LQR是一个多输入，多输出（MIMO）的调节器(仅适用于线性系统)；其目的是在给定代价为二次的代价方程中找出最小代价。
• 在多个输入参数中，可分别设置不同权重。
  此外，由于LQR为状态方程形式，因此输入参数为n*1的形式，而权重矩阵（下文$Q,R$）往往是n*n的对角矩阵，即除了主对角线均为0；故为了表现加权、二次、多输入，代价函数往往为以下形式：($x$,$u$的位置见上文状态空间表达形式)
  $$J=x^T(t)Qx(t)+u^T(t)Ru(t)$$

Q3:LQR的变量、输入、输出

• 变量：状态变量$x$和输入变量$u$，作用在前面表格中已列出。
• 输入：在LQR设计中，我们需要选择合适的状态反馈矩阵K，使得反馈控制器能够有效地控制系统状态的变化，并实现期望的性能指标。
  在状态方程

Q4:LQR的解决思路

接Q1，下一步是选择合适的反馈矩阵$K$使得代价函数$J$最小。
根据式$(3)$,我们另定义一个辅助常量矩阵$P$，
$$\frac{\mathrm{d}}{\mathrm{d}t}{x}^{T}Px=-\left(x^{T}Qx+u^{T}Ru\right)\text{\hspace{1em}(4)}$$

(?)$P$是对称矩阵，$P=P^T>0$

• $P$的作用
  将$(4)$带入$(3)$有
  $$\begin{array}{rl}J& =-{\int }_0^{\infty }\frac{\mathrm{d}}{\mathrm{d}t}{x}^{T}Px\mathrm{d}t\\ & =-\left({x^{T}Px|}_{\infty }-{x^{T}Px|}_0\right)\\ & =-\left(0-{x^{T}Px|}_0\right)\\ & =x^T(0)Px(0)\end{array}\text{\hspace{1em}(5)}$$

  当系统稳定时，$t⟶\infty ,x⟶0$

  可见代价函数只跟初始状态和矩阵$P$有关

带入$u=Kx$,结合式$(2)$、$(4)$，有
$$\begin{array}{rl}{\dot{x}}^{T}Px+x^{T}P\dot{x}+x^{T}Qx+x^T{K}^{T}RKx& =0\\ x^T{A}_{cl}^{T}Px+x^{T}P{A}_{cl}x+x^{T}Qx+x^T{K}^{T}RKx& =0\\ x^T\left(A_{cl}^{T}P+P{A}_{cl}+Q+K^{T}RK\right)x& =0\end{array}\text{\hspace{1em}(6)}$$

代入 $A_{cl}=A-BK$ 可以得到

$$\begin{array}{rl}{A}_{cl}^{T}P+P{A}_{cl}+Q+K^{T}RK& =0\\ (A-BK{)}^{T}P+P(A-BK)+Q+K^{T}RK& =0\\ A^{T}P+PA+Q+K^{T}RK-K^T{B}^{T}P-PBK& =0\end{array}\text{\hspace{1em}(7)}$$

将$K=R^{-1}{B}^{T}P$（暂不推导，见Q4）代入，有
$$\begin{array}{l}{A}^{T}P+PA+Q+{\left(R^{-1}{B}^{T}P\right)}^{T}R\left(R^{-1}{B}^{T}P\right)-{\left(R^{-1}{B}^{T}P\right)}^T{B}^{T}P-PB\left(R^{-1}{B}^{T}P\right)=0\\ \\ A^{T}P+PA+Q-PB{R}^{-1}{B}^{T}P=0\end{array}\text{\hspace{1em}(8)}$$
上式也被称为微分Riccati方程(Algebraic Riccati Equation ，ARE)

到这一步就可以认为$P$是solvable的了，现在已经有很完善的方法能求解ARE了

Q4.1 LQR控制器设计步骤：

步骤一：根据我们想要的期望状态，初步设计好$Q$，$R$（一般凭借经验，可以通过迭代不断调整）

步骤二：根据代数Riccati方程（由系统矩阵组成的等式），情况下求解矩阵P用的是数值解法，很少的情况可以求其解析解。

步骤三：根据$P$，得到反馈矩阵$K$的表达式，得到最优控制序列：

Q5:LQR中做出的…假设？

$K=R^{-1}{B}^{T}P$并不是假设，而是推导的结果，过程如下

反馈矩阵K的推导*

如果只是应用LQR方法，那么推导过程可以不用细看，记住下面的表达式就可以，现在用matlab或是python中的一些库就可以直接求解，应用时理解K的含义就可以。
推导过程应用到矩阵求导相关公式，推荐一个在线矩阵求导网站：Matrix Calculus，可以用来验证自己算的对不对。

推导过程：
观察式$(7)$，$A,B,Q,R,P$都是常值矩阵，唯一可变的是$K$阵，所以问题转换为找到一个$K$使得代价函数最小，下面用到了一些构造，主要关注带$K$的部分，求解的想法是要将$K$包含在一个满足一定约束的式子里面或许可以得到$K$的计算表达式，一种思路是如果我们可以把含有$K$的部分转换成类似 $(M+N{)}^T(M+N)$ 的结构，那么要使得代价最小，就会有 $M+N=0$ ，那么$K$就可以求。

令$R=T^{T}T$，代入式$(7)$，有

$$\begin{gathered} A^{T}P+PA+Q-K^T{B}^{T}P-PBK+K^T{T}^{T}TK=0 \\ {A}^{T}P+PA+Q-K^T{B}^{T}P-PBK+(TK{)}^{T}TK=0\text{\hspace{1em}(9)} \end{gathered}$$

为向 $(M+N{)}^T(M+N)$$ 上面靠，将目标形式展开: $M^{T}M+N^{T}N+M^{T}N+N^{T}M$ ，令 $M=TK$ ，刚好可以满足上面其中一项，剩下的用待定系数，带进去解$N$的表达式:

$$\begin{gathered} M^{T}M+N^{T}N+M^{T}N+N^{T}M=-K^T{B}^{T}P-PBK+(TK{)}^{T}TK,M=TK \\ \Rightarrow N^{T}N+K^T{T}^{T}N+N^{T}TK=-K^T{B}^{T}P-PBK\text{\hspace{1em}(10)} \end{gathered}$$

注意$(10)$左右并不一定相等，只是为了让等式右凑出等式左的样子

继续观察，等式两边都有含$K^T$项和含 $K$ 项 ，先用含 $K$ 项拼凑出$N$:

$$\begin{array}{l}{N}^{T}TK=-PBK\Rightarrow N={\left[-PB\left(T^{-1}\right)\right]}^T\Rightarrow N=-{\left(T^{-1}\right)}^T{B}^T{P}^T=N=\\ -{\left(T^{-1}\right)}^T{B}^{T}P\end{array}\text{\hspace{1em}(11)}$$

将 $N$ 代入，求剩下的部分:

$$N^{T}N+K^T{T}^{T}N+N^{T}TK=-K^T{B}^{T}P-PBK,N=-{\left(T^{-1}\right)}^T{B}^{T}P\text{\hspace{1em}(12)}$$

发现含 $K^T$ 项也消掉了，仅剩下不含 $K$ 的第一项:

$$N^{T}N={\left[{\left(T^{-1}\right)}^T{B}^{T}P\right]}^T\left[{\left(T^{-1}\right)}^T{B}^{T}P\right]=PB{R}^{-1}{B}^{T}P\text{\hspace{1em}(13)}$$

带到代数Riccati方程中有:

$$A^{T}P+PA+Q+{\left[TK-{\left(T^{-1}\right)}^T{B}^{T}P\right]}^T\left[TK-{\left(T^{-1}\right)}^T{B}^{T}P\right]-PB{R}^{-1}{B}^{T}P=0\text{\hspace{1em}(14)}$$

因为:
$$x^T{\left[TK-{\left(T^{-1}\right)}^T{B}^{T}P\right]}^T\left[TK-{\left(T^{-1}\right)}^T{B}^{T}P\right]x\ge 0\text{\hspace{1em}(15)}$$
令$TK-{\left(T^{-1}\right)}^T{B}^{T}P=0$，可以解出$K=R^{-1}{B}^{T}P$

怎么理解u=-Kx

$u=-Kx$是将输入向量$u$用状态变量$x$的线性表达，不妨把这理解为LQR的特征，或者说LQR这一形式就意味着这一假设

稳定性分析

想要分析系统的稳定性，一般采用李雅普诺夫稳定性理论来证明。
首先，定义李雅普诺夫函数：

$$V(x)=x^{T}Px,P=P^{T}P>0\text{\hspace{1em}(16)}$$

$P$为正定常数矩阵，所以 $V(x)$ 是 正定的。(?)
然后，对 $V(x)$ 求 $x$ 的一阶导数：

$$\dot{V}(x)={\dot{x}}^{T}Px+x^{T}P\dot{x}\text{\hspace{1em}(17)}$$

将 $\dot{x}=Ax+Bu,u=-Kx=-R^{-1}{B}^{T}Px$ 带入上式，得到：

$$\dot{V}(x)=x^T\left[A^{T}P-K^T{B}^{T}P+PA-PBK\right]x\text{\hspace{1em}(18)}$$

再代入$K$，得到:

$$\dot{V}(x)=x^T\left[A^{T}P+PA-2PB{R}^{-1}{B}^{T}P\right]x\text{\hspace{1em}(19)}$$

因为通过无限时间LQR设计的 $P$ 会满足上面的代数Riccati方程$(8)$，带进来，得到:

$$\dot{V}(x)=-x^T\left[Q+PB{R}^{-1}{B}^{T}P\right]x\text{\hspace{1em}(20)}$$

因为: $Q>0,R>0,P>0$ ，故 $\dot{V}(x)<0$ ， 系统是渐进稳定的。

上面是从别处搬运的，我只是看了一遍公式无误，自己其实也看不太懂
也有一种说法，是说稳定性取决于状态矩阵A的特征值的符号，利用MATLAB语句[V,F] = eig(A)求得状态矩阵A的特征值以及对应的特征向量。
当全部为实根的时候，只有它们都小于零，系统才是稳定的。

可控性分析

只要满足一些基本条件，LQR的设计过程就能保证得到一个让系统稳定的反馈控制器。

• LQR定理
  令系统 $(A,B)$ 可控，$R$ 和 $Q$ 都是正定的，则闭环系统$(A-BK)$近稳定。

  注意，不管系统的开环稳定性如何，这都是成立的。
  回顾现控的相关知识：可控性可以通过检查可控性矩阵$U=\left[\begin{array}{lllll}B& AB& A^{2}B& \cdots & A^{n-1}B\end{array}\right]$是否满秩来判断

✨matlab实现LQR可视化

模型介绍

物理模型

我们以经典的一阶倒立摆模型为例，进行建模【模型如图】

在此模型中，设

物理量	符号
滑块质量	$M$
摆杆质量	$m$
摆杆转动轴心到杆质心长度【半杆长】	$l$
摆杆在转轴处转动惯量	$J$
小车受到外力	$F$
小车位置	$x$
摆杆与竖直方向夹角【本文顺时针为正】	$\theta$

$$x=\left[\begin{array}{l}{x}_1\\ x_2\\ x_3\\ x_4\end{array}\right]=\left[\begin{array}{c}\theta \\ \dot{\theta }\\ x\\ \dot{x}\end{array}\right],u=F$$

状态方程模型

通过物理建模及化简，最终能得到状态方程$A、B、C、D$四个矩阵的表达式【表达式推导见第二期】
$$\begin{array}{l}\dot{x}=Ax+Bu\\ y=Cx+Du\end{array}\text{\hspace{1em}(1)}$$

代码

% ------------------------------------------
%     本代码使用最经典的一阶倒立摆模型建模
%    
% ------------------------------------------
clc;clear;close all;
g = 9.80665;
m = 1;
M = 0.1;
l = 0.18;  % 半杆长
J = 1/3 * m * (2 * l)^2;

% ------------------------------------
% 创建一个状态空间模型
A21 = m*g*l*(M+m)/(J*(M+m)+M*m*l^2);
A41 = -m^2*g*l^2/(J*(M+m)+M*m*l^2);
A = [0  1  0  0;
    A21 0  0  0;
     0  0  0  1;
    A41 0  0  0];
% [theta, dtheta, x, dx]

B2 = -m*l/(J*(M+m)+M*m*l^2);
B4 = (J+m*l^2)/(J*(M+m)+M*m*l^2);
B = [0;B2;0;B4];

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

D=[0;0];

% ------------------------------------
% 检验状态空间模型的可控性、稳定性分析
eig(A)
%% Qc=ctrb(A,B)
%% rank(Qc)
Qb=obsv(A,C);
rank(Qb)

% ------------------------------------
% 设计 LQR 控制器
Q = [1 0 0 0
     0 1 0 0
     0 0 10 0
     0 0 0 10];
R = 1;
K = lqr(A,B,Q,R)

% ------------------------------------
% 定义输入信号和时间向量
t = 0:0.02:4;
u = 20 * sin(4*t);
%u = zeros(size(t)) * 0.2;

% ------------------------------------
% 模拟系统的响应
% [theta, dtheta, x, dx]
x0 = [30;0;10;0]; % 初始状态
sys=ss(A-B*K,B,C,D); %经过状态量反馈后的新状态矩阵
[y, t, x] = lsim(sys, u, t, x0);

% ------------------------------------
% 绘制系统的输出信号和状态变量的时间响应
figure;
subplot(2,1,1);
plot(t, u, 'r--', t, y(:,1), 'b-', t, y(:,2), 'r-');
legend('输入信号', '输出信号-theta', '输出信号-x');
xlabel('时间');
ylabel('幅值');
title('系统的输入输出信号');
subplot(2,1,2);
plot(t, x(:,1), 'r-', t, x(:,2), 'b--', t, x(:,3), 'black-', t, x(:,4), 'g--');
legend('theta', 'omega', 'x', 'v');
xlabel('时间');
ylabel('幅值');
title('系统的状态变量');
% =========================================

运行结果

Q单位阵，R=10

Q为eig(10,1,10,1)，R=1

可以发现，输入信号的变化对于状态变量的收敛影响不大。

Q6:稳态误差相关控制

先贴ChatGPT的回答，有待笔者补充完善

Q：如果我想让LQR控制器控制结果中出现可被我准确控制的稳态误差，该怎么做？

回答一：Q后加常数

要让LQR控制器在系统稳定后出现可控的稳态误差，可以在成本函数中添加一个稳态误差的惩罚项。这个惩罚项可以使用系统输出与期望输出之间的偏差来表示。

具体来说，在LQR的成本函数中加入一个与系统输出偏差成比例的惩罚项。这可以通过在$Q$矩阵中增加一个与输出偏差平方成比例的权重来实现。可以通过试验和调整权重的大小，以实现对稳态误差的控制。

以下是一个例子，演示如何在LQR控制器中添加一个稳态误差惩罚项：

假设我们有一个二阶系统，其状态空间表示为：

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

我们希望控制系统的稳态误差，并使其快速响应。我们可以使用LQR控制器来优化控制器参数，同时在成本函数中添加一个与输出偏差平方成比例的惩罚项。具体而言，我们可以将$Q$矩阵设置为：

Q = [1 0; 0 10];

这样做可以增加对状态$x_2$的权重，从而加速系统的响应。然后，我们可以添加一个惩罚项来惩罚输出偏差，例如：

Q(2,2) = Q(2,2) + 1000;

这样可以增加对输出偏差的权重，从而减小稳态误差。

最后，我们可以使用lqr函数计算LQR控制器的增益矩阵，并将其应用于系统：

R = 1;
K = lqr(A, B, Q, R);
sys_cl = ss(A-B*K, B, C, D);

通过这样的设置，LQR控制器将通过调整输入信号来控制系统状态，并在稳态时保持输出与期望输出之间的偏差尽可能小。

回答二：加入积分项【LQI】

下一期指路

LQR的理解与运用 第二期——一阶倒立摆在matlab上的LQR实现

参考文档

(个人感觉最好✨→) 线性二次型调节器(LQR)原理详解
https://blog.csdn.net/qq_36133747/article/details/123413115
(推导K→) LQR最优控制方法小结
https://zhuanlan.zhihu.com/p/363033191
(应用篇)RoboMaster平衡步兵机器人控制系统设计
https://zhuanlan.zhihu.com/p/563048952当时的