OpenArmX 前馈补偿 KDL + DOB 实现解析

用 KDL 先补掉已知重力，再用基于动量残差的 DOB 补偿未知摩擦、外扰和模型误差。

控制基线

OpenArmX 的第一版柔顺控制不是直接上复杂外环，而是先建立一个稳定的 MIT PD base loop。每个关节接收 q_des、dq_des、tau_ff、kp、kd 五元组，底层根据位置误差、速度误差和前馈力矩生成最终命令。

这个基线的意义是把平台边界固定下来：位置跟踪由 PD 完成，已知模型项进入 tau_ff，未知扰动由估计器补偿，后续阻抗或导纳控制也能复用同一组接口。

重力是机械臂最稳定、最可建模的一类扰动。用 KDL 根据 URDF、关节位置和重力方向计算每个关节的重力力矩，再写入 tau_ff，可以先把主要静态负载从 PD 环里移走。

这样做有两个好处：一是降低 kp/kd 对抗重力所需的强度，让关节在低刚度下也能保持位置；二是给 DOB 留出更清晰的残差空间。DOB 不应该重复估计已经能由模型解释的重力，否则容易把模型误差、摩擦和噪声混在一起。

DOB 用来处理 KDL 难以覆盖的部分，例如摩擦、外部扰动、负载变化、传动误差和模型不准确。实现上可以基于动量残差估计扰动力矩，再经过低通滤波和限幅后叠加到前馈补偿中。

工程上要特别注意目标附近的抖动。接近目标时位置误差很小，速度估计和静摩擦占比变高，DOB 输出容易在正负方向切换。我的处理思路是保留 KDL 重力补偿为主前馈项，对 DOB 加低速死区、输出限幅和滤波，必要时只在误差或外力超过阈值后启用。

最终控制可以理解为：

tau_cmd = kp * (q_des - q)
        + kd * (dq_des - dq)
        + tau_gravity_kdl
        + tau_disturbance_dob

其中 tau_gravity_kdl 负责已知模型补偿，tau_disturbance_dob 负责剩余误差。二者都不直接绕过 ros2_control 写硬件，而是由控制器 runtime 汇总后统一写入 MIT 五元组接口。

第一步只开 MIT PD，确认位置跟踪稳定、命令方向正确、关节限幅生效。第二步加入 KDL 重力补偿，观察静止姿态下的跟踪误差是否下降，同时检查重力方向和 URDF 惯量参数是否合理。第三步再加入 DOB，并从低增益、强滤波、严格限幅开始。

如果出现目标附近 jitter，优先看速度估计噪声、DOB 滤波截止频率和死区，而不是盲目提高 kd。如果出现缓慢下垂，优先看 KDL 重力补偿方向、关节零位和负载参数。

KDL + DOB 的核心不是“两个补偿叠得越多越好”，而是分清已知项和未知项。KDL 先补掉主要重力，DOB 再补剩余扰动，这样系统更容易调参，也更容易解释失败原因。

这套组合适合作为 OpenArmX 柔顺控制平台的第一阶段：它比纯 PD 更能承受负载和摩擦变化，又比直接引入力传感器外环更容易在现有 ros2_control 链路中落地。