MIT模式中的惯性项求解

一、τff项解释

τff 在 MIT 模式中只是一个控制输入端口，驱动器不关心往里填了什么。它收到一个数值，就把它作为力矩前馈加到 PD 输出上，在工程实现上，τff的内容是一个函数：

针对低速清洁任务：

• τ_gravity：必须做。
• τ_friction：强烈建议做（低速时摩擦占比大）。
• τ_inertia：可以不做（设0），PD控制器会自然产生加速力矩。

累加的实现方式，比如每个控制周期（如 2ms）执行：

tau_ff = (
    gravity_compensation(q_actual)          # 只依赖角度
    + friction_compensation(dq_actual)       # 只依赖速度
    + inertia_compensation(q_actual, ddq_des) # 只依赖加速度（可选）
)
send_mit_command(q_des, dq_des, tau_ff)

三个函数是独立计算、然后累加，它们的输入不同、整定方式不同、触发条件也不同：

二、重力项和摩擦项的获取

前面介绍了通过实验获得了这两项：

• 重力项：在不同角度实测悬浮力矩 → 得到 τg(q) 的查表或插值函数。
• 摩擦项：在不同恒定速度实测平均力矩 → 线性拟合得到 fc 和 fv。

使用方式：

def gravity_compensation(q):
    # 使用查表+线性插值，或多项式拟合
    return interp(q, q_table, tau_g_table)  # 或 polyval(g_coeffs, q)

def friction_compensation(dq):
    # 平滑sign函数避免零速跳变
    return fc * tanh(dq / eps) + fv * dq

三、惯性项获取

3.1 惯性项

当关节加速时，连杆的惯量会产生一个抵抗加速的力矩：

其中 Jeff(q)是关节的有效转动惯量，随关节角度变化（因为质心到关节轴的距离变化）。

3.2 什么时候需要

对于清洁场景，运动速度低、加速度小，惯性项可以暂时不补偿，PD控制器本身的 Kp就能产生足够的加速力矩。

3.3 获取惯性项

有三种方法，按推荐度排序：

方法A：从CAD模型计算（最方便）

如果模型可用，直接从CAD读取连杆质量 m、质心位置、惯性张量，通过正运动学计算任意姿态下的有效惯量 Jeff(q)，然后用以下公式：

其中 q¨从上位机的轨迹规划器获得（不是从编码器差分，因为差分噪声大）。

方法B：动态辨识实验（无需CAD）

原理：在已知加速度下测力矩，反解有效惯量。

步骤：

1. 补偿好重力和摩擦（保证静止悬浮）。
2. 让关节做已知的正弦加速运动，比如 q=Asin⁡(ωt)，其中 ω 选在 1~5 Hz。
3. 记录运动中的力矩输出 τcmd。
4. 由于

，减去已知的重力和摩擦补偿，剩下的部分就是 τ_inertia。

1. 计算加速度 

1. 用最小二乘拟合：

优点：完全实测，不需要CAD。

缺点：加速度信号如果来自编码器差分，噪声大，需要良好滤波。在慢速清洁场景中，惯性项小，辨识精度有限。

方法C：经验公式近似

对于纯旋转关节，如果把连杆简化为一个集中质量 m 位于质心距离 d 处：

可以从重力补偿标定中反向估算出 m⋅d（因为 τg=mgdcos⁡(q)），进而得到：

这种方法精度不高，但对于判断惯性项是否可以忽略非常有用。