Appearance
现代控制理论基础
概念
上一章把系统换成了状态空间的写法，得到了"能控就能任意配置极点"这条硬结论。但极点到底应该放到哪？上一章的答案是"放到 -5"——这是设计者凭经验选的数，不是数学算出来的数。
现代控制理论要解决的正是这个问题：不问"极点放在哪"，而问"什么代价最小"。
一句话说清它是什么：现代控制把"控制得好不好"写成一个可以计算的数（代价函数），然后让数学去求使这个数最小的那个控制器。
| 经典控制 | 现代控制 | |
|---|---|---|
| 描述工具 | 传递函数 G(s) | 状态方程 x' = Ax + Bu |
| 数学模型 | 频域（s 平面） | 时域（状态向量） |
| 适用对象 | 单入单出（SISO） | 多入多出（MIMO） |
| 初始条件 | 被抹掉 | 显式保留 |
| 内部信息 | 看不见（可对消） | 完全可见 |
| 设计方法 | 试凑 + 经验公式（Z-N、根轨迹） | 按指标求解（LQR、极点配置） |
| 最优性 | 无保证 | 在给定指标下最优 |
| 对模型依赖 | 弱 | 强 |
现代控制理论的三根支柱，按本章的顺序：
| 支柱 | 回答的问题 | 关键概念 |
|---|---|---|
| 结构理论 | 这个系统"能不能"被控制、被观测？ | 能控性、能观性（上一章） |
| 稳定性理论 | 不解方程，能不能判定稳定？ | 李雅普诺夫第二法 |
| 最优控制与估计 | 什么样的控制器"最好"？有噪声时怎么估？ | LQR、卡尔曼滤波 |
这一层回答了上一层什么问题：上一章解决了"能不能"，这一章解决"好不好"。"能不能"是资格问题（能控/能观），"好不好"是优化问题（LQR/卡尔曼）——两个问题在数学上是分开的，这也正是现代控制理论的条理所在。
原理
一、稳定性判定的第三条路：李雅普诺夫第二法
前面已经有过两条判稳的路：
| 方法 | 思路 | 局限 |
|---|---|---|
| 劳斯判据 | 排系数表，看第一列是否同号 | 只对线性系统、只判"稳不稳" |
| 奈奎斯特判据 | 看频域曲线是否包围临界点 | 需要画图，判据只管线性 |
| 李雅普诺夫第二法 | 构造一个"能量函数"，看它沿轨迹是否一直下降 | 可推广到非线性 |
第二法的核心思想可以用一句话概括：
如果一个系统的"总能量"随时间单调减少，那它最终必然停在某个能量最低点——不用解方程也知道它稳定。
这里的"能量"不必是真实的物理能量，只要是一个满足条件的标量函数 V(x) 就行。形式化的表述是：
若存在一个正定函数
V(x)（x ≠ 0时V > 0，x = 0时V = 0），使得它沿系统轨迹的导数V̇(x)负定，则该系统在原点渐近稳定。
"正定"与"负定"的含义：V 正定表示它是"碗形"的（原点是最低点）；V̇ 负定表示"沿轨迹走总是往碗底下滑"。两条合起来就是"一定会滑到底"。
对线性系统，这套理论变得完全可操作。取二次型函数 V(x) = xᵀ P x（P 对称正定），则
于是只要指定一个正定的 Q（最简单取单位阵），解矩阵方程
解出的 P 若正定，系统就渐近稳定；这就是李雅普诺夫方程。
这条判据的分量在于：它绕开了求特征值。 对高阶系统，求特征值需要解高次方程；而李雅普诺夫方程是一组线性方程（把 P 的未知元素当变量，按元素展开即可），用线性代数就能解。"把稳定性判定变成解线性方程组"，这是第二法最大的工程价值。
注意 Q 是可以任选的——取单位阵最方便。"Q 取不同值会不会得出不同结论"：不会。定理保证：只要对某一个正定 Q 解出的 P 正定，系统就稳定；如果对某一个 Q 解不出正定 P，那对任何 Q 解出来都不会正定。 所以挑最好算的那个 Q 就行。
二、最优控制：把"好不好"写成一个数
现代控制最漂亮的一步是把"控制得好不好"变成一个可以最小化的标量。对线性系统取二次型代价函数（Linear Quadratic 这个名字就是这么来的）：
这个式子的每一项都有明确的工程含义：
| 项 | 罚什么 | 权重 | 增大权重的后果 |
|---|---|---|---|
xᵀQx | 状态偏离零的程度（"控制精度"） | Q | 更用力地把状态拉回零、响应更快、控制量更大 |
uᵀRu | 控制量的消耗（"控制代价"） | R | 更省控制、响应更慢、超调更小 |
Q 与 R 是一对矛盾的两端：Q 大代表"我不在乎花多少力气，就要把状态压到零"；R 大代表"我很在意执行器的磨损和能耗，宁可慢一点"。整套 LQR 的全部设计工作，就是定这两个矩阵的相对大小。
LQR 的结论（不加证明地给出）：最优控制律是状态的线性反馈
其中 P 是下述代数 Riccati 方程（ARE）的正定对称解：
这个方程与李雅普诺夫方程长得很像，差别只多了中间那个二次项 −PBR⁻¹BᵀP。它不是线性的（含 P 的二次项），所以没有通用解析解，要数值迭代求解。
注意 LQR 给出的闭环极点一定是稳定的——这是定理保证的，不需要另外验证。因为 P 正定，可以证明 A − BK 的所有特征值实部为负。"不用判稳，因为最优解必然稳"，这是 LQR 相对极点配置的一个显著优势。
Q、R 究竟怎么影响结果——这是工程上最需要的手感，用一个最简例子就能看清（下一节示例）：
- 标量系统
A = 0、B = 1：ARE 退化成−P²/R + Q = 0，解出P = sqrt(Q·R)，于是K = P/R = sqrt(Q/R)。 - 所以
K正比于sqrt(Q)、反比于sqrt(R)：R增大到 4 倍，K减半（不是减到 1/4）——这个"平方根关系"是调权重时最实用的一条口诀。
三、卡尔曼滤波：有噪声时的最优观测器
上一章的观测器是确定性的：它假设模型精确、测量精确，用 L(y − C x̂) 把误差拉回来。但真实世界有噪声——过程噪声（模型不准）和测量噪声（传感器不准）。这两种噪声下"怎么加权"，就是卡尔曼滤波要回答的。
卡尔曼滤波的一步更新（离散形式，三个式子）：
| 符号 | 含义 |
|---|---|
P⁻ | 先验方差——"在没看到新测量之前，我对自己的估计有多不确定" |
R | 测量噪声方差——"这次的测量值有多不可信" |
K | 卡尔曼增益——在两个不确定度之间做加权平均 |
z | 本次的测量值 |
x̂⁻、x̂ | 更新前、更新后的状态估计 |
P | 后验方差——更新之后的不确定度（必然比 P⁻ 小） |
第一条式子是整个滤波器的灵魂：
K = P⁻ / (P⁻ + R)——先验越不确定（P⁻大）、测量越可靠（R小），K就越接近 1，就越相信测量；反之就越相信模型。
这与上一章观测器的关系：观测器的增益 L 是手选的常数；卡尔曼的增益 K 是每一步按不确定度自动算出来的——P⁻ 随预测增大、随测量修正减小，K 也随之动态变化。 所以卡尔曼滤波可以看成"会自己调增益的观测器"。
一条漂亮的对偶关系（与能控/能观的对偶同源）：
| LQR（最优控制） | 卡尔曼滤波（最优估计） | |
|---|---|---|
| 对象 | (A, B) | (Aᵀ, Cᵀ) |
| 权重 | Q（状态）、R（控制） | Q（过程噪声）、R（测量噪声） |
| 核心方程 | Riccati 方程 | Riccati 方程（同一个） |
| 结果 | 反馈增益 K | 滤波增益 K |
两者不仅"长得像"，而是数学上完全相同的问题——把 (A,B) 换成 (Aᵀ,Cᵀ)、把 Q/R 换成噪声协方差，LQR 的程序原样就能算卡尔曼滤波。这个对偶是控制理论里最优雅的结果之一。
四、三种稳定性判据的分工
| 方法 | 领域 | 是否充要 | 能否处理非线性 | 能否给裕度 |
|---|---|---|---|---|
| 劳斯判据 | 代数（系数） | 充要 | 否 | 否 |
| 奈奎斯特判据 | 频域（曲线） | 充要 | 否 | 能 |
| 李雅普诺夫第二法 | 时域（能量函数） | 线性时充要；非线性时充分 | 能 | 否 |
看这张表最该注意最后一行：李雅普诺夫第二法在非线性系统上只是充分条件——构造不出 V 不等于不稳定，只说明"这个函数没找到"。这是它唯一的短板，也是它在非线性领域至今仍是研究热点的原因。
五、现代控制的代价与边界
现代控制不是免费的，它有三条硬代价：
| 代价 | 具体表现 |
|---|---|
| 强依赖模型 | LQR、卡尔曼都要求 A、B、Q、R 精确已知。模型错了，最优解就不再最优（甚至可能不稳） |
| 需要全部状态 | 状态反馈 u = −Kx 要求 x 全都测得到；测不到就要装观测器或卡尔曼滤波，代价再翻一层 |
| 参数无物理意义 | K = [23, 7]、R = 1——这些数字和"响应时间""超调量"没有直接的对应关系，要靠反复试算才能建立手感 |
与 PID 的分工：
| 场景 | 建议 |
|---|---|
| 单回路、对象粗糙、要求不高 | PID——不需要模型，调参经验成熟 |
| 多变量耦合、状态可测、要求最优 | LQR / 状态反馈——需要模型，但能给出最优解 |
| 状态测不全 / 噪声大 | 加观测器或卡尔曼滤波 |
一句话总结这条边界：PID 用"不需要模型"换来了鲁棒，现代控制用"需要模型"换来了最优。 两条路都在用，选哪条取决于"模型好不好拿"。
示例
例 1：用李雅普诺夫方程判定稳定性
对 A = [[0, 1], [-2, -3]]（上一章那个系统，特征值 -1、-2），取 Q = I：
text
设 P = [[p11, p12], [p12, p22]]，代入后得三个方程：
-4*p12 = -1 -> p12 = 0.25
2*p12 - 6*p22 = -1 -> p22 = 0.25
p11 - 3*p12 - 2*p22 = 0 -> p11 = 1.25
P = [[1.25, 0.25], [0.25, 0.25]]
核对 A^T P + P A + I = [[0.0e+00, 0.0e+00], [0.0e+00, 0.0e+00]]（应全为 0）
正定判据：一阶主子式 = 1.2500 > 0，二阶主子式 = 0.2500 > 0 -> P 正定解这组方程的过程值得留意：三个未知数 p11、p12、p22，得到三个方程——而且第一个方程 (−4p12 = −1) 里只有 p12 一个未知数，可以直接解出，然后一路代下去。这不是运气：矩阵方程的对称结构决定了"离对角线越远的元素越早被定出"。
正定判据对二阶只要两个数：一阶主子式 p11 与二阶主子式（行列式）。这两个都大于零就是正定——这是西尔维斯特判据在二阶的退化形式。判 P 是否正定，本质上就是判"这个二次型是不是碗形的"。
和特征值法的对比：这套系统只有二阶，求特征值当然更快。但把阶数提到 10 阶，求特征值要解 10 次方程，而李雅普诺夫方程仍然只是一组线性方程——这才是第二法的真正价值所在。
例 2：LQR 的标量例——Q/R 怎么影响增益
取最简单的标量系统 A = 0、B = 1，固定 Q = 1，让 R 变化：
R | P | K | 闭环极点 |
|---|---|---|---|
| 0.25 | 0.5000 | 2.0000 | -2.0000 |
| 1.00 | 1.0000 | 1.0000 | -1.0000 |
| 4.00 | 2.0000 | 0.5000 | -0.5000 |
| 16.00 | 4.0000 | 0.2500 | -0.2500 |
这张表把"调权重"的手感讲清楚了：
R从 1 涨到 4（4 倍），K从 1 降到 0.5（减半）——因为在标量情形下K = sqrt(Q/R)，R翻两番，K只减一半。- 闭环极点就是
−K（标量A − BK = 0 − 1·K），所以**"增大R就等于把极点往虚轴推"**——省了控制量，响应自然变慢。 - 注意
P和K的走向相反：R增大时P在涨（0.5 → 4）、K在跌。这不是矛盾——P是"状态的代价权重"，K = P/R才是实际增益，分母的R涨得更快。
口诀：K 正比于 sqrt(Q)、反比于 sqrt(R)；要速度就加大 Q，要省力就加大 R，而且都是"平方根"级别的敏感度。
例 3：LQR 的二维例——最优解给出的阻尼比
取 A = [[0, 1], [0, 0]]、B = [[0], [1]]、Q = I、R = 1。这样一个"双积分器"系统（位置 + 速度，没有自然阻尼），LQR 给出的解是闭式的、极其干净：
text
ARE 解出 P = [[sqrt3, 1], [1, sqrt3]]，K = R^-1 B^T P = [1, sqrt3]
sqrt(3) = 1.732051，故 K = [1.000000, 1.732051]
闭环 A - BK = [[0, 1], [-1, -sqrt3]]，特征方程 s^2 + 1.732051 s + 1 = 0
特征值 = -0.866025+0.500000j 与 -0.866025-0.500000j
阻尼比 zeta = 0.866025（= sqrt3/2），自然频率 wn = 1.000000为什么说这个结果"干净"：ζ = √3/2 ≈ 0.866025、ωn = 1——两个都是纯数学常数，不是试出来的。
而它给出来的阻尼，比经典控制里那个"最佳阻尼 0.707"还要大。 用 时域分析 的公式核对一下效果：
| 指标 | 公式 | 数值 |
|---|---|---|
| 超调量 | exp(−ζπ/sqrt(1−ζ²)) | 0.433342% |
| 峰值时间 | π / (ωn·sqrt(1−ζ²)) | 6.283185 s（恰好是 2π，因为 ωd = 0.5） |
| 调节时间 (±5%) | 3 / (ζωn) | 3.464102 s |
| 调节时间 (±2%) | 4 / (ζωn) | 4.618802 s |
| 谐振峰值 | 1 / (2ζ·sqrt(1−ζ²)) | 1.154701 |
| 带宽 | 由 ωn、ζ 定 | 0.786151 rad/s |
超调只有 0.43%——几乎看不见。 这就是"最优解自动选了偏大的阻尼"的效果：R = 1 意味着"控制量不算贵"，于是 LQR 选择用较大的反馈把状态压住，代价是响应稍慢（ts(2%) = 4.62 s，而 ωn = 1 说明"慢"是相对于带宽而言的）。
注意 tp = 2π 这个数：它不是巧合，而是 ωd = ωn·sqrt(1 − ζ²) = 1 × 0.5 = 0.5，于是 tp = π/0.5 = 2π。"阻尼比取 √3/2 时 ωd 恰好是 ωn 的一半"是一个值得记住的特例——它让这个例子成为教材里最常用的 LQR 算例。
如果想让响应更快呢？把 Q 增大（更罚状态偏差）——闭环极点会整体向左移动，ωn 变大、ζ 变小（更欠阻尼）。这就是 LQR 的"调参"：不是直接指定极点，而是通过 Q/R 间接移动极点，且移动的方向由数学保证是"朝更优的方向"。
例 4：Riccati 方程的数值迭代
Riccati 方程含 P 的二次项，没有通用闭式解，工程上常用梯度流迭代（把方程左边当成"误差"，沿它下降）：
text
迭代式 P <- P + dt*(A^T P + P A - P B R^-1 B^T P + Q)，dt = 1e-4
迭代 1000 次: P = [[1.099667, 0.099969], [0.099969, 1.009354]]，ARE 残差最大 9.99e-01
迭代 20000 次: P = [[1.780240, 1.024488], [1.024488, 1.726767]]，ARE 残差最大 6.73e-02
迭代 200000 次: P = [[1.732051, 1.000000], [1.000000, 1.732051]]，ARE 残差最大 1.11e-12三行数据正好是三个不同阶段：
- 迭代 1000 次：
P还在往目标爬（1.099667对目标1.732051），残差9.99e-01——几乎没收敛； - 迭代 20000 次：已经相当接近（
1.780240对1.732051，误差2.8%），残差降到6.73e-02； - 迭代 200000 次：
P = [[1.732051, 1.000000], [1.000000, 1.732051]]——与例 3 的闭式解[[√3, 1], [1, √3]]逐位一致，残差1.11e-12已经是机器精度。
这条"数值解收敛到闭式解"的核对，价值很高：例 3 用的是"猜出解析形式再验证"，例 4 用的是"从 P = Q 出发纯迭代"——两条完全独立的路径给出了同一个答案。这和第 10-state 章里"e^{At} 用级数与 Cayley-Hamilton 互证"是同一个方法论。
注意收敛速度：dt = 1e-4 的梯度流需要约 20 万步才能到机器精度——这说明 Riccati 方程虽然"可以迭代解"，但直接梯度流并不高效。工程实现用的是更快的算法（Kleinman 迭代、Schur 方法等），它们能在十几次迭代内收敛。"能算"和"算得快"是两回事，这是数值计算里的常识。
例 5：卡尔曼滤波的一步更新
取 P⁻ = 4（先验方差）、R = 1（测量噪声方差）、x̂⁻ = 10（先验估计）、z = 12（本次测量）：
text
先验方差 P- = 4.0，量测噪声 R = 1.0 -> 卡尔曼增益 K = 0.800000
先验估计 x_hat- = 10.0，量测 z = 12.0 -> 后验估计 x_hat = 11.600000
后验方差 P = 0.800000（比先验方差缩小到 20.00%）三个数字讲清了卡尔曼滤波的全部逻辑：
K = 4/(4+1) = 0.8：先验方差是测量噪声方差的 4 倍——"我对自己不太有信心，更信测量"，所以增益取到0.8（离 1 很近）。如果P⁻ = 1、R = 4，则K = 0.2，那时就该主要相信模型。x̂ = 10 + 0.8×(12 − 10) = 11.6：估计值落在"模型预测"与"本次测量"之间的8:2处。这正是加权平均——权重由不确定度决定。P = (1 − 0.8)×4 = 0.8：不确定度从 4 降到 0.8，缩小到 20%。这一步是卡尔曼滤波"越来越准"的机制：每次测量都把方差砍一截。
P 的递推才是卡尔曼滤波区别于普通观测器的关键：普通观测器的增益 L 是常数（上一章 L = [17, 47] 就是个定值）；卡尔曼滤波每步都根据"当前有多不确定"重算增益——所以它在"测量刚开始（很不可信）"和"收敛之后（很可信）"会表现出完全不同的行为。
例 6：C 实现——李雅普诺夫判稳、LQR 与 Riccati 迭代
/* lqr.c —— 现代控制：李雅普诺夫方程、LQR 最优增益、Riccati 迭代 */
#include <stdio.h>
#include <math.h>
/* 2x2 矩阵：行优先 M[2][2] */
static void mmul(const double A[2][2], const double B[2][2], double R[2][2])
{
int i, j, k;
for (i = 0; i < 2; i++)
for (j = 0; j < 2; j++) {
R[i][j] = 0.0;
for (k = 0; k < 2; k++)
R[i][j] += A[i][k] * B[k][j];
}
}
/* 一维列向量乘一维行向量的外积（B = [0,1]^T 时用来算 P B B^T P） */
static void vecmul(const double a[2], const double b[2], double R[2][2])
{
R[0][0] = a[0] * b[0];
R[0][1] = a[0] * b[1];
R[1][0] = a[1] * b[0];
R[1][1] = a[1] * b[1];
}
int main(void)
{
const double A[2][2] = { {0.0, 1.0}, {-2.0, -3.0} };
const double AT[2][2] = { {0.0, -2.0}, {1.0, -3.0} };
double P[2][2] = { {1.25, 0.25}, {0.25, 0.25} };
double L[2][2], R2[2][2], S[2][2];
int i, j;
printf("=== 一、李雅普诺夫第二法：解 A^T P + P A = -Q（Q 取单位阵）===\n");
printf(" A = [[0, 1], [-2, -3]]\n");
printf(" -4*p12 = -1 -> p12 = 0.25\n");
printf(" 2*p12 - 6*p22 = -1 -> p22 = 0.25\n");
printf(" p11 - 3*p12 - 2*p22 = 0 -> p11 = 1.25\n");
printf(" P = [[%.2f, %.2f], [%.2f, %.2f]]\n", P[0][0], P[0][1], P[1][0], P[1][1]);
mmul(AT, P, L);
mmul(P, A, R2);
for (i = 0; i < 2; i++)
for (j = 0; j < 2; j++)
S[i][j] = L[i][j] + R2[i][j] + (i == j ? 1.0 : 0.0);
printf(" 核对 A^T P + P A + I = [[%.1e, %.1e], [%.1e, %.1e]]（应全为 0）\n",
S[0][0], S[0][1], S[1][0], S[1][1]);
printf(" 正定判据：一阶主子式 = %.4f > 0，二阶主子式 = %.4f > 0 -> P 正定\n",
P[0][0], P[0][0] * P[1][1] - P[0][1] * P[1][0]);
printf(" 结论：V(x) = x^T P x 正定、其导数 = -x^T Q x 负定 -> 渐近稳定\n");
printf("\n=== 二、LQR 标量例：Q = 1、R 变化时增益怎么变 ===\n");
printf(" A = 0，B = 1；ARE 化为 -P^2/R + Q = 0 -> P = sqrt(Q*R)，K = sqrt(Q/R)\n");
printf(" R P K 闭环极点 (-K)\n");
for (i = 0; i < 4; i++) {
const double Rs[4] = { 0.25, 1.0, 4.0, 16.0 };
double Pp = sqrt(1.0 * Rs[i]);
double Kk = Pp / Rs[i];
printf(" %6.2f %7.4f %7.4f %8.4f\n", Rs[i], Pp, Kk, -Kk);
}
printf("\n=== 三、LQR 二维例：闭式解 ===\n");
{
double sq3 = sqrt(3.0);
double re = -sq3 / 2.0;
double im = sqrt(-(sq3 * sq3 - 4.0)) / 2.0;
printf(" A = [[0, 1], [0, 0]]，B = [[0], [1]]，Q = I，R = 1\n");
printf(" ARE 解出 P = [[sqrt3, 1], [1, sqrt3]]，K = [1, sqrt3]\n");
printf(" sqrt(3) = %.6f，故 K = [1.000000, %.6f]\n", sq3, sq3);
printf(" 闭环 A - BK = [[0, 1], [-1, -sqrt3]]，特征方程 s^2 + %.6f s + 1 = 0\n", sq3);
printf(" 特征值 = %+.6f%+.6fj 与 %+.6f%+.6fj\n", re, im, re, -im);
printf(" 阻尼比 zeta = %.6f（= sqrt3/2），自然频率 wn = %.6f\n", sq3 / 2.0, 1.0);
}
printf("\n=== 四、Riccati 方程数值迭代（从 P = Q 出发）===\n");
{
const double Aq[2][2] = { {0.0, 1.0}, {0.0, 0.0} };
const double AqT[2][2] = { {0.0, 0.0}, {1.0, 0.0} };
const int iters[3] = { 1000, 20000, 200000 };
int it, k;
printf(" 迭代式 P <- P + dt*(A^T P + P A - P B R^-1 B^T P + Q)，dt = 1e-4\n");
for (k = 0; k < 3; k++) {
double Pm[2][2] = { {1.0, 0.0}, {0.0, 1.0} };
double dP[2][2], T1[2][2], T2[2][2], PBBP[2][2];
double resid = 0.0;
for (it = 0; it < iters[k]; it++) {
mmul(AqT, Pm, T1);
mmul(Pm, Aq, T2);
vecmul((double[2]) { Pm[0][1], Pm[1][1] },
(double[2]) { Pm[1][0], Pm[1][1] }, PBBP);
for (i = 0; i < 2; i++)
for (j = 0; j < 2; j++) {
dP[i][j] = T1[i][j] + T2[i][j] - PBBP[i][j] + (i == j ? 1.0 : 0.0);
Pm[i][j] += dP[i][j] * 1e-4;
}
}
mmul(AqT, Pm, T1);
mmul(Pm, Aq, T2);
vecmul((double[2]) { Pm[0][1], Pm[1][1] },
(double[2]) { Pm[1][0], Pm[1][1] }, PBBP);
for (i = 0; i < 2; i++)
for (j = 0; j < 2; j++) {
double res = T1[i][j] + T2[i][j] - PBBP[i][j] + (i == j ? 1.0 : 0.0);
if (fabs(res) > resid) resid = fabs(res);
}
printf(" 迭代 %7d 次: P = [[%.6f, %.6f], [%.6f, %.6f]]，ARE 残差最大 %.2e\n",
iters[k], Pm[0][0], Pm[0][1], Pm[1][0], Pm[1][1], resid);
}
}
return 0;
}
c 本站为静态站，不提供在线运行；可复制到本地用 gcc / python 执行
预期输出：
=== 一、李雅普诺夫第二法：解 A^T P + P A = -Q（Q 取单位阵）===
A = [[0, 1], [-2, -3]]
-4*p12 = -1 -> p12 = 0.25
2*p12 - 6*p22 = -1 -> p22 = 0.25
p11 - 3*p12 - 2*p22 = 0 -> p11 = 1.25
P = [[1.25, 0.25], [0.25, 0.25]]
核对 A^T P + P A + I = [[0.0e+00, 0.0e+00], [0.0e+00, 0.0e+00]]（应全为 0）
正定判据：一阶主子式 = 1.2500 > 0，二阶主子式 = 0.2500 > 0 -> P 正定
结论：V(x) = x^T P x 正定、其导数 = -x^T Q x 负定 -> 渐近稳定
=== 二、LQR 标量例：Q = 1、R 变化时增益怎么变 ===
A = 0，B = 1；ARE 化为 -P^2/R + Q = 0 -> P = sqrt(Q*R)，K = sqrt(Q/R)
R P K 闭环极点 (-K)
0.25 0.5000 2.0000 -2.0000
1.00 1.0000 1.0000 -1.0000
4.00 2.0000 0.5000 -0.5000
16.00 4.0000 0.2500 -0.2500
=== 三、LQR 二维例：闭式解 ===
A = [[0, 1], [0, 0]]，B = [[0], [1]]，Q = I，R = 1
ARE 解出 P = [[sqrt3, 1], [1, sqrt3]]，K = [1, sqrt3]
sqrt(3) = 1.732051，故 K = [1.000000, 1.732051]
闭环 A - BK = [[0, 1], [-1, -sqrt3]]，特征方程 s^2 + 1.732051 s + 1 = 0
特征值 = -0.866025+0.500000j 与 -0.866025-0.500000j
阻尼比 zeta = 0.866025（= sqrt3/2），自然频率 wn = 1.000000
=== 四、Riccati 方程数值迭代（从 P = Q 出发）===
迭代式 P <- P + dt*(A^T P + P A - P B R^-1 B^T P + Q)，dt = 1e-4
迭代 1000 次: P = [[1.099667, 0.099969], [0.099969, 1.009354]]，ARE 残差最大 9.99e-01
迭代 20000 次: P = [[1.780240, 1.024488], [1.024488, 1.726767]]，ARE 残差最大 6.73e-02
迭代 200000 次: P = [[1.732051, 1.000000], [1.000000, 1.732051]]，ARE 残差最大 1.11e-12这段代码有两处值得学的写法：
c
/* B = [0, 1]^T，所以 P B B^T P = (P 的第 2 列) × (P 的第 2 行) */
vecmul((double[2]) { Pm[0][1], Pm[1][1] }, (double[2]) { Pm[1][0], Pm[1][1] }, PBBP);第一处：把"矩阵乘法"退化成一个外积。当 B 只有一个非零元素时，P B Bᵀ P 其实只是 P 的某一列乘某一行——用一个两元素的 vecmul 就够了，不必真去做 2×2 的完整乘法。这种"看到结构就简化"的习惯，在嵌入式里是省 flash 与省时间的常规手段。
第二处：复合字面量 (double[2]){ ... } 直接在调用处构造临时数组（C99 特性）。它避免了为临时向量单独声明一个变量，让"这里只是要传两个数"这件事在代码里一目了然。
注意 im = sqrt(-(sq3*sq3 - 4.0)) / 2.0 这个写法：判别式 sq3² − 4 = −1 是负数，代码里显式取了负再开方（而不是引入复数类型），因为已经知道结论是一对共轭复根 −0.866025 ± 0.5j。"知道数学结论、用实数算"是数值代码里很常见的取舍——代价是这段代码不能再处理"根是实根"的情形。
例 7：Python——李雅普诺夫、LQR 两步、Riccati 与卡尔曼
# 现代控制：李雅普诺夫方程、LQR 最优增益、Riccati 迭代
import cmath
import math
def mmul(A, B):
n, m, p = len(A), len(B[0]), len(B)
return [[sum(A[i][k] * B[k][j] for k in range(p)) for j in range(m)] for i in range(n)]
def transpose(A):
return [[A[j][i] for j in range(len(A))] for i in range(len(A[0]))]
def madd(A, B):
return [[A[i][j] + B[i][j] for j in range(len(A[0]))] for i in range(len(A))]
def msub(A, B):
return [[A[i][j] - B[i][j] for j in range(len(A[0]))] for i in range(len(A))]
print("=== 一、李雅普诺夫第二法：解 A^T P + P A = -Q（Q 取单位阵）===")
A = [[0.0, 1.0], [-2.0, -3.0]]
print(" A = [[0, 1], [-2, -3]]")
print(" 设 P = [[p11, p12], [p12, p22]]，代入后得三个方程：")
print(" -4*p12 = -1 -> p12 = 0.25")
print(" 2*p12 - 6*p22 = -1 -> p22 = 0.25")
print(" p11 - 3*p12 - 2*p22 = 0 -> p11 = 1.25")
p11, p12, p22 = 1.25, 0.25, 0.25
P = [[p11, p12], [p12, p22]]
print(f" P = [[{p11:.2f}, {p12:.2f}], [{p12:.2f}, {p22:.2f}]]")
res = madd(madd(mmul(transpose(A), P), mmul(P, A)), [[1.0, 0.0], [0.0, 1.0]])
print(f" 核对 A^T P + P A + I = [[{res[0][0]:.1e}, {res[0][1]:.1e}], "
f"[{res[1][0]:.1e}, {res[1][1]:.1e}]]（应全为 0）")
d1 = p11
d2 = p11 * p22 - p12 * p12
print(f" 正定判据：一阶主子式 = {d1:.4f} > 0，二阶主子式 = {d2:.4f} > 0 -> P 正定")
print(" 结论：取 V(x) = x^T P x，则 V 正定、V 的导数 = -x^T Q x 负定 -> 渐近稳定")
print("\n=== 二、LQR 标量例：Q = 1、R 变化时增益怎么变 ===")
print(" A = 0，B = 1；ARE 化为 -P^2/R + Q = 0 -> P = sqrt(Q*R)，K = P/R = sqrt(Q/R)")
print(" R P K 闭环极点 (-K)")
for R in (0.25, 1.0, 4.0, 16.0):
Pp = math.sqrt(1.0 * R)
K = Pp / R
print(f" {R:6.2f} {Pp:7.4f} {K:7.4f} {-K:8.4f}")
print("\n=== 三、LQR 二维例：闭式解 ===")
print(" A = [[0, 1], [0, 0]]，B = [[0], [1]]，Q = I，R = 1")
print(" ARE 解出 P = [[sqrt3, 1], [1, sqrt3]]，K = R^-1 B^T P = [1, sqrt3]")
sq3 = math.sqrt(3.0)
print(f" sqrt(3) = {sq3:.6f}，故 K = [{1.0:.6f}, {sq3:.6f}]")
bb = sq3
de = 1.0
disc = bb * bb - 4 * de
r1 = (-bb + cmath.sqrt(disc)) / 2.0
r2 = (-bb - cmath.sqrt(disc)) / 2.0
print(f" 闭环 A - BK = [[0, 1], [-1, -sqrt3]]，特征方程 s^2 + {sq3:.6f} s + 1 = 0")
print(f" 特征值 = {r1.real:.6f}{r1.imag:+.6f}j 与 {r2.real:.6f}{r2.imag:+.6f}j")
wn = math.sqrt(de)
zeta = sq3 / (2 * wn)
print(f" 阻尼比 zeta = {zeta:.6f}（= sqrt3/2），自然频率 wn = {wn:.6f}")
print("\n=== 四、Riccati 方程数值迭代（从 P = Q 出发）===")
print(" 迭代式 P <- P + dt*(A^T P + P A - P B R^-1 B^T P + Q)，dt = 1e-4")
Aq = [[0.0, 1.0], [0.0, 0.0]]
Bq = [[0.0], [1.0]]
Qq = [[1.0, 0.0], [0.0, 1.0]]
Pm = [[1.0, 0.0], [0.0, 1.0]]
for it in (1000, 20000, 200000):
Pm = [[1.0, 0.0], [0.0, 1.0]]
for _ in range(it):
PBBP = mmul(mmul(Pm, Bq), mmul(transpose(Bq), Pm))
dP = madd(msub(madd(mmul(transpose(Aq), Pm), mmul(Pm, Aq)), PBBP), Qq)
Pm = madd(Pm, [[dP[i][j] * 1e-4 for j in range(2)] for i in range(2)])
resid = madd(msub(madd(mmul(transpose(Aq), Pm), mmul(Pm, Aq)),
mmul(mmul(Pm, Bq), mmul(transpose(Bq), Pm))), Qq)
mx = max(abs(resid[i][j]) for i in range(2) for j in range(2))
print(f" 迭代 {it:7d} 次: P = [[{Pm[0][0]:.6f}, {Pm[0][1]:.6f}], "
f"[{Pm[1][0]:.6f}, {Pm[1][1]:.6f}]]，ARE 残差最大 {mx:.2e}")
print("\n=== 五、卡尔曼滤波的一步更新（一维标量例）===")
print(" K = P-/(P- + R)；x_hat = x_hat- + K*(z - x_hat-)；P = (1 - K)*P-")
Pm_ = 4.0
R = 1.0
z = 12.0
xh_ = 10.0
K = Pm_ / (Pm_ + R)
xh = xh_ + K * (z - xh_)
Pn = (1.0 - K) * Pm_
print(f" 先验方差 P- = {Pm_:.1f}，量测噪声 R = {R:.1f} -> 卡尔曼增益 K = {K:.6f}")
print(f" 先验估计 x_hat- = {xh_:.1f}，量测 z = {z:.1f} -> 后验估计 x_hat = {xh:.6f}")
print(f" 后验方差 P = {Pn:.6f}（比先验方差缩小到 {Pn / Pm_ * 100:.2f}%）")
print(" 读法：R 越小（量测越可信）K 越接近 1，估计越向量测靠；R 越大 K 越接近 0")
python 本站为静态站，不提供在线运行；可复制到本地用 gcc / python 执行
预期输出：
=== 一、李雅普诺夫第二法：解 A^T P + P A = -Q（Q 取单位阵）===
A = [[0, 1], [-2, -3]]
设 P = [[p11, p12], [p12, p22]]，代入后得三个方程：
-4*p12 = -1 -> p12 = 0.25
2*p12 - 6*p22 = -1 -> p22 = 0.25
p11 - 3*p12 - 2*p22 = 0 -> p11 = 1.25
P = [[1.25, 0.25], [0.25, 0.25]]
核对 A^T P + P A + I = [[0.0e+00, 0.0e+00], [0.0e+00, 0.0e+00]]（应全为 0）
正定判据：一阶主子式 = 1.2500 > 0，二阶主子式 = 0.2500 > 0 -> P 正定
结论：取 V(x) = x^T P x，则 V 正定、V 的导数 = -x^T Q x 负定 -> 渐近稳定
=== 二、LQR 标量例：Q = 1、R 变化时增益怎么变 ===
A = 0，B = 1；ARE 化为 -P^2/R + Q = 0 -> P = sqrt(Q*R)，K = P/R = sqrt(Q/R)
R P K 闭环极点 (-K)
0.25 0.5000 2.0000 -2.0000
1.00 1.0000 1.0000 -1.0000
4.00 2.0000 0.5000 -0.5000
16.00 4.0000 0.2500 -0.2500
=== 三、LQR 二维例：闭式解 ===
A = [[0, 1], [0, 0]]，B = [[0], [1]]，Q = I，R = 1
ARE 解出 P = [[sqrt3, 1], [1, sqrt3]]，K = R^-1 B^T P = [1, sqrt3]
sqrt(3) = 1.732051，故 K = [1.000000, 1.732051]
闭环 A - BK = [[0, 1], [-1, -sqrt3]]，特征方程 s^2 + 1.732051 s + 1 = 0
特征值 = -0.866025+0.500000j 与 -0.866025-0.500000j
阻尼比 zeta = 0.866025（= sqrt3/2），自然频率 wn = 1.000000
=== 四、Riccati 方程数值迭代（从 P = Q 出发）===
迭代式 P <- P + dt*(A^T P + P A - P B R^-1 B^T P + Q)，dt = 1e-4
迭代 1000 次: P = [[1.099667, 0.099969], [0.099969, 1.009354]]，ARE 残差最大 9.99e-01
迭代 20000 次: P = [[1.780240, 1.024488], [1.024488, 1.726767]]，ARE 残差最大 6.73e-02
迭代 200000 次: P = [[1.732051, 1.000000], [1.000000, 1.732051]]，ARE 残差最大 1.11e-12
=== 五、卡尔曼滤波的一步更新（一维标量例）===
K = P-/(P- + R)；x_hat = x_hat- + K*(z - x_hat-)；P = (1 - K)*P-
先验方差 P- = 4.0，量测噪声 R = 1.0 -> 卡尔曼增益 K = 0.800000
先验估计 x_hat- = 10.0，量测 z = 12.0 -> 后验估计 x_hat = 11.600000
后验方差 P = 0.800000（比先验方差缩小到 20.00%）
读法：R 越小（量测越可信）K 越接近 1，估计越向量测靠；R 越大 K 越接近 0这份 Python 里的 transpose 与 msub 两个小函数只在第四节用到（Aᵀ P + P A − P B R⁻¹BᵀP + Q 需要转置与减法），前三节用不到。把工具函数一次性写全、而不是"用到才补"，代价是前几节读起来多两个不相关的函数，好处是第四节读起来干净——两种风格都有道理，关键是别在中间"边写边补"，那会让代码前后不一致。
考点
- 现代控制 vs 经典控制的分工：经典用传递函数、频域、单入单出、靠试凑；现代用状态方程、时域、多入多出、按指标求解。
- 李雅普诺夫第二法：构造正定函数
V，若V̇负定，则渐近稳定——不解方程。 - 线性系统的李雅普诺夫方程
AᵀP + PA = −Q（Q正定）：解出P正定 ⟺ 系统渐近稳定。Q可任选，取单位阵最方便，结论与Q的选择无关。 - 第二法对线性系统是充要条件，对非线性系统只是充分条件（找不到
V不等于不稳定）。 - LQR 的代价函数
J = ∫(xᵀQx + uᵀRu)dt：Q罚状态偏差、R罚控制消耗。 - LQR 结论：
u = −Kx、K = R⁻¹BᵀP，P是 Riccati 方程AᵀP + PA − PBR⁻¹BᵀP + Q = 0的正定解。 - Riccati 方程含
P的二次项，无通用闭式解，需迭代；LQR 给出的闭环必然稳定（定理保证，无需另外判稳）。 Q/R的敏感度：标量情形K = sqrt(Q/R)——K正比于sqrt(Q)、反比于sqrt(R)；R增大 4 倍，K只减半。- 标准算例：
A = [[0,1],[0,0]]、B = [0;1]、Q = I、R = 1→P = [[√3, 1], [1, √3]]、K = [1, √3]、ζ = √3/2 = 0.866025、ωn = 1，超调仅0.433342%。 - 卡尔曼滤波三步：
K = P⁻/(P⁻ + R)、x̂ = x̂⁻ + K(z − x̂⁻)、P = (1 − K)P⁻。K是"先验不确定度"与"测量噪声"之间的加权；P每步必减。 - 对偶关系：LQR 与卡尔曼滤波是同一个 Riccati 问题，
(A, B)换成(Aᵀ, Cᵀ)即可。 - 易错：把
Q、R当成"能随便取、越大越好"（真正决定的是两者之比）；把李雅普诺夫方程当成"能判出具体极点"（它只给"稳定/不稳定"这一个比特的信息）；以为卡尔曼增益是常数（它每步都变）；把"Riccati 方程无解析解"理解成"不能算"（能算，只是要迭代）。
小结
- 现代控制把"极点放哪"换成"什么代价最小"：
J = ∫(xᵀQx + uᵀRu)dt，Q罚状态、R罚控制。 - 李雅普诺夫第二法不解方程即可判稳：找正定
V，看V̇是否负定。线性系统化为AᵀP + PA = −Q，P正定即稳定。 - 本例题：
A = [[0,1],[−2,−3]]、Q = I→P = [[1.25, 0.25], [0.25, 0.25]]，两个主子式1.25、0.25均正。 - LQR 的最优解是线性反馈
u = −Kx，K = R⁻¹BᵀP，P解 Riccati 方程；闭环必然稳定。 - 标量例给出调参口诀：
K = sqrt(Q/R)——R涨 4 倍，K只减半。 - 标准算例给出漂亮的闭式解：
P = [[√3, 1], [1, √3]]、K = [1, √3]、ζ = 0.866025、ωn = 1、超调0.433342%——比经典控制的"最佳阻尼 0.707"更保守。 - Riccati 方程的迭代解收敛到闭式解（残差
1.11×10⁻¹²），但直接梯度流要 20 万步——"能算"和"算得快"是两回事。 - 卡尔曼滤波 = 会自己调增益的观测器：
K = P⁻/(P⁻ + R)，不确定度越大越信测量；本例题K = 0.8、方差缩到 20%。 - LQR 与卡尔曼滤波是对偶的同一个问题。
- 现代控制的代价是强依赖模型、需要全部状态、参数没有物理意义——PID 用"不需要模型"换鲁棒，现代控制用"需要模型"换最优。
回到主线：到这里，控制理论的"理论部分"就讲完了——从开环闭环、到建模、到三套分析工具（时域、根轨迹、频域）、到两大设计方法（PID 经典、LQR 现代）。但这些都还是"算"，还没有"动"。
最后一章回到物理层：控制器算出来的那个数字，最终要驱动一台电机转起来。而电机有它自己的脾气——电枢电阻、反电动势、转差率，还有"转子和负载之间的力学关系"。
下一篇：电机与拖动基础
评论（0）
当前浏览器不允许本地存储，评论无法保存。
还没有评论，来说两句。