二次型最优控制(二)

本文来自潘神的知乎笔记什么是二次型最优控制

代价函数为什么要引入$u$u

前面我们说的都是假设是零输入条件，稳定的系统状态量都会衰减到零，但是绝大多数系统都不会是零输入，比如一般的状态方程为：
$\dot{\boldsymbol{x}}=\boldsymbol{A} \boldsymbol{x}+\boldsymbol{B} \boldsymbol{u}$x˙=Ax+Bu
对于二次型最优控制来说， $P$P是一个非常关键的矩阵，我们在计算 $P$P的时候，是假定 $\dot{\boldsymbol{x}}=\boldsymbol{A} \boldsymbol{x}$x˙=Ax ，也就是输入恒为零的，如果把输入 $u$u也引进来，我们前面的分析就失效了啊，怎么办呢？——解决方法就是使用状态反馈，比如将输入 $u$u 和状态变量建立线性联系，
$\boldsymbol{u}=-\boldsymbol{K} \boldsymbol{x}$u=−Kx
可以是相互较为独立的联系
$u_{1}=-k_{1} x_{1}, \quad u_{2}=-k_{2} x_{2}, \quad \ldots, \quad u_{m}=-k_{m} x_{m}$u1​=−k1​x1​,u2​=−k2​x2​,…,um​=−km​xm​

当然还可以更一般一点

$u_{1}=-k_{1}\left(x_{1}+x_{2}\right), \quad u_{2}=-k_{2}\left(x_{2}+x_{3}\right), \quad \ldots$u1​=−k1​(x1​+x2​),u2​=−k2​(x2​+x3​),…

最通用的形式如下：
$\left[ \begin{array}{c}{\boldsymbol{u}_{1}} \\ {\vdots} \\ {\boldsymbol{u}_{m}}\end{array}\right]=\left[ \begin{array}{ccc}{\boldsymbol{k}_{11}} & {\ldots} & {\boldsymbol{k}_{1 n}} \\ {\vdots} & {\ddots} & {\vdots} \\ {\boldsymbol{k}_{m 1}} & {\ldots} & {\boldsymbol{k}_{m n}}\end{array}\right] \left[ \begin{array}{c}{\boldsymbol{x}_{1}} \\ {\vdots} \\ {\boldsymbol{x}_{n}}\end{array}\right]$⎣⎢⎡​u1​⋮um​​⎦⎥⎤​=⎣⎢⎡​k11​⋮km1​​…⋱…​k1n​⋮kmn​​⎦⎥⎤​⎣⎢⎡​x1​⋮xn​​⎦⎥⎤​

代入状态方程 $\dot{\boldsymbol{x}}=\boldsymbol{A} \boldsymbol{x}+\boldsymbol{B} \boldsymbol{u}$x˙=Ax+Bu就可以得到：
$\dot{\boldsymbol{x}}=\underbrace{(\boldsymbol{A}-\boldsymbol{B K})}_{\text { New } \mathrm{A}} \boldsymbol{x}=\boldsymbol{H} \boldsymbol{x}$x˙= New A(A−BK)​​x=Hx

可见，通过状态反馈，有输入的状态方程变得和无输入在形式上一模一样了，这样就可以借用我们前面所有的分析结论，即损失函数为

$J=\int_{0}^{\infty} \boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x} d t=\boldsymbol{x}(0)^{T} \boldsymbol{P} \boldsymbol{x}(0)$J=∫0∞​xTQxdt=x(0)TPx(0)
矩阵$P$P满足$\boldsymbol{H}^{T} \boldsymbol{P}+\boldsymbol{P} \boldsymbol{H}=-\boldsymbol{Q}$HTP+PH=−Q

举个例子：

$\left[ \begin{array}{c}{\dot{x}_{1}} \\ {\dot{x}_{2}}\end{array}\right]=\underbrace{\left[ \begin{array}{ll}{0} & {1} \\ {0} & {0}\end{array}\right]}_{A} \left[ \begin{array}{l}{x_{1}} \\ {x_{2}}\end{array}\right]+\underbrace{\left[ \begin{array}{l}{0} \\ {1}\end{array}\right] u}_{B} u$[x˙1​x˙2​​]=A[00​10​]​​[x1​x2​​]+B[01​]u​​u
不失一般性，我们假定状态反馈为：
$u=-k_{1} x_{1}-k_{2} x_{2}$u=−k1​x1​−k2​x2​
这样状态方程就变为：

