对倒立摆的LQR控制

1 问题建模

首先对待研究的问题建立数学模型
在倒立摆模型分析这篇文章里，我们已经做了完整的受力分析。最终得到了关于系统变量的微分方程。
$$(M+m)\ddot{x}+b\dot{x}-ml\ddot{\psi }=u$$
$$(I+m{l}^2)\ddot{\psi }-mgl\psi =ml\ddot{x}$$

2 状态空间

可以将状态空间理解为一个包含系统输入、系统输出和状态变量的集合，它们之间的关系可以用一个一阶微分方程表达出来。

$$状态空间（集合）=\left\{\begin{array}{r}系统输入\\ 系统输出\\ 状态变量\end{array}\right\}=一阶微分方程$$
通过观察我们知道，目前系统模型是以二阶微分方程的形式描述的。为了消除方程中的高阶项，可以整理得到下面的式子。
$$\{\begin{array}{l}\dot{x}=\dot{x}\\ \ddot{x}=\frac{m^2{l}^{2}g}{I(m+M)+mM{l}^2}\psi -\frac{b(I+m{l}^2)}{I(m+M)+mM{l}^2}\dot{x}+\frac{(I+m{l}^2)}{I(m+M)+mM{l}^2}u\\ \dot{\psi }=\dot{\psi }\\ \ddot{\psi }=\frac{mlg(m+M)}{I(m+M)+mM{l}^2}\psi -\frac{mlb}{I(m+M)+mM{l}^2}\dot{x}+\frac{ml}{I(m+M)+mM{l}^2}u\end{array}$$
已知系统状态空间方程的标准形式为：
$$\{\begin{array}{l}\dot{x}=Ax+Bu\\ y=Cx+Du\end{array}$$
式中$\dot{x}$表示系统中的一阶微分项，$y$表示系统状态。以矩阵运算的形式表示系统的状态空间方程：
$$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\psi }\\ \ddot{\psi }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{b(I+m{l}^2)}{I(m+M)+mM{l}^2}& \frac{m^2{l}^{2}g}{I(m+M)+mM{l}^2}& 0\\ 0& 0& 0& 1\\ 0& -\frac{mlb}{I(m+M)+mM{l}^2}& \frac{mlg(m+M)}{I(m+M)+mM{l}^2}& 0\end{array}\right]\times \left[\begin{array}{c}x\\ \dot{x}\\ \psi \\ \dot{\psi }\end{array}\right]+\left[\begin{array}{c}0\\ \frac{(I+m{l}^2)}{I(m+M)+mM{l}^2}\\ 0\\ \frac{ml}{I(m+M)+mM{l}^2}\end{array}\right]u$$
$$y=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\times \left[\begin{array}{c}x\\ \dot{x}\\ \psi \\ \dot{\psi }\end{array}\right]+\left[\begin{array}{c}0\end{array}\right]\times u$$

3 LQR控制器设计

L（Linear）Q（Quadratic）R（Regulator），直译为线性二次型控制器。
可以通过加入反馈，使系统最终能达到稳定状态，但如何选取最好的系统特征值（极点）在系统收敛的前提下实现最优的收敛过程，这是我们目前要考虑的问题。
这里我们引入目标函数（价值函数），使系统在收敛的同时，满足$J$最小：
$$\begin{gathered} J={\int }_{t_0}^{t_f}[X^{T}QX+U^{T}RU]dt \\ min(J) \end{gathered}$$

1. $Q$是一个对角矩阵，$X^{T}QX=a{x}_1^2+b{x}_2^2+c{x}_3^2+......$，当系统中的状态变量$x\ne 0$时，我们可以通过调节$Q$中元素的值来改变该变量的对$J$的影响。$Q$中较大的一项对应在收敛过程中优先考虑的系统变量。
2. 同理，$U^{T}RU$则代表了系统输入$U$对价值函数$J$的影响。因为$J$是积分的形式，当矩阵$R$中某一项较大，则意味着我们希望该项对应的系统输入能够快速收敛到0。这么做的现实意义往往是以最小的代价（例如能耗）实现系统的稳态。
   式子中代表系统输入的$U$在一个能够实现自稳定的系统中（例如我们这里设计的倒单摆系统）代表控制器反馈回路的输出。

