一、为什么要使用 PLL 估算速度
在电机 FOC 控制中，编码器可以直接测量转子角度，但不能直接测量转子速度。
最简单的方法是使用相邻两次编码器角度作差：
速度 = (本次角度 - 上次角度)/采样周期
写成变量形式：
omega = (theta_now - theta_last) / Ts
其中：
- theta_now：本次编码器角度
- theta_last：上次编码器角度
- Ts：采样周期
- omega：计算得到的速度
这种方法虽然简单，但是编码器的量化误差、采样抖动和角度噪声都会被差分放大，导致计算出来的速度波动较大。
PLL 的思路不是直接对角度作差，而是在内部建立一个估算角度，让估算角度不断追踪编码器测量角度，同时得到比较平滑的估算速度。
二、PLL 中的变量
为了避免不同编辑器出现公式显示异常，本文采用以下变量名称：
- theta_meas：编码器测量角度
- theta_est：PLL 估算角度
- omega_est：PLL 估算速度
- error_theta：测量角度与估算角度之间的误差
- k1：角度快速校正增益
- k2：速度积分校正增益
- Ts：PLL 执行周期
PLL 的核心思想可以概括为：
测量角度与估算角度作差
↓
得到角度误差
↓
k1 支路快速修正角度
k2 支路逐渐修正速度
↓
速度积分得到估算角度
↓
估算角度反馈，再次计算误差
三、首先计算角度误差
编码器测量角度与 PLL 估算角度之间的误差为：
cpp
error_theta = wrap(theta_meas - theta_est)其中 wrap() 表示角度环绕处理。
如果角度使用弧度，一般把角度误差限制在：
cpp
error_theta = 2°例如：
cpp
theta_meas = 1°
theta_est = 359°如果直接相减：
cpp
error_theta = 1° - 359° = -358°但两个角度实际只相差 2°。
经过角度环绕处理后：
cpp
error_theta = 2°因此，编码器 PLL 必须进行角度环绕处理，否则在角度经过 0° 或 360° 时，会产生一个非常大的错误角度差。
四、k1 支路：快速修正估算角度
k1 支路直接对当前角度误差进行比例放大：
cpp
omega_corr = k1 × error_theta其中 omega_corr 是角速度校正量。
这个校正量会与 PLL 当前的估算速度相加：
cpp
theta_est 的变化速度 = omega_est + omega_corr也就是：
cpp
d(theta_est)/dt = omega_est + k1 × error_theta然后对它进行积分，得到新的估算角度：
cpp
theta_est = 积分[omega_est + k1 × error_theta]k1 支路的作用
如果编码器测量角度在估算角度前面：
cpp
error_theta > 0那么：
cpp
k1 × error_theta > 0PLL 会增加估算角度的变化速度，让估算角度快速追赶编码器角度。
如果估算角度跑得太快：
cpp
error_theta < 0那么 k1 支路会降低估算角度的变化速度。
因此，k1 支路的作用是：
根据当前角度误差，立即产生一个角速度校正量，让估算角度快速跟随编码器角度。
k1 的单位
角度误差的单位是：rad。k1 乘以角度误差后，需要得到角速度：
cpp
k1 × rad = rad/s因此 k1 的单位是：k1 单位 = 1/s
也就是"每秒"。
例如：
cpp
k1 = 100 1/s
error_theta = 0.1 rad那么 k1 支路产生的角速度校正量为：
cpp
omega_corr = 100 × 0.1
= 10 rad/s意思是：
当前存在 0.1 rad 的角度误差时，k1 支路会额外产生 10 rad/s 的追赶速度。
五、k2 支路：逐渐修正估算速度
k2 支路同样来自角度误差：
cpp
speed_change_rate = k2 × error_theta然后经过积分，得到 PLL 的估算速度：
cpp
omega_est = 积分[k2 × error_theta]也可以写成：
cpp
d(omega_est)/dt = k2 × error_thetak2 支路的作用
如果：
cpp
error_theta > 0说明估算角度持续落后于编码器角度，PLL 会逐渐增加估算速度。
如果：
cpp
error_theta < 0说明估算角度跑得太快，PLL 会逐渐降低估算速度。
如果：
cpp
error_theta = 0速度不再被修正，估算速度保持当前值。
因此，k2 支路的作用是：
cpp
k2 × error_theta根据持续存在的角度误差，逐渐修正并保持 PLL 的估算速度。
六、为什么角度误差积分后可以得到速度
从真实物理关系来说，速度确实是角加速度积分得到的：
角速度 = 角加速度的积分
也就是：
cpp
omega = 积分[alpha]但是在 PLL 中：
并不是电机真实的角加速度，而是 PLL 根据角度误差构造出来的"估算速度变化率"。
可以将其理解为一种虚拟角加速度：
cpp
虚拟角加速度 = k2 × error_theta当估算角度持续落后时，PLL 认为当前估算速度太小，于是让估算速度不断增加。
当估算角度持续超前时，PLL 认为当前估算速度太大，于是让估算速度不断减小。
所以 PLL 不需要直接测量电机的真实角加速度，也能根据角度误差逐渐估算出速度。
七、k2 的单位
估算速度的单位是：rad/s
估算速度变化率的单位是：rad/s²
因为：
cpp
k2 × error_theta = 估算速度变化率所以：
cpp
k2 × rad = rad/s²最终得到：k2 单位 = 1/s²
因此：
- k1 的单位是 1/s
- k2 的单位是 1/s²
八、框图中的两个 1/s 是什么
PLL 框图中的 1/s 表示积分器。
第一个积分器的作用是：
cpp
k2 × error_theta
↓ 积分
omega_est也就是角度误差经过 k2 后积分，得到估算速度。
第二个积分器的作用是：
cpp
omega_est + k1 × error_theta
↓ 积分
theta_est也就是估算速度和角度校正量相加后，再积分得到估算角度。
需要区分下面两个概念：
- k1 的单位为 1/s，表示"每秒"。
- 框图中的 1/s 表示积分运算。
虽然它们都写成 1/s，但含义不同。
九、k1 和 k2 两条支路的区别
| 支路 | 输出量 | 主要作用 | 类似的控制作用 |
|---|---|---|---|
| k1 支路 | 角速度校正量 | 快速减小当前角度误差 | 比例作用 P |
| k2 支路 | 估算速度变化率 | 修正并保持长期估算速度 | 积分作用 I |
可以简单记忆为：
k1 负责让当前角度赶紧对齐，k2 负责把长期速度估准确。
十、PLL 的完整数学关系
PLL 的角度误差为：
cpp
error_theta = wrap(theta_meas - theta_est)估算速度的变化率为：
cpp
d(omega_est)/dt = k2 × error_theta估算角度的变化率为：
cpp
d(theta_est)/dt = omega_est + k1 × error_theta因此，PLL 的完整工作过程是：
cpp
1. 读取编码器测量角度 theta_meas
2. 计算角度误差：
error_theta = wrap(theta_meas - theta_est)
3. k2 支路更新估算速度：
d(omega_est)/dt = k2 × error_theta
4. k1 支路产生角速度校正量：
omega_corr = k1 × error_theta
5. 估算速度与角速度校正量相加：
angle_rate = omega_est + omega_corr
6. 对 angle_rate 积分得到新的估算角度
7. 将估算角度反馈，继续与编码器角度比较十一、PLL 参数的计算
PLL 是一个二阶系统，它的特征方程为：
cpp
s² + k1 × s + k2标准二阶系统的特征方程为：
cpp
s² + 2 × zeta × wn × s + wn²对比两者可以得到：
cpp
k1 = 2 × zeta × wn
k2 = wn²其中：
- wn：PLL 的自然角频率，单位为 rad/s
- zeta：阻尼比，没有单位
- 常用阻尼比可以取 0.707
例如：
cpp
zeta = 0.707
wn = 100 rad/s则：
cpp
k1 = 2 × 0.707 × 100
= 141.4 1/s
k2 = 100²
= 10000 1/s²PLL 带宽越高：
- 角度和速度响应越快
- 动态延迟越小
- 编码器噪声更容易进入速度估算
PLL 带宽越低：
- 估算速度更加平滑
- 编码器噪声更小
- 速度响应更加滞后
因此，PLL 带宽并不是越高越好，需要在动态响应和速度噪声之间进行平衡。
十二、离散化后的计算过程
单片机不能直接执行连续积分，需要按照固定周期进行离散累加。
估算速度更新公式为：
cpp
omega_est =
omega_est
+ k2 × error_theta × Ts估算角度更新公式为：
cpp
theta_est =
theta_est
+ (omega_est + k1 × error_theta) × Ts其中 Ts 是 PLL 的真实执行周期。
例如 PLL 每 100 us 执行一次：
cpp
Ts = 100 us
= 0.0001 s如果执行频率为 20 kHz：
cpp
Ts = 1 / 20000
= 0.00005 s代码中的 Ts 必须与 PLL 的真实调用频率一致，否则 PLL 的实际带宽和计算出来的参数会不一致。
十三、C 语言实现
cpp
typedef struct
{
float angle_est; /* 估算角度，rad */
float speed_est; /* 估算速度，rad/s */
float k1; /* 角度校正增益，1/s */
float k2; /* 速度校正增益，1/s^2 */
float Ts; /* PLL执行周期，s */
} EncoderPLL_t;
角度环绕函数：
static float WrapToPi(float angle)
{
const float PI = 3.14159265358979323846f;
const float TWO_PI = 6.28318530717958647692f;
while (angle >= PI)
{
angle -= TWO_PI;
}
while (angle < -PI)
{
angle += TWO_PI;
}
return angle;
}
PLL 更新函数：
void EncoderPLL_Update(EncoderPLL_t *pll, float angle_measured)
{
float angle_error;
/* 计算测量角度与估算角度之间的误差 */
angle_error =
WrapToPi(angle_measured - pll->angle_est);
/*
* k2支路：
* 角度误差经过k2后积分，更新估算速度
*/
pll->speed_est +=
pll->k2 * angle_error * pll->Ts;
/*
* k1支路：
* k1乘以角度误差得到角速度校正量
* 再与估算速度相加，积分得到估算角度
*/
pll->angle_est +=
(pll->speed_est
+ pll->k1 * angle_error)
* pll->Ts;
/* 将估算角度限制到 -π 到 π */
pll->angle_est =
WrapToPi(pll->angle_est);
}十四、实际使用注意事项
1. 必须处理角度环绕
编码器角度经过 0° 和 360° 的交界处时，必须计算最短方向角度误差。
2. PLL 周期必须正确
代码中的 Ts 必须等于 PLL 的真实执行周期。
3. 启动时初始化估算角度
第一次读取到有效编码器角度时，建议执行：
cpp
pll.angle_est = angle_measured;
pll.speed_est = 0.0f;这样可以避免启动瞬间出现很大的角度误差和速度冲击。
4. 异常编码器数据不能更新 PLL
如果出现以下情况，不应该直接使用当前角度更新 PLL：
- 编码器 CRC 校验失败
- SPI 通信失败
- 编码器角度跳变超过合理范围
- 磁编码器数据无效
- 编码器掉线
5. 机械角度和电角度不能混用
如果 PLL 输入的是机械角度，输出首先得到的是机械角速度。
电角速度需要通过极对数换算：
cpp
电角速度 = 机械角速度 × 极对数6. 带宽不能设置过高
PLL 带宽应该明显低于采样频率。
带宽过高会导致：
- 速度噪声增大
- 编码器量化误差被放大
- 估算速度出现振荡
- 离散系统稳定性下降
十五、总结
编码器 PLL 的本质是建立一个估算角度，并让它不断跟踪编码器测量角度。
核心关系为：
角度误差：
cpp
error_theta = wrap(theta_meas - theta_est)估算速度变化率：
cpp
d(omega_est)/dt = k2 × error_theta估算角度变化率：
cpp
d(theta_est)/dt =
omega_est + k1 × error_theta其中：
- k1 支路根据当前角度误差，快速修正估算角度。
- k2 支路根据持续存在的角度误差，逐渐修正估算速度。
- 第一个积分器得到估算速度。
- 第二个积分器得到估算角度。
- 估算角度反馈后，继续与编码器测量角度比较。
一句话总结：
估算速度用于预测角度，角度误差用于修正估算角度和估算速度；k1 管快速跟随，k2 管长期速度。