三阶锁相环（3rd-Order PLL）：无感FOC转子位置与速度观测

1. 问题链：差分/反正切 → 二阶 PLL → 三阶 PLL

FOC 的控制质量取决于"当前转子在哪儿、转多快"。位置来源有两条路，都需要一个平滑的速度信号，而速度从来不是直接测出来的：

场景	位置来源	速度怎么办	痛点
带编码器	编码器直接给电角度 θ_enc	差分 ω=(θ_k-θ_k-1)/T_s，或 PLL 跟踪	差分把量化台阶放大成噪声毛刺（17 bit 台阶 4.8e-5 rad @50μs → ≈1 rad/s），见第 2 节
无感	反电动势观测器给 e_α/e_β	反正切直接算位置，或 PLL 跟踪	反正切把反电动势高频噪声原样放大到角度上，低速信噪比差

两条路殊途同归：都有一个位置/角度信号，需要把它变成干净的速度信号。朴素做法（差分/反正切）都会放大噪声；二阶 PLL（type-2）能平滑跟踪匀速，却在匀加速工况留下稳态误差 e_ss=α/K_i；三阶 PLL（type-3）把误差归零、还内生速度和加速度。三条路精度逐级上升：

方案	做法	问题
差分 / 反正切	直接差分算速度 / atan2 直接算位置	量化台阶/高频噪声被放大成毛刺，速度不可用或噪声大，无滤波能力
二阶 PLL（type-2）	鉴相 + PI 环路滤波器锁相位	恒速无稳态误差；但匀加速（频率斜坡）下有稳态误差 α/K_i，带宽平方越大误差越小，低带宽工况误差可达 0.5°~1° 电角度
三阶 PLL（type-3）	环路滤波器再叠加一个积分器	匀加速无稳态误差，对加速度斜坡误差只剩 j/K_ii；代价是多一个状态、离散化要注意稳定

这里"二阶/三阶"指的是环路滤波器的型别（type），不是状态数。一个 PLL 里如果有 k 个纯积分器（含压控振荡器 NCO 那个），就叫 type-k。工程上 type-1（一阶环）常用于解调，type-2 是伺服/无感位置跟踪的主流，type-3 专门为"高动态加减速 + 低带宽"的矛盾场景准备——正是 FOC 高速起停、负载突变时的痛点。

2. 编码器量化噪声：差分测速为什么不可用

编码器只给位置。若速度用每拍差分得到：

差分测速的量化噪声放大

ω_diff = (θ_k - θ_k-1) / T_s，噪声 σ_ω ≈ q / T_s

q 是编码器量化台阶。17 bit：q = 2π/2¹⁷ ≈ 4.8e-5 rad；50 μs（20 kHz）步长下 σ_ω ≈ 4.8e-5 / 5e-5 ≈ 0.96 ≈ 1 rad/s 的毛刺——速度环直接吃这个信号，Iq 指令会跟着抖

这不是算法缺陷，是信息论决定的：每拍只有 ±q/2 的角度信息，差分把它除以 T_s 放大。M/T 法、取平均都只能缓解不能根治。而 PLL 完全不同：

PLL 为什么能给出干净速度：状态方程内生

PLL 把编码器角度当成参考信号去跟踪：量化噪声从误差通道 ε = θ_enc - θ̂ 进入闭环，经过开环增益的积分作用被三阶极点衰减——低频（真实速度变化）被无损通过，高频（量化毛刺）被滤波。速度不是"算"出来的，而是状态方程内生产生（θ̂ 的导数），天然连续无毛刺。这正是三阶 PLL 观测器作为整机唯一速度源的底气：在 FOC ISR 每拍执行一次，把电角度重构成电角速度 + 加速度两个状态量，速度环、位置环、观测器都从它取值。

同样的逻辑也适用于无感场景：反正切直接算位置时反电动势谐波/噪声全进角度，而 PLL 让它们从误差通道进、被极点滤掉。两个场景共用的就是同一个 type-3 环路结构。

3. PLL 的阶与型：type-1/2/3 对典型输入的稳态误差

把 PLL 看作相位域的线性反馈系统：输入是真实相位 θ(t)，输出是估计相位 θ̂(t)，误差 ε = θ - θ̂。开环传递函数 L(s) = N(s)/s，其中 1/s 是 NCO 的积分，N(s) 是环路滤波器：

