电机控制Bode图（2）- 测试机械共振与结构谐振

摘要

本文面向伺服驱动、FOC 电机控制、丝杆平台、皮带传动机构和高动态运动平台等场景，系统说明如何利用 Bode 图和 Nyquist 图识别机械共振、结构谐振和编码器噪声问题，并给出基于频域分析的优化思路，包括陷波器设计、低通滤波、速度观测优化、机械刚度提升和环路重整定。文中重点回答以下问题：共振和噪声在 Bode 图上表现为什么；在 Nyquist 图上如何理解"稳定但危险"的状态；测试时输入、输出、注入点和观测量分别是什么；如何根据测试结果确定是否需要增加陷波器；陷波器参数如何选取；优化后如何验证效果。

1. 引言

在电机控制系统中，很多"电流环、速度环、位置环参数看起来没问题，但设备就是会叫、会振、会抖、会发热"的问题，本质上并不是单纯的 PI 参数问题，而是以下几类现象在作怪：

1. 机械共振：例如联轴器、丝杆、皮带、悬臂负载带来的弹性振动；
2. 结构谐振：例如安装底座、支架、平台、外壳的柔性模态；
3. 编码器噪声：例如量化误差、插值误差、抖动、布线干扰、速度差分放大噪声；
4. 控制与机械耦合：例如滤波器延时、低刚度结构、外环过高带宽共同导致稳定裕度下降。

这些问题在时域波形中通常表现为：

1. 阶跃响应有振铃；
2. 定点时有细小抖动；
3. 特定速度段有啸叫；
4. 位置保持时电流抖动增大；
5. 某些频率附近噪声明显放大。

但如果只看时域，很容易看到现象，却看不清原因。因此工程上更有效的方法是：

1. 用 Bode 图识别幅值峰值、相位突降和噪声抬升；
2. 用 Nyquist 图判断系统距离稳定边界还有多远；
3. 再根据峰值频率和裕度结果决定是否加入陷波器、调整增益、降低带宽或优化机械结构。

本文的目标，就是把"共振怎么测、噪声怎么看、陷波器怎么加、优化后怎么验"讲清楚。

1.1 本章小结

本章说明了本文的应用背景：很多振动和噪声问题仅靠时域难以定位，需要用 Bode 图和 Nyquist 图从频域与复平面角度做联合分析。后文将围绕测试对象、特征识别、陷波器设计和实际案例展开。

2. 测试对象与问题定义

在伺服系统中，机械共振、结构谐振和编码器噪声通常主要体现在速度环与位置环，但根源不一定都在控制器本身。

2.1 典型问题链路

这个链路中，问题可能出现在：

1. 机械侧：柔性联轴器、长丝杆、皮带、悬臂、安装底座；
2. 传感侧：编码器噪声、速度微分噪声、接地干扰；
3. 控制侧：带宽过高、相位裕度不足、滤波延时过大。

2.2 本文测试关注点

本文重点关注三类频域特征：

1. 机械共振峰：Bode 图上表现为某频率附近幅值显著抬升；
2. 编码器噪声抬升区：高频段幅值曲线不再正常下降，甚至出现"噪声地板抬高"；
3. 结构谐振引起的相位突降：相位在某段频率快速下降，Nyquist 轨迹逼近临界点。

2.3 测试输入与输出定义

在实际测试时，常见定义如下：

1. 速度环共振测试

   • 输入：速度给定 ω∗\omega^*ω∗ 上叠加扫频扰动；
   • 输出：反馈速度 ω\omegaω；
   • 目标：识别机械共振峰、速度估计噪声和相位裕度变化。

2. 位置环结构谐振测试

   • 输入：位置给定 θ∗\theta^*θ∗ 上叠加小幅扫频扰动；
   • 输出：反馈位置 θ\thetaθ 或位置误差 eθe_\thetaeθ；
   • 目标：识别柔性结构峰值与稳定边界。

3. 编码器噪声测试

   • 输入：可在位置或速度指令上叠加极小幅扫频，也可在静止状态下采集反馈噪声频谱；
   • 输出：位置反馈、速度反馈、速度估计值；
   • 目标：判断噪声主要出现在编码器本体、速度微分还是控制闭环放大环节。

2.4 本章小结

本章把问题定义清楚了：我们不是单纯测"带宽"，而是借助频域响应去定位机械共振、结构谐振和编码器噪声，并明确了测试时常用的输入输出定义。

3. 机械共振、编码器噪声和结构谐振在 Bode 图上的表现

3.1 机械共振的典型表现

机械共振在 Bode 图上最典型的表现是：

1. 在某个频率附近出现幅值峰值；
2. 峰值前后相位快速下滑；
3. 峰值越高，通常表示阻尼越差。

例如，当速度闭环在 140 Hz 附近存在柔性联轴器模态时，可能出现：

1. 幅值从 -2 dB 突然抬升到 +4 dB；
2. 相位在 100 Hz ~ 180 Hz 内快速多下降 40° ~ 80°；
3. 阶跃响应中伴随 120 Hz ~ 160 Hz 的振铃。

3.2 结构谐振的典型表现

结构谐振更常见于位置环，表现为：

1. 闭环带宽附近或更高一点的频段出现窄峰；
2. 峰值频率与平台、支架、丝杆、负载模态有关；
3. 在位置保持或微小往返运动中会放大为尖锐啸叫或细碎振动。

3.3 编码器噪声的典型表现

编码器噪声通常不是"窄峰"，而更像：

1. 高频段幅值不正常抬高；
2. 低速时速度反馈抖动加重；
3. 使用差分求速度时，噪声频谱被高频放大。

在 Bode 图上常见现象是：

1. 高频段曲线不再平滑下降；
2. 幅值曲线出现起伏毛刺；
3. 相位数据在高频段明显发散；
4. 重复测试时高频结果不一致。

3.4 三类问题的对比识别

问题类型	幅值曲线特征	相位曲线特征	时域表现
机械共振	局部明显峰值	峰值附近快速下滑	振铃、啸叫、某频段抖动
结构谐振	窄峰或多峰	多模态相位快速下降	平台抖动、壳体共鸣
编码器噪声	高频段抬高、曲线毛刺	高频相位发散	静止抖动、速度估计毛刺

