上一篇讲了导纳控制，核心是"力→位置"——用外力驱动末端运动。但实际工业场景中，很多任务既需要精确的位置跟踪，又需要控制接触力。比如机器人拧螺丝——位置和姿态要精确，同时轴向力不能太大也不能太小。

这种"既要位置又要力"的需求，就是力位混合控制要解决的问题。说白了，它把笛卡尔空间分成两个区域：某些方向做位置控制，另一些方向做力控制。

一、为什么需要力位混合？

先想一个场景：机器人在桌面上画圆。

画笔需要沿着桌面移动（位置控制），同时笔尖要保持一定的按压力（力控制）。这两个需求发生在同一个末端，但控制目标完全不同。

如果你只用位置控制——画笔要么飘在空中（力太小），要么戳破桌面（力太大）。如果你只用力控制——笔尖的位置根本没法精确跟踪圆形轨迹。

力位混合控制的思路很直接：在不同的方向上分配不同的控制模式。

画圆的例子里：

• 水平面内（X、Y方向）：位置控制，精确跟踪圆形轨迹
• 垂直方向（Z方向）：力控制，保持恒定的按压力

这就是"混合"的含义——同一时刻，不同方向用不同的控制策略。

二、选择矩阵——力控和位置控制的"分界线"

力位混合控制的核心是一个选择矩阵S。它是一个对角矩阵，对角线元素为0或1。

S_i = 1 表示第i个方向做位置控制 S_i = 0 表示第i个方向做力控制

import numpy as np

# 6自由度末端：[x, y, z, rx, ry, rz]
# 场景：桌面上画圆
# X, Y方向位置控制；Z方向力控制
# 姿态全部位置控制

S_pos = np.diag([1, 1, 0, 1, 1, 1])  # 位置控制选择
S_force = np.diag([0, 0, 1, 0, 0, 0])  # 力控制选择

# 验证：S_pos + S_force = I
assert np.allclose(S_pos + S_force, np.eye(6))

选择矩阵的关键性质：S_pos + S_force = I（单位矩阵）。每个方向要么做位置控制，要么做力控制，不能同时做两个，也不能都不做。

三、力位混合的控制架构

经典的力位混合控制架构如下：

期望位置 x_d ──→ 位置控制器 ──→ 位置控制输出 τ_pos
                                      ↓
                                    S_pos
                                      ↓
期望力 F_d ──→ 力控制器 ──→ 力控制输出 τ_force
                                      ↓
                                    S_force
                                      ↓
                              τ = S_pos*τ_pos + S_force*τ_force
                                      ↓
                              关节力矩指令 → 电机

位置控制器通常是PID，输入是期望位置和实际位置的偏差。力控制器也是PID，输入是期望力和实际力的偏差。两个输出通过选择矩阵加权求和，得到最终的关节力矩。

来看一段简化代码：

class HybridForcePositionController:
    def __init__(self, S_pos, S_force):
        self.S_pos = S_pos
        self.S_force = S_force
        self.pos_pid = PIDController(kp=100, ki=1, kd=10)
        self.force_pid = PIDController(kp=0.5, ki=0.01, kd=0.05)

    def compute(self, x_d, x_curr, F_d, F_curr, J):
        # 位置控制部分
        pos_error = x_d - x_curr
        F_pos = self.S_pos @ self.pos_pid.compute(pos_error)

        # 力控制部分
        force_error = F_d - F_curr
        F_force = self.S_force @ self.force_pid.compute(force_error)

        # 笛卡尔空间力 → 关节力矩
        tau = J.T @ (F_pos + F_force)
        return tau

这里有个细节：力控制器的增益通常比位置控制器小很多。因为力的量级和位置的量级差很多——1mm的位置偏差和1N的力偏差，对系统的影响完全不同。

四、约束坐标系——选择矩阵在哪个坐标系下？

这是力位混合控制最容易搞混的地方。选择矩阵S定义在哪个坐标系下？

答案是：约束坐标系（constraint frame）。

约束坐标系是根据任务定义的，不是基坐标系，也不是工具坐标系。比如：

