Related: Coding Projects 2 & 3
Topics: Observer / Feedforward / System Type / Noise
Overview / 总述
为什么需要超越 PI？
在 Lecture 2 中我们建立了统一电机模型。在 Coding Project 1–2 中你已经体验了 PI 调参和前馈解耦。但 PI 只能"被动"地用误差去纠正——它不知道扰动是什么，只能等误差出现后再反应。
本讲介绍一种"主动"策略：用观测器估计扰动，然后前馈补偿。我们将从三个维度评价运动控制系统：
- 指令跟踪 (Command Tracking)——跟踪速度给定有多快、多准？
- 扰动抑制 (Disturbance Rejection)——抗负载转矩变化有多强？
- 噪声抑制 (Noise Attenuation)——对测量噪声有多敏感？
这三者之间存在根本性的权衡：不可能同时最优。本讲将用传递函数语言精确描述这一权衡。
6.1 Motion Equation / 运动方程
符号定义与运动方程
我们从 PMSM 的机械运动方程出发，定义本讲所有变量：
| 符号 | 含义 | 单位 |
|---|
| $\theta_m$ | 机械转子角度 | rad |
| $\Omega = \dot{\theta}_m$ | 机械角速度 | rad/s |
| $\Omega^*$ | 速度参考指令 | rad/s |
| $T_e$ | 电磁转矩（实际值） | Nm |
| $T_e^*$ | 转矩参考（控制器输出） | Nm |
| $T_L$ | 负载转矩 | Nm |
| $J$ | 实际转动惯量 | kg·m² |
| $J_n$ | 控制器中使用的标称惯量 | kg·m² |
| $B$ | 粘性摩擦系数 | Nm·s/rad |
| $\delta_p$ | 位置传感器量化噪声 | rad |
运动方程为：
$$J\dot{\Omega} = T_e - B\Omega - T_L$$
(1)
定义"总扰动" $d_{to}$
控制器发出的是转矩参考 $T_e^*$，而非实际转矩 $T_e$。将运动方程改写为以 $T_e^*$ 为输入：
$$\dot{\Omega} = \frac{1}{J} T_e^* - \underbrace{\frac{T_e^* - T_e + B\Omega + T_L}{J}}_{d_n\text{ (标称扰动)}} = b\, T_e^* + d_n$$
(2a) — $b = 1/J$ 是实际控制增益
由于实际惯量 $J$ 未知，控制器用标称值 $J_n$ 代替，定义 $b_n = 1/J_n$，则：
$$\dot{\Omega} = b_n T_e^* + \underbrace{(b - b_n)T_e^* + d_n}_{d_{to}\text{ (总扰动)}}$$
(2b) — $d_{to}$ 包含负载转矩、摩擦、惯量失配等一切未建模动态
核心定义：$d_{to}$ 是从控制器角度看的"总扰动"——它把所有控制器不知道的东西（负载、摩擦、参数误差、未建模动态）打包成一个标量。如果我们能估计 $d_{to}$，就可以前馈补偿掉它。
定义控制增益比 $k_b$
定义 $k_b = b_n / b = J / J_n$，即实际惯量与标称惯量的比值：
- $k_b = 1$：惯量匹配，无参数失配
- $k_b > 1$：实际惯量大于标称（例如负载比预期重）
- $k_b < 1$：实际惯量小于标称
6.2 Feedback Control Law / 反馈控制律
基于观测器的速度控制器
目标：让速度跟踪误差 $e_s = \Omega^* - \Omega$ 以指数速率衰减，即 $\dot{e}_s = -k_{ps} e_s$，其中 $k_{ps} > 0$ 是反馈增益（单位：rad/s），决定了闭环响应速度。
将 $\dot{e}_s = \dot{\Omega}^* - \dot{\Omega}$ 代入 (2b)，解出转矩参考：
$$T_e^* = \frac{\dot{\Omega}^* + k_{ps}(\Omega^* - \Omega) - d_{to}}{b_n}$$
(3) — 理想控制律（需要已知 $\Omega$ 和 $d_{to}$）
问题：$\Omega$ 和 $d_{to}$ 都不能直接测量。我们需要一个观测器来估计它们。用估计值 $\hat{\Omega}$ 和 $\hat{d}_{to}$ 替代：
$$T_e^* = \frac{\dot{\Omega}^* + k_{ps}(\Omega^* - \hat{\Omega}) - \hat{d}_{to}}{b_n}$$
(4) — 实际控制律
6.3 Observer / 观测器
从位置测量估计速度和扰动
转子位置 $\theta_m$ 由编码器测量，但含有量化噪声 $\delta_p$：
$$\theta_m^{\text{meas}} = \theta_m + \delta_p$$
我们用一个观测器来同时估计位置、速度和扰动。观测器的状态为 $[\hat{\theta}_m,\, \hat{\Omega},\, \hat{d}_{to},\, \hat{p}_{to},\, \ldots]$，其中 $\hat{p}_{to} = \dot{\hat{d}}_{to}$ 是扰动变化率的估计。
3 阶观测器：假设扰动恒定
内部模型假设 $\dot{d}_{to} = 0$（扰动不随时间变化）。状态：$[\hat{\theta}_m,\, \hat{\Omega},\, \hat{d}_{to}]$。
$$\begin{aligned}
\dot{\hat{\theta}}_m &= \ell_1 \underbrace{(\theta_m^{\text{meas}} - \hat{\theta}_m)}_{e_\theta\text{ (位置误差)}} + \hat{\Omega} \\
\dot{\hat{\Omega}} &= \ell_2\, e_\theta + (b_n T_e^* + \hat{d}_{to}) \\
\dot{\hat{d}}_{to} &= \ell_3\, e_\theta
\end{aligned}$$
(5a) — 3 阶观测器动态方程
其中 $e_\theta = \theta_m^{\text{meas}} - \hat{\theta}_m$ 是位置估计误差，$\ell_1, \ell_2, \ell_3$ 是观测器增益。
4 阶观测器：假设扰动为斜坡
内部模型假设 $\dot{d}_{to} = p_{to}$，$\dot{p}_{to} = 0$（扰动以恒定速率变化）。状态：$[\hat{\theta}_m,\, \hat{\Omega},\, \hat{d}_{to},\, \hat{p}_{to}]$。
$$\begin{aligned}
\dot{\hat{\theta}}_m &= \ell_1\, e_\theta + \hat{\Omega} \\
\dot{\hat{\Omega}} &= \ell_2\, e_\theta + (b_n T_e^* + \hat{d}_{to}) \\
\dot{\hat{d}}_{to} &= \ell_3\, e_\theta + \hat{p}_{to} \\
\dot{\hat{p}}_{to} &= \ell_4\, e_\theta
\end{aligned}$$
(5b) — 4 阶观测器动态方程
关键区别：第三行多了 $+ \hat{p}_{to}$（扰动变化率的前馈），第四行用 $\ell_4 e_\theta$ 驱动对扰动变化率的估计。
增益设计：极点配置法
将观测器的所有特征根（极点）配置在 $s = -\omega_{ob}$，其中 $\omega_{ob}$ 是观测器带宽（单位：rad/s）。利用二项式展开：
$$\text{3 阶：特征多项式 } \Delta_3 = (s + \omega_{ob})^3 = s^3 + 3\omega_{ob}s^2 + 3\omega_{ob}^2 s + \omega_{ob}^3$$
$\Rightarrow\; \ell_1 = 3\omega_{ob},\; \ell_2 = 3\omega_{ob}^2,\; \ell_3 = \omega_{ob}^3 / b_n,\; \ell_4 = 0$
$$\text{4 阶：特征多项式 } \Delta_4 = (s + \omega_{ob})^4 = s^4 + 4\omega_{ob}s^3 + 6\omega_{ob}^2 s^2 + 4\omega_{ob}^3 s + \omega_{ob}^4$$
$\Rightarrow\; \ell_1 = 4\omega_{ob},\; \ell_2 = 6\omega_{ob}^2,\; \ell_3 = 4\omega_{ob}^3 / b_n,\; \ell_4 = \omega_{ob}^4$
（注：代码中 $b_n = n_{pp}/J_s$，所以 $\ell_3$ 的表达式写成 $\omega_{ob}^3 \cdot J_s / n_{pp}$。）
$\omega_{ob}$ 是观测器最关键的调参旋钮：
- $\omega_{ob}$ 越大 → 估计收敛越快，但放大更多测量噪声
- $\omega_{ob}$ 越小 → 噪声更小，但扰动跟踪有延迟
- 经验法则：$\omega_{ob}$ 通常取速度环带宽的 3–10 倍
- 高阶观测器需要更小的 $\omega_{ob}$（因为噪声放大更严重）
6.4 Closed-Loop Transfer Functions / 闭环传递函数
三条通道：跟踪、扰动、噪声
将控制律 (4) 和观测器 (5) 代入运动方程 (2b)，可以推导出闭环输出由三条路径叠加而成：
$$\boxed{\Omega(s) = \Phi_r(s)\,\Omega^*(s) \;+\; \Phi_d(s)\,d_n(s) \;+\; \Phi_n(s)\,\big(-s\delta_p(s)\big)}$$
(6) — 三通道分解
其中 $-s\delta_p(s)$ 是位置噪声经求导后变成的速度测量噪声 $\delta_n(s)$。
统一表达式
为了简洁地表达不同观测器的闭环性能，定义辅助传递函数 $G_3(s)$：
$$G_3(s) = \Delta_n + k_{ps} s G_1(s) - G_2(s)$$
(7) — $G_3(s)$ 的定义
其中 $\Delta_n = (s + \omega_{ob})^n$ 是观测器特征多项式（$n = 3$ 或 $4$），$G_1(s)$ 和 $G_2(s)$ 由观测器如何估计速度和扰动的方式决定（见后文 §6.8 的具体展开）。
三条传递函数的统一形式为：
$$\Phi_r(s) = \frac{\Delta_n(s + k_{ps})}{\Delta_n(s + k_{ps}) + (k_b - 1)s\,G_3(s)}$$
(8a) — 跟踪传递函数
$$\Phi_d(s) = \frac{k_b\, G_3(s)}{\Delta_n(s + k_{ps}) + (k_b - 1)s\,G_3(s)}$$
(8b) — 扰动传递函数
$$\Phi_n(s) = 1 - s\,\Phi_d(s)$$
(8c) — 噪声传递函数（与扰动互补）
关键观察：
- 当 $k_b = 1$（惯量匹配）：$\Phi_r(s) = 1$，系统完美跟踪任何参考指令
- $\Phi_n + s\Phi_d = 1$——这是一个互补约束，改善扰动抑制必然恶化噪声抑制
- $G_3(s)$ 完全决定了扰动和噪声的频率特性——不同的观测器结构对应不同的 $G_3(s)$
6.5 Command Tracking / 指令跟踪
跟踪性能
从 (8a) 可知，跟踪性能完全取决于 $k_b$：
- $k_b = 1$：$\Phi_r = 1$，完美跟踪
- $k_b > 1$（实际惯量大于标称）：超调、振荡
- $k_b < 1$（实际惯量小于标称）：响应变快但噪声抑制变差
反馈增益 $k_{ps}$ 决定了闭环带宽：$k_{ps}$ 越大，阶跃响应越快（上升时间 $t_r \approx 2.2/k_{ps}$），但对噪声也越敏感。
与 Coding Project 2 的联系：在 Project 2 Part 1 中你扫描了 FOC_desired_VLBW_HZ，本质上就是在调节 $k_{ps}$。你观察到的上升时间-超调量权衡，正是 $\Phi_r(s)$ 的频域特性在时域的体现。
6.6 Disturbance Rejection / 扰动抑制
扰动通道与系统型别
扰动传递函数 $\Phi_d(s)$ (8b) 描述了负载转矩变化对速度的影响。以 $k_b = 1$ 为例：
$$\Phi_d(s)\big|_{k_b=1} = \frac{G_3(s)}{\Delta_n(s + k_{ps})}$$
(9)
$G_3(s)$ 在 $s = 0$ 处的行为决定了系统对不同类型扰动的稳态抑制能力。
系统型别的视角
经典控制理论中，系统的"型别"（system type）定义为开环传递函数中 $s = 0$ 处的积分器个数。Type I 系统对阶跃输入零稳态误差，Type II 对斜坡零稳态误差，以此类推。
现在我们换一个视角：把负载转矩 $T_L$ 当作"输入"，转速 $\Omega$ 当作"输出"，来分析这条扰动通道的型别。
核心洞察：观测器的扩展状态 = 扰动通道中的级联积分器
观测器每多估计一阶扰动的导数，就在扰动抑制通道中多串联一个积分器，系统型别提升一级：
| 观测器阶数 | 扰动通道积分器 | 型别 | 能无差跟踪的扰动 |
|---|
| 3 阶 | 1 个 | Type I | 阶跃 $T_L$（恒定负载） |
| 4 阶 | 2 个 | Type II | 斜坡 $T_L$（线性增加负载） |
| 5 阶 | 3 个 | Type III | 抛物线 $T_L$ |
从 $G_3(s)$ 验证
对于 3 阶观测器（我们代码中的默认配置），$G_3(s)$ 在 $s \to 0$ 时包含一个 $s$ 的因子：
$$G_3(s)\big|_{\text{3-rd order}} \sim c_1 \cdot s \quad \text{as } s \to 0$$
$\Rightarrow\; \Phi_d(0) = 0$：对阶跃扰动的稳态响应为零
但对斜坡扰动 $d_n = at$（$D_n(s) = a/s^2$），终值定理给出非零稳态误差。
对于 4 阶观测器，$G_3(s)$ 在 $s = 0$ 处多一个零点：
$$G_3(s)\big|_{\text{4-th order}} \sim c_2 \cdot s^2 \quad \text{as } s \to 0$$
$\Rightarrow\; s\Phi_d(s)\big|_{s=0} = 0$：对斜坡扰动的稳态响应也为零
与参考跟踪通道的类比：在参考跟踪通道中，PI 控制器的积分器（$K_i/s$）使系统对阶跃参考为 Type I。观测器的扩展状态在扰动通道中扮演了同样的角色——这和你在本科自动控制原理中学的系统型别判据完全一致，只不过"输入"从参考指令换成了扰动。
低频 vs 高频扰动抑制
从 Bode 图分析 $\Phi_d(j\omega)$ 的幅值：
- 低频扰动抑制能力主要由观测器的扰动估计性能（即 $G_3(s)$ 的零点阶数）决定
- 高频扰动抑制能力主要由反馈增益 $k_{ps}$ 决定，与观测器阶数关系不大
思考题
如果我们不用高阶观测器，而是在速度 PI 控制器中再串联一个积分器（变成 PII），能不能也实现 Type II 的扰动抑制？为什么实际中不这么做？
提示：积分器加在反馈控制器中会同时影响所有三条通道（跟踪、扰动、噪声），而观测器的积分器只加在扰动估计通道中——这就是为什么观测器方案更灵活。
6.7 Noise Attenuation / 噪声抑制
高阶观测器的代价
从 (8c) 我们有互补关系：
$$\Phi_n(s) + s\,\Phi_d(s) = 1$$
(10) — 互补约束
基本物理约束：$|\Phi_n(j\omega)| + |j\omega \cdot \Phi_d(j\omega)| \geq 1$ 在所有频率上成立。在低频改善扰动抑制（让 $\Phi_d$ 更小），必然导致在某些频率上 $\Phi_n$ 变大（噪声放大）。这是无法通过调参绕过的。
观测器阶数与噪声衰减斜率
不同的观测器在高频段对噪声的衰减速率不同。更高阶的观测器如果把更多阶数用于提升扰动抑制（更多积分器），留给噪声衰减的"余量"就更少：
- 3 阶观测器中的最佳噪声抑制方案：高频衰减 $-60$ dB/dec
- 4 阶观测器中的最佳噪声抑制方案：高频衰减 $-80$ dB/dec（因为总阶数更高，有更多余量）
- 但 4 阶中追求最佳低频扰动抑制的方案：高频噪声衰减仅 $-40$ dB/dec
速度测量噪声不是白噪声——它由编码器量化误差经求导产生，是有色噪声，功率谱随转速变化。因此 $\omega_{ob}$ 不能只看理论 Bode 图，还要考虑实际噪声特性。
6.8 Observer Structures / 观测器结构比较
同一阶数下的不同观测器
同一阶数的观测器（如 3 阶），根据如何组合观测到的速度和扰动来构成反馈，可以得到不同的控制器结构。每种结构对应不同的 $G_1(s)$、$G_2(s)$ 和 $G_3(s)$，从而有不同的扰动-噪声权衡。
$G_1(s)$ 和 $G_2(s)$ 的物理含义
回顾观测器的估计输出可以统一写成：
$$\hat{\Omega}(s) = \Omega_m(s) - G_1(s)\,\tilde{\theta}_m(s), \qquad \hat{d}_{to}(s) = \frac{G_2(s)}{s}\,\tilde{\theta}_m(s)$$
(11)
其中 $\Omega_m = s\theta_m^{\text{meas}}$ 是直接从位置求导得到的（含噪声的）速度，$\tilde{\theta}_m = \theta_m^{\text{meas}} - \hat{\theta}_m$ 是位置估计误差。
- $G_1(s)$：决定观测器如何"滤波"速度估计——$G_1$ 的高频行为决定噪声抑制
- $G_2(s)$：决定观测器如何从位置误差中提取扰动估计——$G_2$ 的低频行为决定扰动跟踪
代码中的观测器（3 阶位置观测器）
我们仿真代码中默认使用的 3 阶观测器，位置误差驱动所有三个状态，对应：
$$G_1(s) = 3\omega_{ob}s + 3\omega_{ob}^2,\qquad G_2(s) = \omega_{ob}^3 / b_n$$
$$G_3(s) = (s+\omega_{ob})^3 + k_{ps}s(3\omega_{ob}s + 3\omega_{ob}^2) - \omega_{ob}^3/b_n$$
$G_3(s)$ 在 $s=0$ 处：$G_3(0) = \omega_{ob}^3(1 - 1/b_n) + \ldots$
低频扰动抑制：$+20$ dB/dec（Type I）。高频噪声衰减：$-60$ dB/dec。
4 阶观测器（代码中切换到 else 分支）
扩展一阶扰动导数后：
$$G_1(s) = 4\omega_{ob}s^2 + 6\omega_{ob}^2 s,\qquad G_2(s) = (4\omega_{ob}^3 s + \omega_{ob}^4)/b_n$$
$G_3(s)$ 在 $s = 0$ 处有两个零点 $\sim s^2$，所以：
- 低频扰动抑制：$+40$ dB/dec（Type II）——能无差跟踪斜坡负载
- 高频噪声衰减：$-60$ dB/dec——与 3 阶的最佳方案相同
这是一个很好的折中方案：比 3 阶有更强的低频抗扰能力，同时高频噪声抑制不退化。
6.9 Two-DOF Control / 二自由度控制
分离跟踪与抗扰
标准 PI 控制器只有一个自由度——$K_p$ 和 $K_i$ 同时影响跟踪和抗扰。加入观测器前馈后，控制律自然变成二自由度 (2DOF) 结构：
$$i_q^* = \underbrace{K_p(\omega^* - \hat{\omega}) + K_i \int (\omega^* - \hat{\omega}) dt}_{\text{PI 反馈（决定跟踪性能）}} + \underbrace{\frac{\hat{d}_{to}}{1.5\, n_{pp}\, K_A}}_{\text{扰动前馈（决定抗扰性能）}}$$
(12) — PI + 前馈 = 二自由度控制
好处：
- 跟踪性能由 PI 增益（$K_p$, $K_i$）独立调节
- 抗扰性能由观测器带宽（$\omega_{ob}$）和阶数独立调节
- 两者可以分别优化，而不像纯 PI 那样互相牵制
这就是 Coding Project 3 中你要实现的内容——在 FOC() 函数中将 total_disrubance_feedforward 接入 cmd_idq[1]。
6.10 Robustness / 鲁棒性
$k_b$ 对系统稳定性的影响
从 (8a) 可以看出，当 $k_b \neq 1$ 时，分母出现 $(k_b - 1)sG_3(s)$ 项，可能改变闭环极点位置。
- $k_b > 1$：超调和振荡增加（闭环极点向虚轴移动），但噪声抑制改善
- $k_b < 1$：响应变快、无超调，但噪声更大，且低频抗扰变差
- 重要结论：低频扰动抑制能力越强的观测器方案，对 $k_b$ 变化越敏感（稳定裕度更小）
实际中 $k_b$ 的来源：负载惯量变化（机器人手臂不同姿态）、标称惯量标定误差。通常先在控制器中使用较小的 $J_n$（即 $k_b > 1$），因为实际负载往往比预期重。
与 Coding Project 2 的联系：在 Project 2 Part 3 中你通过修改 init_Js 引入了惯量失配，本质上就是在改变 $k_b$。你观察到的性能退化，正是 $\Phi_r(s)$ 和 $\Phi_d(s)$ 的分母因 $(k_b-1)$ 项而改变的结果。
Summary / 总结
三方权衡的全景图
核心结论
- 闭环系统的性能由三条传递函数决定：$\Phi_r$（跟踪）、$\Phi_d$（扰动）、$\Phi_n$（噪声）
- $\Phi_n + s\Phi_d = 1$——扰动抑制和噪声抑制互补，不可兼得
- 观测器每多扩展一阶状态，就在扰动通道中多一个积分器——系统型别提升一级（Type I → II → III）
- $\omega_{ob}$ 是最关键的设计参数——决定扰动估计速度与噪声水平的权衡
- 惯量失配 $k_b \neq 1$ 同时影响三个通道；低频抗扰越强的方案对 $k_b$ 越敏感
- PI + 观测器前馈 = 二自由度控制——跟踪和抗扰可以分别调节
Appendix / 附录
与文献中术语的联系
本讲的内容在文献中属于扰动/不确定性估计与抑制 (Disturbance/Uncertainty Estimation and Attenuation, DUEA) 框架。不同的研究社区对同一类方法使用了不同的名称：
| 本讲术语 | 文献术语 | 说明 |
|---|
| 3 阶/4 阶观测器 |
扩展状态观测器 (Extended State Observer, ESO) |
ESO 是 ADRC 框架中的核心组件。当观测器只比系统模型高一阶时就是经典 ESO |
| 高阶观测器 |
广义比例积分观测器 (Generalized Proportional-Integral Observer, GPIO) |
GPIO 是 ESO 的推广——扩展状态阶数可以任意高。文献中也称 Generalized ESO |
| "用观测器估计扰动 + 前馈补偿" |
自抗扰控制 (Active Disturbance Rejection Control, ADRC) |
ADRC 由韩京清提出，核心思想就是本讲介绍的：把所有不确定性视为"总扰动"，用 ESO 在线估计并补偿 |
| PI + 前馈 = 二自由度控制 |
2DOF Robust Control based on DUEA |
本讲的控制律 (4) 加上观测器，本质上就是一个 2DOF 控制系统 |
| $G_1(s),\, G_2(s),\, G_3(s)$ |
辅助传递函数 |
由 Zuo et al. (IEEE Trans. IE, 2022) 提出的统一分析框架。$G_1$ 描述速度估计、$G_2$ 描述扰动估计、$G_3$ 综合两者 |
| ESO_1, ESO_4, ESO_8 等编号 |
同阶 ESO 的不同组合方案 |
同一阶数的观测器可以通过不同方式组合速度/扰动估计，得到不同的 $G_1/G_2/G_3$，从而有不同的抗扰-噪声权衡。数字编号来自上述文献 |
推荐阅读
- [1] Zuo, Chen, Zhu, Lee. "Different Active Disturbance Rejection Controllers Based on the Same Order GPI Observer," IEEE Trans. Industrial Electronics, vol. 69, no. 11, Nov. 2022.——本讲的主要参考。提出了统一的 $G_1/G_2/G_3$ 分析框架，比较了 6 种三阶和 10 种四阶观测器方案
- [2] Gao, Z. "Scaling and bandwidth-parameterization based controller tuning," American Control Conference, 2003.——线性 ADRC 的参数化方法，$\omega_{ob}$ 带宽调参的理论基础
- [3] Han, J. "From PID to Active Disturbance Rejection Control," IEEE Trans. Industrial Electronics, vol. 56, no. 3, 2009.——ADRC 的奠基性综述
为什么本讲不直接从 ADRC 入手？
ADRC 是一个完整的控制框架，有自己的术语体系。但其核心思想——用观测器估计总扰动、前馈补偿——可以从经典控制的系统型别视角自然推导出来。本讲选择先从运动方程、观测器、传递函数出发，建立直觉后再与 ADRC 对接。这样你会发现：ADRC 并不是一个全新的东西，它是经典反馈 + 观测器前馈的自然延伸。