三类环路滤波器（型别）

type-1：N(s) = K_p | type-2：N(s) = K_p + K_i/s | type-3：N(s) = K_p + K_i/s + K_ii/s²

开环极点个数 = type 数：s 分母的幂次就是积分器个数。型别越高，能无差跟踪的多项式输入次数越高

由终值定理，对多项式型输入 θ(t)=a·t^m/m!（m 阶），稳态误差 e_ss = lim_s→0 s·E(s)。把典型工况代入：

输入工况	θ(t) 形式	type-1	type-2	type-3
相位阶跃（突跳）	θ₀	0	0	0
频率阶跃（恒速）	ω₀·t	ω₀/K_p	0	0
频率斜坡（匀加速）	½·α·t²	∞	α/K_i	0
加速度斜坡（jerk）	⅙·j·t³	∞	∞	j/K_ii

⚠️ 二阶 PLL 的"看不见的误差"

恒速时 type-2 误差为 0，所以大多数无感方案用二阶就够了——但在匀加速段（起动、加减速、负载突变后的重新稳定）误差瞬时显现：e_ss = α/K_i，K_i ≈ ω_c²，即误差 ≈ α/ω_c²。带宽 ω_c=500 rad/s、加速度 α=2000 rad/s² 时误差 8 mrad≈0.46° 电角度，看着不大，但注意它叠加在 dq 电流方向上，会周期性产生同幅度的转矩误差分量，且随加速度峰值出现——高速换相边缘就是出错的位置。

4. 三阶 PLL 观测器结构：整机唯一速度源

三阶 PLL 观测器由四块组成：鉴相器（把输入角度变成相位误差）、转矩模型前馈（从 iq 直接算出加速度，无感场景用）、三阶环路滤波器（型别 3，只修正模型/跟踪误差）、NCO 积分器（把速度积成位置）。输入角度有两种来源，鉴相器相应有两种形态：

模块	输入 → 输出	作用
位置源（二选一）	编码器 θ_enc / 反电动势观测器 e_αβ	带编码器：直接给电角度；无感：滑模/龙伯格/磁链观测器给 αβ 反电动势
鉴相器	θ_enc/e_αβ, θ̂ → ε	编码器形态：ε = θ_enc - θ̂（直接做差）；无感形态：交叉积 ε = e_βcosθ̂ - e_αsinθ̂，归一化后 ≈ Δθ
转矩模型前馈	iq, ψ_f, p, J → α_m	由 iq 经电磁转矩公式算出加速度前馈 α_m=p·(1.5·p·ψ_f·iq - T̂_L)/J，直接驱动速度积分（无感低速关键，见下）
三阶环路滤波器	ε → 速度修正 Δω	K_p + K_i/s + K_ii/s²，量化/模型噪声从 ε 通道进入、被三阶极点衰减
NCO 积分	Δω + ω₀ → θ̂	速度积分出位置，构成第 3 个积分器（type-3 的根基）

为什么三阶一定要输入 iq？（无感场景）

纯误差驱动（只有 ε 输入）的三阶环虽然数学上"对匀加速无差"，但加速度全靠 K_ii·ε 慢慢积分"学"出来——收敛慢、动态差，且反电动势在低速几乎为零，ε 的信噪比很差，根本学不动。工程上的三阶观测器必须并联一条 iq 前馈通路：iq 是电流环的输出、几乎无延迟，id=0 控制下电磁转矩 T_e=1.5·p·ψ_f·iq 与转速无关，把它折算成加速度直接灌进速度积分，环路只剩"模型误差"要修——所以同样的 ω_c 下动态快一个量级，低速也能站得住。这一条正是"无感低速也能稳"的关键。带编码器场景不缺反电动势，但同样可以把 iq 前馈用于重载加减速的动态补偿。

转矩模型前馈（iq → 加速度，电角度域）

T_e = 1.5·p·ψ_f·i_q， α_m = p·(T_e - T̂_L) / J

p 极对数、J 转子+负载折算惯量（kg·m²）、T̂_L 负载/扰动转矩估计（可由 DOB 或积分状态给出）。id=0 控制下 Te 只取决于 iq，零速同样成立——这是低速无感立得住的原因