$\dot{\boldsymbol{x}}=\boldsymbol{H x}=\left[ \begin{array}{cc}{0} & {1} \\ {-k_{1}} & {-k_{2}}\end{array}\right] \left[ \begin{array}{l}{x_{1}} \\ {x_{2}}\end{array}\right]$x˙=Hx=[0−k1​​1−k2​​][x1​x2​​]
为简单起见，我们假设$\bm{Q=I}$Q=I，此时：
$J=\int_{0}^{\infty} \boldsymbol{x}^{T} \boldsymbol{x} d t=\boldsymbol{x}(0)^{T} \boldsymbol{P} \boldsymbol{x}(0)$J=∫0∞​xTxdt=x(0)TPx(0)

数学上可以证明，若$k_1$k1​和 $k_2$k2​都没有约束，则当 $k_1=k_2=k$k1​=k2​=k时，损失函数 $J$J取得极小值。具体可参见Richard C. Dorf & Robert H. Bishop“Modern Control Systems Twelfth Edition”。此时：
$\dot{\boldsymbol{x}}=\boldsymbol{H x}=\left[ \begin{array}{cc}{0} & {1} \\ {-k} & {-k}\end{array}\right] \left[ \begin{array}{l}{x_{1}} \\ {x_{2}}\end{array}\right]$x˙=Hx=[0−k​1−k​][x1​x2​​]

可以得到满足$\boldsymbol{H}^{T} \boldsymbol{P}+\boldsymbol{P} \boldsymbol{H}=-\boldsymbol{Q}$HTP+PH=−Q的$P$P为
$\boldsymbol{P}=\left[ \begin{array}{cc}{\frac{1+2 k}{2 k}} & {\frac{1}{2 k}} \\ {\frac{1}{2 k}} & {\frac{k+1}{2 k^{2}}}\end{array}\right]$P=[2k1+2k​2k1​​2k1​2k2k+1​​]
嘉定初始状态$\boldsymbol{x}^{T}(0)=\left[ \begin{array}{ll}{1} & {0}\end{array}\right]$xT(0)=[1​0​]，带入代价函数$J=\boldsymbol{x}(0)^{T} \boldsymbol{P} \boldsymbol{x}(0)=1+\frac{1}{2 k}$J=x(0)TPx(0)=1+2k1​，当$J$J取极小值时，$k$k趋近于无穷大，当$k$k很大时，意味着什么呢？
$u(t)=-k\left[x_{1}(t)+x_{2}(t)\right]$u(t)=−k[x1​(t)+x2​(t)]
这样计算下来的 $u$u 就会非常大。但是，我们通常的输入信号往往会受到物理上的限制，比如运动控制最常用的电机，其控制信号为输入电压，就受到外接电源的限制，航空低压电源一般是28Vdc，我们们就必须对输入进行约束：

$|u(t)| \leq 28$∣u(t)∣≤28
这样最大可接受的 $k$k取值为：

$k_{\max }=\frac{|u|_{\max }}{x_{1}(0)}=28$kmax​=x1​(0)∣u∣max​​=28
带入代价函数
$J_{\min }=1+\frac{1}{2 k_{\max }}=1.02$Jmin​=1+2kmax​1​=1.02
和 $u$u的理论最小值1非常接近，也就是说，我们无限制提高 $u$u 明没有明显改善系统的性能，但是付出的代价很大。之所以会出现 $u$u 很大的原因，是因为在我们的代价函数里面只有误差 $e$e 积累量，而控制量 $u$u 没有任何体现。怎么解决呢？——也很好办，我们参照误差的处理方式，将控制量 $u$u 也引入到代价函数里面，而且形式也保持一致，具体如下：

$J=\int_{0}^{\infty}\left(\boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x}+\boldsymbol{u}^{T} \boldsymbol{R} \boldsymbol{u}\right) d t$J=∫0∞​(xTQx+uTRu)dt

其中矩阵 $Q$Q和 $R$R是自己定义的值，用来调整误差量和控制量之前的权重。

如何实现一般二次型最优控制

