4. 惯量实时辨识与补偿

任务定义

在张力控制系统中，当卷径变化或物料规格切换时，负载惯量随之改变，导致速度环动态响应失配，加减速阶段产生张力波动。本案例实现一个基于递推最小二乘法（RLS）的惯量实时辨识器，并将辨识结果用于前馈转矩补偿，使速度环在惯量变化时仍能保持一致的动态响应。

惯量辨识原理与实现

采用电机运动方程作为辨识模型：

Te - TL = J · dω/dt

其中Te为电磁转矩，TL为负载转矩（含摩擦），J为转动惯量。将方程离散化并整理为最小二乘形式：

y(k) = φT(k) · θ

其中y(k) = Te(k) - TL(k)，φ(k) = [ω(k) - ω(k-1)] / Ts，θ = J。实际中TL未知，将其作为待辨识参数之一，扩展状态向量：

y(k) = [Δω/Ts, 1] · [J, TL]T

其中Δω = ω(k) - ω(k-1)。RLS算法在线更新参数估计值。

// rls_inertia_estimator.h
#ifndef RLS_INERTIA_ESTIMATOR_H
#define RLS_INERTIA_ESTIMATOR_H

typedef struct {
    float theta[2];      // [0]=J估计, [1]=TL估计
    float P[4];          // 2x2协方差矩阵, 按行存储
    float lambda;        // 遗忘因子(0.95~0.99)
    float Ts;            // 采样周期(s)
    float w_prev;        // 上一次速度(rad/s)
    float Te_prev;       // 上一次电磁转矩(N·m)
    int initialized;
} RLS_Estimator;

void RLS_Init(RLS_Estimator *est, float Ts, float lambda);
void RLS_Reset(RLS_Estimator *est, float J_init, float TL_init);
float RLS_Update(RLS_Estimator *est, float w, float Te);

#endif

// rls_inertia_estimator.c
#include "rls_inertia_estimator.h"

void RLS_Init(RLS_Estimator *est, float Ts, float lambda) {
    est->Ts = Ts;
    est->lambda = lambda;
    est->initialized = 0;
}

void RLS_Reset(RLS_Estimator *est, float J_init, float TL_init) {
    est->theta[0] = J_init;
    est->theta[1] = TL_init;
    // 初始协方差: 对角阵, 对角元素取较大值以加速收敛
    est->P[0] = 100.0f;
    est->P[1] = 0.0f;
    est->P[2] = 0.0f;
    est->P[3] = 100.0f;
    est->w_prev = 0.0f;
    est->Te_prev = 0.0f;
    est->initialized = 1;
}

float RLS_Update(RLS_Estimator *est, float w, float Te) {
    if (!est->initialized) {
        RLS_Reset(est, 0.001f, 0.0f);
        est->w_prev = w;
        est->Te_prev = Te;
        return est->theta[0];
    }

    float dw = w - est->w_prev;
    float acc = dw / est->Ts;       // 角加速度
    float phi[2] = {acc, 1.0f};     // 回归向量

    // 计算增益 K = P*phi / (lambda + phi^T*P*phi)
    float Pphi[2];
    Pphi[0] = est->P[0] * phi[0] + est->P[1] * phi[1];
    Pphi[1] = est->P[2] * phi[0] + est->P[3] * phi[1];

    float denom = est->lambda + phi[0] * Pphi[0] + phi[1] * Pphi[1];
    float K[2];
    K[0] = Pphi[0] / denom;
    K[1] = Pphi[1] / denom;

    // 预测误差
    float y = Te - est->Te_prev;    // 转矩差分消除稳态负载
    float y_pred = phi[0] * est->theta[0] + phi[1] * est->theta[1];
    float err = y - y_pred;

    // 更新参数
    est->theta[0] += K[0] * err;
    est->theta[1] += K[1] * err;

    // 更新协方差 P = (I - K*phi^T) * P / lambda
    float P_new[4];
    float IKP[4];
    // I - K*phi^T
    IKP[0] = 1.0f - K[0] * phi[0];
    IKP[1] = -K[0] * phi[1];
    IKP[2] = -K[1] * phi[0];
    IKP[3] = 1.0f - K[1] * phi[1];

    // (I-K*phi^T) * P
    float temp[4];
    temp[0] = IKP[0] * est->P[0] + IKP[1] * est->P[2];
    temp[1] = IKP[0] * est->P[1] + IKP[1] * est->P[3];
    temp[2] = IKP[2] * est->P[0] + IKP[3] * est->P[2];
    temp[3] = IKP[2] * est->P[1] + IKP[3] * est->P[3];

    P_new[0] = temp[0] / est->lambda;
    P_new[1] = temp[1] / est->lambda;
    P_new[2] = temp[2] / est->lambda;
    P_new[3] = temp[3] / est->lambda;

    // 对称化处理
    float avg01 = (P_new[1] + P_new[2]) * 0.5f;
    est->P[0] = P_new[0];
    est->P[1] = avg01;
    est->P[2] = avg01;
    est->P[3] = P_new[3];

    // 惯量下限保护
    if (est->theta[0] < 0.0001f) {
        est->theta[0] = 0.0001f;
    }

    est->w_prev = w;
    est->Te_prev = Te;
    return est->theta[0];
}

