机器人关节电流环怎么去噪：LMS与NLMS自适应滤波Q15定点七步讲透

调试 机器人 关节 FOC 电流环 时最常见的现象是：一阶滞后把电流波形抹平了，力矩响应也跟着迟钝；滑动平均把开关纹波滤掉了，突变负载时估计值却慢半拍。根子在于固定系数滤波器用一组"死"参数对抗一组"活"干扰。结论先给三条：第一，去噪问题的理论最优解是维纳滤波器——最小均方误差（MMSE）意义下的线性滤波，但它需要信号与噪声的统计先验，现场拿不到；第二，LMS 算法用随机梯度下降在线逼近维纳解，不需要先验，系数随工况自动调整，是工程上唯一能在关节里跑起来的"最优滤波"；第三，MCU 上落地用 NLMS 归一化版本加 Q15 定点化，16 抽头每样本约 48 次乘加，200MHz 主频下负载不足 1%。下面按噪声来源、理论根基、算法参数、定点实现、验证与落地七步讲透。

一、关节电流环的噪声从哪来

先看清对手。关节模组里电流采样信号链是：采样电阻 → 差分放大 → ADC → 电流环。这条链上的干扰按《数字信号处理与噪声分析》（Vaseghi）第二章的噪声分类可以对号入座：逆变器 PWM 开关动作通过功率地与信号地的公共阻抗耦合进采样回路，属于典型的电磁噪声；MOSFET 开关瞬间的大电流跳变叠加在线路阻抗上产生阶梯状纹波；放大器与 ADC 自身引入宽带热噪声与量化噪声；编码器线缆与电机动力线捆扎走线时还有容性/感性耦合。建立模型时，工程上把这些干扰近似为加性噪声——观测信号 y = x + n，其中 x 是真实电流分量，n 是噪声。噪声统计特性平稳时，固定滤波器尚可应付；但关节转速一变，PWM 谐波的位置与幅度全跟着变，噪声变成非平稳过程，固定滤波器就失效了。

二、理论根基：从维纳滤波到 LMS

维纳滤波器回答了"最优线性滤波长什么样"：在向量空间里，把期望信号投影到观测信号张成的子空间，得到的滤波器使均方误差最小——Vaseghi 书第六章给出其向量空间投影解释与最小均方误差分析，并给出加性噪声去除、信道均衡、多传感器时间对齐等应用。维纳解的形式是 w = R⁻¹p，其中 R 是输入自相关矩阵、p 是输入与期望信号的互相关向量。问题恰恰在这里：算 R 和 p 需要信号与噪声的统计先验，而关节的噪声统计随转速、负载、温度漂移，先验永远过时。

LMS（最小均方）算法的聪明之处是用"走一步看一步"的随机梯度下降绕开先验：系数沿瞬时误差梯度的反方向更新，w(n+1) = w(n) + 2μ·e(n)·x(n)。它不需要知道 R 和 p，误差本身会告诉系数该往哪调——干扰大的频段系数自动凹陷，形成自适应陷波；工况变了，系数跟着挪。代价是收敛慢于牛顿类算法，且稳态存在失调（misadjustment）。NLMS（归一化 LMS）在 LMS 基础上把步长除以输入能量，一次解决输入幅度波动导致的收敛速度不稳定问题，是工程实现的首选。

三、算法参数：一张表定 LMS 与 NLMS

两族算法的递推结构与参数意义对比如下，选型只看三行：先验需求、稳态失调、计算复杂度。

对比项	固定系数滤波（一阶滞后/滑动平均）	维纳滤波	LMS / NLMS
系数来源	设计期固定	离线计算 w=R⁻¹p	在线递推，随误差更新
统计先验	不需要	需要 R、p 全部统计	不需要，误差即反馈
非平稳跟踪	无能力	统计变了就要重算	自动跟踪工况漂移
稳态误差	滞后与纹波不可兼得	MMSE 理论下界	接近维纳解，存在失调
每样本运算量	1~N 次加法	矩阵求逆 O(M³)	约 2~3M 次乘加（M 抽头）
风险	响应变慢	先验失配即失效	步长过大发散

LMS 与 NLMS 的关键参数直接决定成败：

参数	LMS	NLMS	工程取值
更新式	w(n+1)=w(n)+2μ·e(n)·x(n)	w(n+1)=w(n)+μ·e(n)·x(n)/(δ+‖x(n)‖²)	δ 取 0.001~0.01 防除零
收敛条件	0 < μ < 2/(M·Px)，Px 为输入平均功率	0 < μ < 2，与输入能量无关	NLMS 取 0.05~0.5
收敛速度	随输入能量波动	归一化后近似恒定	16 抽头几十到几百个样本收敛
稳态失调	与 μ 成正比	与 μ 成正比	收敛快与失调小是跷跷板，现场折中