对于状态方程：
$\dot{\boldsymbol{x}}=\boldsymbol{A x}+\boldsymbol{B u}$x˙=Ax+Bu
状态反馈为：
$\boldsymbol{u}(t)=-\boldsymbol{K} \boldsymbol{x}(t)$u(t)=−Kx(t)
按照上一节的介绍，定义代价函数为：
、
$J=\int_{0}^{\infty}\left(\boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x}+\boldsymbol{u}^{T} \boldsymbol{R} \boldsymbol{u}\right) d t$J=∫0∞​(xTQx+uTRu)dt
其中$Q$Q和$R$R都是是对称矩阵。
$\dot{\boldsymbol{x}}=\boldsymbol{A} \boldsymbol{x}-\boldsymbol{B} \boldsymbol{K} \boldsymbol{x}=(\boldsymbol{A}-\boldsymbol{B} \boldsymbol{K}) \boldsymbol{x}$x˙=Ax−BKx=(A−BK)x
注意$A-B K$A−BK是要保持系统稳定的，即$A-B K$A−BK特征值都具有负实部，我们这个状态方程代入到代价函数里面：

$\begin{aligned} J &=\int_{0}^{\infty}\left(\boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x}+\boldsymbol{x}^{T} \boldsymbol{K}^{T} \boldsymbol{R} \boldsymbol{K} \boldsymbol{x}\right) d t \\ &=\int_{0}^{\infty} \boldsymbol{x}^{T}\left(\boldsymbol{Q}+\boldsymbol{K}^{T} \boldsymbol{R} \boldsymbol{K}\right) \boldsymbol{x} d t \end{aligned}$J​=∫0∞​(xTQx+xTKTRKx)dt=∫0∞​xT(Q+KTRK)xdt​
和前面的处理方式一致，我还是要找个这个积分的原函数
$\frac{d}{d t}\left(\boldsymbol{x}^{T} \boldsymbol{P} \boldsymbol{x}\right)=-\boldsymbol{x}^{T}\left(\boldsymbol{Q}+\boldsymbol{K}^{T} \boldsymbol{R} \boldsymbol{K}\right) \boldsymbol{x}$dtd​(xTPx)=−xT(Q+KTRK)x

其中 $4P$4P为实对称矩阵，把状态方程带进去，就可以得到 $P$P需要满足

$(A-B K)^{T} P+P(A-B K)=-\left(Q+K^{T} R K\right)$(A−BK)TP+P(A−BK)=−(Q+KTRK)
则有
$\begin{aligned} J &=\int_{0}^{\infty} \boldsymbol{x}^{T}\left(\boldsymbol{Q}+\boldsymbol{K}^{T} \boldsymbol{R} \boldsymbol{K}\right) \boldsymbol{x} d t \\ &=-\boldsymbol{x}^{T} \boldsymbol{P}\left.\boldsymbol{x}\right|_{0} ^{\infty} \\ &=\boldsymbol{x}^{T}(0) \boldsymbol{P} \boldsymbol{x}(0)-\boldsymbol{x}^{T}(\infty) \boldsymbol{P} \boldsymbol{x}(\infty) \end{aligned}$J​=∫0∞​xT(Q+KTRK)xdt=−xTPx∣0∞​=xT(0)Px(0)−xT(∞)Px(∞)​
当然，对于稳定的系统$x(\infty) \rightarrow 0$x(∞)→0，因此可以得到：
$J=\boldsymbol{x}^{T}(0) \boldsymbol{P} \boldsymbol{x}(0)$J=xT(0)Px(0)

当然，计算 $P$P和$K$K 还是相对比较繁琐的，我们不用手动去算了，MATLAB提供有现成的函数，语法为：

K = lqr(A,B,Q,R)
[K,P,E] = lqr(A,B,Q,R)

$K.P$K.P相信不用解释，都能看懂， $E$E 为$(A-BK)$(A−BK) 特征值。

例如：
$\boldsymbol{A}=\left[ \begin{array}{ccc}{0} & {1} & {0} \\ {0} & {0} & {1} \\ {-35} & {-27} & {-9}\end{array}\right], \quad \boldsymbol{B}=\left[ \begin{array}{l}{0} \\ {0} \\ {1}\end{array}\right]$A=⎣⎡​00−35​10−27​01−9​⎦⎤​,B=⎣⎡​001​⎦⎤​
代价函数定义为：
$J=\int_{0}^{\infty}\left(\boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x}+u^{T} R u\right) d t$J=∫0∞​(xTQx+uTRu)dt
定义权重矩阵：
$\boldsymbol{A}=\left[ \begin{array}{lll}{1} & {0} & {0} \\ {0} & {1} & {0} \\ {0} & {0} & {1}\end{array}\right], R=1$A=⎣⎡​100​010​001​⎦⎤​,R=1

MATLAB代码为;