交叉积鉴相（无感形态，小角度线性化）

ε = e_β·cos θ̂ - e_α·sin θ̂ = |E|·sin(θ_e - θ̂) ≈ |E|·(θ_e - θ̂)

|E| = √(e_α²+e_β²)。除以 |E| 归一化后，ε 就是纯相位差（rad），环路增益不再随转速/磁链波动；编码器形态则直接 ε = θ_enc - θ̂，无需归一化

注意这里的三个积分器：环路滤波器里两个（Ki/s 和 Kii/s²）+ NCO 一个（1/s），所以是 type-3。速度估计直接取环路滤波器内部状态 ω̂，位置取 θ̂——一个观测器同时给出转速和电角度，直接喂给速度环与 Park 变换；在带编码器的整机里它就是唯一速度源（速度环、位置环、观测器共用），在无感系统里它同时充当位置观测器。环路对 ε 的"型别"决定稳态误差（对匀加速无差），α_m 前馈决定动态——两者叠加才是工程上的三阶观测器。

5. 三阶环路滤波器设计：极点配置与整定

type-3 的闭环特征方程：

闭环传函与特征方程

Φ(s) = (K_ps² + K_is + K_ii) / (s³ + K_ps² + K_is + K_ii)

把极点全部放在 s = -ω_c（三重实根，临界阻尼，无超调）：s³ + 3ω_cs² + 3ω_c²s + ω_c³ → 对应系数相等

三阶 PLL 整定公式（ω_c 为环路带宽）

K_p = 3ω_c， K_i = 3ω_c²， K_ii = ω_c³

对比 type-2 常用 K_p=2ζω_c（ζ=0.707）、K_i=ω_c²。三阶在相同 ω_c 下比例项更大、还多了二次积分项

选 ω_c 的工程约束和二阶完全一样：要高于反电动势的基频跟踪需求（≥ 若干倍最高电角频率），但必须低于反电动势观测器自身的带宽和 PWM 开关频率的 1/10 量级，否则环路会把观测器的噪声和采样纹波"跟"进去。典型取值：高速无感 ω_c 300~1500 rad/s，低速重载往低取。

注意 iq 前馈只加在输入通路，不改变闭环特征方程——K_p/K_i/K_ii 依然按 ω_c 配置，前馈项 α_m 的作用是把"真实加速度"提前喂给积分器，让 ε 环路只需要收敛模型误差，带宽感受不到前馈的存在。

带宽 ω_c	K_p (1/s)	K_i (1/s²)	K_ii (1/s³)	适用场景
300 rad/s	900	2.7×10⁵	2.7×10⁷	低速重载、观测器噪声大
500 rad/s	1500	7.5×10⁵	1.25×10⁸	通用高速无感（本文仿真默认）
1000 rad/s	3000	3.0×10⁶	1.0×10⁹	观测器干净、PWM ≥ 20 kHz

6. 稳态误差定量对比：二阶 vs 三阶

以 ω_c=500 rad/s 为例：二阶 ζ=0.707 → K_i=2.5×10⁵；三阶 → K_ii=1.25×10⁸。输入匀加速 θ=½αt²，理论误差分别为 α/K_i 和 0。用与文末 C 代码相同的离散结构在 T_s=0.1 ms 下仿真 0.3 s 取末值：

加速度 α (rad/s²)	二阶仿真误差 (mrad)	二阶理论 α/K_i (mrad)	三阶仿真误差 (mrad)	等效电角度 (°)
500	2.000	2.000	≈0	0.11°（仅二阶）
1000	4.000	4.000	≈0	0.23°（仅二阶）
2000	8.000	8.000	≈0	0.46°（仅二阶）
4000	16.000	16.000	≈0	0.92°（仅二阶）

低带宽工况更明显：ω_c=300 rad/s 时 K_i=9×10⁴，α=2000 rad/s² 的二阶误差 = 22.2 mrad = 1.27° 电角度——对 8 极对电机就是 0.16° 机械角，在齿轮箱输出端会被减速比进一步放大。三阶 PLL 把这些全部归零；它只剩对 jerk（加速度斜坡）的残余误差 j/K_ii，例如 j=2×10⁴ rad/s³ 时仅 0.16 mrad≈0.009°，工程上可忽略。