四、Q15 定点实现：16 抽头 NLMS

关节 MCU 没有浮点也要跑得动。以下 Q15 定点 NLMS 参考实现（16 抽头，fs=16kHz 与 PWM 同步采样），每次电流环中断调用一次：

/* Q15 定点 NLMS 自适应滤波器：关节电流环去噪
   fs = 16kHz（PWM 同步采样），M = 16 抽头
   每样本 ≈ 3M 次乘加 ≈ 48 MAC，200MHz 主频下占用 < 1% */
#include <stdint.h>

#define NLMS_M     16        /* 抽头数 */
#define Q15_ONE    32767
#define NLMS_MU    24576     /* 步长 0.75 (Q15)，现场按口诀扫描后回退 */
#define NLMS_DELTA 64        /* δ 防除零，Q15 域 */

typedef struct {
    int16_t  w[NLMS_M];      /* 滤波器系数 Q15 */
    int16_t  x[NLMS_M];      /* 输入历史环形缓冲 */
    int32_t  px2;            /* 输入能量估计 ‖x‖² 的慢速估计 */
    uint8_t  head;
} nlms_t;

static inline int16_t sat_q15(int32_t v)
{
    if (v >  32767) return  32767;   /* 饱和防溢出：系数漂移的兜底 */
    if (v < -32768) return -32768;
    return (int16_t)v;
}

int16_t nlms_run(nlms_t *f, int16_t xin, int16_t d)
{
    f->x[f->head] = xin;

    /* 1) 滤波输出 y = w^T x（Q30 累加后右移 15） */
    int32_t acc = 0;
    for (int i = 0; i < NLMS_M; i++) {
        int16_t xv = f->x[(f->head + 1 + i) & (NLMS_M - 1)];
        acc += (int32_t)f->w[i] * xv;         /* Q15*Q15=Q30 */
    }
    int16_t y = (int16_t)(acc >> 15);

    /* 2) 误差 e = d - y，d 为期望信号（如相邻两相信号的差分估计） */
    int16_t e = sat_q15((int32_t)d - y);

    /* 3) 输入能量慢速估计，一阶 IIR：P = 0.9P + 0.1·x² */
    f->px2 += (((int32_t)xin * xin) - f->px2) / 10;
    if (f->px2 < NLMS_DELTA) f->px2 = NLMS_DELTA;

    /* 4) 归一化步长 mu_n = MU * e / P，Q15 域用移位实现除法 */
    int32_t mu_norm = (int32_t)((int64_t)NLMS_MU * e /
                                (f->px2 >> 4));

    /* 5) 系数更新 w += mu_norm · x，逐项饱和 */
    for (int i = 0; i < NLMS_M; i++) {
        int16_t xv = f->x[(f->head + 1 + i) & (NLMS_M - 1)];
        int32_t dw = (int32_t)((int64_t)mu_norm * xv >> 15);
        f->w[i] = sat_q15((int32_t)f->w[i] + dw);
    }

    f->head = (uint8_t)((f->head + 1) & (NLMS_M - 1));
    return y;                                  /* 滤波后的电流值 */
}

五、验证与七步落地

自适应滤波器"跑通"和"能用"是两回事，验证抓三个指标：噪声抑制量（滤波前后误差功率之比，dB）、力矩阶跃响应延迟（与固定滤波器对比）、系数稳定性（μ 更新量的稳态方差）。把流程收拢成七步：

步骤	动作	判据 / 参考值
1　建模型	按 y=x+n 建加性噪声模型，记录各工况噪声谱	静止/匀速/变速三工况各录一组
2　定参考	选定期望信号 d：干净参考、延迟线预测或相邻相估计	参考与噪声相关、与信号不相关
3　定抽头	M 由需要抵消的干扰最长时延差决定	M=8~32 起步，MAC 预算内取值
4　扫步长	μ 从小到大扫描，观察收敛与系数漂移	NLMS μ=0.05~0.5，振铃即回退
5　定点化	Q15 实现，能量估计用一阶 IIR，逐项饱和	无溢出告警，系数稳态不贴满量程
6　验指标	噪声抑制 dB、力矩阶跃延迟、带宽不缩水	抑制 ≥10dB 且电流环带宽下降 <5%
7　留监测	运行时输出误差功率与 μ 有效值	发散趋势（误差持续增大）即复位系数

本文基于《数字信号处理与噪声分析》（Saeed V. Vaseghi，Advanced Digital Signal Processing and Noise Reduction, 2nd Edition，John Wiley & Sons, 2000）中噪声建模与维纳滤波章节等技术资料整理优化，并结合机器人关节电流环调试的工程实践进行了系统化梳理。