以倒立摆为例，学习设计LQR控制器
本文先介绍LQR算法，然后以一阶倒立摆为例，设计一个LQR控制器
以一阶倒立摆为例,学习使用LQR控制器
@author: Liao Yufei
本文中，当不描述文中所指的一阶倒立摆系统时，\(x、y、u、e、A、B、C、D、K、S、J\)等符号均代表对普遍性的状态空间方程和LQR理论的描述。
一、线性二次型调节器(LQR)
1、最优控制和线性二次型调节器的关系
最优控制的目的是设计出最优的控制策略，我们希望能用最少的能量、最高的效率去达成对系统的控制。最优控制理论涉及到极小值原理、变分法、动态规划等理论。最优控制对问题的描述一般包括运动方程、状态约束条件、目标集、容许控制集、性能指标等等。
线性二次型调节器是基于最优控制理论的一种控制器，其所选取的性能指标函数(代价函数)为二次型。
2、代价函数(cost function)简介
代价函数通常是用于寻找最优解的一种函数，一般用符号J表示，其可用于代表解决问题所需的代价。代价函数中通常包含了预测值和真实值间的差异。使用代价函数的目的是用最少的能量达到最优的控制效果，在算法设计中，通常希望所设计的代价函数的值能够达到最小。
在许多算法中都看到代价函数的身影，比如最优控制、机器学习等等。
在最优控制中，通常可以看到将代价函数称为性能指标函数。
LQR中选取的代价函数为二次型。对于二次型的代价函数来说，可以在其前面乘\(\frac{1}{2}\)，这是为了在原理公式的推导中简化计算，使结果简洁。如果只是使用LQR控制器，不会涉及到LQR原理的深入计算。
3、线性二次型有限时间最优状态调节器
（1）连续时间系统有限时间最优状态调节器
假设一个线性系统： \[ \begin{align} \dot{x}(t) = A(t)x(t) + B(t)u(t)\\ y(t)=C(t)x(t)+D(t)u(t) \end{align} \]
此情况下代价函数(也称为性能指标)一般设置为： \[ \begin{align} J=\frac{1}{2}x^{T}(t_f)S(t_f)x(t_f)+\frac{1} {2}\int_{t_0}^{t_f} [x^{T}(t)Q(t)x(t) + u^{T}(t)R(t)u(t)] dt \label{2eq}\\ \end{align} \]
（2）离散时间系统有限时间最优状态调节器
初始条件为\(x(0) = x_0\)
代价函数一般设置为： \[ \begin{align} J = \frac{1}{2}x^{T}(N)S(N)x(N) + \frac{1}{2}\sum_{k=0}^{N-1}[x^{T}(k)Q(k)x(k)+u^{T}(k)R(k)u(k)] \end{align} \]
4、线性二次型无限时间最优状态调节器
一般我们所说的LQR是指无限时间上的线性二次型调节器。
（1）连续时间系统无限时间最优状态调节器
a.对于线性时变系统
描述系统的微分方程中有系数为时间的函数，此时其无限时间上的LQR问题相当于是在\(t_f\)趋于无穷下的有限时间LQR问题。由于当\(t\)趋于0时，系统状态稳定，表示系统终端状态的项数值也趋于0
b.对于线性定常系统
\[ \begin{align} \dot{x}(t) = Ax(t) + Bu(t)\\ y(t)=Cx(t)+Du(t) \end{align} \]
A、B、C和D矩阵均为常矩阵。
代价函数一般设置为： \[ \begin{align} J=\frac{1}{2}\int_{0}^{\infty} [x^{T}(t)Qx(t) + u^{T}(t)R u(t) ]dt \\ \end{align} \]
可以看到此代价函数没有表示终端状态的项，因为当时间趋于无穷时，系统状态稳定，\(x(t)\)趋近于0。
Q是半正定常数矩阵、R是正定常数矩阵。一般设置Q和R为对角线矩阵。Q和R矩阵内的元素值大小可以理解为对状态向量和系统输入向量中相应元素的权重分配。
对于线性负反馈闭环系统有（\(K\)为反馈增益矩阵）： \[ \begin{align} u(t) = -Kx(t) \end{align} \] 将式(8)代入式(7)，可得到： \[ \begin{align} J = \frac{1}{2}\int_{0}^{\infty} x^{T}(t)(Q+K^TRK)x(t)dt \end{align} \]
当\((A,B)\)为可镇定对，\(u^*(t)\)是线性定常系统无限时间LQR问题存在的唯一最优控制器。其中\(P\)矩阵是式(12)所示的\(Riccati\)黎卡提方程的非负定解。 \[ \begin{align} u^*(t) = -R^{-1}B^TPx(t) \end{align} \]
\[ \begin{align} 令&K = R^{-1}B^{T}P\\ \end{align} \]
补充：对于线性定常系统，\((A,B)\)是可镇定对，意为存在反馈增益矩阵\(K\)，使得\(A-BK\)稳定。 \[ \begin{align} &A^TP + PA + Q - PBR^{-1}B^{T}P = 0\\ \end{align} \]
可用如下方式得到该\(Riccati\)黎卡提方程：
假设存在常数矩阵P，使 \[ \begin{align} \frac{\mathrm{d}(x^T(t)Px(t))}{\mathrm{d}t} = -x^{T}(t)(Q+K^TRK)x(t) \end{align} \] 将上式展开，可得到： \[ \begin{align} \dot{x}^T(t)Px(t)+x^T(t)P\dot{x}(t)= -x^{T}(t)(Q+K^TRK)x(t) \end{align} \] 对于线性负反馈系统\(\dot{x}(t)=Ax(t)+Bx(t)\)，结合式(8)可得到： \[ \begin{align} \dot{x}(t)=(A-BK)x(t) \end{align} \] 将式(15)代入式(14)： $$ \[\begin{align} &x^T(t){(A-BK)}^TPx(t)+x^T(t)P(A-BK)x(t)+x^{T}(t)(Q+K^TRK)x(t) = 0\\ &x^T(t)(A^TP+PA+Q+K^TRK-K^TBP-PBK)x(t)=0\\ \end{align}\] \[ 为使式(17)恒成立，可得： \] \[\begin{align} A^TP+PA+Q+K^TRK-K^TBP-PBK=0 \end{align}\] \[ 代入式(11)化简可得： \] A^TP + PA + Q - PBR{-1}B{T}P = 0 \[ 当期望的状态变量不为0时，可将代价函数设置为： \] \[\begin{align} J=\frac{1}{2}\int_{0}^{\infty} [e^{T}(t)Qe(t) + u^{T}(t)R u(t) ]dt \\ \end{align}\] \[ \] 其中\(e(t) = x(t)-x_{ref}(t)\) $$
（2）离散时间系统无限时间最优状态调节器
初始条件为\(x(0) = x_0\)
\[ \begin{align} J = \frac{1}{2}\sum_{k=0}^{N-1}[x^{T}(k)Qx(k)+u^{T}(k)Ru(k)] \end{align} \]
二、一阶倒立摆力学模型
补充：平行轴定理
设\(I_c\)代表刚体对于质心轴的转动惯量，该刚体质量为M，设与质心轴垂直距离为\(l\)的轴1，则该刚体相对于轴1的转动惯量为\(I = I_c + Ml^2\)
| 符号 | 意义 | 单位 |
|---|---|---|
| \(m\) | 摆杆质量 | \(kg\) |
| \(M\) | 滑块质量 | \(kg\) |
| \(l\) | 摆杆质心到摆杆转动点的距离 | \(m\) |
| \(b\) | 滑块与导轨阻尼系数 | \(N·s/m\) |
| 符号 | 意义 | 设定正方向 | 单位 |
|---|---|---|---|
| \(\phi\) | 摆杆与竖直方向的夹角 | 图示为正 | ° |
| \(\dot{\phi}\) | 摆杆与竖直方向的夹角角速度 | 顺时针 | °/\(s\) |
| \(\ddot{\phi}\) | 摆杆与竖直方向的夹角角加速度 | 与Y轴正方向平行 | °/\(s^2\) |
| \(x_m\) | 滑块位移 | 与x轴正方向平行 | \(m\) |
| \(\dot{x}_m\) | 滑块速度 | 与x轴正方向平行 | \(m/s\) |
| \(\ddot{x}_m\) | 滑块加速度 | 与x轴正方向平行 | \(m/s^{2}\) |
进行力学分析可建立以下方程：
\[ \begin{align} & F = (m + M) \ddot{x}_m + ml\ddot{\phi}cos\phi - ml\dot{\phi}^2 sin\phi + b\dot{x}_m \end{align} \] \[ \begin{align} & (J + ml^2)\ddot{\phi} = mglsin\phi - m\ddot{x}_mlcos\phi \end{align} \]
三、设计LQR控制器
设计LQR控制器需要精确的数学模型。如果无法直接测量获得系统状态向量里所有的状态值，则需要设计状态观测器来估计无法测量的状态值。
以一阶倒立摆为例介绍LQR控制器的设计步骤：
1、对被控制的系统进行建模
（1）建立该系统中各变量的关系
对图1所示一阶倒立摆系统进行力学分析，能够得到方程(21)、方程(22)。可以看出一阶倒立摆系统是一个非线性系统。
（2）建立状态向量
选取一组数量最少的、能够完全表征系统运动状态的变量作为状态变量，建立状态向量。
对于如图1所示一阶倒立摆系统来说，关于变量\(x_m\)的微分方程最高阶导数项为\(\ddot{x}_m\)，关于变量\(\phi\)的微分方程最高阶导数项为\(\ddot{\phi}\)，结合实际情况分析，故用滑块位移\(x_m\)、滑块速度\(\dot{x}_m\)、摆杆与竖直方向夹角\(\phi\)以及该夹角的角速度\(\dot{\phi}\)便能表征该系统的运动状态，选取这四个变量为状态变量，构造状态向量\(X\) \[ \begin{align} X = \left[ \begin{array}{ccc} {x_m}\\ \dot{x}_m\\ {\phi}\\ \dot{\phi} \end{array} \right] \\ \end{align} \]
（3）确定该被控系统的输入和输出
对于如图1所示一阶倒立摆系统来说，其系统的输入是除了系统内力和重力之外的外部力，即图中的\(F\)力，故该一阶倒立摆系统的输入\(U=F\)，可将滑块位移\(x_m\)和摆杆与竖直方向夹角\(\phi\)看作系统的输出 \[ \begin{align} Y = \left[ \begin{array}{ccc} {x_m}\\ {\phi}\\ \end{array} \right] \\ \end{align} \]
（4）建立状态空间方程
状态空间方程是用来描述系统输入、输出和状态变量之间关系的方程组。
一般的状态空间方程表达式为： \[ \begin{align} \left\{ \begin{array}{lr} \dot{x} = Ax + Bu\\\notag y=Cx+Du \end{array} \right.\notag \end{align} \]
a.对于线性系统
需要将物理模型的微分方程和传递函数转化为状态空间方程。
b.对于非线性系统
需要将非线性系统在平衡点处进行线性化。
需要建立系统状态向量、系统输入关系和时间的关系，其关系方程可用下列式(25)来表示。 \[ \begin{align} \dot{x(t)} = f(x(t),u(t),t) \end{align} \]
用该方程分别对状态向量\(x\)和系统输入\(u\)求雅可比矩阵，所求的雅可比矩阵分别为状态空间方程中的A、B矩阵。再根据关系列写出C、D矩阵。
建立本文中一阶倒立摆系统的状态空间方程
容易该一阶倒立摆系统的状态向量一阶导数向量为： \[ \begin{align} \dot{X} = \left[ \begin{array}{ccc} \dot{x}_m\\ \ddot{x}_m\\ \dot{\phi}\\ \ddot{\phi} \end{array} \right] \end{align} \]
由于该系统是非线性系统，故需要对该系统在平衡点附近进行线性化，实际的倒立摆的稳定工作点处摆角较小，可认为平衡点为\(\phi = 0\)，\(\dot{\phi}=0\)
对于平衡点\(\phi = 0\)附近来说，由等价无穷小公式：\(sin\phi \sim \phi ,1-cos\phi \sim \frac{\phi^2}{2}\)，可以得到在平衡点\(\phi = 0\)附近，\(sin\phi \approx 0,cos\phi \approx 1\)
第一种计算方法：
将\(\dot{X}\)中不属于状态向量\(X\)元素、系统输入\(U\)元素、可直接测量得到的已知常量的元素，分别用这三种变量去表示。
利用matlab，用状态向量的一阶导数向量分别对状态向量和输入求雅克比矩阵： \(A_1\)(因篇幅过长，暂时不在此给出)
$$ \[\begin{align} &B_1=\left[\begin{array}{c} 0\\ \frac{ml^2 +J}{\sigma_1 }\\ 0\\ -\frac{lm\cos \left(\phi \right)}{\sigma_1 } \end{array}\right]\\ &\sigma_1 =-l^2 m^2 {\cos \left(\phi \right)}^2 +l^2 m^2 +Ml^2 m+Jm+JM \end{align}\] $$
代入\(\phi = 0\)，\(\dot{\phi}=0\)有： \[ \begin{align} A_1 = \left[ \begin{array}{ccc} 0 & 1 & 0 & 0\\ 0 & -\frac{(J+ml^2)b}{Mml^2+J(m+M)}& -\frac{gm^2l^2}{Mml^2+J(m+M)} & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{blm}{Mml^2+J(m+M)} & \frac{lmg(M+m)}{Mml^2+J(m+M)} & 0\\ \end{array} \right] \end{align} \]
\[ \begin{align} &B_1=\left[\begin{array}{c} 0\\ \frac{ml^2 +J}{Mml^2+J(m+M)}\\ 0\\ -\frac{lm}{Mml^2+J(m+M) } \end{array}\right]\\ \end{align} \]
代入系统中摆杆质量、摆杆转动惯量、滑块质量、摆杆质心到摆杆转动点的距离和滑块与导轨阻尼系数，即可求得式(29)、式(30)所示雅可比矩阵的数值矩阵。
该一阶倒立摆系统输出：\(Y = C_1X + D_1U\)
容易得到：\(C_1=\left[ \begin{array}{ccc} 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 \\ \end{array} \right] , D_1=\left[ \begin{array}{ccc} 0 \\ 0 \\ \end{array} \right]\)
另一种计算方法：
先利用在平衡点\(\phi = 0\)附近，\(sin\phi \approx 0,cos\phi \approx 1\)，化简方程(21)、(22)，然后再进行线性化，求雅可比矩阵。
2、设计LQR控制器
（1）绘制控制方框图
示例
一阶倒立摆LQR控制方框图如下：
（2）求解和设置参数
由\(u(t)=-Kx(t)\)可得该一阶倒立摆系统的反馈增益矩阵为： \[ K_1=\left[ \begin{array}{ccc} k_{11} & k_{12} & k_{13} & k_{14} \\ \end{array} \right] \]
a.设置系统初值
b.设置Q和R矩阵
由于目标是使得代价函数\(J\)最小，当半正定加权矩阵\(Q\)值设置得越大，\(x(t)\)就需要越小，\(x(t)\)衰减速度会更快达到0，系统能更快达到稳定；当正定加权矩阵R值设置得越大。系统衰减速度会变慢。
一般选取Q和R为对角矩阵，此时Q矩阵对角元素应设置为非负且所有元素不能全设为0；R矩阵对角元素应全设置为正数。
c.求解\(P\)和\(K\)
由于LQR的计算十分复杂，所以可以利用matlab等软件来进行计算求解，可以正确使用函数matlab中lqr()、lqrd()或者dlqr()等函数。
d.求解\(u\)
\(u(t)=-Kx(t)\)
（3）按照设计的控制方框图进行仿真
（4）部署代码
参考文献：
[1] 钟宜生.最优控制.清华大学出版社.2015.
[2] 林乐天. 基于卡尔曼滤波器的一阶倒立摆控制研究[D].哈尔滨工业大学,2012.
[3] 田涛. 基于鲁棒最优控制的舵减摇控制系统研究[D].哈尔滨工程大学,2022.
[4]LQR控制器— 线性二次型调节器 Linear Quadratic Regulator20. LQR控制器— 线性二次型调节器 Linear Quadratic Regulator - 知乎 (zhihu.com)