伺服控制的负载转矩扰动观测器 (TL DOB) 简谈

余生皆假期-

402人浏览 · 2026-05-13 12:40:23

余生皆假期- · 2026-05-13 12:40:23 发布

一、电机转动方程

1.1 电机转动方程公式

$J \ddot{\theta} = K_t i_q + d$

J = 转动惯量 (kg·m²)
θ = 角位置 (rad)
$\dot{\theta}$ = 弧度每秒 (rad/s)
$\ddot{\theta}$ = 位置的二阶导数角加速度 (rad/s²)
Kt = 电机转矩常数 (N·m/A)
iq = q 轴电流 (A)
d = 未知扰动 (N·m)

描述的是电机轴的力学动态关系，本质上就是旋转形式的牛顿第二定律。

这条公式告诉我们，电机轴的角加速度 $\ddot{\theta}$ 是由两部分力矩共同决定的：

电机自己产生的控制转矩 $K_ti_q$
外部未知扰动 d (摩擦、负载、齿槽等)

用通俗话说：

想让电机快转 → 增大电流 iq
电机转动中遇到阻力 → 扰动 d 会减慢加速度 (比如 d 包含静摩擦，基本为常量；黏滞摩擦，它转速成正比)

因为等号左侧有是角位置的二阶导，所以这条公式也被称为：

电机 / 旋转机械的二阶动力学方程

1.2 电机扰动演示

伺服电机在运行时会受到各种未知干扰：

负载转矩 (搬东西)
摩擦力 (轴承、丝杆)
齿槽转矩 (电机结构固有)
重力 (垂直轴)

1.2.1 无扰动

恒定负载稳定输出在 80Nm：

1.2.2 等效齿槽转矩

加入一定周期和幅值的齿槽转矩扭矩输出：

1.2.3 齿槽转矩、偏心/重力负载、传感谐波

多重转矩负载输出：

二、DOB 基本思路

2.1 理想的控制策略

回顾电机转动方程公式公式，我们可以发现：

$J \ddot{\theta} = K_t i_q + d$

电机产生的净转矩 = 惯性引起的角加速度 + 外部扰动
$J \ddot{\theta}$ = 惯性产生的转矩
$K_t i_q$ = 控制产生的转矩
$d$ = 未知扰动 (齿槽转矩、传感器谐波、磁阻转矩波动等)

我们的目标是观测出未知扰动 d，其包括 (齿槽转矩、传感器谐波、磁阻转矩波动等)，从而让系统抵消未知扰动，输出效果：

$J \ddot{\theta} \approx K_t i_q$

2.2 DOB 的基本思路

使用系统模型预测电机输出：

$T_{model}=J\ddot{\theta}$

$T_{model}$ = 模型输出
$J \ddot{\theta}$ = 惯性产生的转矩

我们用预测的输出减去实际的输出，就得到了预估的扰动 d：

$\hat{d} = J \ddot{\theta} - K_t i_q$
$J \ddot{\theta}$ = 惯性产生的转矩
$K_t i_q$ = 控制产生的转矩
$\hat{d}$ = 预估的扰动

最后根据一些办法得到根据得到的扰动的周期规律，给 iq 输出做前馈控制抵消掉扰动。

除此之外，DOB 因为是前馈控制，对于突然变动的负载引起的转速 / 力矩突变也有较好的抑制效果。因为电流环 PI 的误差依赖积分，然而观测扰动却可以先于 PI 做前馈控制，从而在极端的情况下稳定住系统。

2.3 DOB 观测器概述

2.3.1 三阶观测器的状态空间模型

状态量	说明
x1	$\theta$ (两个周期的位移量，类似速度)
x2	$\dot{\theta}$ (速度)
x3	d (扰动)

于是系统可以写成：

$\dot{x_1}=x_2$

$\dot{x_1}$ = 的导数，速度
$x_2$ = 加速

$J\dot{x_2}=K{i_q}+x_3$

$\dot{x_2}$ = 角度的二阶导数角加速度 (rad/s²)
K = 电机转矩常数 (N·m/A)
iq = q 轴电流 (A)
$x_3$ = 扰动

2.3.2 构造 x1、x2、x3 状态方程

2.3.2.1 状态方程 x3

