状态空间入门（现代控制理论）

11.0 线性代数速成

向量和矩阵：向量是一列数，矩阵是一张数表。\(\mathbb{R}^{n}\) 表示 \(n\) 维列向量，\(n\times m\) 矩阵有 \(n\) 行 \(m\) 列。

矩阵乘法：\(AB\) 的第 \(i\) 行第 \(j\) 列元素 = \(A\) 的第 \(i\) 行与 \(B\) 的第 \(j\) 列对应相乘再相加。要求 \(A\) 的列数等于 \(B\) 的行数。一般 \(AB\ne BA\)。例：

单位矩阵 \(I\)：对角线为 1、其余为 0，\(IA=AI=A\)。逆矩阵 \(A^{-1}\)：满足 \(AA^{-1}=I\)。2×2 矩阵的逆：

特征值：满足 \(Av=\lambda v\)（\(v\ne0\)）的数 \(\lambda\)，由特征方程 \(\det(\lambda I-A)=0\) 求出。例：

后面会看到：矩阵 \(A\) 的特征值就是系统的极点。实际计算交给 numpy.linalg.eigvals。

11.1 状态与状态方程

状态：一组变量，知道它们在某一时刻的值和之后的输入，就能确定系统以后的全部行为。通常取储能元件对应的量：位置和速度（动能、势能）、电容电压、电感电流、温度。

例 1　弹簧—质量—阻尼（第2章例 1）：\(m\ddot y+f\dot y+ky=F\)。取 \(x_1=y\)（位移）、\(x_2=\dot y\)（速度），则 \(\dot x_1=x_2\)，\(\dot x_2=\frac1m(F-kx_1-fx_2)\)：

例 2　直流电机（第2章例 3）：取 \(x_1=i\)（电流）、\(x_2=\omega\)（转速），输入电压 \(u\)：

和传递函数相比，电流这个内部变量也在模型里了，所以能直接加电流限幅、做电流环。

由传递函数得到状态方程：\(G(s)=\dfrac{b_{n-1}s^{n-1}+\cdots+b_0}{s^n+a_{n-1}s^{n-1}+\cdots+a_0}\) 可以写成可控标准型：\(A\) 的最后一行是 \(-a_0,-a_1,\ldots,-a_{n-1}\)，上面是一条错位的单位对角线，\(B=[0,\ldots,0,1]^T\)，\(C=[b_0,b_1,\ldots,b_{n-1}]\)。plant.py 里的 Plant.__init__ 就是在构造这个形式；本笔记的交互组件也用它做仿真。

11.2 与传递函数的关系

零初始条件下对状态方程取拉氏变换：\(sX=AX+BU\Rightarrow X=(sI-A)^{-1}BU\)，所以

\((sI-A)^{-1}\) 的分母是 \(\det(sI-A)\)，所以传递函数的极点就是 \(A\) 的特征值（如果没有零极点对消）。同一个系统可以有无数种状态方程（取不同的状态变量），但特征值相同。

import numpy as np
import control as ct

m, f, k = 1.0, 2.0, 5.0
A = np.array([[0, 1], [-k / m, -f / m]])
B = np.array([[0], [1 / m]])
C = np.array([[1, 0]])
D = np.array([[0]])
sys = ct.ss(A, B, C, D)
print(np.linalg.eigvals(A))     # [-1.+2.j -1.-2.j]，就是传递函数的极点
print(ct.tf(sys))                # 1 / (s^2 + 2 s + 5)，分子里可能出现 1e-16 量级的数值误差，忽略即可

11.3 状态方程的解与离散化

状态方程的解（矩阵指数 \(e^{At}=I+At+\frac{(At)^2}{2!}+\cdots\)）：

第一项是零输入响应，第二项是零状态响应，和拉氏变换那一章的分解一样。如果输入在每个采样周期内保持不变（零阶保持），就得到精确的离散状态方程：

plant.py 的 _zoh 函数用一个技巧同时算出 \(A_d\) 和 \(B_d\)：把 \(\begin{bmatrix}A&B\\0&0\end{bmatrix}T_s\) 做矩阵指数，结果的左上角是 \(A_d\)，右上角是 \(B_d\)。python-control 里对应 ct.c2d(sys, Ts, method="zoh")。

11.4 能控性与能观性

• 能控：通过输入能把系统从任意状态转移到任意状态。不能控的部分，控制器根本影响不到它，如果它本身不稳定，就无论如何也镇定不了。判据：能控性矩阵 \(\begin{bmatrix}B&AB&\cdots&A^{n-1}B\end{bmatrix}\) 满秩。
• 能观：通过输出能推算出所有状态。不能观的状态，从测量里看不出来，也就无法估计。判据：能观性矩阵 \(\begin{bmatrix}C^T&(CA)^T&\cdots&(CA^{n-1})^T\end{bmatrix}^T\) 满秩。

print(np.linalg.matrix_rank(ct.ctrb(A, B)), np.linalg.matrix_rank(ct.obsv(A, C)))   # 2 2：能控且能观

直观例子：两个完全一样的水箱用同一根进水管同时供水，只能控制「两个液位的和」，不能单独控制「两个液位的差」，所以不完全能控。

11.5 状态反馈与极点配置

如果所有状态都能测到，就用它们全部来反馈：

