从滤波延迟到零滞后跟踪：使用龙伯格观测器拯救国产编码器

在电机控制（FOC）领域，我们经常会陷入一个令人头疼的工程死锁：“带宽与相位滞后的权衡陷阱”。当你为了控制成本使用了一款高频噪声极大的国产编码器时，直接对位置求导得到的速度信号往往充满了令人崩溃的毛刺。

传统的做法是加一个极重的低通滤波器（LPF）。毛刺确实没了，但随之而来的是严重的 相位滞后（延迟） 。一旦电机进行高动态的加减速，控制器就会“慢半拍”，轻则伺服刚性大幅下降，重则直接引发系统高频震荡。

那么，有没有一种方法能够 “既无延迟，又能完美去噪” 呢？为了解答这个问题，我们需要跟着经典控制理论的脚步，看看工程师们是如何一步步被逼出“龙伯格观测器（Luenberger Observer）”这个终极杀器的。

---

1. 消除传感器影响的两次“失败”尝试

在理想的控制系统中，传感器应该是完美无瑕的，即传递函数 $G_S(s)=1$。但现实中，编码器不仅有延迟，还有严重的量化噪声和高频干扰（$G_S(s)\ne 1$）。

为了消除这层影响，初学者或早期的工程师通常会尝试以下两种极其自然、但最终证明行不通的方案：

尝试一：硬刚物理——串联逆向滤波器

既然传感器是一个带延迟的低通滤波器，那我直接在软件里给它乘上一个“逆向函数” $G_{SEst}^{-1}(s)$ 不就行了？

• 理论上：可以完美抵消延迟。
• 现实中：低通滤波器的逆运算通常包含微分。众所周知，微分操作是高频噪声的“放大器”。国产编码器本来就充满量化噪声和 EMI 干扰，一旦经过微分，系统会被巨大的高频噪声瞬间淹没。
• 结论：完全依赖传感器。在低频段（稳态）运行良好，但在高频段（动态加减速）会产生无法控制的巨大噪声。

尝试二：抛弃物理——纯软件开环预测

既然传感器高频噪声那么大，那我干脆不用传感器参与高频计算了！我直接在微控制器里写一个电机的数学模型 $G_{PEst}(s)$。我给电机发了多少电流指令，就同时发给我的代码，让代码算出电机“应该”跑多快。

• 理论上：纯数学计算，绝对没有任何噪声，且绝对零延迟。
• 现实中：任何电机的物理模型都不可能 100% 准确，且物理世界总是存在摩擦力等不可测的扰动。由于模型计算速度和位置需要不断积分，即使只有 0.1% 的微小误差，随着时间的推移，积分器也会让软件里的假想位置和真实电机的实际位置偏离十万八千里。
• 结论：完全依赖数学模型。在高频段（瞬态响应）极其完美，但在低频段（稳态）会产生致命的积分漂移。

---

2. 龙伯格观测器：完美融合两者的绝妙设计

如果你仔细对比上面两种失败方案，会发现它们的优缺点恰好是互补的！
那么，能不能把它们结合起来？这就诞生了龙伯格观测器。

它的核心思想是：高频看模型，低频看传感器。

1. 建立平行世界（前馈通道）：在控制器内部，我们依然运行那个纯数学的电机模型。当给电机下发电流（转矩）时，模型在同一微秒内瞬间算出理论加速度并积分出速度。这保证了系统面对突变指令时，具有毫无延迟、毫无噪声的高频动态响应。
2. 引入补偿器（闭环纠偏）：为了解决“积分漂移”的痛点，我们终于让传感器出场了。我们将真实传感器测得的带噪位置，减去我们模型预测的位置，得到一个观测误差 $E_O(s)$。
3. 高增益 PI 融合：把这个误差送入一个 PI 调节器（观测器补偿器 $G_{CO}(s)$）。如果模型跑偏了，PI 调节器就会温柔地把模型拉回正轨。因为 PI 调节器本质上是一个低通滤波器，它完美地把传感器的高频毛刺挡在了门外，只利用传感器的低频平均值来修正模型。

---

3. 数学推导：从连续物理世界到离散代码

在连续时间域（$t$）中，一个标准的永磁同步电机机械轴转动方程为：
$$\frac{d\omega }{dt}=\frac{K_t\cdot i_q-T_L}{J}$$
其中 $\omega$ 是电机转速，$i_q$ 是 $q$ 轴电流，$K_t$ 是转矩常数，$J$ 是转动惯量，$T_L$ 是负载扰动。

我们的龙伯格观测器构建了一个一模一样的数学模型，并引入误差 PI 补偿：
$$\frac{d\hat{\omega }}{dt}=\underset{\text{纯模型前馈 (高频零延迟)}}{\underbrace{\frac{K_t}{J}\cdot i_q}}+\underset{\text{传感器误差修正 (低频防漂移)}}{\underbrace{K_i\cdot (\theta -\hat{\theta })}}$$