3.5 本章小结

本章给出了三类问题在 Bode 图上的辨识方法：共振更像"峰值"，噪声更像"高频抬高与毛刺"，结构谐振则常表现为多模态窄峰和相位急剧下降。

4. Nyquist 图如何判断风险与稳定边界

4.1 为什么看 Nyquist 图

Bode 图善于读峰值、带宽和相位，但 Nyquist 图更适合回答一个更本质的问题：

这个共振峰会不会把系统推到失稳边界附近？

4.2 Nyquist 判据的基本形式

闭环特征方程为：

1+L(s)=0(4-1) 1+L(s)=0 \tag{4-1} 1+L(s)=0(4-1)

Nyquist 判据指出：

Z=N+P(4-2) Z=N+P \tag{4-2} Z=N+P(4-2)

其中：

1. ZZZ 是闭环右半平面零点数；
2. PPP 是开环右半平面极点数；
3. NNN 是 Nyquist 曲线对 (−1,0)(-1,0)(−1,0) 的包围数。

在大多数伺服系统设计中，目标是 P=0P=0P=0，因此只要 Nyquist 轨迹危险地逼近或错误包围 (−1,0)(-1,0)(−1,0)，系统就会变得脆弱甚至失稳。

4.3 共振与 Nyquist 轨迹的关系

当共振峰存在时，通常会带来：

1. 幅值上升；
2. 相位额外下降；
3. Nyquist 轨迹在某个频率附近向 (−1,0)(-1,0)(−1,0) 方向凸出。

这时即便系统还没有失稳，也可能已经进入：

1. 轻微碰撞就振；
2. 负载一换就振；
3. 温度变化后就变差；
4. 增益略升就失稳。

4.4 Nyquist 图示意流程

4.5 本章小结

本章说明了 Nyquist 图的价值：它不是替代 Bode 图，而是帮助判断"峰值是否已经危险地接近稳定边界"。对共振问题，Bode 用来定位，Nyquist 用来判风险。

5. 测试方法与流程

5.1 速度环机械共振测试流程

5.2 位置环结构谐振测试流程

5.3 编码器噪声测试流程

5.4 扫频参数建议

测试对象	工作点示例	扰动幅值	频率范围	采样建议
速度环机械共振	300 rpm	10~20 rpm	1~300 Hz	每点 10~12 周期
位置环结构谐振	0° 附近	0.1°~0.5°	0.5~100 Hz	每点 8~10 周期
编码器噪声	静止/10 rpm	极小或不注入	频谱观察到 500 Hz 以上	长时间采样 FFT

5.5 本章小结

本章给出的是落地测试流程。工程上最常见做法是：速度环找机械峰，位置环找结构峰，编码器噪声则通过静止/低速采样和高频响应异常联合判断。

6. 怎样解决与优化共振问题

共振问题的优化思路通常不是单一手段，而是多种方法组合。

6.1 调低带宽或回退增益

这是最直接但有性能代价的方法。

适用于：

1. 峰值就在交越频率附近；
2. PM 已明显不足；
3. 设备以稳定优先而不是动态优先。

优点：

1. 实施简单；
2. 风险小；
3. 易快速验证。

缺点：

1. 响应变慢；
2. 无法根治结构问题；
3. 可能影响跟随性能。

6.2 加入陷波器 Notch Filter

陷波器是工程上最常用的共振抑制方法之一，用于在某个共振频率附近做局部衰减。

常见连续域二阶陷波器形式：

Gnotch(s)=s2+2ζzωns+ωn2s2+2ζpωns+ωn2(6-1) G_{notch}(s)=\frac{s^2+2\zeta_z\omega_n s+\omega_n^2}{s^2+2\zeta_p\omega_n s+\omega_n^2} \tag{6-1} Gnotch(s)=s2+2ζpωns+ωn2s2+2ζzωns+ωn2(6-1)

其中：

1. ωn=2πfn\omega_n=2\pi f_nωn=2πfn 为目标陷波中心频率；
2. ζz\zeta_zζz 决定零点阻尼；
3. ζp\zeta_pζp 决定极点阻尼；
4. 通常要求 ζz<ζp\zeta_z < \zeta_pζz<ζp，这样在 fnf_nfn 附近形成衰减槽。

6.3 低通滤波与观测滤波优化

除了陷波器，低通滤波器也是非常常见的处理方法，特别适合以下情况：

1. 高频噪声整体抬高，而不是单一窄峰；
2. 编码器速度估计存在明显高频毛刺；
3. 共振频率远高于控制带宽，只需要削弱高频激发。

常见做法包括：

1. 对速度反馈增加一阶或二阶低通；
2. 对位置微分得到的速度估计增加平滑窗口；
3. 对电流前馈或转矩指令增加适度滚降；
4. 降低噪声耦合通道的高频增益。

但低通的缺点也很明显：

1. 会引入相位滞后；
2. 可能降低相位裕度；
3. 若截止频率设得过低，会直接拖慢环路动态。

因此，低通滤波更适合处理"高频噪声宽带抬升"，而不适合替代对明确窄峰的陷波处理。

6.4 Lead/Lag 校正与相位补偿

当问题不只是峰值过高，而是交越频率附近相位余量不足时，可以考虑加入超前校正（Lead）或适度的滞后-超前组合补偿。

典型用途：

1. 在不明显降低带宽的前提下补一点相位；
2. 在峰值附近改善开环形状；
3. 配合陷波器一起使用，抵消部分额外相位损失。

优点：

1. 可以直接改善 PM；
2. 对提升 Nyquist 安全距离很有帮助；
3. 适合"系统不一定有很强共振，但裕度偏紧"的场景。

缺点：

1. 参数整定比单纯降增益更复杂；
2. 如果共振峰很高，仅靠 lead 往往不够；
3. 需要反复看 Bode 和 Nyquist 才能确认效果。

6.5 输入整形与指令预滤波

有些振动问题，并不是闭环本身一直不稳，而是指令激发了机械模态。这时除了改闭环，还可以从命令端做优化。

