一级倒立摆控制 —— LQR 控制器设计及 MATLAB 实现

系列文章目录

最优控制介绍
一级倒立摆控制 —— 系统建模（传递函数模型与状态空间方程表示）
一级倒立摆控制 —— PID 控制器设计及 MATLAB 实现
一级倒立摆控制 —— MPC 控制器设计及 MATLAB 实现

---

---

一、一阶倒立摆系统的动力学方程

1.1 系统变量表：

参数	符号	数值
小车质量	$M$	0.5 kg
摆杆质量	$m$	0.2 kg
小车摩擦系数	$b$	0.1 N/m/sec
摆杆转动轴心到杆质心的长度	$l$	0.3 m
摆杆转动惯量	$I$	0.006 kg*m^2
施加在小车上的力	$F$
小车位置坐标	$x$
摆杆偏离竖直方向的角度	$\theta$

1.2 一阶倒立摆系统的动力学方程

一阶倒立摆系统的动力学方程用状态空间表示为
$$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\varphi }\\ \ddot{\varphi }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& \frac{-(I+m{l}^2)b}{I(M+m)+Mm{l}^2}& \frac{m^{2}g{l}^2}{I(M+m)+Mm{l}^2}& 0\\ 0& 0& 0& 1\\ 0& \frac{-mlb}{I(M+m)+Mm{l}^2}& \frac{mgl(M+m)}{I(M+m)+Mm{l}^2}& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \varphi \\ \dot{\varphi }\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$$
$$\mathbf{y}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\varphi }\\ \ddot{\varphi }\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]u$$
将上述表格系统值代入得状态空间方程为
$$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\\ \dot{\varphi }\\ \ddot{\varphi }\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -0.1818& 2.6727& 0\\ 0& 0& 0& 1\\ 0& -0.4545& 31.1818& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \varphi \\ \dot{\varphi }\end{array}\right]+\left[\begin{array}{c}0\\ 1.8182\\ 0\\ 4.5455\end{array}\right]u$$
$$\mathbf{y}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ \varphi \\ \dot{\varphi }\end{array}\right]+\left[\begin{array}{c}0\\ 0\end{array}\right]u$$

---

二、LQR控制

系统中输出为小车的位置 $x$ 和摆杆与竖直方向的角度 $\varphi$，小车在 $x$ 方向上移动 0.2 m，设计要求如下：

1. 位移 $x$ 和摆角 $\varphi$ 的稳定时间小于 5s
2. 位移 $x$ 的上升时间小于 0.5s
3. 摆角 $\varphi$ 不超过 20°
4. 位移 $x$ 和摆角 $\varphi$ 的稳态误差小于 2％

LQR 控制非常适用于多输入多输出变量控制，因此控制要求中保证摆角范围的同时，还要求位移大小。
采用全状态反馈控制，控制框图如下

2.1 确定系统开环极点

在一阶倒立摆系统中，$r$ 表示小车位置的阶跃指令，4 个状态变量为小车的位置 $x$ 和速度 $\dot{x}$，摆杆的角度 $\varphi$ 和角速度 $\dot{\varphi }$，输出为为小车的位置 $x$ 和摆杆角度 $\varphi$。

参数	符号
位置阶跃指令	$r$
小车位移	$x$
小车速度	$\dot{x}$
摆杆角度	$\varphi$
摆杆角速度	$\dot{\varphi }$

目标为设计一个控制器，当给系统一个阶跃参考值时，摆杆摆动但最终保持竖直状态，小车则移动到指定的位置。

确定系统的开环极点，代码如下

M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;

p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices

