4. 笛卡尔空间力矩控制

好，咱们今天聊点硬核的。从关节空间到笛卡尔空间的映射，说白了就是——你拧电机，但你想控制的是手爪的位置和力。这中间隔了一层「几何变换」。

我刚开始做机器人时，总觉得关节空间控制就够了。直到有一次，我要让机器人用恒力去擦玻璃。关节空间控制？根本搞不定。因为玻璃表面不是平的，关节角度一变，力就跑了。嗯，这就是为什么我们需要笛卡尔空间力矩控制。

4.1 从关节空间到笛卡尔空间：雅可比矩阵

先问一个问题：你拧关节电机，末端执行器会怎么动？

答案藏在雅可比矩阵里。雅可比矩阵 J 把关节速度映射到末端速度：

v = J(q) · q_dot

其中：

• v 是末端在笛卡尔空间的速度（6维：线速度+角速度）
• q_dot 是关节速度
• J(q) 是雅可比矩阵，随关节位置变化

那力和力矩呢？根据虚功原理，关节力矩 τ 和末端力 F 的关系是：

τ = J^T(q) · F

注意，这里是转置，不是逆矩阵。这个公式太重要了。我当年在调试一个六轴协作机器人时，就是靠这个公式把末端的力传感器读数映射回关节力矩，才让重力补偿跑起来的。

4.2 笛卡尔空间力矩控制结构

有了映射关系，我们就可以在笛卡尔空间设计控制器了。结构是这样的：

1. 在笛卡尔空间计算期望力 F_des
2. 用 τ = J^T · F_des 映射到关节空间
3. 加上重力补偿、摩擦力补偿
4. 输出到电机

我习惯把这种控制叫做「外环-内环」结构：

• 外环：笛卡尔空间力/位置控制器
• 内环：关节空间力矩控制器（通常是电流环）

你想想看，外环负责「想做什么」，内环负责「怎么做到」。分工明确。

4.3 阻抗控制的概念引入

好，现在问题来了：如果机器人末端碰到障碍物，怎么办？

纯位置控制会硬碰硬，力会瞬间飙升。纯力控制又容易不稳定。那有没有一种控制，既能跟踪位置，又能对接触力有柔顺响应？

有。这就是阻抗控制。

阻抗控制的核心思想是：让机器人末端表现出一个「虚拟弹簧-阻尼」系统的行为。

数学上，我们定义：

M · (x_ddot - x_ddot_d) + D · (x_dot - x_dot_d) + K · (x - x_d) = -F_ext

其中：

• M, D, K 是虚拟的质量、阻尼、刚度矩阵
• x_d 是期望位置
• x 是实际位置
• F_ext 是外部施加的力

说白了，就是让机器人「假装」自己是一个弹簧阻尼系统。你推它，它让开；你松手，它回来。

4.4 笛卡尔阻抗控制的实现

把阻抗控制放到笛卡尔空间，控制律变成：

F_des = M · x_ddot_d + D · (x_dot_d - x_dot) + K · (x_d - x)
τ_des = J^T(q) · F_des + τ_gravity(q) + τ_friction(q_dot)

注意最后两项：重力补偿和摩擦力补偿。没有它们，机器人连自重都撑不住。

我曾经在一个项目中，忘记加重力补偿，结果机器人一上电就「啪」地砸下来。嗯，从那以后我写代码第一件事就是加重力补偿。

4.5 知识体系结构图

下面这张图总结了本章的核心逻辑：

4.6 一个简单的实现示例

下面是一个笛卡尔阻抗控制的伪代码框架。实际项目中，我一般用 C++ 写实时控制，但这里用 Python 风格方便理解：

class CartesianImpedanceController:
    def __init__(self, M, D, K):
        self.M = M  # 虚拟质量矩阵 (6x6)
        self.D = D  # 虚拟阻尼矩阵 (6x6)
        self.K = K  # 虚拟刚度矩阵 (6x6)
        
    def compute(self, q, q_dot, x_d, x_dot_d, x_ddot_d):
        # 1. 正运动学：关节 → 笛卡尔
        x = forward_kinematics(q)
        J = compute_jacobian(q)
        
        # 2. 阻抗控制律
        delta_x = x - x_d
        delta_x_dot = J @ q_dot - x_dot_d
        
        F_des = self.M @ x_ddot_d \
              + self.D @ (-delta_x_dot) \
              + self.K @ (-delta_x)
        
        # 3. 映射到关节空间
        tau_des = J.T @ F_des
        
        # 4. 补偿
        tau_des += gravity_compensation(q)
        tau_des += friction_compensation(q_dot)
        
        return tau_des

4.7 总结

这一节我们聊了：

• 雅可比矩阵怎么把关节空间和笛卡尔空间连起来
• 笛卡尔空间力矩控制的基本结构
• 阻抗控制的核心思想：虚拟弹簧-阻尼系统
• 实现时的注意事项和调试技巧

说实话，阻抗控制是我个人最喜欢的控制方式之一。它让机器人有了「手感」。你推它，它知道让；你拉它，它知道跟。这种柔顺性，在很多场景下比纯位置控制好用得多。