常见方法包括：

1. 指令预滤波；
2. S 曲线加减速；
3. 输入整形（Input Shaping）；
4. 对特定频率做前馈抑制。

这类方法尤其适合：

1. 机械模态频率明确；
2. 问题主要发生在启停、换向、快速定位；
3. 不希望大改闭环控制器本体。

优点：

1. 不一定增加闭环相位损失；
2. 对"激励型振动"常非常有效；
3. 可以与现有控制器并行叠加。

缺点：

1. 对外部扰动抑制有限；
2. 更适合轨迹激发问题，不适合纯反馈噪声问题；
3. 参数依赖模态识别准确度。

6.6 双惯量建模、观测器与主动阻尼

对于电机---联轴器---负载这类明显双惯量或多惯量系统，单纯加陷波器只是表层处理。更深入的方法包括：

1. 建立双惯量模型；
2. 引入扭振观测器；
3. 加主动阻尼（Active Damping）；
4. 使用负载侧速度或扭矩估计补偿。

这类方法适合：

1. 共振峰随工况变化明显；
2. 单一陷波器效果有限；
3. 设备是高性能伺服或高端运动平台。

优点：

1. 更接近系统真实物理结构；
2. 对抑制扭振更有针对性；
3. 在复杂机械系统中往往比堆叠滤波器更稳健。

缺点：

1. 建模和整定复杂；
2. 对参数准确性要求高；
3. 工程实现门槛高于普通 PI + notch。

6.7 提高结构刚度或机械阻尼

如果问题根源在机械结构，则应考虑：

1. 提高安装底座刚度；
2. 更换更高刚度联轴器；
3. 缩短悬臂长度；
4. 改善丝杆支撑；
5. 增加阻尼材料或结构支撑。

6.8 更换传动方案或传感方案

如果系统长期受限于结构模态或测量品质，工程上还可以从硬件方案层面优化：

1. 联轴器改为更高刚度型号；
2. 丝杆改短或更换支撑方式；
3. 皮带传动改为更高刚度传动结构；
4. 编码器从低分辨率增量式升级为高分辨率绝对式；
5. 必要时增加负载侧传感器。

这种方法虽然成本较高，但优点也最直接：

1. 从源头降低共振和噪声；
2. 降低控制补偿复杂度；
3. 往往能带来最稳健的长期效果。

6.9 优化编码器与速度观测

若问题根源是编码器噪声，则可考虑：

1. 优化编码器布线和接地；
2. 提升编码器分辨率或插值质量；
3. 调整速度估计算法；
4. 使用合适的低通滤波，但避免过大延时；
5. 降低差分噪声放大。

6.10 多种优化方法对比表

方法	更适合解决的问题	优点	缺点	是否需重点复查 PM/GM
陷波器	明确窄带共振峰	定点压峰效果好	会引入局部相位影响	是
低通滤波	高频宽带噪声、编码器毛刺	实现简单	延时增加明显	是
Lead/Lag 补偿	裕度偏紧、交越区相位不足	可提升相位裕度	参数整定更复杂	是
输入整形/预滤波	启停换向激发模态	不一定损伤闭环裕度	对外扰抑制有限	需要
主动阻尼/观测器	双惯量扭振、多模态复杂系统	针对性强	建模门槛高	是
机械加固	结构柔性、安装刚度不足	根治能力强	成本高、改动大	需要复测
传感器升级	编码器噪声、测量品质差	从源头改善噪声	成本较高	需要复测

6.11 优化顺序建议

推荐顺序：

1. 先确认是不是测试假象；
2. 再区分机械峰还是编码器噪声；
3. 若是机械峰，优先确认是否可接受回退带宽；
4. 若需要保留动态性能，再设计陷波器；
5. 若峰值太多或漂移太大，优先改机械结构。

进一步细化时，可按下面顺序执行：

1. 先区分是窄峰还是宽带噪声；
2. 窄峰优先考虑陷波、主动阻尼或机械加固；
3. 宽带噪声优先考虑观测优化和低通滤波；
4. 若主要在启停时被激发，优先考虑输入整形；
5. 若 Nyquist 轨迹整体过近，需考虑 lead 补偿或整体降带宽；
6. 若问题随工况变化太大，优先改机械方案，而不是堆补偿器。

6.12 优化方法选型流程图

下面给出一个适合现场使用的"按现象选方法"流程图。实际使用时，建议先用 Bode 图判断问题形态，再结合 Nyquist、PM/GM 和阶跃波形做二次确认。

6.13 现场选型简化口诀

为了方便现场快速判断，也可以直接按下面口诀理解：

1. 看到窄峰先想 notch；
2. 看到高频噪声地板抬高先想低通和观测；
3. 看到交越区相位掉得快先想 lead 或降带宽；
4. 只在启停换向时振先想输入整形；
5. 多峰、漂移大、换负载就变，优先怀疑机械结构。

6.14 本章小结

本章补充说明了：处理共振和噪声并不只有陷波器一种办法。除了 notch，还可以用低通滤波、lead 补偿、输入整形、主动阻尼、机械加固和传感器升级等多种手段。工程上应根据"窄峰 / 宽带噪声 / 裕度不足 / 结构柔性"不同问题类型选择组合方案。

7. 陷波器设计方法

7.1 什么时候该加陷波器

通常满足以下条件之一时，可以考虑加陷波器：

1. Bode 图存在明显窄峰；
2. 峰值频率比较稳定；
3. 峰值正好卡在速度环或位置环的有效工作频段；
4. 回退带宽会明显损害性能；
5. Nyquist 轨迹因该峰值明显逼近 (−1,0)(-1,0)(−1,0)。

7.2 陷波中心频率如何选

中心频率一般选在峰值频率附近：

fn≈fpeak(7-1) f_n \approx f_{peak} \tag{7-1} fn≈fpeak(7-1)

例如 Bode 图在 148 Hz 出现峰值，则可先取：

fn=148 Hz(7-2) f_n=148\text{ Hz} \tag{7-2} fn=148 Hz(7-2)