A = [0      1              0           0;
     0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;
     0      0              0           1;
     0 -(m*l*b)/p       m*g*l*(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];

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 = eig(A)

输出为

poles =

         0
   -0.1428
   -5.6041
    5.5651

可以看到，系统只有一个右极点，开环系统是不稳定的。

2.2 设计 LQR 控制器

根据控制框图，需要确定状态反馈矩阵（向量）K，使用状态反馈矩阵的前提是状态变量是可测的。
下面使用 MATLAB 的 lqr() 命令，该命令可以返回最佳状态反馈控制器增益矩阵，该前提是假设系统为线性，代价函数为二次型，

2.2.1 确定系统可控性

在设计 LQR 控制器之前，需要验证系统的可控性。系统可控意味着可以在有限时间内（在系统的物理约束条件下）将系统的状态驱动至任意位置。如果系统是可控的，可控性矩阵的秩为 n。

MATLAB 代码如下

co = ctrb(sys_ss);
controllability = rank(co)

输出为

controllability =

     4

2.2.2 设计 LQR 控制器

用线性二次型控制方法来确定状态反馈增益矩阵 $K$，$R$ 和 $Q$ 分别表示在最优化的代价函数中控制输入和误差的相对权重。最简单的情况是令 $R=1,Q=C^{\prime }C$，这表示代价函数中对控制输入和对状态变量有相同权重。通过调节控制器中矩阵 $Q$ 中的非零元素得到合适的响应。
观察矩阵 $Q$ ，MATLAB 代码如下

Q = C'*C

输出为

Q =

     1     0     0     0
     0     0     0     0
     0     0     1     0
     0     0     0     0

矩阵 $Q$ 中（1,1）位置的元素表示小车位移的权重，（3,3）位置的元素表示摆杆角度的权重。
权重 $R$ 保持为 1，最终起作用的是 $Q$ 和 $R$ 的相对值，而不是绝对值。
根据 $Q$ 和 $R$ 设计 LQR 控制器。MATLAB 代码如下

Q = C'*C;
R = 1;
K = lqr(A,B,Q,R)

Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];

states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'r'};
outputs = {'x'; 'phi'};

sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;
r =0.2*ones(size(t));
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response with LQR Control')

输出为

K =

   -1.0000   -1.6567   18.6854    3.4594

橙色表示摆杆角度变化曲线，蓝色表示小车位移变化曲线。
回顾控制器的设计要求

1. 位移 $x$ 和摆角 $\varphi$ 的稳定时间小于 5s
2. 位移 $x$ 的上升时间小于 0.5s
3. 摆角 $\varphi$ 不超过 20°
4. 位移 $x$ 和摆角 $\varphi$ 的稳态误差小于 2％

摆杆和小车的超调量满足要求，稳定时间需要调整，小车位移 $x$ 的上升时间需要减小，小车位移误差需要调整。

2.2.3 改变矩阵 $Q$ 改进 LQR 控制器

对于小车和摆杆状态变量的稳定时间和上升时间，调整矩阵 $Q$ 发现，增大矩阵 $Q$ 中（1,1）和（3,3）位置的元素可以使稳定时间和上升时间下降，减小摆杆移动的角度。这是以增加控制为代价减小误差，即减小稳定时间和上升时间。但是更多的控制意味着需要更大的能量输入。

MATLAB代码如下

Q = C'*C;
Q(1,1) = 5000;
Q(3,3) = 100
R = 1;
K = lqr(A,B,Q,R)

Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];

states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'r'};
outputs = {'x'; 'phi'};