A = [0 1 0;0 0 1;-35 -27 -9];
B = [0;0;1];
Q = [1 0 0;0 1 0;0 0 1];
R = [1];
[K,P,E] = lqr(A,B,Q,R)

计算结果如下:

K =
0.0143 0.1107 0.0676
P =
4.2625 2.4957 0.0143
2.4957 2.8150 0.1107
0.0143 0.1107 0.0676
E =
-5.0958
-1.9859 + 1.7110i
-1.9859 - 1.7110i

假设状态向量的初始值为

$\boldsymbol{x}(0)=\left[ \begin{array}{l}{1} \\ {0} \\ {0}\end{array}\right]$x(0)=⎣⎡​100​⎦⎤​
状态反馈为 $u=-K x$u=−Kx ,则状态方程变为：
$\dot{\boldsymbol{x}}=(\boldsymbol{A}-\boldsymbol{B K}) \boldsymbol{x}$x˙=(A−BK)x
MATLAB代码如下：

sys = ss(A-B*K, eye(3),eye(3),0);
t = 0:0.01:8;
x = initial(sys,[1;0;0],t);
x1 = [1 0 0]*x';
x2 = [0 1 0]*x';
x3 = [0 0 1]*x';
h1=figure(1);
set(h1,'Position',[200 300 1000 300],'Color','white')
subplot(1,3,1); plot(t,x1,'k','linewidth',1.5); grid
xlabel('t (sec)'); ylabel('x1')
subplot(1,3,2); plot(t,x2,'k','linewidth',1.5); grid
xlabel('t (sec)'); ylabel('x2')
subplot(1,3,3); plot(t,x3,'k','linewidth',1.5); grid
xlabel('t (sec)'); ylabel('x3')

各状态变量变化如下：

在现代控制理论中，我们进行状态反馈，都是根据极点进行配置，有了LQR，我们就可以很方便地找到最佳极点了。

终值定理及系统的型别

前面说的，都是零输入情况，即状态方程为：

$\dot{\boldsymbol{x}}=\boldsymbol{A x}$x˙=Ax

或通过状态反馈整定成零输入系统，实际上我们碰到更多的可能是跟随系统，或者叫伺服系统，是有输入的，那怎么研究这种系统呢？我们先有必要介绍一下什么是系统的型别（type），先从终值定理开始。
如果一个函数 $f(s)$f(s) 在空间 $(0, \infty)$(0,∞)是有界的（bounded），则
$\lim _{t \rightarrow \infty} f(t)=\lim _{s \rightarrow 0} s F(s)$t→∞lim​f(t)=s→0lim​sF(s)
一个很简洁的表达式，其中 $f(s)$f(s) 和$F(s)$F(s) 为拉普拉斯变换对，这称之为终值定理。这个公式有啥意义呢？——它提供了一种方式，当时间趋向于无穷大时，可以在频域计算时域的值，因为我们很多系统是在频域描述的，这样我们就可以在不进行拉普拉斯变化的情况下计算极限值或者说是稳定值，特别是在计算稳定误差的时候。

对于一个常见的闭环控制系统，误差为：
$E(s)=R(s)-C(s)$E(s)=R(s)−C(s)
输出和误差的关系为：
$C(s)=E(s) G(s)$C(s)=E(s)G(s)
所以可以得到误差和输入的关系为：
$\mathrm{E}(\mathrm{s})=\frac{1}{1+G(\mathrm{s})} \mathrm{R}(\mathrm{s})$E(s)=1+G(s)1​R(s)
根据终值定理，有
$\lim _{t \rightarrow \infty} e(t)=\lim _{s \rightarrow 0} s E(s)$t→∞lim​e(t)=s→0lim​sE(s)
代入误差的表达式，可以计算稳态误差为：
$e_{s s}=\lim _{t \rightarrow \infty} e(t)=\lim _{s \rightarrow 0} \frac{s R(s)}{1+G(s)}$ess​=t→∞lim​e(t)=s→0lim​1+G(s)sR(s)​

当输入信号为阶跃信号时（拉普拉斯变换为$\frac{1}{s}$s1​ ）：

$e_{s s}=\lim _{s \rightarrow 0} \frac{s}{1+G(s)} \frac{1}{s}$ess​=s→0lim​1+G(s)s​s1​

可见， $G(s)$G(s)就变得非常关键了，把 $G(s)$G(s)写成如下形式：
$G(s)=\frac{K \prod_{i=1}^{p}\left(T_{i} s+1\right)}{s^{N} \prod_{j=1}^{q}\left(T_{j} s+1\right)}$G(s)=sN∏j=1q​(Tj​s+1)K∏i=1p​(Ti​s+1)​