工程上也可取 145 Hz、150 Hz 附近做对比试验。

7.3 陷波深度与带宽如何选

陷波太浅压不住峰值，太深又会引入不必要相位影响。一般可按以下思路：

1. 峰值高出周围 3 dB ~ 5 dB：先试中等陷波；
2. 峰值高出周围 6 dB 以上：考虑更深陷波；
3. 峰值很窄：陷波带宽可窄一些；
4. 峰值随工况漂移：陷波带宽要略宽，但要重新检查相位裕度。

7.4 陷波器设计流程

7.5 陷波器设计经验表

项目	工程建议
中心频率	取峰值频率或略低/略高做微调
陷波宽度	峰值窄则窄，峰值漂移大则略加宽
陷波深度	以"压低峰值且不过度损失相位"为原则
验证方式	必须复测 Bode、PM/GM、Nyquist 和阶跃
风险点	陷波器过多会叠加相位延时

7.6 数字陷波器、Lead 和低通的离散化实现

在驱动器、DSP、MCU 或 FPGA 中，补偿器最终都要落到离散时间差分方程上。工程上最常见做法是：

1. 先在连续域设计目标传递函数；
2. 再用 Tustin（双线性变换）或匹配极零法离散化；
3. 最后整理成二阶或一阶 IIR 差分方程写入代码。

常用双线性变换为：

s≈2Ts⋅1−z−11+z−1(7-3) s \approx \frac{2}{T_s}\cdot\frac{1-z^{-1}}{1+z^{-1}} \tag{7-3} s≈Ts2⋅1+z−11−z−1(7-3)

其中 TsT_sTs 为控制周期。

---

7.6.1 数字二阶陷波器的一般形式

连续域二阶陷波器：

Gnotch(s)=s2+2ζzωns+ωn2s2+2ζpωns+ωn2(7-4) G_{notch}(s)=\frac{s^2+2\zeta_z\omega_n s+\omega_n^2}{s^2+2\zeta_p\omega_n s+\omega_n^2} \tag{7-4} Gnotch(s)=s2+2ζpωns+ωn2s2+2ζzωns+ωn2(7-4)

离散化后通常可写成：

Gnotch(z)=b0+b1z−1+b2z−21+a1z−1+a2z−2(7-5) G_{notch}(z)=\frac{b_0+b_1 z^{-1}+b_2 z^{-2}}{1+a_1 z^{-1}+a_2 z^{-2}} \tag{7-5} Gnotch(z)=1+a1z−1+a2z−2b0+b1z−1+b2z−2(7-5)

对应差分方程：

yk=−a1yk−1−a2yk−2+b0xk+b1xk−1+b2xk−2(7-6) yk=-a_1 yk-1-a_2 yk-2+b_0 xk+b_1 xk-1+b_2 xk-2 \tag{7-6} yk=−a1yk−1−a2yk−2+b0xk+b1xk−1+b2xk−2(7-6)

其中：

1. xkxkxk 是输入，例如速度误差、速度反馈或位置指令；
2. ykykyk 是滤波输出；
3. a1,a2,b0,b1,b2a_1,a_2,b_0,b_1,b_2a1,a2,b0,b1,b2 是离散化后的系数。

如果用"数字谐振频率 + 半径"的常见工程写法，也可直接写成：

Gnotch(z)=1−2cos⁡(ω0)z−1+z−21−2rcos⁡(ω0)z−1+r2z−2(7-7) G_{notch}(z)=\frac{1-2\cos(\omega_0)z^{-1}+z^{-2}}{1-2r\cos(\omega_0)z^{-1}+r^2 z^{-2}} \tag{7-7} Gnotch(z)=1−2rcos(ω0)z−1+r2z−21−2cos(ω0)z−1+z−2(7-7)

其中：

1. ω0=2πfnTs\omega_0=2\pi f_n T_sω0=2πfnTs；
2. fnf_nfn 为目标陷波频率；
3. rrr 越接近 1，陷波越窄；
4. rrr 越小，陷波越宽。

对应差分方程可写成：

yk=2rcos⁡(ω0)yk−1−r2yk−2+xk−2cos⁡(ω0)xk−1+xk−2(7-8) yk=2r\cos(\omega_0)yk-1-r^2 yk-2+xk-2\cos(\omega_0)xk-1+xk-2 \tag{7-8} yk=2rcos(ω0)yk−1−r2yk−2+xk−2cos(ω0)xk−1+xk−2(7-8)

这类写法特别适合现场快速实现窄带 notch。

7.6.2 数字 Lead 补偿器的离散化

连续域 Lead 补偿器常写为：

Glead(s)=K⋅1+Tzs1+Tps,Tz>Tp(7-9) G_{lead}(s)=K\cdot\frac{1+T_z s}{1+T_p s},\quad T_z>T_p \tag{7-9} Glead(s)=K⋅1+Tps1+Tzs,Tz>Tp(7-9)

它的作用是在某个频段提供正相位补偿，从而提升 PM。

用 Tustin 离散化后：

Glead(z)=b0+b1z−11+a1z−1(7-10) G_{lead}(z)=\frac{b_0+b_1 z^{-1}}{1+a_1 z^{-1}} \tag{7-10} Glead(z)=1+a1z−1b0+b1z−1(7-10)

其一阶差分方程为：

yk=−a1yk−1+b0xk+b1xk−1(7-11) yk=-a_1 yk-1+b_0 xk+b_1 xk-1 \tag{7-11} yk=−a1yk−1+b0xk+b1xk−1(7-11)

如果把双线性变换直接代入，可得到：

b0=K⋅1+2TzTs1+2TpTs,b1=K⋅1−2TzTs1+2TpTs,a1=1−2TpTs1+2TpTs(7-12) b_0=K\cdot\frac{1+\frac{2T_z}{T_s}}{1+\frac{2T_p}{T_s}},\qquad b_1=K\cdot\frac{1-\frac{2T_z}{T_s}}{1+\frac{2T_p}{T_s}},\qquad a_1=\frac{1-\frac{2T_p}{T_s}}{1+\frac{2T_p}{T_s}} \tag{7-12} b0=K⋅1+Ts2Tp1+Ts2Tz,b1=K⋅1+Ts2Tp1−Ts2Tz,a1=1+Ts2Tp1−Ts2Tp(7-12)