7. 交互仿真：匀加速位置跟踪误差对比

拖动加速度滑块，看同样 ω_c=500 rad/s 下二阶（蓝）与三阶（红）PLL 跟踪 θ=½αt² 的相位误差曲线。二阶误差上升并停在 α/K_i（虚线为理论值），三阶误差冲一下后归零：

二阶 vs 三阶 PLL：匀加速跟踪稳态误差对比（ωc=500 rad/s）

加速度 α 1000 rad/s²

蓝=二阶 PLL 误差（稳态 α/Ki），红=三阶 PLL 误差（稳态 0）。虚线=二阶理论稳态值。

观察要点：

三阶 PLL 的误差曲线只在启动瞬间有一个小突起（由积分器初始建立引起），随后收敛到 0——而二阶 PLL 的误差一路爬到 α/K_i 并保持，加速度越大稳态误差越大；
用低带宽（如 ω_c=300）时二阶误差显著增大，这正是重载低速无感场景；三阶对带宽的敏感度低得多；
代价在动态过程：三阶有更长的调节时间和更大的超调潜力，所以 K_ii 不宜再加大——严格按 ω_c³ 取，别"顺手加一点"。
仿真口径说明：本仿真三条曲线都是纯误差驱动（不给 iq）——这是"最坏情况"对照，证明 type-3 的型别本身就把稳态误差归零。工程版再叠加 iq 转矩前馈（第 4/8 节）后，启动突起更小、收敛更快、低速也能立住。

7.2 演示 B：编码器量化差分 vs 三阶 PLL 测速

17 bit 编码器、20 kHz 采样：真实速度 ω₀=30 rad/s 时，差分测速被量化台阶 q≈4.8e-5 rad 放大成 ≈±1 rad/s 的毛刺；三阶 PLL 跟踪量化后的电角度，速度由状态方程内生产生，平滑地跟住 30 rad/s。拖动转速滑块看对比：

差分 vs 三阶 PLL 测速：编码器量化噪声对比（17bit · 20kHz）

真实转速 ω₀ 30 rad/s

橙=差分速度（±q/Ts 毛刺），蓝=三阶 PLL 速度（平滑）。毛刺峰峰值见标注。

观察要点：

差分毛刺幅度 ≈ q/T_s = 4.8e-5/5e-5 ≈ 0.96 rad/s，与转速无关——只由量化台阶和采样周期决定；转速越低，相对噪声越大；
PLL 输出速度的毛刺被三阶极点衰减到 1% 量级，且无相位滞后累积（type-3 对恒速无差）；这就是"状态方程内生速度"的含义；
速度环直接吃差分信号会让 Iq 指令同频抖动（速度环增益越高越明显），换成 PLL 速度源后整机噪声明显下降——这也是伺服里"编码器 + 软件测速源"的标准做法。

8. 数字离散化：嵌套积分结构 + iq 前馈

三阶 PLL 在 DSP/MCU 上的标准实现是三个嵌套的前向欧拉积分器——前一个积分器的输出作为后一个的"前馈"，同时把 iq 的转矩模型加速度 α_m 直接叠加进速度积分：

离散更新（Ts 为控制周期，含 iq 前馈）

α_m = p·(1.5·p·ψ_f·i_q - T̂_L) / J
α̂ += K_ii·ε·T_s
ω̂ += (K_i·ε + α_m + α̂)·T_s
θ̂ += (K_p·ε + ω̂)·T_s

i_q 从电流环直接来（无延迟、零速也有效）：α_m 承担"真实加速度"的绝大部分，ε 环路只用 K_p/K_i/K_ii 修正模型误差（J/ψ_f/T̂_L 不准的部分）

离散化约束

前向欧拉对 type-3 的稳定条件比 type-2 更苛刻：要求 ω_c·T_s 明显小于 1（经验值 ≤0.1~0.3）。ω_c=500 rad/s 时建议 T_s ≤ 0.2 ms；若控制周期固定且偏大（如 1 kHz 速环 + 位置观测），要么降低 ω_c，要么改用 Tustin（双线性）离散——代价是公式稍复杂、且出现代数环，需拆开半步更新。α_m 前馈是开环量，不参与稳定性判定，但 J 估计偏差过大时它本身会变成扰动，需与 T̂_L 积分状态协同收敛。