我们把 $N$N称之为系统的型别（Type），它表示传递函数的极点在坐标系原点的个数。为啥这个数字特别关键呢？接下来我就仔细分析一下，先把 $G(s)$G(s)写 代入稳态误差方程：

$e_{s s}=\lim _{s \rightarrow 0} \frac{1}{1+G(s)}=\frac{1}{1+G(0)}$ess​=s→0lim​1+G(s)1​=1+G(0)1​

其中
$G(0)=\lim _{s \rightarrow 0} \frac{K \prod_{i=1}^{p}\left(T_{i} s+1\right)}{s^{N} \prod_{j=1}^{q}\left(T_{j} s+1\right)}$G(0)=s→0lim​sN∏j=1q​(Tj​s+1)K∏i=1p​(Ti​s+1)​

对于Type 0系统， $N=0$N=0，此时
$G(0)=\lim _{s \rightarrow 0} \frac{K \prod\left(T_{i} s+1\right)}{\prod\left(T_{j} s+1\right)}=K$G(0)=s→0lim​∏(Tj​s+1)K∏(Ti​s+1)​=K
稳态误差为
$e_{s s}=\frac{1}{1+K}$ess​=1+K1​
可见，闭环系统有稳态差的，当然，提高开环增益 $K$K可以减小误差。
对于Type 1系统， $N=1$N=1，此时
$G(0)=\lim _{s \rightarrow 0} \frac{K \prod\left(T_{i} s+1\right)}{s \prod\left(T_{j} s+1\right)}=\infty$G(0)=s→0lim​s∏(Tj​s+1)K∏(Ti​s+1)​=∞
稳态误差为
$e_{s s}=\frac{1}{1+\infty}=0$ess​=1+∞1​=0
可见，闭环系统稳态差为零。于是我们可以得出如下结论：对于Type 0系统，如果前向通道没有积分器，单位阶跃响应会有稳态误差，这个误差可以通过提高 开环增益$K$K来减小，但是会导致稳定问题。如果想消除稳态误差，必须提高系统的Type，通常的办法就是在控制器上加积分器。

如何最优化一个跟踪控制系统（Type 1）

上一节之所以介绍终值定理及系统的type，目的就是进行跟踪伺服控制，因为跟踪控制器的选择和被控对象的type有关。现在假设有一个系统：
$\begin{aligned} \dot{\boldsymbol{x}} &=\boldsymbol{A} \boldsymbol{x}+\boldsymbol{B} u \\ y &=\boldsymbol{C x} \end{aligned}$x˙y​=Ax+Bu=Cx​
当然，我们可以通过状态方程得出系统的传递函数，现在假设传递函数里面有一个积分器，我们该如何设计状态反馈，以实现一个很好的跟随特性，由终值定理可以知道，此时若想阶跃信号的稳态误差为零，控制器部分是不需要加积分的，我们可以设计成如下框图的构型。

我们最终目标是获得一个如下方程：
$\dot{\boldsymbol{e}}=\overline{\boldsymbol{A}} e$e˙=Ae
其中$\overline{\boldsymbol{A}}$A特征值都具有负的实部，这样就能保证$e \rightarrow 0$e→0

具体可以这么操作，令

$u=-\left[ \begin{array}{llllll}{0} & {k_{2}} & {k_{3}} & {\ldots} & {k_{n}}\end{array}\right] \left[ \begin{array}{c}{x_{1}} \\ {x_{2}} \\ {x_{3}} \\ {\vdots} \\ {x_{n}}\end{array}\right]+k_{1}\left(r-x_{1}\right)$u=−[0​k2​​k3​​…​kn​​]⎣⎢⎢⎢⎢⎢⎡​x1​x2​x3​⋮xn​​⎦⎥⎥⎥⎥⎥⎤​+k1​(r−x1​)
$=-\boldsymbol{K} \boldsymbol{x}+k_{1} r$=−Kx+k1​r
其中
$\boldsymbol{K}=\left[ \begin{array}{lllll}{k_{1}} & {k_{2}} & {k_{3}} & {\dots} & {k_{n}}\end{array}\right]$K=[k1​​k2​​k3​​…​kn​​]
则状态方程转化为
$\dot{\boldsymbol{x}}(t)=(\boldsymbol{A}-\boldsymbol{B} \boldsymbol{K}) \boldsymbol{x}(t)+\boldsymbol{B} k_{1} r(t)$x˙(t)=(A−BK)x(t)+Bk1​r(t)
对于一个伺服跟随系统，我们期望的是当$t \rightarrow \infty$t→∞时有$y(\infty)$y(∞)趋近于$r$r，也就是
$\dot{\boldsymbol{x}}(\infty)=(\boldsymbol{A}-\boldsymbol{B} \boldsymbol{K}) \boldsymbol{x}(\infty)+\boldsymbol{B} k_{1} r(\infty)$x˙(∞)=(A−BK)x(∞)+Bk1​r(∞)
假如我们的输入是单位阶跃信号，我们有$r(\infty)=r(t)=r$r(∞)=r(t)=r，把以上两式相减即可得到：
$\dot{\boldsymbol{x}}(t)-\dot{\boldsymbol{x}}(\infty)=(\boldsymbol{A}-\boldsymbol{B} \boldsymbol{K})(\boldsymbol{x}(t)-\boldsymbol{x}(\infty))$x˙(t)−x˙(∞)=(A−BK)(x(t)−x(∞))
即