工程上使用 Lead 时要特别注意：

1. Lead 虽然能补相位，但也可能提高高频增益；
2. 若机械峰就在补偿频段附近，必须结合 notch 一起验证；
3. 加完后一定复查 Nyquist 轨迹是否更安全，而不是只看 PM 数值。

7.6.3 数字一阶低通滤波器的离散化

连续域一阶低通：

Glp(s)=ωcs+ωc(7-13) G_{lp}(s)=\frac{\omega_c}{s+\omega_c} \tag{7-13} Glp(s)=s+ωcωc(7-13)

其中截止频率 fc=ωc/(2π)f_c=\omega_c/(2\pi)fc=ωc/(2π)。

离散后可写成：

Glp(z)=b0+b1z−11+a1z−1(7-14) G_{lp}(z)=\frac{b_0+b_1 z^{-1}}{1+a_1 z^{-1}} \tag{7-14} Glp(z)=1+a1z−1b0+b1z−1(7-14)

对应差分方程：

yk=−a1yk−1+b0xk+b1xk−1(7-15) yk=-a_1 yk-1+b_0 xk+b_1 xk-1 \tag{7-15} yk=−a1yk−1+b0xk+b1xk−1(7-15)

若采用 Tustin 形式，则：

b0=b1=ωcTs2+ωcTs,a1=ωcTs−22+ωcTs(7-16) b_0=b_1=\frac{\omega_c T_s}{2+\omega_c T_s},\qquad a_1=\frac{\omega_c T_s-2}{2+\omega_c T_s} \tag{7-16} b0=b1=2+ωcTsωcTs,a1=2+ωcTsωcTs−2(7-16)

如果采用更常见的指数平滑写法，也可直接写为：

yk=αyk−1+(1−α)xk(7-17) yk=\alpha yk-1+(1-\alpha)xk \tag{7-17} yk=αyk−1+(1−α)xk(7-17)

其中：

α=e−2πfcTs(7-18) \alpha=e^{-2\pi f_c T_s} \tag{7-18} α=e−2πfcTs(7-18)

这在 MCU 中实现最简单，但频率特性与严格 Tustin 形式略有差异。

7.6.4 三种离散补偿器的代码实现模板

下面给出现场最常用的 C 代码：

typedef struct 
{
    float a1, a2;
    float b0, b1, b2;
    float x1, x2;
    float y1, y2;
} IIR2_Filter;

float IIR2_Run(IIR2_Filter *f, float x)
{
    float y = -f->a1 * f->y1 - f->a2 * f->y2
              + f->b0 * x + f->b1 * f->x1 + f->b2 * f->x2;

    f->x2 = f->x1;
    f->x1 = x;
    f->y2 = f->y1;
    f->y1 = y;
    return y;
}

若是一阶 Lead 或低通，则可用更简单的模板：

typedef struct 
{
    float a1;
    float b0, b1;
    float x1;
    float y1;
} IIR1_Filter;

float IIR1_Run(IIR1_Filter *f, float x)
{
    float y = -f->a1 * f->y1 + f->b0 * x + f->b1 * f->x1;
    f->x1 = x;
    f->y1 = y;
    return y;
}

7.6.5 离散实现时的工程注意事项

1. 陷波器中心频率不能太接近 Nyquist 频率，否则离散误差明显；
2. 采样频率至少应为目标补偿频率的 15~20 倍，这样频率特性更可信；
3. 离散化后必须重新画离散系统 Bode 图，不能只看连续域设计值；
4. 多级滤波器串联时，量化误差和数值溢出要单独检查；
5. 固定点实现时，要验证系数量化对 PM/GM 的影响；
6. 所有补偿器上线前都应做阶跃、扫频和 Nyquist 三重验证。

7.6.6 数值示例

设控制周期为：

Ts=0.0005 s(2 kHz 采样)(7-19) T_s=0.0005\text{ s} \quad (2\text{ kHz 采样}) \tag{7-19} Ts=0.0005 s(2 kHz 采样)(7-19)

若要设计 150 Hz 的数字陷波器，则数字角频率为：

ω0=2π×150×0.0005≈0.4712 rad(7-20) \omega_0=2\pi \times 150 \times 0.0005 \approx 0.4712\text{ rad} \tag{7-20} ω0=2π×150×0.0005≈0.4712 rad(7-20)

若先取 r=0.95r=0.95r=0.95，则：

cos⁡(ω0)≈0.8910(7-21) \cos(\omega_0)\approx 0.8910 \tag{7-21} cos(ω0)≈0.8910(7-21)

可得分母项近似为：

1−2×0.95×0.8910 z−1+0.952z−2(7-22) 1-2\times0.95\times0.8910\,z^{-1}+0.95^2 z^{-2} \tag{7-22} 1−2×0.95×0.8910z−1+0.952z−2(7-22)

即：

1−1.6929z−1+0.9025z−2(7-23) 1-1.6929z^{-1}+0.9025z^{-2} \tag{7-23} 1−1.6929z−1+0.9025z−2(7-23)

分子项近似为：

1−1.7820z−1+z−2(7-24) 1-1.7820z^{-1}+z^{-2} \tag{7-24} 1−1.7820z−1+z−2(7-24)

因此差分方程近似可写为：

yk=1.6929yk−1−0.9025yk−2+xk−1.7820xk−1+xk−2(7-25) yk=1.6929yk-1-0.9025yk-2+xk-1.7820xk-1+xk-2 \tag{7-25} yk=1.6929yk−1−0.9025yk−2+xk−1.7820xk−1+xk−2(7-25)

这个例子说明：只要确定了目标频率、采样周期和带宽参数，就能直接写出数字滤波器差分方程，方便嵌入控制代码。

7.7 本章小结

本章除了给出陷波器的工程设计思路，还补充了数字陷波器、Lead 和低通滤波器的离散化公式与差分方程。对驱动器实际开发而言，连续域设计只是第一步，离散实现与系数量化验证同样关键。

