3. 关节空间力矩控制：PD+重力补偿控制

好，咱们今天聊点实在的。前面讲了力矩控制的基本概念，现在该动手了。我个人觉得，PD+重力补偿是进入力矩控制世界最好的一扇门。为什么？因为它简单、有效，而且你很快就能看到效果。

3.1 为什么是PD+重力补偿？

你想想看，一个机械臂关节，如果只是简单给个力矩让它动，会发生什么？

嗯，它会掉下来。因为重力一直在拉着它。

所以我们需要两件事：

• PD控制：让关节按照我们期望的位置和速度运动
• 重力补偿：抵消重力带来的影响，让控制更精准

说白了，PD负责「怎么动」，重力补偿负责「别掉下去」。

3.2 单关节模型长什么样？

一个单关节，说白了就是一个旋转的刚体。它的动力学方程很简单：

J·q̈ + b·q̇ + G(q) = τ

这里：

• J：转动惯量（关节的「惯性」）
• b：阻尼系数（摩擦、阻力）
• G(q)：重力矩（跟关节角度有关）
• τ：我们施加的力矩

我在项目中遇到过一个问题：很多人直接把电机手册上的惯量拿来用，结果控制效果一塌糊涂。为什么？因为负载变了。你夹个重物，惯量就变了。所以实际项目中，我建议你在线辨识或者至少留个余量。

3.3 重力补偿怎么算？

对于单关节，重力矩其实很好算：

G(q) = m·g·L·sin(q)

其中：

• m：连杆质量
• g：重力加速度
• L：质心到关节轴的距离
• q：关节角度

3.4 代码实现：一个简单的单关节力矩控制器

好，咱们直接上代码。这是我个人习惯的写法，清晰、可调试。

// 单关节PD+重力补偿控制器
// 作者：资深工程师老张
// 说明：这是一个位置模式的力矩控制器

class SingleJointTorqueController {
private:
    float Kp;           // 比例增益
    float Kd;           // 微分增益
    float mass;         // 连杆质量 (kg)
    float length;       // 质心到关节距离 (m)
    float gravity;      // 重力加速度 (m/s^2)
    
    float prev_error;   // 上一次误差，用于计算微分
    float prev_velocity; // 上一次速度估计
    
public:
    // 构造函数
    SingleJointTorqueController(float kp, float kd, float m, float l) {
        Kp = kp;
        Kd = kd;
        mass = m;
        length = l;
        gravity = 9.81;
        prev_error = 0.0;
        prev_velocity = 0.0;
    }
    
    // 计算重力补偿项
    float gravityCompensation(float q) {
        return mass * gravity * length * sin(q);
    }
    
    // 计算控制力矩
    float compute(float q_desired, float q_actual, float dt) {
        // 1. 计算位置误差
        float error = q_desired - q_actual;
        
        // 2. 计算速度（用差分近似）
        float velocity = (error - prev_error) / dt;
        
        // 3. PD控制项
        float pd_term = Kp * error + Kd * velocity;
        
        // 4. 重力补偿项
        float gravity_term = gravityCompensation(q_actual);
        
        // 5. 总力矩
        float torque = pd_term + gravity_term;
        
        // 6. 更新状态
        prev_error = error;
        prev_velocity = velocity;
        
        return torque;
    }
    
    // 设置增益（调试用）
    void setGains(float kp, float kd) {
        Kp = kp;
        Kd = kd;
    }
};

3.5 参数怎么调？

调参是个手艺活。我一般这么干：

1. 先调Kp：从小到大慢慢加，直到关节开始有轻微振荡
2. 再加Kd：消除振荡，让响应变平滑
3. 验证重力补偿：让关节停在某个角度，看它会不会滑下来

给你一个参考表格（基于我常用的一个轻型关节）：

参数	推荐范围	我的经验值
Kp	10 - 100	35
Kd	1 - 20	5
重力补偿系数	0.8 - 1.2倍理论值	0.95

3.6 核心逻辑流程图

下面这张图，是我画的一个单关节力矩控制器的核心流程。你看一眼就明白了。

3.7 实际调试中的坑

最后，分享几个我踩过的坑：

• 重力补偿不准：如果你发现关节在某个角度停不住，八成是重力补偿算错了。检查一下质心位置和连杆质量。
• 微分项噪声：速度信号噪声大，微分项就会乱跳。我建议加个一阶低通滤波，截止频率设在5-10Hz。
• 积分饱和：虽然PD没有积分项，但如果你后面加了积分，记得做抗饱和处理。

好了，这一节就到这。代码拿去跑跑看，有问题随时找我。