速度观测器：锁相环、M/T法与观测器性能对比

1. 为什么速度反馈这么关键

编码器/磁编码器直接给出的是角度，而速度环需要的是转速——两者之间差着一个"求导/估计"环节。速度反馈的质量（噪声、滞后、分辨率）直接决定了：

速度环带宽上限：反馈噪声大会迫使滤波加重，滤波滞后又吃掉相位裕度（见《三闭环带宽分配》）
低速性能：低速时角度变化慢、量化步长占比大，测速误差被放大
整机振动与发热：速度反馈噪声会通过 PI 放大成 iq 抖动，表现为振动和 MOS 发热

所以"怎么测速"不是小问题。主流方法可归为三条路线：M/T 计数法（硬件测频测周）、锁相环 PLL（相位跟踪）、状态观测器（模型估计）。

2. M/T 法：数脉冲，最传统的思路

编码器每转发出固定数量的脉冲，测速 = 数脉冲。按计数策略分三种：

M 法（测频）：固定时间窗 T 内数脉冲数 m，ω = 2π·m/(P·T)。高速时脉冲多、精度高；低速时一个窗内可能只有几个甚至 0 个脉冲，误差极大
T 法（测周）：测相邻脉冲的时间间隔，ω = 2π/(P·Δt)。低速时周期长、精度高；高速时周期太短、时间分辨率不够
M/T 法：高速用 M 法、低速用 T 法，或同时测脉冲数与时钟数。宽范围内都可用，是编码器测速的经典方案

量化误差（±1 计数）

M法：Δω/ω = 1/m（脉冲数越少误差越大） · T法：Δω/ω = 1/N_clk（计数越少误差越大）

优点：实现极简（计数器即可）、无模型、硬件天然支持。缺点：速度更新率受脉冲频率限制、低速误差大、信号"阶梯化"（量化噪声明显）。适合要求不高的通用变频，不适合高性能伺服的低速高刚性场合。

3. 锁相环（PLL）测速：把测速变成锁相

PLL 的思路完全不同：不"算"差分，而是让一个内部振荡器去锁定外部角度信号。一旦锁定，内部振荡器的角频率就等于真实速度。

二阶 PLL 测速结构

鉴相：e = sin(θ_meas − θ̂)

环路滤波（PI）：ω̂ = K_p·e + K_i·∫e dt

积分（VCO）：θ̂ = ∫ω̂ dt

K_p = 2ζ·ω_pll、K_i = ω_pll²，ζ=1 取临界阻尼，ω_pll 为环路带宽

PLL 是闭环跟踪而非开环差分：角度噪声被环路滤波器平滑，输出速度天然连续、无差分放大。带宽 ω_pll 是可设计的旋钮——带宽高则响应快但吃噪声，带宽低则平滑但滞后。

优点：平滑、带宽可调、对编码器角度噪声/量化不敏感，中高速性能优秀，广泛用于伺服速度提取。缺点：低速时若角度分辨率不足，相位误差大；锁定过程有初始瞬态；本质仍是"跟随"而非"预测"。

4. 状态观测器测速：借模型之力

前两种方法只用了"角度"这一个信息。状态观测器额外用上机械模型（J·dω/dt = T_e − T_d），把测量和模型融合，估计出更平滑、可预测的速度。

龙伯格角速度观测器

dθ̂/dt = ω̂ + L₁·(θ_meas − θ̂)

dω̂/dt = (K_t·i_q − T̂_d)/J + L₂·(θ_meas − θ̂)

增益 L₁=2ω_obs、L₂=ω_obs²（临界阻尼）；加入转矩项后还能同时估计扰动

与《转矩扰动观测》一脉相承：同一个观测器框架既能出速度、又能出扰动。观测器的优势是模型约束带来的天然抗噪——速度不会"乱跳"，因为它受物理方程约束；还能预测（前馈），减少测量滞后。

缺点：依赖模型参数 J（不准则估计有偏）；实现比前两者复杂；需要 i_q、T̂_d 等额外输入。

方法	原理	动态带宽	抗噪/量化	低速性能	模型依赖	实现成本
M/T 法	脉冲计数/测周	中（受更新率）	差（阶梯化）	差（M法）	无	★☆☆
锁相环 PLL	相位跟踪	高（带宽可设）	好（闭环平滑）	中（看分辨率）	无	★★☆
状态观测器	模型+测量融合	高（受J限制）	最优（模型约束）	好	有（J）	★★★

5. 怎么选：三个决策点

成本/简单优先 → M/T 法（通用变频、对动态要求不高的场合）
中高性能、无模型、好落地 → PLL（大多数伺服的速度反馈首选，带宽按"能稳则尽量高"调）
要求平滑+可前馈+低速高刚性 → 状态观测器（配合 J 辨识与扰动观测使用）

✅ 工程共识

高性能伺服常混用：PLL/观测器给出平滑速度进速度环，同时用 M/T 计数做低速档的校验与启动判定。速度环带宽能设多高，往往就卡在速度反馈这一环上——反馈越干净，速度环越敢往快调。