8. MATLAB 画图与陷波器设计代码

8.1 机械共振模型与 Bode 图示例

clear; clc; close all;
s = tf('s');

fn = 150;                % 共振频率 Hz
wn = 2*pi*fn;
zeta1 = 0.03;            % 低阻尼共振

G_res = wn^2 / (s^2 + 2*zeta1*wn*s + wn^2);
G_lp  = 2*pi*120 / (s + 2*pi*120);
G = G_lp * G_res;

figure;
bode(G);
grid on;
title('机械共振示例 Bode 图');

8.2 Nyquist 图示例

clear; clc; close all;
s = tf('s');
L = 30 / ((0.002*s+1)*(0.01*s+1)) * (2*pi*150)^2 / (s^2 + 2*0.03*2*pi*150*s + (2*pi*150)^2);

figure;
nyquist(L);
grid on;
title('存在共振峰时的 Nyquist 图');

8.3 陷波器设计示例

clear; clc; close all;
s = tf('s');

fn = 150;
wn = 2*pi*fn;
zeta_z = 0.02;
zeta_p = 0.15;

Notch = (s^2 + 2*zeta_z*wn*s + wn^2) / (s^2 + 2*zeta_p*wn*s + wn^2);

G0 = 30 / ((0.002*s+1)*(0.01*s+1)) * (2*pi*150)^2 / (s^2 + 2*0.03*2*pi*150*s + (2*pi*150)^2);
G1 = G0 * Notch;

figure;
margin(G0);
grid on;
title('补偿前裕度图');

figure;
margin(G1);
grid on;
title('加入陷波器后的裕度图');

8.4 连续域到离散域的 MATLAB 仿真对比代码

下面这段代码用于把前文的连续域机械共振模型、陷波器和低通滤波器离散化，并对比：

1. 连续域与离散域的 Bode 曲线差异；
2. 连续域与离散域的 Nyquist 轨迹差异；
3. 加入数字陷波器前后的时域响应变化；
4. c2d 离散化结果与手工差分方程实现是否一致。

适用场景：

1. 验证 7.6 节离散化后的补偿器是否仍满足设计预期；
2. 检查采样周期是否足够支撑目标补偿频率；
3. 在驱动器代码落地前，先离线验证数字滤波器效果。

clear; clc; close all;

%% 1) 连续域模型
s = tf('s');
Ts = 0.0005;                    % 2 kHz 采样

fn_res = 150;                   % 机械共振频率
wn_res = 2*pi*fn_res;
zeta_res = 0.03;

fc_lp = 120;                    % 速度观测低通
wc_lp = 2*pi*fc_lp;

% 连续域机械共振 + 低通对象
G_res_c = wn_res^2 / (s^2 + 2*zeta_res*wn_res*s + wn_res^2);
G_lp_c  = wc_lp / (s + wc_lp);
G_c = G_lp_c * G_res_c;

%% 2) 连续域陷波器设计
fn_notch = 150;
wn_notch = 2*pi*fn_notch;
zeta_z = 0.02;
zeta_p = 0.15;

Notch_c = (s^2 + 2*zeta_z*wn_notch*s + wn_notch^2) / ...
          (s^2 + 2*zeta_p*wn_notch*s + wn_notch^2);

G_comp_c = minreal(G_c * Notch_c);

%% 3) 离散化
G_d        = c2d(G_c, Ts, 'tustin');
Notch_d    = c2d(Notch_c, Ts, 'tustin');
G_comp_d   = minreal(c2d(G_comp_c, Ts, 'tustin'));

disp('连续域陷波器 Notch_c = ');
Notch_c
disp('离散域陷波器 Notch_d = ');
Notch_d

%% 4) Bode 对比：连续域 vs 离散域
figure('Name','Bode对比：连续域/离散域');
bode(G_c, 'b', G_d, 'r--', {2*pi*5, 2*pi*800});
grid on;
legend('连续域 G_c', '离散域 G_d');
title('机械共振对象：连续域与离散域 Bode 对比');

figure('Name','Bode对比：加陷波后');
bode(G_comp_c, 'b', G_comp_d, 'r--', {2*pi*5, 2*pi*800});
grid on;
legend('连续域 G_{comp,c}', '离散域 G_{comp,d}');
title('加陷波后：连续域与离散域 Bode 对比');

%% 5) Nyquist 对比
figure('Name','Nyquist对比：未补偿');
nyquist(G_c, 'b', G_d, 'r--');
grid on;
legend('连续域 G_c', '离散域 G_d');
title('未补偿系统 Nyquist 对比');

figure('Name','Nyquist对比：加陷波后');
nyquist(G_comp_c, 'b', G_comp_d, 'r--');
grid on;
legend('连续域 G_{comp,c}', '离散域 G_{comp,d}');
title('加陷波后 Nyquist 对比');

%% 6) 裕度对比
[Gm_c, Pm_c, Wcg_c, Wcp_c] = margin(G_comp_c);
[Gm_d, Pm_d, Wcg_d, Wcp_d] = margin(G_comp_d);

fprintf('\n===== 加陷波后裕度对比 =====\n');
fprintf('连续域: PM = %.2f deg, GM = %.2f dB, Wcp = %.2f Hz\n', ...
    Pm_c, 20*log10(Gm_c), Wcp_c/(2*pi));
fprintf('离散域: PM = %.2f deg, GM = %.2f dB, Wcp = %.2f Hz\n', ...
    Pm_d, 20*log10(Gm_d), Wcp_d/(2*pi));

%% 7) 时域对比：扫频附近的正弦输入
t = 0:Ts:0.25;
f_test = 150;                   % 在共振附近测试
u = sin(2*pi*f_test*t);

y_c0 = lsim(G_c, u, t);
y_c1 = lsim(G_comp_c, u, t);
y_d0 = lsim(G_d, u, t);
y_d1 = lsim(G_comp_d, u, t);

figure('Name','时域正弦响应对比');
subplot(2,1,1);
plot(t, u, 'k:', t, y_c0, 'b', t, y_d0, 'r--', 'LineWidth', 1.1);
grid on; xlabel('Time (s)'); ylabel('Amplitude');
legend('输入','连续域未补偿','离散域未补偿');
title('150 Hz 附近：未补偿时域响应');

