1、前言

文章将基于博客倒立摆自动控制学习笔记（二）的数学模型，设计LQR控制器，即

$$\begin{array}{rl}& F-N_{车}-b_1\dot{x}=M\ddot{x}\\ & N_{杆}-f=m(\ddot{x}-\ddot{\theta }lcos\theta +{\dot{\theta }}^{2}lsin\theta )\\ & P_{杆}-mg=-m(\ddot{\theta }lsin\theta +{\dot{\theta }}^{2}lcos\theta )\\ & P_{杆}lsin\theta +N_{杆}lcos\theta -b_2\dot{\theta }=I\ddot{\theta }\\ & N_{车}=N_{杆}\\ & f=\frac{1}{2}{C}_d\rho \dot{x}|\dot{x}|A_{直立}cos\theta \end{array}\text{\hspace{1em}(1-1)}$$

2、系统模型

这里我们先把各个参数的值放上：

计算特征面积时，假设摆杆是0.032m×0.032m×1m的立方体木杆，密度为500kg/m3。
对于倒立摆系统而言，完全表征系统的最少状态变量¹为$\mathbf{X}=[\theta ,\dot{\theta },x,\dot{x}{]}^T$，并由输入$F$驱动该系统。

控制目标是将倒立摆控制到$[\theta ,\dot{\theta },x,\dot{x}{]}^T=[0,0,0,0{]}^T$的状态。参考周彬的《线性系统理论》，忽略掉泰勒展开的高阶项，可以在工作点$[\theta ,\dot{\theta },x,\dot{x}{]}^T=[0,0,0,0{]}^T$附近的小范围区域近似线性化：
$$\{\begin{array}{l}sin\theta \approx \theta \\ cos\theta \approx 1\\ {\dot{\theta }}^2\approx 0\\ \dot{x}|\dot{x}|\approx 0\\ si{n}^2\theta \approx 0\\ co{s}^2\theta \approx 1\end{array}\text{\hspace{1em}(2-1)}$$
式(1-1)利用以下代码化简：（equation.mlx）

% 解出状态方程表达式（字母表达式）
clear; clc;
syms x x_dot x_ddot F N f theta theta_dot theta_ddot P  % 参数
% 初始化参数
M = 1;  % 小车质量（kg）
m = 0.5;  % 摆杆质量（kg）
l = 0.5;   % 转动关节到摆杆质心的长度（m）
b_1 = 0.3;  % 小车移动阻尼
b_2 = 0;   % 摆杆转动阻尼
I = (m*l^2) / 3;   % 摆杆转动惯量
g = 9.8;  % 重力加速度
C_d = 1.2;   % 阻力系数
rho = 1.293;   % 空气密度（kg/m3）
A0 = 0.032;    % 直立状态的特征面积，假设摆杆是0.032m*0.032m*1m的立方体木杆，密度为500kg/m3
theta0 = 30 * pi/180;   % 摆杆初始角度（rad）
theta_d = 0 * pi/180;   % 摆杆目标角度（rad）
x_d = 0;    % 目标位置

eq1 = F-N-b_1*x_dot == M*x_ddot;
eq2 = N-f == m * (x_ddot - theta_ddot*l*cos(theta)+theta_dot^2*l*sin(theta));
eq3 = P-m*g == -m*(theta_ddot*l*sin(theta)+theta_dot^2*l*cos(theta));
eq4 = P*l*sin(theta)+N*l*cos(theta)-b_2*theta_dot == I*theta_ddot;
eq5 = f == C_d*rho*x_dot*abs(x_dot)*A0*cos(theta);

eqns = [eq1, eq2, eq3, eq4, eq5];

vars = [x_ddot, theta_ddot, N, P, f];

[x_ddot, theta_ddot, N, P, f] = solve(eqns, vars)

并近似线性化，得到：
$$\begin{array}{rl}& \ddot{\theta }=19.6\theta -0.4\dot{x}+1.33F\\ & \ddot{x}=3.27\theta -0.27\dot{x}+0.89F\end{array}\text{\hspace{1em}(2-2)}$$
转换成状态空间方程：
$$\dot{\mathbf{X}}=\mathbf{A}\mathbf{X}+\mathbf{B}F\text{\hspace{1em}(2-3)}$$
其中
$$\begin{array}{rl}& \mathbf{A}=\left[\begin{array}{cccc}0& 1& 0& 0\\ 19.6& 0& 0& -0.4\\ 0& 0& 0& 1\\ 3.27& 0& 0& -0.27\end{array}\right]\\ & \mathbf{B}=\left[\begin{array}{c}0\\ 1.33\\ 0\\ 0.89\end{array}\right]\end{array}\text{\hspace{1em}(2-4)}$$

3、连续时间LQR控制介绍

内容参考文章推导倒立摆模型并使用LQR进行位置跟踪控制、文章【控制理论】离散及连续的LQR控制算法原理推导以及笔者在学校鲁棒控制课的课程资料。本节内容主要讨论无限时间调节器(Infinite Time Regulator)的情况。
考虑线性系统：
$$\begin{array}{rl}& \dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u},\mathbf{x}(0)={\mathbf{x}}_0,t\in [0,\infty ]\\ & J(\mathbf{u}(\cdot ))={\int }_0^{\infty }({\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u})dt\end{array}\text{\hspace{1em}(3-1)}$$
其中$\mathbf{Q}={\mathbf{D}}^T\mathbf{D}\ge 0$，$\mathbf{R}>0$。（注：这里是默认了目标状态为$\mathbf{x}=0$）
假设$(\mathbf{A},\mathbf{B})$是可控的且$(\mathbf{A},\mathbf{D})$是可观测的，需要找到一个最优矩阵$\mathbf{K}$和最优的控制输入$u^{\ast }(t)=-\mathbf{K}\mathbf{x}(t)$，使得代价函数$J(\mathbf{u}(\cdot ))$最小化。
将$u^{\ast }(t)=-\mathbf{K}\mathbf{x}(t)$带入代价函数，有
$$J=\frac{1}{2}{\int }_0^{\infty }{\mathbf{x}}^T(\mathbf{Q}+{\mathbf{K}}^T\mathbf{R}\mathbf{K})\mathbf{x}dt\text{\hspace{1em}(3-2)}$$
可以看到，等号右边积分号内为二次型。为了去掉积分号，假设存在一个常量对称矩阵$\mathbf{P}$使得
$$\begin{array}{rl}& \frac{d}{dt}({\mathbf{x}}^T\mathbf{P}\mathbf{x})=-{\mathbf{x}}^T(\mathbf{Q}+{\mathbf{K}}^T\mathbf{R}\mathbf{K})\mathbf{x}\\ & J=\frac{1}{2}{\mathbf{x}}^T(0)\mathbf{P}\mathbf{x}(0)\end{array}\text{\hspace{1em}(3-3)}$$
对一个二次型进行求导，并代入$\dot{\mathbf{x}}={\mathbf{A}}_c\mathbf{x},{\mathbf{A}}_c=\mathbf{A}-\mathbf{B}\mathbf{K}$，有以下推导
$$\begin{array}{rl}\frac{d}{dt}({\mathbf{x}}^T\mathbf{P}\mathbf{x})& =[\frac{\partial }{\partial \mathbf{x}}({\mathbf{x}}^T\mathbf{P}\mathbf{x}){]}^T\frac{d\mathbf{x}}{dt}\\ & =[(\mathbf{P}+{\mathbf{P}}^T)\mathbf{x}{]}^T\dot{\mathbf{x}}\\ & ={\mathbf{x}}^T\mathbf{P}\dot{\mathbf{x}}+{\dot{\mathbf{x}}}^T\mathbf{P}\mathbf{x}\\ & ={\mathbf{x}}^T({\mathbf{A}}_c^T\mathbf{P}+\mathbf{P}{\mathbf{A}}_c)\mathbf{x}\end{array}\text{\hspace{1em}(3-4)}$$
结合式(3-3)和式(3-4)有
$$\begin{array}{r}{\mathbf{x}}^T({\mathbf{A}}_c^T\mathbf{P}+\mathbf{P}{\mathbf{A}}_c+\mathbf{Q}+{\mathbf{K}}^T\mathbf{R}\mathbf{K})\mathbf{x}=0\end{array}\text{\hspace{1em}(3-5)}$$
由于状态向量$\mathbf{x}$不可能恒为0，因此有
$$\begin{array}{rl}& {\mathbf{A}}_c^T\mathbf{P}+\mathbf{P}{\mathbf{A}}_c+\mathbf{Q}+{\mathbf{K}}^T\mathbf{R}\mathbf{K}=0\\ & {\mathbf{A}}^T\mathbf{P}+\mathbf{P}\mathbf{A}+\mathbf{Q}+{\mathbf{K}}^T\mathbf{R}\mathbf{K}-{\mathbf{K}}^T{\mathbf{B}}^T\mathbf{P}-\mathbf{P}\mathbf{B}\mathbf{K}=0\end{array}\text{\hspace{1em}(3-6)}$$
式(3-6)第二个式子可转换为
$$\begin{array}{rl}& {\mathbf{A}}^T\mathbf{P}+\mathbf{P}\mathbf{A}+\mathbf{Q}-\mathbf{P}\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{P}=-(\mathbf{K}-{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{P})\mathbf{R}(\mathbf{K}-{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{P}{)}^T\end{array}\text{\hspace{1em}(3-7)}$$
当$\mathbf{K}={\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{P}$，即等号右边=0时，左边的$\mathbf{P}$可以取到“最小”，从而使得代价函数$J=\frac{1}{2}{\mathbf{x}}^T(0)\mathbf{P}\mathbf{x}(0)$最小。（注：这里笔者暂时没有找到严格证明，为便于记忆，可以把等号左边看作是$\mathbf{P}$的“二次多项式”）
此时
$$\begin{array}{rl}& {\mathbf{A}}^T\mathbf{P}+\mathbf{P}\mathbf{A}+\mathbf{Q}-\mathbf{P}\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{P}=0\end{array}\text{\hspace{1em}(3-8)}$$
式(3-8)就是著名的连续时间代数Riccati方程。根据方程解出 $\mathbf{P}$ ，就能得到 $\mathbf{K}$ 。
注：文章【数理知识】Riccati 黎卡提 system里面有讲Riccati方程的解法，连续的和离散的都有，不过实际做控制设计的时候调现成的库即可。

4、LQR控制器设计

式(2-3)和式(2-4)已经给出了系统的状态空间方程。考虑到状态空间中每个状态以及输入的量纲都不一样，为了方便设计，尽可能将状态量限制在[-1, 1]的范围内，先将状态转换成无量纲的状态量。
$$\{\begin{array}{l}{\theta }_N=\theta /(0.26rad)\\ {\dot{\theta }}_N=\dot{\theta }/(3rad/s)\\ x_N=x/(1m)\\ {\dot{x}}_N=\dot{x}/(3m/s)\\ F_N=F/(50N)\end{array}\text{\hspace{1em}(4-1)}$$
即
$$\begin{array}{rl}& {\mathbf{X}}_N={\mathbf{T}}_N\mathbf{X}\\ & {\mathbf{T}}_N=\left[\begin{array}{cccc}3.85& 0& 0& 0\\ 0& 0.33& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0.33\end{array}\right]\end{array}\text{\hspace{1em}(4-2)}$$
$$\begin{array}{rl}& {\dot{\mathbf{X}}}_N={\mathbf{A}}_N{\mathbf{X}}_N+{\mathbf{B}}_N{F}_N\\ & {\mathbf{A}}_N=\mathbf{A}{\mathbf{T}}_N^{-1}=\left[\begin{array}{cccc}0& 3& 0& 0\\ 5.96& 0& 0& -1.2\\ 0& 0& 0& 3\\ 0.85& 0& 0& -0.81\end{array}\right]\\ & {\mathbf{B}}_N=50\mathbf{B}=\left[\begin{array}{c}0\\ 66.5\\ 0\\ 44.5\end{array}\right]\end{array}\text{\hspace{1em}(4-3)}$$
设置$\mathbf{Q}$、$R$如下
$$\begin{array}{rl}& \mathbf{Q}=\left[\begin{array}{cccc}4& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 4& 0\\ 0& 0& 0& 1\end{array}\right]\\ & R=0.25\end{array}\text{\hspace{1em}(4-4)}$$
在matlab中可使用函数lqr函数直接得出$\mathbf{K}$。
去matlab试一下效果：

角度变化情况：（蓝线是目标，黄线是实际）

位置变化情况：（蓝线是目标，黄线是实际）

不得不说在模型已知的情况下，LQR还是很吊的，随手设的$\mathbf{Q}$、$R$就能调出来收敛的效果。
未完待续。

5、（番外）关于级联PID控制器的踩坑记录

先把式(2-2)贴过来
$$\begin{array}{rl}& \ddot{\theta }=19.6\theta -0.4\dot{x}+1.33F\\ & \ddot{x}=3.27\theta -0.27\dot{x}+0.89F\end{array}\text{\hspace{1em}(2-2)}$$
用下面的代码

% 解出从推力到摆角/位移的传递函数（字母表达式）
clear; clc;
syms s x F theta % 参数

eq1 = s^2*theta == 19.6*theta-0.4*s*x+1.33*F;
eq2 = s^2*x == 3.27*theta-0.27*s*x+0.89*F;

eqns = [eq1, eq2];

vars = [theta, x];

% 解方程
sol = solve(eqns, vars);

% 提取theta和x的解x
theta_sol = sol.theta;
x_sol = sol.x;

% 整理传递函数
G_theta_F = theta_sol / F;
G_x_F = x_sol / F;

% 使用collect函数按s的幂次合并同类项
[G_theta_F_num, G_theta_F_den] = numden(G_theta_F);  % 提取分子分母
G_theta_F_num_collected = collect(G_theta_F_num, s);  % 按s合并分子
G_theta_F_den_collected = collect(G_theta_F_den, s);  % 按s合并分母
G_theta_F = G_theta_F_num_collected / G_theta_F_den_collected

[G_x_F_num, G_x_F_den] = numden(G_x_F);  % 提取分子分母
G_x_F_num_collected = collect(G_x_F_num, s);  % 按s合并分子
G_x_F_den_collected = collect(G_x_F_den, s);  % 按s合并分母
G_x_F = G_x_F_num_collected / G_x_F_den_collected

可以解出传递函数的表达式：
$$\begin{array}{rl}& G_{\theta }(s)=\frac{\Theta (s)}{F(s)}=\frac{1.33s+0.0031}{s^3+0.27s^2-19.6s-3.984}\\ & G_x(s)=\frac{X(s)}{F(s)}=\frac{0.89s^2-13.0949}{s(s^3+0.27s^2-19.6s-3.984)}\end{array}\text{\hspace{1em}(5-1)}$$
特征方程出现负系数，很明显系统不稳定。
以级联PID控制方案进行设计，级联PID控制流程图如下：

$$G_{x-\theta }(s)=\frac{\Theta (s)}{X(s)}=\frac{1.33s^2+0.0031s}{0.89s^2-13.0949}\text{\hspace{1em}(5-2)}$$
其中带下标 $d$ 的是给定量。
先看内环，加入第一篇笔记随手调的控制器（$K_p=100$，$K_i=0.5$，$K_d=10$）。
开环传递函数：
$$\begin{array}{rl}{G}_1(s)& =(\frac{0.5}{s}+100+10s)\frac{1.33s+0.0031}{s^3+0.27s^2-19.6s-3.984}\\ & =\frac{(10s^2+100s+0.5)(1.33s+0.0031)}{s(s^3+0.27s^2-19.6s-3.984)}\end{array}\text{\hspace{1em}(5-3)}$$
特征方程：
$$D(s)=s(s^3+0.27s^2-19.6s-3.984)\text{\hspace{1em}(5-4)}$$
为简单起见，我们直接看括号里那一部分即可，画劳斯表：

第一列有一次变号，表明有一个正实部特征根($P=1$)。
画出奈奎斯特图：

注意matlab画的图频率范围是$(-\infty ,\infty )$。要使用奈奎斯特判据，还要画一条增补线（红色）。曲线(-1, j0)的圈数和开环正实部特征根个数相等时系统稳定，容易看出，图线绕了(-1, j0)负1圈，所以系统不稳定。（尴尬）
后来想了一下，也问了一下AI，仿真摆角能收敛的原因是摆角的初始状态不为零（传递函数无法体现初始状态的信息），由于系统不稳定零输入响应也发散，所以闭环PID造就了一个同样发散的零状态响应“压制”住了零输入响应，使得输出信号看起来不发散，但内部状态是发散的（比如小车位置 $x$ ）。
注意观察 $G_1(s)=\frac{(10s^2+100s+0.5)(1.33s+0.0031)}{s(s^3+0.27s^2-19.6s-3.984)}$每个括号中的常数项，当$s\to j{0}_{+}$时，由于积分环节的存在，$G_1(s)$总会位于实轴上方无穷远处，因此奈奎斯特曲线的增补线一定会顺时针包围(-1, j0)点。
考虑将积分环节去除，改用比例控制试试，比例系数=100：

看起来还是有点难绷，图线绕了(-1, j0)点0圈，尝试了下增加微分环节或者增加串联滞后校正，也没搞出来让图线绕(-1, j0)点1圈，实现系统稳定。

代码地址：https://github.com/ouClassmate/Pendulum-Control-LQR.git