4. 力位混合控制：自然约束与人工约束、选择矩阵、并行控制架构

各位工程师朋友，欢迎来到第四章。这一章我们聊点硬核的——力位混合控制。

说实话，我刚入行那会儿，觉得位置控制就够用了。直到有一次做精密装配，机器人硬怼进去，零件直接崩飞了。嗯，从那以后我才真正理解：有些场景，光控位置不行，得让机器人学会「温柔地用力」。

4.1 自然约束与人工约束

先问一个问题：为什么拧螺丝的时候，机器人不能像人一样自然？

因为环境给机器人设了两道坎——自然约束和人工约束。这两个概念是 Mason 在 1981 年提出的，到现在依然是力控的基石。

4.1.1 自然约束

自然约束，说白了就是物理世界强加的限制。你没法改变它，只能顺从它。

举个例子：

• 机械臂末端贴着桌面移动——桌面限制了 Z 方向的位置，这就是自然位置约束
• 机械臂推一堵墙——墙限制了 X 方向的速度，同时产生了法向力，这就是自然力约束

我个人习惯把自然约束分成两类：

约束类型	表现形式	例子
自然位置约束	位置/速度被限制	末端接触桌面，Z方向不能动
自然力约束	力/力矩被限制	推墙时，法向力由接触决定

你想想看，自然约束其实是个「二选一」的游戏：要么位置被限制，要么力被限制，不会同时出现。这就是 Mason 说的自然约束的正交性。

4.1.2 人工约束

人工约束就灵活多了。它是我们人为设定的控制目标。

比如：

• 在桌面划直线——我们设定 X、Y 方向的位置轨迹（人工位置约束）
• 同时保持 Z 方向接触力为 5N（人工力约束）

关键来了：人工约束必须与自然约束互补。

什么意思？

• 自然约束限制了位置的方向 → 我们就用力去控
• 自然约束限制了力的方向 → 我们就用位置去控

我曾经在打磨项目中踩过这个坑：工件表面有弧度，我却在法线方向用力控，结果力控和位置控打架，末端抖得像筛子。后来改成法线方向力控、切线方向位置控，立马稳了。

4.2 选择矩阵

好，约束分清楚了，怎么告诉控制器「哪个方向控位置，哪个方向控力」？

答案就是选择矩阵。

4.2.1 选择矩阵的定义

选择矩阵 S 是一个对角矩阵，对角线元素是 0 或 1：

• S = 1 → 该方向用力控制
• S = 0 → 该方向用位置控制

对于六轴机械臂，我们通常用 6×6 的矩阵：

S = diag([s_x, s_y, s_z, s_rx, s_ry, s_rz])

其中：
s_x = 1 → X方向力控
s_x = 0 → X方向位置控

注意，选择矩阵也有个互补矩阵 S' = I - S，用来选另一组方向。

4.2.2 实际怎么用

我举个例子，你在做抛光：

• Z 方向要恒力接触工件 → S_z = 1
• X、Y 方向要跟踪轨迹 → S_x = 0, S_y = 0
• 姿态保持固定 → S_rx = 0, S_ry = 0, S_rz = 0

选择矩阵就是：

S = diag([0, 0, 1, 0, 0, 0])

然后控制律写成：

τ = S * τ_force + (I - S) * τ_position

说白了，就是力控和位置控各管各的，互不干扰。

4.3 并行控制架构

有了选择矩阵，接下来就是怎么搭控制框架了。目前主流的是并行控制架构。

4.3.1 架构原理

并行架构的核心思想很简单：力控和位置控同时计算，然后通过选择矩阵合并。

我画了一张图帮你理解：

你看，整个流程很清晰：

1. 位置控制器和力控制器并行计算，互不等待
2. 选择矩阵 S 提取力控输出，I-S 提取位置控输出
3. 两者相加，得到最终的关节力矩指令

4.3.2 代码实现

下面是一个简化版的并行控制代码，我用 Python 风格写：

class HybridForcePositionController:
    def __init__(self, S_matrix):
        self.S = S_matrix          # 选择矩阵
        self.I_minus_S = np.eye(6) - S_matrix  # 互补矩阵
        
        # 位置控制器参数
        self.Kp_pos = 100.0
        self.Kd_pos = 20.0
        
        # 力控制器参数
        self.Kp_force = 0.5
        self.Ki_force = 0.1
        
    def compute(self, x_desired, f_desired, x_actual, f_actual, dt):
        # 1. 位置控制环
        pos_error = x_desired - x_actual
        tau_pos = self.Kp_pos * pos_error - self.Kd_pos * (x_actual - self.x_prev) / dt
        
        # 2. 力控制环
        force_error = f_desired - f_actual
        self.force_integral += force_error * dt
        tau_force = self.Kp_force * force_error + self.Ki_force * self.force_integral
        
        # 3. 通过选择矩阵合并
        tau = self.S @ tau_force + self.I_minus_S @ tau_pos
        
        self.x_prev = x_actual
        return tau

4.4 实际应用中的坑与对策

讲了这么多理论，来点实战经验。

4.4.1 模式切换时的冲击

从纯位置模式切到力位混合模式，最容易出问题。为什么？

因为切换瞬间，力控输出突然从 0 跳到一个非零值，产生冲击。

我的解决办法：

• 加一个软启动，让力控输出在 0.5 秒内逐渐切入
• 或者用力控输出限幅，每次变化不超过 5%

4.4.2 选择矩阵的切换策略

有些任务需要动态切换选择矩阵。比如装配时：

• 接近阶段 → 全位置控（S = 全0）
• 接触阶段 → 法线方向切到力控（S = diag([0,0,1,0,0,0])）
• 插入阶段 → 再加一个方向力控（S = diag([0,0,1,0,0,1])）

我曾经在切换时没做平滑过渡，结果机器人「咯噔」一下，工件直接飞了。后来我加了过渡矩阵，让 S 矩阵的值在 0.2 秒内线性变化，问题就解决了。

4.4.3 力传感器噪声

力控好不好，传感器是关键。我踩过的坑：

• 传感器安装偏了 → 力控方向全错
• 传感器温漂 → 力控输出慢慢飘走
• 高频噪声 → 力控抖得不行

我的建议：

• 每次上电做零点校准
• 加一个低通滤波器，截止频率 10-20Hz
• 如果条件允许，用加速度补偿消除惯性力干扰

好了，这一章就到这里。力位混合控制说难不难，说简单也不简单。关键是多动手调，多踩坑，慢慢就有感觉了。

---