LQR——从公式到建模

LQR——从公式到建模

1、选用经典的倒立摆模型（闭环控制）

​

Goal：$\varphi =0$

小球 $X$ 方向位移：

​ $X=Lsin\varphi +\delta$

线性化：$\varphi \to 0sin\varphi =\varphi$

​ $X=L\varphi +\delta$

X方向力：

​ $F_x=m\ddot{x}$

对 F 进行受力分析

$F_x=Fsin\varphi =F\varphi$

$F_y=Fcos\varphi =F$

​ $F\varphi =mL\ddot{\varphi }+m\ddot{\delta }$ — 1

Y方向力：

​ $F_y=mg$

​ $F=mg$ — 2

​ 联立1、2式：$mg\varphi =mL\ddot{\varphi }+m\ddot{\delta }$

​ 把$m$消掉得系统的动态方程：

​ $\ddot{\varphi }-\frac{g}{L}\varphi +\frac{1}{L}\ddot{\delta }=0$

1. State Space

​ $x_1=\varphi$ $\Rightarrow$ $\dot{x_1}=\dot{\varphi }=x_2$

​ $x_2=\dot{\varphi }$ $\Rightarrow$ $\dot{x_2}=\ddot{\varphi }=\frac{g}{L}\varphi -\frac{1}{L}\ddot{\delta }=\frac{g}{L}{x}_1-u$

令$u=\frac{1}{L}\ddot{\delta }$

​ 状态空间方程：

​ $\left[\begin{array}{c}\dot{x_1}\\ \dot{x_2}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ \frac{g}{L}& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ -1\end{array}\right]u$

2. Open Loop

​ $A=\left[\begin{array}{cc}0& 1\\ \frac{g}{L}& 0\end{array}\right]$

​ $\left|\lambda I-A\right|=0\Rightarrow \left|\begin{array}{cc}\lambda & -1\\ -\frac{g}{L}& \lambda \end{array}\right|={\lambda }^2-\frac{g}{L}\Rightarrow \lambda =\pm \sqrt{\frac{g}{L}}$

$\lambda$有大于0的成分，不稳定！

3. Controllable?

​ $C_o=\left[\begin{array}{cc}B& AB\end{array}\right]=\left[\begin{array}{cc}0& -1\\ -1& 0\end{array}\right]$ $Rank\left(C_o\right)=2\Rightarrow 可控$

4. Let $u=-\left[\begin{array}{cc}{k}_1& k_2\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]$ 使得${\lambda }_1={\lambda }_2=-1$

​ $\left[\begin{array}{c}\dot{x_1}\\ \dot{x_2}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ \frac{g}{L}& 0\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ -1\end{array}\right]\left[\begin{array}{cc}-k_1& -k_2\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ \frac{g}{L}+k_1& k_2\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]$

​ 即$A_{CL}=\left[\begin{array}{cc}0& 1\\ \frac{g}{L}+k_1& k_2\end{array}\right]$

求$A_{CL}$特征值，令$\left|\lambda I-A_{CL}\right|=0$

得：$k_1=-1-\frac{g}{L}$ $k_2=-2$

​ 因此，输入$u=-\left[\begin{array}{cc}-1-\frac{g}{L}& -2\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]$

​ $=\left[\begin{array}{cc}1+\frac{g}{L}& 2\end{array}\right]\left[\begin{array}{c}\varphi \\ \dot{\varphi }\end{array}\right]$

2、引入LQR

**Goal：**选取不同的特征值$\lambda$，来决定系统是如何靠近这个状态的

上面为了简便，所以将${\lambda }_1={\lambda }_2=-1$，即保持系统稳定即可

引入Cost Function（目标函数，能量函数）

​ $minJ={\int }_0^{\infty }\left(X^{T}QX+U^{T}RU\right)dt$

$X^{T}QX$为正（二次型），展开为$a{x_1}^2+b{x_2}^2+c{x_3}^2\cdots \cdots$，表达了Penalty的概念：当$x\ne 0$时，J会根据ABC大小有不同的反应

$U^{T}RU$代表了整个输入对Cost Function的影响。R越大U对J的影响越大

LQR的意思是在同时满足系统稳定性的同时，去寻找Cost Function的最小值

3、Simulink仿真

​ 状态空间方程：
$${\dot{x}}_1=x_2$$

$${\dot{x}}_2=10x_1-u$$

$$u=-k_1{x}_1-k_2{x}_2$$

令$L=1,g=10$，可得$\dot{x_1}和\dot{x_2}$

​ 在Simulink中可搭建如下仿真：

1. 假设为开环系统（$k=0$）

先令$k_1=k_2=0$，构成开环系统

运行后点击Scope，可看出系统无法收敛（系统输出为无穷而非震荡是因为把$sin\varphi =\varphi$了）

2. 用LQR得到不同的K值

​ 在Matlab命令行中输入

clear all
A=[0 1;10 0]
B=[0;-1]
Q=[100 0;0 1]
R=0.01;
K=lqr(A,B,Q,R)

希望系统有快速的反应，因此Q中$x_1$的系数为100，但并不关心速度，因此$x_2$系数为1

同时并不关心输入大小为多少，因此R为0.01

​ 代码可输出：

K = -110.4988 -17.9164

3. 选取不同的Q和R

将上一个系统作为Q较大的子系统，现在再创建一个R较大的子系统

clear all
A=[0 1;10 0]
B=[0;-1]
Q=[1 0;0 1]
R=100;
K=lqr(A,B,Q,R)

不再关心收敛速度，因此Q的$x_1$系数为1

更关心输入，希望很小的输入就能完成收敛，因此$R=100$

代码可输出：

K = -20.0005 -6.3254

4. 结果讨论

黄线为Q较大的情况，红线为R较大的情况

通过选取不同的Q和R矩阵，可以获得不同的系统表现，对于黄线，需要更快达到收敛效果，对于红线，需要更高的能耗比