subplot(2,1,2);
plot(t, u, 'k:', t, y_c1, 'b', t, y_d1, 'r--', 'LineWidth', 1.1);
grid on; xlabel('Time (s)'); ylabel('Amplitude');
legend('输入','连续域加陷波','离散域加陷波');
title('150 Hz 附近：加陷波后时域响应');

%% 8) 手工差分方程验证数字陷波器
[b_notch, a_notch] = tfdata(Notch_d, 'v');

x = u(:);
y_manual = zeros(size(x));
for k = 3:length(x)
    y_manual(k) = -a_notch(2)*y_manual(k-1) - a_notch(3)*y_manual(k-2) ...
                  + b_notch(1)*x(k) + b_notch(2)*x(k-1) + b_notch(3)*x(k-2);
end

y_builtin = lsim(Notch_d, u, t);

figure('Name','手工差分方程验证');
plot(t, y_builtin, 'b', t, y_manual, 'r--', 'LineWidth', 1.1);
grid on; xlabel('Time (s)'); ylabel('Amplitude');
legend('lsim(Notch_d)','手工差分方程');
title('数字陷波器：系统函数与差分方程结果对比');

%% 9) 离散极点零点图
figure('Name','离散陷波器极零图');
pzmap(Notch_d); grid on;
title('数字陷波器零极点图');

8.5 根据实验数据识别峰值频率

clear; clc; close all;

f = [10 20 30 50 80 100 120 140 150 160 180 200 250];
mag_db = [-0.1 -0.2 -0.4 -0.8 -1.4 -2.0 -1.5 1.2 3.6 2.0 -1.0 -3.2 -6.0];
phase_deg = [-5 -9 -12 -20 -30 -40 -55 -78 -96 -118 -135 -148 -160];

[peak_val, idx] = max(mag_db);
fprintf('Peak frequency = %.1f Hz\n', f(idx));
fprintf('Peak magnitude = %.2f dB\n', peak_val);

figure;
subplot(2,1,1); semilogx(f, mag_db, 'o-'); grid on; ylabel('Mag(dB)');
subplot(2,1,2); semilogx(f, phase_deg, 's-'); grid on; ylabel('Phase(deg)'); xlabel('Hz');

8.6 本章小结

本章提供了可直接扩展的 MATLAB 代码，覆盖共振模型、Nyquist 图、陷波器设计、连续域/离散域对比以及峰值识别。工程中可以把这些脚本作为离线验证工具，与实测数据表和驱动器代码实现相互校验。

9. 详细案例 1：速度环机械共振识别与陷波优化

9.1 已知条件

某伺服平台速度环在 300 rpm 附近工作时存在明显啸叫，参数如下：

参数	数值
电流环带宽	1.2 kHz
速度 PI 参数	Kp=0.42K_p=0.42Kp=0.42，Ki=10K_i=10Ki=10
速度工作点	300 rpm
扰动函数	ω∗(t)=300+12sin⁡(2πft)\omega^*(t)=300+12\sin(2\pi f t)ω∗(t)=300+12sin(2πft) rpm
扫频范围	10 ~ 250 Hz
采样周期	1 ms
每点采样周期数	12

测试波形示意图

9.2 Bode 图测试结果

频率 Hz	输入幅值 rpm	输出幅值 rpm	幅值 dB	相位 deg
10	12	11.9	-0.07	-6
20	12	11.8	-0.15	-10
30	12	11.5	-0.37	-14
50	12	10.9	-0.84	-22
80	12	10.0	-1.58	-33
100	12	9.2	-2.31	-44
120	12	10.1	-1.49	-58
140	12	13.8	1.22	-79
150	12	18.2	3.62	-96
160	12	15.1	2.00	-118
180	12	10.7	-1.00	-135
200	12	8.3	-3.20	-148

可见 150 Hz 附近存在明显共振峰，峰值约 +3.6 dB。

9.3 Nyquist 风险判断

根据开环频响换算可知，150 Hz 附近的额外峰值和相位突降导致 Nyquist 轨迹明显向 (−1,0)(-1,0)(−1,0) 点凸出。系统尚未失稳，但负载稍变化就可能进入高风险区。

9.4 陷波器设计与优化

根据峰值，初选：

1. 中心频率 fn=150f_n=150fn=150 Hz；
2. 中等陷波深度；
3. 窄带陷波，避免影响过多低频相位。

优化后复测结果示意：

指标	优化前	优化后
峰值频率	150 Hz	150 Hz
峰值高度	+3.6 dB	-1.2 dB
相位裕度 PM	38°	52°
听感啸叫	明显	基本消失
阶跃振铃	明显	可接受

参数整定建议表

观察现象	当前表现	可能原因	调整建议
150 Hz 峰值	+3.6 dB	机械柔性共振	优先在 150 Hz 附近设计陷波器
PM 偏低	38°	峰值叠加相位下降	加陷波后重新调整速度 PI
啸叫明显	有	共振被速度环激发	除陷波外检查联轴器和安装刚度
阶跃振铃	明显	峰值阻尼不足	陷波后再评估是否适当回退 KpK_pKp

9.5 结论

这是典型的"单一主峰机械共振"案例。对这类问题，陷波器最有效，但一定要在优化后重新检查 PM、GM 和 Nyquist 轨迹，而不是只看峰值是否被压低。

9.6 本章小结

本章给出了一个标准的机械共振优化案例：先用 Bode 图识别峰值，再用 Nyquist 判断风险，最后设计陷波器并复测验证。

10. 详细案例 2：编码器噪声识别与速度观测优化

10.1 已知条件

某平台低速爬行时速度反馈抖动明显，但机械部分未发现明显共振。测试条件如下：

参数	数值
编码器类型	增量式 2500 线
电子齿轮换算后分辨率	10000 counts/rev
低速工作点	10 rpm
速度采样周期	1 ms
速度算法	差分 + 一阶低通
位置环状态	关闭

测试波形示意图