9. 代码实现（C语言，可编译模块）

9.1 三阶 PLL 观测器核心（带 iq 转矩前馈）

/* 三阶 PLL 转子位置/速度观测器（无感 FOC 用，含 iq 前馈）
 * 输入：相位误差 eps（rad，来自交叉积鉴相，已归一化）
 *       iq（A，电流环输出的 q 轴电流，id=0 控制）
 * 输出：估计位置 theta（电角度 rad）、估计速度 omega（电角速度 rad/s）
 * 整定：wc 为环路带宽（rad/s），ts 为控制周期（s）
 *   Kp = 3*wc;  Ki = 3*wc*wc;  Kii = wc*wc*wc;
 *   psif 永磁磁链(Wb)、pp 极对数、j 转子+负载折算惯量(kg·m^2, 辨识值)
 */
#include <math.h>

typedef struct {
    float kp, ki, kii;   /* 环路滤波系数 */
    float psif;        /* 永磁磁链 Wb */
    float pp;          /* 极对数 */
    float j;           /* 惯量 kg·m^2 */
    float tl_hat;      /* 负载/扰动转矩估计 N·m */
    float ts;          /* 采样周期 s */
    float theta;       /* 位置估计 rad（电角度） */
    float omega;       /* 速度估计 rad/s（电角速度） */
    float alpha_int;   /* 加速度积分修正 rad/s^2 */
} pll3_t;

void pll3_init(pll3_t *p, float wc, float ts,
              float psif, float pp, float j)
{
    p->kp   = 3.0f * wc;
    p->ki   = 3.0f * wc * wc;
    p->kii  = wc * wc * wc;
    p->psif = psif;  p->pp = pp;  p->j = j;
    p->tl_hat = 0.0f;
    p->theta = p->omega = p->alpha_int = 0.0f;
}

/* 单步执行：输入相位误差 eps 与 iq，返回估计位置 */
float pll3_step(pll3_t *p, float eps, float iq)
{
    float te     = 1.5f * p->pp * p->psif * iq;          /* 电磁转矩（id=0） */
    float alpha_m = p->pp * (te - p->tl_hat) / p->j;       /* iq 转矩模型加速度前馈 */

    p->alpha_int += p->kii * eps * p->ts;               /* 加速度积分修正（模型误差） */
    p->omega += (p->ki * eps + alpha_m + p->alpha_int) * p->ts;  /* 速度：修正 + iq 前馈 + 加速度积分 */
    p->theta += (p->kp * eps + p->omega) * p->ts;       /* 位置：修正 + 速度积分 */
    return p->theta;
}

9.2 交叉积鉴相 + 反电动势幅值归一化

/* 输入：反电动势估计 e_alpha/e_beta，当前位置估计 th_hat
 * 返回：归一化相位误差 ε ≈ sin(θe - θ̂)，小角度下 ≈ Δθ（rad）
 * 归一化目的：|E|=ωe·ψf 随转速变化，不归一化则环路增益漂移 */
float pll3_phase_err(float e_alpha, float e_beta, float th_hat)
{
    float ca = cosf(th_hat), sa = sinf(th_hat);
    float err = e_beta * ca - e_alpha * sa;   /* |E|·sin(θe - θ̂) */
    float amp = sqrtf(e_alpha * e_alpha + e_beta * e_beta);
    if (amp < 1e-6f) return 0.0f;   /* 零速/停转保护 */
    return err / amp;
}

9.3 使用示例：Sensorless 任务循环

/* 50kHz 电流环里每 10 拍调用一次（5kHz 位置更新，Ts=0.2ms）
 * iq 取当前电流环的 q 轴指令/反馈（Id=0 控制） */
void Sensorless_Task(void)
{
    static pll3_t pll;
    float e_alpha, e_beta, eps, iq, theta_hat, omega_hat;

    pll3_init(&pll, 500.0f, 2e-4f, 0.062f, 4.0f, 1.2e-4f);
    /* ωc=500 rad/s, Ts=0.2ms, ψf=62mWb, 4 极对, J≈0.12e-3 kg·m²（辨识值） */
    for (;;) {
        /* 反电动势观测器（滑模/龙伯格）每拍更新 e_alpha / e_beta */
        eps = pll3_phase_err(e_alpha, e_beta, pll.theta);
        iq  = CurrentLoop_GetIq();              /* 本拍 q 轴电流 */
        theta_hat = pll3_step(&pll, eps, iq);
        omega_hat = pll.omega;
        /* theta_hat → Park 变换；omega_hat → 速度环反馈 */
    }
}