闭环极点是 \(A-BK\) 的特征值。只要系统能控，就可以通过选 \(K\) 把闭环极点放到任何想要的位置（复数极点成对）。这比 PID 只能调两三个参数灵活得多。\(k_r\) 用来保证稳态时输出等于设定值。

例　双积分对象（质量为 1 的滑块，输入是力 \(u\)，输出是位置 \(x\)）：\(A=\begin{bmatrix}0&1\\0&0\end{bmatrix}\)，\(B=\begin{bmatrix}0\\1\end{bmatrix}\)。取 \(u=k_1(r-x)-k_2\dot x\)，闭环特征方程 \(s^2+k_2s+k_1=0\)。希望 \(\omega_n=2\)、\(\xi=0.7\)，对比标准二阶形式得 \(k_1=\omega_n^2=4\)，\(k_2=2\xi\omega_n=2.8\)。

A2 = np.array([[0, 1], [0, 0]])
B2 = np.array([[0], [1]])
wn, xi = 2.0, 0.7
K = ct.place(A2, B2, np.roots([1, 2 * xi * wn, wn**2]))
print(K)                         # [[4.  2.8]]

11.6 状态观测器

实际中往往只能测到一部分状态（比如只有位置传感器，没有速度传感器）。观测器用模型跟着系统一起「跑」，再用测量值不断修正：

估计误差 \(e=x-\hat x\) 满足 \(\dot e=(A-LC)e\)。只要系统能观，就能选 \(L\) 让 \(A-LC\) 的特征值任意放置，误差就会按设定的速度衰减到 0。选 \(L\) 和选 \(K\) 是对偶问题：对 \((A^T,C^T)\) 做极点配置即可。

• 观测器极点一般取控制器极点的 2～5 倍快（太快会放大测量噪声）；
• 分离原理：控制器和观测器可以分别设计，合在一起后的闭环极点就是两组极点的并集；
• 在单片机里实现时，把观测器离散化（第8章），每个采样周期更新一次。

C2 = np.array([[1, 0]])                                   # 只测位置
L = ct.place(A2.T, C2.T, [-5 + 5j, -5 - 5j]).T             # 观测器极点 -5±5j
print(L.ravel())                                           # [10. 50.]

11.7 LQR：用「代价」来设计

极点配置要你自己决定极点放在哪，系统维数一高就很难凭直觉选。LQR（线性二次型调节器）换一种问法：在「状态偏离」和「控制量大小」之间取最好的折中，使

最小。\(Q\) 越大表示越在乎状态偏差（要快、要准），\(R\) 越大表示越舍不得用控制量（要省力、不饱和）。解出来的最优控制就是状态反馈 \(u=-Kx\)，\(K=R^{-1}B^TP\)，\(P\) 由黎卡提方程求得（交给 ct.lqr）。LQR 得到的闭环一定稳定，而且有很好的鲁棒性（相位裕度至少 60°）。

权重怎么选（Bryson 规则）：\(Q_{ii}=\dfrac{1}{(\text{状态 }i\text{ 允许的最大偏差})^2}\)，\(R=\dfrac{1}{(\text{允许的最大控制量})^2}\)，先用这个起步，再按仿真结果调整。

K, P, E = ct.lqr(A2, B2, np.diag([1.0, 1.0]), np.array([[1.0]]))
print(K, E)          # [[1. 1.7321]]  闭环极点 -0.866±0.5j

对双积分对象，LQR 增益有解析解：\(k_1=\sqrt{q_1/r}\)，\(k_2=\sqrt{q_2/r+2k_1}\)。上面的组件就是用它算的。第13章会用 LQR 设计平衡小车（倒立摆）的控制器。

状态反馈本身没有积分作用，模型不准或有恒定扰动时会有稳态误差。常用的办法是把误差的积分作为一个额外的状态加进去，再对增广后的系统做 LQR。

11.8 卡尔曼滤波的直觉

观测器的 \(L\) 是自己选的。如果知道模型有多不准（过程噪声）、传感器有多吵（测量噪声），能不能选出「最好」的 \(L\)？卡尔曼滤波就是答案：它在每一步都按两者的不确定度，最优地决定「相信模型多少、相信测量多少」。

一维例子：测量一个基本不变（或缓慢变化）的量，传感器噪声方差 \(r\)，模型认为它每步可能随机变化，方差 \(q\)。

def kalman_1d(z, q, r, x0, p0):
    x, p, out = x0, p0, []
    for zk in z:
        p = p + q                # 预测：不确定度增加
        K = p / (p + r)          # 卡尔曼增益：越相信测量，K 越接近 1
        x = x + K * (zk - x)     # 用测量修正
        p = (1 - K) * p
        out.append(x)
    return np.array(out), K

# 真值 1.0，测量噪声 σ=0.3：q=1e-3 时稳态增益 0.100，估计误差标准差 0.056（原始测量 0.30）

稳态时 \(K\) 是常数，这时一维卡尔曼滤波就是一个指数滑动平均（第8章的一阶低通）。卡尔曼滤波的价值在于：多维、多传感器、模型随时间变化时，它能自动算出最优的融合方式。矩阵形式：

实际例子：用 IMU 测倾角。陀螺仪测角速度，积分得到角度，短时间很准但会慢慢漂移；加速度计能测出重力方向，从而得到角度，没有漂移但受振动影响很大。卡尔曼滤波可以把两者融合。很多项目用更简单的互补滤波，效果接近：

相当于对陀螺仪的角度做高通、对加速度计的角度做低通，再相加。

本章小结

练习