$\dot{e}(t)=(A-B K) e(t)$e˙(t)=(A−BK)e(t)

可见，只要我们合理设计矩阵$(A-B K)$(A−BK)的极点就可以。

举个简单的例子，有一个系统，有3个状态变量：

$A=\left[ \begin{array}{ccc}{0} & {1} & {0} \\ {0} & {0} & {1} \\ {0} & {-2} & {-3}\end{array}\right], B=\left[ \begin{array}{l}{0} \\ {0} \\ {1}\end{array}\right]$A=⎣⎡​000​10−2​01−3​⎦⎤​,B=⎣⎡​001​⎦⎤​
$C=\left[ \begin{array}{lll}{1} & {0} & {0}\end{array}\right], D=\left[ \begin{array}{l}{0}\end{array}\right]$C=[1​0​0​],D=[0​]

此系统的特征函数为：
$|\lambda \boldsymbol{I} s-\boldsymbol{A}|=s \lambda\left(\lambda s^{2}+3 \lambda s+2\right)$∣λIs−A∣=sλ(λs2+3λs+2)
可见被控对象为Type 1 型，因此当输入为阶跃信号时，控制器部分不需要积分器。假设状态反馈为：

$\begin{aligned} u &=k_{1}\left(r-x_{1}\right)-\left(k_{2} x_{2}+k_{3} x_{3}\right) \\ &=k_{1} r-\left(k_{1} x_{1}+k_{2} x_{2}+k_{3} x_{3}\right) \end{aligned}$u​=k1​(r−x1​)−(k2​x2​+k3​x3​)=k1​r−(k1​x1​+k2​x2​+k3​x3​)​令

$\boldsymbol{K}=\left[ \begin{array}{lll}{k_{1}} & {k_{2}} & {k_{3}}\end{array}\right]$K=[k1​​k2​​k3​​]
假设系统的代价函数为：

$J=\int_{0}^{\infty}\left(\boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x}+\boldsymbol{u}^{T} \boldsymbol{R} \boldsymbol{u}\right)$J=∫0∞​(xTQx+uTRu)

其中误差权重矩阵为：

$\boldsymbol{Q}=\left[ \begin{array}{rrr}{100} & {0} & {0} \\ {0} & {1} & {0} \\ {0} & {0} & {1}\end{array}\right]$Q=⎣⎡​10000​010​001​⎦⎤​

控制量权重为：

$\boldsymbol{R}=0.01$R=0.01

MATLAB代码如下：

A=[0 1 0;0 0 1;0 -2 -3];
B=[0;0;1];
C = [1 0 0];
D = [0];

Q=[100 0 0;0 1 0;0 0 1];
R=[0.01];
K=lqr(A,B,Q,R)
t=0:0.01:5;
sys = ss(A-B*K, B*K(1),C,D);
[y,t,x]=step(sys,t);
figure('Color','white')
plot(t,y,'k','linewidth',1.5);grid on;
title('Unit step Response of LQR controller');
xlabel('t,sec');ylabel('Output y=x1');

figure('Color','white');
plot(t,x,'linewidth',1.5);grid on;
title('Unit step Response x1,x2,x3,versus t');
xlabel('t,sec');ylabel('x1,x2,x3');
legend('x1','x2','x3')

系统的阶跃响应为：

状态变量分别为：

如何最优化一个跟踪控制系统（Type 0）