转矩补偿与速度环集成

辨识得到的惯量估计值J_hat用于计算加速度前馈转矩：

// speed_controller_with_compensation.c
#include "rls_inertia_estimator.h"

// 速度环PI参数(基于标称惯量Jn整定)
#define Jn_NOMINAL   0.002f   // 标称惯量 kg·m²
#define KP_SPEED     0.15f    // 速度环比例增益
#define KI_SPEED     0.02f    // 速度环积分增益
#define TS_CONTROL   0.001f   // 控制周期 1ms

typedef struct {
    RLS_Estimator rls;
    float J_hat;
    float w_ref_prev;
    float integrator;
    float speed_ref;
} SpeedCtrl;

void SpeedCtrl_Init(SpeedCtrl *sc) {
    RLS_Init(&sc->rls, TS_CONTROL, 0.97f);
    RLS_Reset(&sc->rls, Jn_NOMINAL, 0.0f);
    sc->J_hat = Jn_NOMINAL;
    sc->w_ref_prev = 0.0f;
    sc->integrator = 0.0f;
    sc->speed_ref = 0.0f;
}

// 主控制函数: 输入速度反馈w和电磁转矩Te, 输出转矩指令
float SpeedCtrl_Step(SpeedCtrl *sc, float w_ref, float w_fb, float Te) {
    // 1. 惯量辨识(仅在加减速时有效)
    float dw_ref = w_ref - sc->w_ref_prev;
    if (dw_ref > 0.5f || dw_ref < -0.5f) {  // 速度变化超过阈值才更新
        sc->J_hat = RLS_Update(&sc->rls, w_fb, Te);
    }
    sc->w_ref_prev = w_ref;

    // 2. 速度误差PI控制
    float err = w_ref - w_fb;
    float P_out = KP_SPEED * err;
    sc->integrator += KI_SPEED * err * TS_CONTROL;
    // 积分限幅
    if (sc->integrator > 5.0f) sc->integrator = 5.0f;
    if (sc->integrator < -5.0f) sc->integrator = -5.0f;

    // 3. 加速度前馈补偿(使用辨识惯量)
    float acc_ref = (w_ref - sc->w_ref_prev) / TS_CONTROL;
    float ff_torque = sc->J_hat * acc_ref;

    // 4. 总转矩指令
    float torque_cmd = P_out + sc->integrator + ff_torque;
    return torque_cmd;
}

仿真验证

以下代码模拟惯量从0.002突变为0.006的场景，对比有无补偿时的速度跟踪误差：

// simulation_test.c
#include <stdio.h>
#include <math.h>
#include "speed_controller_with_compensation.c"

#define SIM_TIME   2.0f    // 仿真时长(秒)
#define STEP_MS    1.0f    // 步长(ms)

typedef struct {
    float J_actual;
    float w;
    float Te;
} Plant;

void Plant_Step(Plant *p, float torque_cmd, float dt) {
    // 模拟实际电机: J_actual * dw/dt = torque_cmd - 摩擦(忽略)
    float dw = (torque_cmd) / p->J_actual * dt;
    p->w += dw;
    p->Te = torque_cmd;  // 反馈电磁转矩
}