为了在微控制器（MCU）中运行，我们设定采样周期为 $T_s$，使用欧拉向前法（Euler Forward）进行一阶离散化，移项后得到了代码中实际运行的速度迭代公式：
$$\hat{\omega }(k+1)=\hat{\omega }(k)+T_s\cdot \left(\frac{K_t}{J}\cdot i_q(k)\right)+T_s\cdot K_i\cdot (\theta (k)-\hat{\theta }(k))$$

---

4. C 语言代码落地与映射

将上述离散化公式落实到 MCU 代码中，我们需要处理好非线性的角度卷绕（Rollover），并将 PI 补偿器注入到状态积分中。核心代码片段如下：

/* ========================================================================= */
/* RUN LUENBERGER OBSERVER                                                   */
/* ========================================================================= */

// 1. 获取当前 q 轴反馈电流 iq(k)
float iq_current = motor.current_q; 

// 2. 计算物理模型理论加速度: accel = (Kt / J) * iq
//    注: motor.Kt_counts_per_A_sec2 是已折算至编码器计数值单位的系数
float accel_from_model = iq_current * motor.Kt_counts_per_A_sec2; 

// 3. 预测当前位置状态: θ_hat(k) = θ_hat(k-1) + Ts * ω_hat(k)
encoder->pos_cpr_counts += CURRENT_MEASURE_PERIOD * encoder->vel_estimate_counts;

// 4. 计算位置观测误差: delta_pos = θ(k) - θ_hat(k) (包含过零处理)
float delta_pos_cpr_counts = (float)(encoder->count_in_cpr - (int)encoder->pos_cpr_counts);
while(delta_pos_cpr_counts > +ENC_CPR_DIV){delta_pos_cpr_counts -= ENC_CPR_F;}
while(delta_pos_cpr_counts < -ENC_CPR_DIV){delta_pos_cpr_counts += ENC_CPR_F;}

// 5. 反馈修正项 (补偿器): Ts * Ki * (θ(k) - θ_hat(k))
float observer_error_integral_term = CURRENT_MEASURE_PERIOD * encoder->pll_ki * delta_pos_cpr_counts;

// 6. 模型前馈项: Ts * (Kt / J) * iq(k)
float observer_feedforward_term = CURRENT_MEASURE_PERIOD * accel_from_model;

// 7. 更新速度估计值: ω_hat(k+1) = ω_hat(k) + 前馈 + 反馈
encoder->vel_estimate_counts += (observer_feedforward_term + observer_error_integral_term);

// 8. 闭环修正位置状态，防止积分漂移
encoder->pos_cpr_counts += CURRENT_MEASURE_PERIOD * encoder->pll_kp * delta_pos_cpr_counts;
while(encoder->pos_cpr_counts > ENC_CPR){encoder->pos_cpr_counts -= ENC_CPR_F;}
while(encoder->pos_cpr_counts < 0){encoder->pos_cpr_counts += ENC_CPR_F;}

---

5. 调参哲学的转变：为何带宽可以骤降？

如果你之前使用过纯粹的位置跟踪锁相环（PLL），你会发现计算 PI 增益的公式是完全一样的：$K_p=2\zeta {\omega }_n$，$K_i={\omega }_n^2$。

但是，加入物理前馈后，参数设定的逻辑发生了断崖式的改变。

在纯 PLL 中，电机加速产生的所有速度变化，都必须先转化为位置误差，再由 PI 硬生生逼出来。为了跟上动态，你被迫把带宽（${\omega }_n$）设得极高，结果国产编码器的高频噪声被原封不动地放大了。

而在龙伯格观测器中，速度的动态变化由物理模型（前馈通道）包揽了。PI 调节器不再负责追踪高频动态，它现在只有一个任务：修正低频漂移。

因此，你完全可以将带宽降低到原来的 1/10 甚至更低。带宽越低，滤波器对编码器噪声的压制能力就越恐怖；而得益于前馈的存在，系统依然能保持极佳的动态响应，真正做到了“又静又快”。

---

6. 工程实践避坑：惯量失配 ($J$ 的动态变化)

在机器人应用中，负载惯量随着机械臂姿态的变化而剧烈改变。如果你在代码中仅仅填写了电机手册上的“转子惯量”，会导致前馈加速度远大于实际加速度（模型过于自信）。这会引发严重的超调和系统震荡。

工程调参法则：宁可“高估”，不要“低估”。

1. 大减速比关节（如谐波减速器）：折算到电机轴端的负载惯量极小，直接使用转子惯量 $J_{rotor}$ 通常即可稳定运行。
2. 直驱 / DD 马达：必须加上负载折算惯量。如果算不准，凭直觉把手册上的转子惯量乘以 2 到 3 倍填入模型。虽然这会牺牲微乎其微的动态前馈比例，让 PI 多承担一点修正工作，但能换取极强的稳定性底线。
3. 极致性能要求：由上位机实时计算机器人动力学矩阵，并将动态的 $J$ 下发给驱动器更新观测器系数。

结语

从传统的信号滤波跨越到状态观测器，本质上是从“纯信号域”走向“物理域”的思想转变。这不仅仅是填补低端传感器缺陷的妥协之举，更是迈向高性能伺服控制的必经之路。