们目前涉及的倒立摆系统只有一个输入，即倒立摆所受到的外部牵引力$U$，所以矩阵$R$仅有一个元素。另外有四个系统状态变量，分别是$x,\dot{x},\psi ,\dot{\psi }$，因此对角矩阵$Q$的规模为$4\times 4$。
在MATLAB中，我们可以调用$lqr()$函数生成满足$J$最小的反馈矩阵$K$。$K$对应的就是各个系统变量反馈路径中的增益。即：
$$U=[K_1,K_2,K_3,K_4]\times \left[\begin{array}{c}x\\ \dot{x}\\ \psi \\ \dot{\psi }\end{array}\right]$$

---

下图是倒立摆开环系统的阶跃响应。显然系统是不收敛的。

---

引入LQR反馈后系统的阶跃响应

4 MATLAB 代码

clc; 
clear; 
close all;

% Parameters:
%   m: mass of pendulum (kg)
%   M: mass of cart (kg)
%   b: dampling coefficient
%   I: rotional inertia
%   g: acceleration of gravity
%   L: the distance from mass center to the hinge

m = 3.375;
M = 5.40;
b = 0.01;
I = 0.0703125;
g = 9.80665;
L = 0.25;

%  create transfer function model
s = tf('s');

q = (m + M) * (I + m * L^2) - (m * L)^2;

P_cart = (((I + m * L^2) / q) * s^2 - (m * g * L / q)) / ...
    (s^4 + (b * (I + m * L^2)) * s^3 / q - ((M + m) * m * g * L) * s^2 / q - b * m * g * L * s / q);

P_pend = (m * L * s / q) / ...
    (s^3 + (b * (I + m * L^2)) * s^2 / q - ((M + m) * m * g * L) * s / q - b * m * g * L / q);

sys_tf = [P_cart; P_pend];

inputs = {'u'}; 
outputs = {'x'; 'phi'};
set(sys_tf,'InputName',inputs);
set(sys_tf,'OutputName',outputs);

sys_tf

% create state-space model
p = I * (m + M) + m * M * L^2;

A = [0, 1, 0, 0;
     0, -b * (I + m * L^2) / p, (m^2 * L^2 * g) / p, 0;
     0, 0, 0, 1;
     0, -(m * L * b) / p, m * L * g * (M + m) / p, 0];
 
B = [0;
     (I + m * L^2) / p;
     0;
     m * L / p];

C = [1, 0, 0, 0;
     0, 0, 1, 0];
 
D = [0;
     0];

% definitions of system variables
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'}; outputs = {'x'; 'phi'};

sys_ss = ss(A, B, C, D, 'statename', states, 'inputname', inputs, 'outputname', outputs);

% poles of open loop system
poles = pole(sys_tf);
poles

% impulse response of system
t = 0: 0.01: 1;
impulse(sys_ss, t);

% step response of system
t = 0: 0.01: 1;
step(sys_ss, t);


% LQR simulation
% Q matrix of LQR controller
Q = [1000, 0, 0, 0;
     0, 0, 0, 0;
     0, 0, 500, 0;
     0, 0, 0, 0];
 
R = 0.1;

% optimal gain matrix K
K = lqr(A, B, Q, R);

Ac = A - B * K;
sys_lqr = ss(Ac, B, C, D, 'statename', states, 'inputname', inputs, 'outputname', outputs);

% impulse response of system
t = 0: 0.01: 2;
impulse(sys_lqr, t);100

% step response of system
t = 0: 0.01: 3;
step(sys_lqr, t);