int main() {
    SpeedCtrl sc;
    SpeedCtrl_Init(&sc);

    Plant plant;
    plant.J_actual = 0.002f;
    plant.w = 0.0f;
    plant.Te = 0.0f;

    float t = 0.0f;
    float w_ref = 0.0f;
    float dt = STEP_MS / 1000.0f;

    // 记录最大跟踪误差
    float max_err = 0.0f;
    float max_err_nocomp = 0.0f;

    // 无补偿对照组(固定惯量)
    SpeedCtrl sc_nc;
    SpeedCtrl_Init(&sc_nc);
    sc_nc.J_hat = Jn_NOMINAL;  // 固定不更新

    Plant plant_nc;
    plant_nc.J_actual = 0.002f;
    plant_nc.w = 0.0f;
    plant_nc.Te = 0.0f;

    int step = 0;
    while (t < SIM_TIME) {
        // 速度曲线: 0~0.5s加速到100rad/s, 0.5~1.0s匀速, 1.0~1.5s减速到0
        if (t < 0.5f) w_ref = 200.0f * t;
        else if (t < 1.0f) w_ref = 100.0f;
        else if (t < 1.5f) w_ref = 100.0f - 200.0f * (t - 1.0f);
        else w_ref = 0.0f;

        // 0.8s时惯量突变
        if (fabs(t - 0.8f) < dt/2) {
            plant.J_actual = 0.006f;
            plant_nc.J_actual = 0.006f;
        }

        // 有补偿控制
        float torque_cmd = SpeedCtrl_Step(&sc, w_ref, plant.w, plant.Te);
        Plant_Step(&plant, torque_cmd, dt);

        // 无补偿控制
        float torque_cmd_nc = SpeedCtrl_Step(&sc_nc, w_ref, plant_nc.w, plant_nc.Te);
        Plant_Step(&plant_nc, torque_cmd_nc, dt);

        // 记录误差
        float err = fabs(w_ref - plant.w);
        float err_nc = fabs(w_ref - plant_nc.w);
        if (err > max_err) max_err = err;
        if (err_nc > max_err_nocomp) max_err_nocomp = err_nc;

        // 打印关键点数据
        if (step % 500 == 0) {
            printf("t=%.2fs J_hat=%.5f w_ref=%.1f w=%.1f err=%.3f | w_nc=%.1f err_nc=%.3f\n",
                   t, sc.J_hat, w_ref, plant.w, err, plant_nc.w, err_nc);
        }

        t += dt;
        step++;
    }

    printf("\n=== 结果 ===\n");
    printf("有补偿最大误差: %.3f rad/s\n", max_err);
    printf("无补偿最大误差: %.3f rad/s\n", max_err_nocomp);
    printf("最终惯量辨识值: %.5f kg·m² (实际0.006)\n", sc.J_hat);
    return 0;
}

编译与运行

gcc -o sim_test simulation_test.c -lm
./sim_test

验证方法与判定标准

运行仿真程序后，观察输出数据：

  1. 惯量辨识收敛：在0.8s惯量突变后，J_hat应在约0.3~0.5s内从0.002逐渐逼近0.006，最终稳态值在0.005~0.007范围内（遗忘因子0.97下辨识精度约±10%）；
  2. 补偿效果对比：惯量突变后的加减速阶段（0.8s~1.5s），有补偿的最大速度跟踪误差应明显小于无补偿的误差，预期有补偿误差不超过无补偿的50%；
  3. 系统稳定性：整个仿真过程中速度响应无振荡发散，辨识值无负值或异常跳变。
惯量辨识与转矩补偿系统框图 ω_ref Σ − 速度PI Kp, Ki + T_e* 电机+负载 J_actual 编码器 ω_fb T_e (反馈) RLS辨识器 J_hat, T_L 前馈计算 J_hat × a_ref 加速度计算 dω_ref/dt 信号说明 转矩/速度主回路 速度反馈 转矩反馈(辨识输入) 前馈补偿路径 参考速度(加速度源)

关键参数说明

参数值说明
遗忘因子 λ0.97越小跟踪越快，但噪声敏感度增加
采样周期 Ts1ms与控制周期一致
初始协方差 P₀100较大值加速初始收敛
惯量辨识触发阈值0.5 rad/s速度变化过小时不更新，避免噪声累积
惯量下限保护0.0001 kg·m²防止辨识值非物理

判定标准：仿真输出中，惯量突变后辨识值收敛至实际值的±10%以内，且补偿后的最大速度误差相比无补偿降低至少50%，即认为本案例实现有效。若收敛过慢，可适当减小遗忘因子至0.95；若噪声过大导致辨识波动，则增大遗忘因子至0.99。

← 上一章 📖 返回目录 下一章 →