6. 交互式演示：三种测速方法同台竞技

下面是同一组带量化的角度信号（编码器四倍频分辨率、角度噪声），三种方法各自估计速度：M/T（红）、PLL（蓝）、观测器（绿），真实速度（灰）。切速度模式、调 PLL/观测器带宽和编码器分辨率，看谁跟得快、谁更稳。

M/T vs PLL vs 观测器测速对比仿真

速度模式

编码器线数 1000

PLL 带宽 60 Hz

观测器带宽 80 Hz

选择速度模式与参数，对比三种测速方法的响应与噪声。

观察要点：

低速/高分辨率不足时，M/T 法（红）明显"阶梯化"、误差大；PLL（蓝）与观测器（绿）平滑
阶跃时观测器（绿）因为带了模型/预测，通常比 PLL 更快到达新速度且超调可控
把 PLL/观测器带宽调高：跟得更快但噪声变大；调低：平滑但滞后——这就是速度反馈的"快与稳"权衡
把编码器线数调小（分辨率差）：三者误差都变大，但观测器相对更扛

7. 代码实现（C语言）

7.1 二阶 PLL 测速

/* 二阶 PLL 测速：锁定角度，输出角频率 = 速度 */
typedef struct {
    float kp, ki;   /* 环路滤波器 PI（Kp=2ζω, Ki=ω²） */
    float theta_hat, /* 内部角度估计 */
          omega_hat,  /* 输出速度估计 rad/s */
          integ;      /* 积分项 */
} pll_speed_t;

void pll_speed_init(pll_speed_t *p, float bw_hz)
{
    const float w = 2.0f * 3.14159265f * bw_hz;
    p->kp = 2.0f * w;        /* 临界阻尼 ζ=1 */
    p->ki = w * w;
    p->theta_hat = 0.0f; p->omega_hat = 0.0f; p->integ = 0.0f;
}

float pll_speed_update(pll_speed_t *p, float theta_meas, float ts)
{
    float e = sinf(theta_meas - p->theta_hat); /* 鉴相 */
    p->integ += p->ki * e * ts;                 /* 环路积分 */
    p->omega_hat = p->kp * e + p->integ;
    p->theta_hat += p->omega_hat * ts;          /* VCO 积分 */
    return p->omega_hat;
}

7.2 龙伯格角速度观测器

/* 角速度观测器：dθ/dt=ω + L1·e；dω/dt=(Kt·iq-Td_hat)/J + L2·e */
typedef struct {
    float j, kt;       /* 惯量与转矩常数 */
    float l1, l2;      /* 观测器增益（极点配置） */
    float theta_hat, omega_hat, td_hat;
} speed_obs_t;

void speed_obs_init(speed_obs_t *o, float j, float kt, float bw_hz)
{
    const float w = 2.0f * 3.14159265f * bw_hz;
    o->j = j; o->kt = kt;
    o->l1 = 2.0f * w; o->l2 = w * w;      /* 临界阻尼 */
    o->theta_hat = 0.0f; o->omega_hat = 0.0f; o->td_hat = 0.0f;
}

float speed_obs_update(speed_obs_t *o, float theta_meas,
                       float iq, float ts)
{
    float e = theta_meas - o->theta_hat;
    o->theta_hat += (o->omega_hat + o->l1 * e) * ts;
    o->omega_hat += ((o->kt * iq - o->td_hat) / o->j + o->l2 * e) * ts;
    o->td_hat   += (-o->l2 * o->kt * e) * ts * 0.1f; /* 扰动估计（可选，慢回路） */
    return o->omega_hat;
}

7.3 M/T 法（M 法）

/* M 法测速：固定窗内角度差 / 窗时间（低速误差大，可切 T 法） */
float mt_speed_update(float theta_meas, float theta_prev, float window)
{
    float dtheta = theta_meas - theta_prev;   /* 窗内角度增量（已量化） */
    return dtheta / window;                    /* ω = Δθ/ΔT */
}

8. 工程要点与常见误区

速度反馈质量决定速度环上限：反馈越干净，速度环越敢往快调；反之被迫加滤波反而更滞后
M/T 别在高性能低速场合硬用：量化误差在低速被放大成明显的"阶梯"，会让速度环震荡
PLL 带宽是核心旋钮：从速度环带宽的 2~3 倍起步，调到"跟得上又不抖"为止
观测器先校 J：J 不准观测器速度会有偏，务必先做惯量辨识（配合《转矩扰动观测》一起用）
别让测速噪声污染电流环：速度噪声经速度环放大成 iq 抖动 → 振动/发热，观察 iq 波形可反查测速噪声
低速档可混用：低速用 T 法/观测器，高速用 M 法，切换要做滞回

✅ 一句话记住

M/T 法靠数脉冲（简单但低速差）、PLL 靠锁相位（平滑带宽可调）、观测器靠借模型（最稳可预测但依赖 J）。高性能伺服：PLL/观测器给速度环供干净的速度，M/T 做校验兜底。