假如现在被控对象是Type 0 型，对于Type 0系统，若激励是单位阶跃信号$1/ s$1/s，此时
$e_{s s}=\lim _{s \rightarrow 0} \frac{s}{1+G(s)} \frac{1}{s}=\frac{1}{1+G(0)}$ess​=s→0lim​1+G(s)s​s1​=1+G(0)1​

可见，还有静态误差的，那怎么办呢？——我们需要在前向通道增加积分器才能保证阶跃激励的稳态误差为零，我们假设$G^{\prime}(s)=\frac{k}{s} G(s)$G′(s)=sk​G(s)
那么
$e_{s s}^{\prime}=\lim _{s \rightarrow 0} \frac{s}{1+\frac{k}{s} G(s)} \frac{s}{s}=\lim _{s \rightarrow 0} \frac{s}{1+k G(0)}=0$ess′​=s→0lim​1+sk​G(s)s​ss​=s→0lim​1+kG(0)s​=0

具体怎么操作呢？——我们可以增加一个新状态变量（state）来表征输出和输入之间误差，这个状态变量还得是积分形式。状态变量定义如下：
$\boldsymbol{x}_{i}(t)=\int \boldsymbol{y}(t)-\boldsymbol{r}(t) d t$xi​(t)=∫y(t)−r(t)dt

两边都微分一下，就是：

$\begin{aligned} \dot{\boldsymbol{x}}_{i}(t) &=\boldsymbol{y}(t)-\boldsymbol{r}(t) \\ &=C \boldsymbol{x}(t)-\boldsymbol{r}(t) \end{aligned}$x˙i​(t)​=y(t)−r(t)=Cx(t)−r(t)​

把这个新变量扩充到原来的状态方程