10. 与位置源搭配：编码器直连与无感观测

三阶 PLL 是"角度跟踪器"，上游给它什么角度源，它就把什么重构成速度。两种接法：

接法	鉴相输入	要点
编码器直连	ε = θ_enc - θ̂	不需要归一化；电角度直接进环路。θ̂ 跟踪 θ_enc 时速度 ω̂ 内生、无差分毛刺——整机唯一速度源。注意 θ_enc 的绝对零点与 Park 用角一致，避免初始 ±π 模糊
无感观测	交叉积鉴相 ε = e_βcosθ̂ - e_αsinθ̂	上游是反电动势观测器（滑模/龙伯格/磁链），必须幅值归一化，且观测器带宽 ≥ 3~5×PLL 带宽

无感接法的三条铁律（幅值归一化、带宽适配、低速切换协同）：

幅值归一化是前提：反电动势幅值 |E| = ω_e·ψ_f 随转速变化（低速时很小），若鉴相输出直接用 ε=|E|·Δθ，环路增益随转速漂移，整定等于白做。归一化后增益恒定，低速也能保持带宽；
观测器带宽必须高于 PLL 带宽：滑模/磁链观测器的滤波时间常数决定了上游反电动势的相移，若 PLL 带宽逼近观测器带宽，会在环路里引入额外相位滞后，三阶的"无差"特性会被上游相移破坏；
低速切换协同：低速反电动势信噪比差时，普遍的做法是 I/F 强拖（见《I/F功角自稳定驱动》），速度起来后再切 PLL 观测。切换瞬间 θ̂ 与真实角可能有偏差，三阶 PLL 的积分器会以 (ω_c) 量级带宽快速收敛——切换后先给一小段"观测器建立窗口"再使能速度环，避免误差冲击。

11. 工程避坑清单

坑	现象	对策
加速度积分饱和	长时间大加减速后 α̂ 越积越大，恢复匀速时位置持续超调/振荡	对 α̂ 加限幅（如 ±2×实际最大加速度）；ω̂ 也限幅到合理范围；必要时 α̂ 低通
K_ii 手动调大	本来稳定的环开始低频振荡，甚至发散	严格按 ω_c³ 取值；要更快就用更高的 ω_c，别单独抬 K_ii
离散步长过大	仿真稳定、上机发散（前向欧拉条件被破坏）	ω_c·T_s ≤ 0.3；不行就降 ω_c 或改 Tustin
零速/低速归一化除零	ε 突变、位置跳变	\|E\| < 阈值时强制 ε=0（见 9.2 的 1e-6 保护）
上游相移不匹配	三阶"无差"在实机上变成明显稳态误差	确认反电动势观测器滤波时间常数带来的滞后，PLL 带宽留足裕量
与反正切切换	切换瞬间位置跳变	统一入口：反正切只用于初值对齐（θ̂ 初值=atan2 结果），此后全程 PLL 跟踪，不频繁切换
漏接 iq 前馈（只喂 ε）	低速跟不住、加减速相位滞后明显，K_ii 越调越振荡	必须把 iq 经转矩模型折算成加速度叠加进速度积分（9.1 的 alpha_m），环路只修模型误差；低速信噪比差的正是靠这条前馈立住
J/ψ_f/T̂_L 辨识不准	α_m 前馈本身变成扰动，位置出现同频误差	惯量用离线辨识值起步、在线辨识微调；T̂_L 用积分状态或 DOB 平滑估计，避免前馈把负载当加速度
编码器初始角度零点不对齐	Park 用角与 θ̂ 相差固定角度，转矩/位置环偏差	初始化时用编码器零点标定 θ̂ 初值；电角度 = 机械角×p 的换算与极对数对齐
编码器毛刺/坏点（磁编偶发跳变）	ε 瞬间变大，θ̂ 和 ω̂ 被拉偏	ε 限幅（如 ±π/4）+ 跳变检测：\|ε\| 超阈值时不更新积分器只更新位置，下一拍自动恢复