$\hat{x}_3 = \int [\omega_o^3 (x_1 - \hat{x}_1)] dt$

$\hat{x}_3$ = 当前的扰动值估计
$\omega_o$ = 观测增益系数
$(x_1 - \hat{x}_1)$ = 位移量观测误差
$x_1$ = 编码器位移量
$\hat{x}_1$ = 编码器位移量预估

2.3.2.2 状态方程 x2

这个 x2 预估公式根据电机转动方程公式 $J \ddot{\theta} = K_t i_q + d$ 变形得到。

$\hat{x}_2 = \int [3\omega_o^2 (x_1 - \hat{x}_1) + b_0 u + \hat{x}_3] dt$

$\hat{x}_2$ = 当前速度估计
$\omega_o$ = 观测增益系数
$(x_1 - \hat{x}_1)$ = 观测误差，测量误差减观测误差得到
$x_1$ = 编码器位移量
$\hat{x}_1$ = 编码器位移量预估
$b_0$ = 已知的电机控制增益，转矩系数除以转动惯量 (Kt/J)
$u$ = 控制输入指令，q 轴电流指令
$\hat{x}_3$ = 当前时刻的扰动值估计

2.3.2.1 状态方程 x1

位移量本质上就是速度，所以这里没有积分。

$\hat{x}_1 = \left( \hat{x}_2 + 3\omega_o(x_1 - \hat{x}_1) \right) dt$

$\hat{x}_1$ = 当前角度估计
$\hat{x}_2$ = 当前速度估计
$\omega_o$ = 观测增益系数
$(x_1 - \hat{x}_1)$ = 角度观测误差
$x_1$ = 角度
$\hat{x}_1$ = 角度位置

2.3.3 x1、x2、x3 的 ωo 的定义

因为我们在 DOB 中使用的 ESO 是三阶系统，所以在上面公式中所使用的观测增益系数 $\omega_o$ 系数不一样，其中三个状态有三个极点：

$(s+\omega_0)^3$

$s^3 + 3\omega_0 s^2 + 3\omega_0^2 s + \omega_0^3$

l1 = 3*w0;
l2 = 3*w0*w0;
l3 = w0*w0*w0;

角度、速度和扰动分别越容易被积分和误差干扰，所以就越使用更大的增益。

2.4 代码演示

将连续的积分离散后的代码如下：

/*
参数名             含义                              参数名             含义

pos_now_pulse      当前编码器累计脉冲值              iq_now_A           当前 q轴电流(A)

wn_Hz              DOB观测器带宽(Hz)                kt_j               Kt/J（转矩常数 / 转动惯量）

dt                 控制周期(s)                      enc_line_inv       编码器线数倒数

iq_max             最大允许电流(A)                  speed_max          最大允许速度(rad/s)
*/
void tl_dob_update(int64_t pos_now_pulse, float iq_now_A, float wn_Hz, float kt_j, float dt, float enc_line_inv, float iq_max, float speed_max)
{
    // 1. 编码器脉冲差 -> 角位移

    int64_t dp = pos_now_pulse - pos_last;
    pos_last = pos_now_pulse;

    float y = (float)dp * 2.0f * PI * enc_line_inv;

    // 2. 观测器带宽
    float w0 = 2.0f * PI * wn_Hz;

    float l1 = 3.0f * w0;
    float l2 = 3.0f * w0 * w0;
    float l3 = w0 * w0 * w0;

    // 3. 位置误差
    float err = y - dob_x1;

    // 4. 扰动观测 x3
    float x3_max = iq_max * kt_j;
    dob_x3 += l3 * err * dt;
    dob_x3 = clampf(dob_x3, -x3_max, +x3_max);

    // 5. 速度观测 x2
    float acc_est = kt_j * iq_now_A + dob_x3 + l2 * err;
    dob_x2 += acc_est * dt;
    dob_x2 = clampf(dob_x2, -speed_max, +speed_max);

    // 6. 下一拍位移预测 x1
    dob_x1 = (dob_x2 + l1 * err) * dt;
    dob_x1 = clampf(dob_x1, -PI, +PI);

    // 7. 输出
    ev_rad_s = dob_x2;
    iq_com_A = dob_x3 / kt_j;
}