10.2 测试结果

静止和低速时做频谱分析，得到：

频率 Hz	噪声幅值（归一化）
20	0.05
50	0.08
100	0.13
150	0.18
200	0.24
300	0.31
400	0.36
500	0.39

特点是：

1. 没有很尖锐的单峰；
2. 高频整体抬升；
3. 多次测试曲线有轻微波动。

这更符合编码器量化噪声与速度差分噪声放大的特征。

10.3 优化方法

尝试以下优化：

1. 调整速度估计算法窗口；
2. 适度降低速度低通截止频率；
3. 优化编码器接地和屏蔽；
4. 保持速度环带宽不过度逼近噪声主区。

优化后结果示意：

指标	优化前	优化后
300 Hz 噪声幅值	0.31	0.19
500 Hz 噪声幅值	0.39	0.22
低速速度抖动	明显	明显减轻
速度环 PM	48°	45°

说明：优化后噪声下降了，但也引入了一点额外相位滞后，因此不能无限加强滤波。

参数整定建议表

观察现象	当前表现	可能原因	调整建议
高频噪声抬升	明显	编码器量化 + 差分放大	优化速度估计算法和采样窗口
无明显窄峰	是	不是典型机械共振	不建议直接上陷波器
低速抖动	明显	测量噪声主导	优先调滤波和布线，不是优先调 PI
PM 略下降	优化后略降	低通滤波带来延时	滤波后必须重新检查 PM

10.4 结论

这个案例说明：不是所有"抖"都是共振。编码器噪声在频域上通常更像高频地板抬高，而不是窄峰。对此优先应做传感和观测优化，而不是直接加陷波器。

10.5 本章小结

本章强调了"先分型，再下药"的原则。编码器噪声问题和机械共振问题在 Bode 图上的形态不同，对应优化方法也不同。

11. 详细案例 3：位置环结构谐振与多手段联合优化

11.1 场景描述

某丝杆平台位置环在 0° 附近保持时有细碎振动，阶跃时出现长尾振铃。参数如下：

参数	数值
电流环带宽	1 kHz
速度环带宽	110 Hz
位置环比例增益	28
扰动函数	θ∗(t)=0+0.3∘sin⁡(2πft)\theta^*(t)=0+0.3^\circ\sin(2\pi f t)θ∗(t)=0+0.3∘sin(2πft)
扫频范围	0.5 ~ 80 Hz
采样周期	1 ms

测试波形示意图

11.2 Bode 结果

频率 Hz	输入幅值 deg	输出幅值 deg	幅值 dB	相位 deg
1	0.30	0.30	0.00	-2
2	0.30	0.30	-0.02	-4
5	0.30	0.29	-0.29	-10
10	0.30	0.28	-0.60	-21
20	0.30	0.25	-1.58	-45
30	0.30	0.23	-2.31	-67
40	0.30	0.24	-1.94	-92
45	0.30	0.29	-0.30	-110
50	0.30	0.34	1.09	-129
60	0.30	0.21	-3.10	-151

50 Hz 附近存在明显结构峰值。

对应裕度结果：

指标	数值
开环交越频率	34 Hz
相位裕度 PM	31°
增益裕度 GM	5 dB

11.3 优化方法

该案例没有只采用一种手段，而是联合处理：

1. 将位置环比例增益从 28 降到 24；
2. 在 50 Hz 附近增加窄带陷波；
3. 检查平台安装支撑，增加侧向刚度。

优化后结果：

指标	优化前	优化后
50 Hz 峰值	+1.09 dB	-2.20 dB
PM	31°	49°
GM	5 dB	8.5 dB
保持抖动	明显	基本消失
阶跃振铃	长尾	明显缩短

参数整定建议表

观察现象	当前表现	可能原因	调整建议
50 Hz 结构峰	明显	支撑刚度不足、外环过高	先回退位置增益，再加窄带陷波
PM 偏低	31°	峰值带来额外相位损失	目标提升到 45°以上
保持抖动	明显	峰值被外环持续激发	降外环增益优先级高于继续追带宽
振铃长尾	明显	阻尼不足	联合机械刚度优化更有效

11.4 结论

这个案例说明：结构谐振往往不是单靠陷波器就能完美解决。对于柔性平台，更常见的有效方案是"回退外环增益 + 陷波补偿 + 机械加固"的联合优化。

11.5 本章小结

本章展示了结构谐振类问题的典型处理方式：不能只盯控制参数，还要结合机械改造与补偿器设计，才能兼顾动态与稳定性。

12. 测试报告建议写法

12.1 基本信息

建议写清：

1. 电机和驱动器型号；
2. 负载形式；
3. 编码器类型；
4. 控制周期与采样周期；
5. 当前控制参数版本。

12.2 测试条件

建议写清：

1. 工作点；
2. 扰动函数；
3. 扫频范围；
4. 每点采样周期数；
5. 测量量和滤波设置；
6. 环境或安装状态。

12.3 结果字段示例

问题类型	工作点	峰值频率/噪声区	PM	GM	优化方式	优化后结果
机械共振	300 rpm	150 Hz	38°	5.5 dB	150 Hz 陷波	峰值显著下降
编码器噪声	10 rpm	300~500 Hz	48°	8 dB	观测优化+低通调整	高频噪声下降
结构谐振	0° 保持	50 Hz	31°	5 dB	回退增益+陷波+机械加固	保持振动减轻

12.4 本章小结

本章给出的是测试报告写法建议。对共振与噪声类问题，报告不能只写"有问题/已解决"，而应把峰值频率、裕度变化和优化手段一起记录。

13. 常见误区

1. 看到振动就直接加陷波器；
2. 没分清机械共振和编码器噪声；
3. 陷波器加完后不复测 PM、GM 和 Nyquist；
4. 只关注某个峰值下降，不关注整体相位损失；
5. 结构刚度问题完全依赖控制补偿解决；
6. 编码器噪声问题单纯靠提高带宽压制。

13.1 本章小结

本章强调的是方法论：先识别类型，再选择手段，再闭环验证结果。避免"先补偿、后理解"的错误顺序，能大幅减少反复试错。