$\left\{\begin{array}{l}{\dot{\boldsymbol{x}}(t)=A \boldsymbol{x}(t)+B \boldsymbol{u}(t)} \\ {\boldsymbol{y}(t)=C \boldsymbol{x}(t)+D \boldsymbol{u}(t)}\end{array}\right.${x˙(t)=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t)​

可以得到

$\left[ \begin{array}{c}{\dot{\boldsymbol{x}}(t)} \\ {\dot{\boldsymbol{x}}_{i}(t)}\end{array}\right]=\left[ \begin{array}{ll}{A} & {0} \\ {C} & {0}\end{array}\right] \left[ \begin{array}{c}{\boldsymbol{x}(t)} \\ {\boldsymbol{x}_{i}(t)}\end{array}\right]+\left[ \begin{array}{c}{B} \\ {0}\end{array}\right] \boldsymbol{u}(t)+\left[ \begin{array}{c}{0} \\ {-I}\end{array}\right] \boldsymbol{r}(t)$[x˙(t)x˙i​(t)​]=[AC​00​][x(t)xi​(t)​]+[B0​]u(t)+[0−I​]r(t)

我们把扩展的矩阵起个新的名字，分别为$A_a$Aa​，$B_a$Ba​，$B_r$Br​。还拿我们熟悉的DC motor的例子，我们把误差状态变量增加进去，就可以得到新的状态方程：

$\left[ \begin{array}{c}{\dot{\theta}(t)} \\ {\dot{\omega}(t)} \\ {\dot{i}(t)} \\ {\dot{\nu}(t)}\end{array}\right]=\left[ \begin{array}{cccc}{0} & {1} & {0} & {0} \\ {0} & {-\frac{K_{f}}{J}} & {\frac{K}{J}} & {0} \\ {0} & {-\frac{K}{L}} & {-\frac{R}{L}} & {0} \\ {1} & {0} & {0} & {0}\end{array}\right] \left[ \begin{array}{c}{\theta(t)} \\ {\omega(t)} \\ {i(t)} \\ {\nu(t)}\end{array}\right]+\left[ \begin{array}{c}{0} \\ {0} \\ {\frac{1}{L}} \\ {0}\end{array}\right] u(t)+\left[ \begin{array}{c}{0} \\ {0} \\ {0} \\ {-1}\end{array}\right] r(t)$⎣⎢⎢⎡​θ˙(t)ω˙(t)i˙(t)ν˙(t)​⎦⎥⎥⎤​=⎣⎢⎢⎡​0001​1−JKf​​−LK​0​0JK​−LR​0​0000​⎦⎥⎥⎤​⎣⎢⎢⎡​θ(t)ω(t)i(t)ν(t)​⎦⎥⎥⎤​+⎣⎢⎢⎡​00L1​0​⎦⎥⎥⎤​u(t)+⎣⎢⎢⎡​000−1​⎦⎥⎥⎤​r(t)
以及新的输出方程：

$y(t)=\underbrace{\left[ \begin{array}{cccc}{1} & {0} & {0} & {0}\end{array}\right]}_{C_{a}} \left[ \begin{array}{c}{\theta(t)} \\ {\omega(t)} \\ {i(t)} \\ {\nu(t)}\end{array}\right]$y(t)=Ca​[1​0​0​0​]​​⎣⎢⎢⎡​θ(t)ω(t)i(t)ν(t)​⎦⎥⎥⎤​

式中 $\nu(t)$ν(t)就是前面的 $x_{i}(t)$xi​(t)。画成框图就是：

根据上面的框图，可以得到输入 $u(t)$u(t)的表达式为：
$\boldsymbol{u}(t)=-K_{c} \boldsymbol{x}(t)-K_{i} \boldsymbol{\nu}(t)=-\left[ \begin{array}{cc}{K_{c}} & {K_{i}}\end{array}\right] \left[ \begin{array}{c}{\boldsymbol{x}(t)} \\ {\boldsymbol{\nu}(t)}\end{array}\right]$u(t)=−Kc​x(t)−Ki​ν(t)=−[Kc​​Ki​​][x(t)ν(t)​]

令$\boldsymbol{x}_{\boldsymbol{a}}(t)=\left[ \begin{array}{c}{\boldsymbol{x}(t)} \\ {\boldsymbol{\nu}(t)}\end{array}\right]$xa​(t)=[x(t)ν(t)​]以及$K_{a}=\left[ \begin{array}{ll}{K_{c}} & {K_{i}}\end{array}\right]$Ka​=[Kc​​Ki​​]
则可以得到有输出反馈的系统状态方程：

$\dot{\boldsymbol{x}}_{a}(t)=\underbrace{\left(A_{a}-B_{a} K_{a}\right)}_{\text { new } \mathrm{A}} \boldsymbol{x}_{a}(t)+B_{r} \boldsymbol{r}(t)$x˙a​(t)= new A(Aa​−Ba​Ka​)​​xa​(t)+Br​r(t)

以及输出方程

$\boldsymbol{y}(t)=C_{a} \boldsymbol{x}_{a}(t)$y(t)=Ca​xa​(t)
因此，闭环控制系统的极点分布为：
$\left|\lambda I-\left(A_{a}-B_{a} K_{a}\right)\right|=0$∣λI−(Aa​−Ba​Ka​)∣=0
也就是说，我们只要配置矩阵 $A_{a}-B_{a} K_{a}$Aa​−Ba​Ka​的极点就可以了，现在我们要用LQR方法来确定最佳的极点配置。
假设系统的代价函数为：
$J=\int_{0}^{\infty}\left(\boldsymbol{x}^{T} \boldsymbol{Q} \boldsymbol{x}+\boldsymbol{u}^{T} \boldsymbol{R} \boldsymbol{u}\right)$J=∫0∞​(xTQx+uTRu)

其中误差权重矩阵为：

$\boldsymbol{Q}=\left[ \begin{array}{cccc}{1} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {1 e 4}\end{array}\right]$Q=⎣⎢⎢⎡​1000​0000​0000​0001e4​⎦⎥⎥⎤​
控制量权重为：
$\boldsymbol{R}=0.001$R=0.001
注意，$x_{4}(t)$x4​(t) 分配的权重比较大，是因为其代表着跟踪误差，是我们比较关心的量。具体代码如下：

L = 1e-3;%inductance
R = 1; %resistance
J = 5e-5; %inetia
Kf = 1e-4;%damping
K = 0.1;% torque constant

A = [0 1 0
    0 -Kf/J K/J
    0 -K/L -R/L];
B = [0 ; 0 ; 1/L];
C = [1  0  0];
D = 0;

Aa = [0 1 0 0
      0 -Kf/J K/J 0
      0 -K/L -R/L 0
      1 0 0 0];
Ba = [0 ; 0 ; 1/L ; 0 ];
Br = [0 ; 0 ; 0; -1];
Ca = [1 0 0 0];
Da = [0];

Q=[1 0 0 0;0 0 0 0;0 0 0 0;0 0 0 1e4];
R=0.001;

Ka = lqr(Aa,Ba,Q,R);
Kc=Ka(1:3);
Ki=Ka(4);

SIMULINK模型：

系统输出如下：