• 桌面画圆：约束坐标系的Z轴垂直于桌面，XY轴在桌面内
• 孔轴装配：约束坐标系的Z轴沿孔的轴线方向
• 曲面打磨：约束坐标系的Z轴沿曲面法线方向

约束坐标系可以随任务变化。比如机械臂在曲面上打磨，曲面法线方向一直在变，约束坐标系也要跟着变。

def get_constraint_frame(surface_normal):
    """根据曲面法线构建约束坐标系"""
    z_axis = surface_normal / np.linalg.norm(surface_normal)
    # 选一个不平行于z_axis的向量
    if abs(z_axis[0]) < 0.9:
        temp = np.array([1, 0, 0])
    else:
        temp = np.array([0, 1, 0])
    x_axis = np.cross(temp, z_axis)
    x_axis /= np.linalg.norm(x_axis)
    y_axis = np.cross(z_axis, x_axis)
    # 6x6旋转矩阵
    R = np.eye(6)
    R[:3, :3] = np.column_stack([x_axis, y_axis, z_axis])
    R[3:, 3:] = R[:3, :3]
    return R

每次控制循环都要更新约束坐标系，把力和位置误差变换到约束坐标系下，再乘以选择矩阵。这个变换如果做错了，力控方向就会"跑偏"。

五、工业应用案例

力位混合控制在工业界应用非常广泛，举几个典型场景：

案例1：轴孔装配。Z方向力控制（保证插入力不超过阈值），XY和姿态方向位置控制（保证对准）。这是最经典的力位混合应用。

案例2：去毛刺。刀具沿工件表面运动，法线方向力控制（保持恒定切削力），切线方向位置控制（跟踪轮廓）。选择矩阵根据表面法线实时更新。

案例3：插拔连接器。插入时Z方向力控制，XY方向位置控制。拔出时反过来——先力控检测是否松动，再位置控制拔出。

案例4：精密研磨。法向力控制保证研磨压力恒定，切向位置控制保证轨迹精度。研磨力通常在5-20N范围，位置精度要求0.1mm以内。

之前做AMR底盘装配的项目，有一个工序是把电池连接器插入插槽。这个任务就是典型的力位混合——XY方向对准位置，Z方向控制插入力。力控PID调了两天，关键是力控增益不能太大，否则连接器会被"砸"进去。

六、面试实战

Q：力位混合控制和阻抗控制有什么区别？ A：力位混合控制在不同方向分别做位置控制和力控制，两者是独立的。阻抗控制不区分方向，统一用"力-位置关系"来描述。力位混合适合方向明确的任务（如垂直按压），阻抗适合需要全方位柔顺的任务（如拖拽示教）。

Q：选择矩阵能不是对角矩阵吗？ A：理论上可以，但工程上几乎都用对角矩阵。对角矩阵的物理意义清晰——每个方向独立选择控制模式。非对角矩阵意味着方向之间的耦合，调试难度大增。

Q：力位混合控制能处理未知环境吗？ A：比较困难。选择矩阵需要预先知道哪个方向该力控、哪个方向该位置控。如果环境几何未知，选择矩阵没法确定。这种情况用阻抗控制更合适。

Q：力控方向的PID怎么调？ A：力控PID和位置PID的调法类似，但增益量级不同。一般先只开P，从小到大加，直到力响应够快但不振荡。然后加D抑制振荡，最后加I消除稳态误差。力控频率通常比位置控制低，所以增益要相应减小。

小结

力位混合控制的核心：用选择矩阵把笛卡尔空间分成"位置控制区"和"力控制区"。不同方向用不同的控制策略，互不干扰。

选择矩阵定义在约束坐标系下，约束坐标系根据任务定义。力控方向和位置控制方向的切换通过矩阵元素0/1实现。

工业应用中最常见的场景是"法向力控+切向位置控"——打磨、装配、插拔都是这个套路。

下一篇我们聊关节空间控制和笛卡尔空间控制的选型——这两种控制策略的差异比你想的要大。

---

如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。

「机器人软件开发面试·从入门到精通」连载系列 

上一篇：第202篇 导纳控制——力控场景的另一种实现思路

下一篇预告：第204篇 关节空间控制vs笛卡尔空间控制——两种控制策略的选型

有任何问题欢迎评论区留言，我会尽量回复。