sys_cl = ss(Ac,Bc,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;
r =0.2*ones(size(t));
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response with LQR Control')

输出为

Q =

        5000           0           0           0
           0           0           0           0
           0           0         100           0
           0           0           0           0


K =

  -70.7107  -37.8345  105.5298   20.9238

2.2.4 增加预补偿环节改进 LQR 控制器

如上框图所示，通过增加预补偿环节 $\overline{N}$ 实现稳态误差最小。预补偿环节 $\overline{N}$ 可以通过函数 rscale.m 实现，rscale.m 文件内容如下

         function[Nbar]=rscale(a,b,c,d,k)
         % Given the single-input linear system:
         %       .
         %       x = Ax + Bu
         %       y = Cx + Du
         % and the feedback matrix K,
         %
         % the function rscale(sys,K) or rscale(A,B,C,D,K)
         % finds the scale factor N which will
         % eliminate the steady-state error to a step reference
         % for a continuous-time, single-input system
         % with full-state feedback using the schematic below:
         %
         %                         /---------\
         %      R         +     u  | .       |
         %      ---> N --->() ---->| X=Ax+Bu |--> y=Cx ---> y
         %                -|       \---------/
         %                 |             |
         %                 |<---- K <----|
         %
         %8/21/96 Yanjie Sun of the University of Michigan
         %        under the supervision of Prof. D. Tilbury
         %6/12/98 John Yook, Dawn Tilbury revised
         error(nargchk(2,5,nargin));
         % --- Determine which syntax is being used ---
         nargin1 = nargin;
         if (nargin1==2),	% System form
         		[A,B,C,D] = ssdata(a);
         		K=b;
         elseif (nargin1==5), % A,B,C,D matrices
         		A=a; B=b; C=c; D=d; K=k;
         else error('Input must be of the form (sys,K) or (A,B,C,D,K)')
         end;
         % compute Nbar
         s = size(A,1);
         Z = [zeros([1,s]) 1];
         N = inv([A,B;C,D])*Z';
         Nx = N(1:s);
         Nu = N(1+s);
         Nbar=Nu + K*Nx;  

修改矩阵 $C$ 以说明参考仅仅针对小车位移。
MATLAB代码如下

Cn = [1 0 0 0];
sys_ss = ss(A,B,Cn,0);
Nbar = rscale(sys_ss,K)

输出为

Nbar =

  -70.7107

加入预补偿环节 $\overline{N}$ 之后MATLAB代码为

sys_cl = ss(Ac,Bc*Nbar,Cc,Dc,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;
r =0.2*ones(size(t));
[y,t,x]=lsim(sys_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response with Precompensation and LQR Control')

可以看到稳态误差，上升时间，稳定时间，超调量均符合要求。

2.2.5 基于状态观测器的 LQR 控制器设计

以上全状态反馈控制器是假设系统状态变量都是可测的，但是很多情况下并不现实。因此需要设计状态估计器，带全状态估计器的控制系统框图如下所示

在设计估计器之前，我们首先要验证系统是否可观测。可观测性决定了我们是否能根据测量到的系统输出估计出系统的状态。与验证可控性的过程类似，如果系统的可观测性矩阵是满级的，那么该系统就是可观测的。可观测矩阵的定义如下
$$\begin{array}{c}\mathbf{O}=\left[\begin{array}{c}C\\ CA\\ C{A}^2\\ ⋮\\ C{A}^{n-1}\end{array}\right]\end{array}$$
如下图所示，我们可以使用 MATLAB 命令 obsv 来构建可观测性矩阵，并使用秩命令来检查其秩。

ob = obsv(sys_ss);
observability = rank(ob)

输出为

observability =
     4

由于可观测矩阵为 8x4，秩为 4，因此它是全秩矩阵，我们的系统是可观测的。这种情况下的可观测矩阵不是方阵，因为我们的系统有两个输出。请注意，如果我们只能测量摆角输出，就无法估计系统的全部状态。这一点可以从 obsv(A,C(2,:)) 产生的可观测性矩阵不是全秩矩阵这一事实中得到验证。

既然我们知道可以估算出系统状态，那么现在就来介绍一下设计状态估算器的过程。根据上述图表，状态估计的动态方程如下。
$$\begin{array}{c}\dot{\hat{\mathbf{x}}}=A\hat{\mathbf{x}}+Bu+L(\mathbf{y}-\hat{\mathbf{y}})\end{array}$$
该方程的核心与闭环控制相似，最后一项是基于反馈的修正。具体来说，最后一项是根据实际输出 $y$ 和估计输出 $\hat{y}$ 之间的差值来修正状态估计值。现在我们来看看状态估计误差的动态变化（动态变化由导数体现）。
$$\begin{array}{c}\dot{\hat{\mathbf{e}}}=\dot{\mathbf{x}}-\dot{\hat{\mathbf{x}}}=(A\mathbf{x}+Bu)-(A\hat{\mathbf{x}}+Bu+L(C\mathbf{x}-C\hat{\mathbf{x}}))\end{array}$$
因此，状态估计误差动态描述为
$$\begin{array}{c}\dot{\hat{\mathbf{e}}}=(A-LC)\mathbf{e}\end{array}$$
如果矩阵 $A-LC$ 是稳定的（具有负特征值），误差将趋近于零（$\hat{\mathbf{x}}$ 将趋近于 $\mathbf{x}$）。与控制的情况一样，收敛速度取决于估计器的极点（$A-LC$ 的特征值）。由于我们计划将状态估计值作为控制器的输入，因此我们希望状态估计值的收敛速度快于整个闭环系统的收敛速度。也就是说，我们希望观测器极点的收敛速度快于控制器极点的收敛速度。通常的指导原则是让估计极点比最慢的控制器极点快 4-10 倍。如果测量受到噪声干扰或传感器测量存在一般误差，则估计器极点速度过快会造成问题。

根据这一逻辑，我们必须首先找到控制器极点。为此，请将以下代码复制到 m 文件末尾。如果使用更新后的 $Q$ 矩阵，您将在 MATLAB 命令窗口中看到以下极点。

poles = eig(Ac)

poles =
  -8.4910 + 7.9283i
  -8.4910 - 7.9283i
  -4.7592 + 0.8309i
  -4.7592 - 0.8309i

最慢极点的实部等于 -4.7592 ，因此，我们将估计器的极点设在 -40 。由于闭环估计器的动态由矩阵 ($A-LC$) 决定，而矩阵 ($A-LC$) 的形式与决定状态反馈系统动态的矩阵 ($A-BK$) 相似，因此我们可以使用与寻找状态反馈增益 $K$ 相同的命令来寻找估计器增益 $L$。具体来说，由于对$A-LC$进行转置后，特征值保持不变，并得到与$A-BK$形式完全一致的结果$A^{\prime }-C^{\prime }{L}^{\prime }$，因此我们可以使用acker或place命令。考虑到放置命令不能放置乘数大于 1 的极点，我们将如下放置观察者极点。在 m 文件中添加以下命令来计算 $L$ 矩阵并生成下图所示的输出。

P = [-40 -41 -42 -43];
L = place(A',C',P)'

L =
   1.0e+03 *
    0.0826   -0.0010
    1.6992   -0.0402
   -0.0014    0.0832
   -0.0762    1.7604

我们使用两个输出（摆锤的角度和小车的位置）来设计观测器。

现在，我们将把之前的状态反馈控制器与状态估计器结合起来，得到完整的补偿器。由此产生的闭环系统由以下矩阵方程描述。

$$\begin{array}{c}\left[\begin{array}{c}\dot{\mathbf{x}}\\ \dot{\mathbf{e}}\end{array}\right]=\left[\begin{array}{cc}A-BK& BK\\ 0& A-LC\end{array}\right]\left[\begin{array}{c}\mathbf{x}\\ \mathbf{e}\end{array}\right]+\left[\begin{array}{c}B\overline{N}\\ 0\end{array}\right]r\end{array}$$
$$\begin{array}{c}\mathbf{y}=\left[\begin{array}{cc}C& 0\end{array}\right]\left[\begin{array}{c}\mathbf{x}\\ \mathbf{e}\end{array}\right]+\left[\begin{array}{c}0\end{array}\right]r\end{array}$$
在 m 文件末尾添加以下命令即可在 MATLAB 中实现上述闭环系统。运行 m 文件后，将生成如图所示的阶跃响应。

Ace = [(A-B*K) (B*K);
       zeros(size(A)) (A-L*C)];
Bce = [B*Nbar;
       zeros(size(B))];
Cce = [Cc zeros(size(Cc))];
Dce = [0;0];

states = {'x' 'x_dot' 'phi' 'phi_dot' 'e1' 'e2' 'e3' 'e4'};
inputs = {'r'};
outputs = {'x'; 'phi'};

sys_est_cl = ss(Ace,Bce,Cce,Dce,'statename',states,'inputname',inputs,'outputname',outputs);

t = 0:0.01:5;
r = 0.2*ones(size(t));
[y,t,x]=lsim(sys_est_cl,r,t);
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
title('Step Response with Observer-Based State-Feedback Control')

这一响应与假设我们可以完全访问状态变量时的响应几乎完全相同。这是因为观测器极点的速度很快，而且我们为观测器假设的模型与实际工厂的模型（包括相同的初始条件）完全相同。因此，只需花费最少的控制精力，就能满足所有设计要求。无需进一步迭代。

这个例子说明，使用状态空间法控制多输入、多输出系统要比我们介绍的其他方法容易得多。

---