31.1.3 控制理论基础：力/位混合控制

可视化演示

演示机器人力/位混合控制原理：选择矩阵分解任务空间、力控与位控双环路协同、以及接触力与位置响应动态调节过程。

基本概念（对应思维导图一级分支 1）

第一次用钢笔的人多半会犯同一个错：紧握笔杆，每一笔都狠狠往下戳纸，结果不是笔尖劈叉，就是晕开一团墨。老师傅点破要害：“手腕管走位，指腹管施压。” 你这才发现，写字其实是两条并行的指令流：纸面内的横向运笔决定“写到哪”，垂直向下的指力决定“用多大力”——方向不同，目标不同，却在同一支笔上同时发生。

机器人面对的任务也有同样的结构。机械臂末端夹着打磨头或销钉，在空间中运动时，有些方向可以自由活动，比如沿工件表面的切向滑动；有些方向却被环境“顶住”，比如法向方向上一旦接触，就产生了力。力/位混合控制的核心，就是把末端任务空间按方向拆成两个互补的集合：位置控制子空间负责“去哪”，力控制子空间负责“用多大力”。

这种拆分有清晰的理论依据。环境对运动施加的限制称为自然约束：某个方向被环境限死，速度只能为零，而力可以自由增长；某个方向环境不设限，速度自由，而力恒为零。控制器要做的是在速度受限的方向上设定期望力，在速度自由的方向上设定期望轨迹，这两类人工约束与自然约束一一对应、恰好互补，实现了“每个方向都有人管，又没有人抢”。用维数语言表述，就是位置子空间与力子空间拼起来恰好铺满整个任务空间：

$$\displaystyle \dim(\mathcal{V}_p) + \dim(\mathcal{V}_f) = n$$

1. 自由度总数 n：$n$ 是末端任务空间中可控制的方向总数，例如六自由度机械臂末端可取 $n=6$。

$$n$$

2. 位置子空间：$\mathcal{V}_p$ 是所有采用位置控制的方向构成的集合，负责“走到哪”。

$$\mathcal{V}_p$$

3. 力子空间：$\mathcal{V}_f$ 是所有采用力控制的方向构成的集合，负责“用多大力”。

$$\mathcal{V}_f$$

4. 互补关系：两子空间维数之和恰等于总自由度，方向互不重叠、也无遗漏。

$$\dim(\mathcal{V}_p) + \dim(\mathcal{V}_f) = n$$

等你掌握了这份分工，笔尖自然就不抖了。老师那句“腕走准、指压稳”，其实就是力/位混合控制最朴素的口诀：走位的方向归位置管，发力的方向归力管。

选择矩阵（对应思维导图一级分支 2）

假如你左手拿笔、右手拿章，同时在一张纸上又写又盖，事先不分工，两只手会在同一个位置打起来。机器人的位置环和力环也一样，必须有一张“分工表”，在每个方向上都写清楚：这里归谁管。

这张分工表在数学上叫选择矩阵（Selection Matrix）$\mathbf{S}$。任务空间有 $n$ 个方向时，$\mathbf{S}$ 是 $n\times n$ 的对角矩阵，对角线元素只能取 0 或 1。$s_i=1$ 表示第 $i$ 个方向交给力控制环，$s_i=0$ 表示交给位置控制环。它就像给每个方向发一张通行证：方向不同，权限不同，但一个方向只拿得到一张证。

$\mathbf{S}$ 的设计原则是“一方向一模式”：位置控制和力控制不重叠、不遗漏。环境把哪个方向的运动限死了，那个方向就填 1 用力控；环境对哪个方向不设限，那个方向就填 0 用位控。以插销入孔为例：垂直于孔壁的两个方向受孔壁约束，$\mathbf{S}$ 对应元素为 1；沿孔轴线的进给方向需要跟踪轨迹，对应元素为 0。整个任务的柔顺性，全藏在这张小小的对角阵里。

$$\displaystyle \mathbf{S} = \mathrm{diag}(s_1, s_2, \ldots, s_n), \qquad s_i \in \{0,\,1\}$$

1. 矩阵形式：$\mathbf{S}$ 是 $n\times n$ 对角矩阵，非对角线元素全为 0，表示各方向独立分配。

$$\mathbf{S} = \mathrm{diag}(s_1, s_2, \ldots, s_n)$$

2. 二元取值：每个对角元 $s_i$ 只取 0 或 1，不允许中间值。

$$s_i \in \{0,\ 1\}$$

3. 力控方向：$s_i=1$ 时第 $i$ 个方向由力控制环接管。

$$s_i = 1 \Rightarrow \text{第}\ i\ \text{方向为力控}$$

4. 位控方向：$s_i=0$ 时第 $i$ 个方向由位置控制环接管。

$$s_i = 0 \Rightarrow \text{第}\ i\ \text{方向为位控}$$

笔尖之所以不打架，是因为大脑早就把“横向”和“纵向”的通行证分好了。机器人的选择矩阵，就是把这套潜意识的默契写成一张明明白白的表。

控制架构（对应思维导图一级分支 3）

回到写字。你的神经系统里其实跑着两条互不干扰的指令流：一条告诉手腕“下一笔画到哪”，一条告诉指腹“保持这个压强”。它们各自计算，最终在笔杆上汇合。力/位混合控制的架构，正是把这种“双通道并行”搬进了控制器。

架构由两条并列的回路组成。位置控制环接收期望位置指令，输出位置修正力；力控制环接收期望力指令，输出力修正力。两条回路的输出汇入选择矩阵这扇“分诊门”：$\mathbf{S}$ 放行力环在力控方向的输出，$\mathbf{I}-\mathbf{S}$ 放行位环在位控方向的输出。两路信号叠加后，再经坐标变换映射到各关节的力矩，驱动机械臂运动。

这套由 Raibert 与 Craig 于 1981 年提出的经典架构，其控制律写作：

$$\displaystyle \mathbf{F} = \mathbf{S}\,\mathbf{F}_c + (\mathbf{I} - \mathbf{S})\,\mathbf{K}_p\,\Delta\mathbf{x}$$

1. 合成命令：$\mathbf{F}$ 是最终作用在末端执行器上的力/力矩向量。

$$\mathbf{F}$$

2. 力通道项：选择矩阵 $\mathbf{S}$ 选中力控方向，放行力环输出的期望力指令 $\mathbf{F}_c$。

$$\mathbf{S}\,\mathbf{F}_c$$

3. 位通道项：互补矩阵 $\mathbf{I}-\mathbf{S}$ 选中位控方向，放行位置环输出。

$$(\mathbf{I} - \mathbf{S})\,\mathbf{K}_p\,\Delta\mathbf{x}$$

4. 位置增益与误差：$\mathbf{K}_p$ 是位置增益矩阵，$\Delta\mathbf{x}$ 是期望位置与实际位置的误差向量。

$$\mathbf{K}_p\,\Delta\mathbf{x}$$

一横一竖，最终汇成一个完整的字。位置环和力环各写各的，再由选择矩阵合成，这就是“混合”的真正含义：并行、分工、合流。

稳定性分析（对应思维导图一级分支 4）

你有没有这样的经历：越想写稳，手反而越抖。笔尖刚触到纸面的一刹那，如果用力过猛，笔尖会弹起来再砸下去，留下一串“蚯蚓”般的抖动线。写字发抖，是因为“从悬空到接触”的状态切换没处理好。机器人也一样：在自由空间与接触空间之间的切换点，是稳定性风险最集中的地方。

力/位混合控制的稳定性风险主要来自三处。其一是接触稳定性：环境刚度越大，力环增益越难整定，增益偏大就会震荡，就像在钢板上写字远比在软垫上写字难稳。其二是切换稳定性：笔尖落纸的瞬间系统动力学突然改变，若没有过渡设计，力信号会在接触瞬间跳变。其三是参数敏感度：位置环与力环共用同一套机械臂，任一环增益变动都会通过动力学相互影响，鲁棒性差时一个小扰动就能诱发振荡。

工程上常用“刚度—阻尼匹配”的思想来分析。把接触过程近似成一个二阶系统，其中机器人自身刚度 $k_r$ 与环境刚度 $k_e$ 串联成等效刚度 $k_{eq}$：

$$\displaystyle \begin{aligned} m\ddot{x} + c\dot{x} + k_{eq}\,x &= F_d \\ k_{eq} &= \frac{k_r\,k_e}{k_r + k_e} \end{aligned}$$

1. 惯性项：$m\ddot{x}$ 是末端等效质量产生的惯性力，它决定了接触的动态快慢。

$$m\ddot{x}$$

2. 阻尼项：$c\dot{x}$ 消耗接触能量，阻尼不足时系统会反复震荡。

$$c\dot{x}$$

3. 恢复项：$k_{eq}\,x$ 是接触面提供的弹性恢复力，把末端“推”回平衡位置。

$$k_{eq}\,x$$

4. 等效刚度：机器人刚度 $k_r$ 与环境刚度 $k_e$ 串联，二者中较小的一个主导接触特性。

$$\frac{k_r\,k_e}{k_r + k_e}$$

5. 稳定条件：$m$、$c$、$k_{eq}$ 均为正时系统才稳定；力反馈符号接反会使等效刚度变负，系统立即发散。

$$m > 0,\ c > 0,\ k_{eq} > 0$$

稳定性分析的结论可以浓缩成一句话：给笔尖装一层合适的缓冲垫。阻尼足够大，笔尖轻轻落下；刚度匹配得当，接触不尖叫。机器人与世界打交道时的种种“发抖生病”，大多源于这三项系数失衡。

性能优化（对应思维导图一级分支 5）

会写字之后，追求就变成了“又快又稳又好看”。用力太保守，字迹发虚；用力太激进，墨水洇纸。高手的指尖总能在毫秒之间微调压力，机器人的性能优化也追求同样的细腻。

优化手段通常四管齐下。一是力位加权调节：在力控方向与位控方向之间分配优先级，装配时可以“先保护工件、再追求速度”。二是过渡过程优化：在自由空间到接触空间之间安排过渡指令，让期望力从零平滑爬升，而不是瞬间压满。三是柔顺增益整定：调节力环的比例、积分增益与位置环的刚度、阻尼，使系统获得理想的动态响应。四是前馈补偿：把期望力、重力、摩擦力等已知量直接前馈到指令中，减轻反馈环的负担。

其中，过渡过程优化最常用也最直观的手法叫“力斜坡”——接触前让期望力按斜坡逐渐升到目标值：

$$\displaystyle F_d(t) = F_{target}\,\min\!\left(1,\ \frac{t}{t_{ramp}}\right)$$

1. 最终目标力：$F_{target}$ 是接触稳定后要维持的期望接触力。

$$F_{target}$$

2. 斜坡时间：$t_{ramp}$ 是力从零升到目标值所用的时间，决定过渡的快慢。

$$t_{ramp}$$

3. 归一化因子：$\min(1,\ t/t_{ramp})$ 保证指令不超过目标力。

$$\min\!\left(1,\ \frac{t}{t_{ramp}}\right)$$

4. 平滑过渡：$F_d(t)$ 随时间连续爬升，避免接触瞬间的力冲击。

$$F_d(t) = F_{target}\,\min\!\left(1,\ \frac{t}{t_{ramp}}\right)$$

优化的终点，是一支笔在纸上流畅游走：起笔不重、行笔不抖、收笔不飘。它靠的不是某一招，而是权重、过渡、增益、前馈四件套的协同。

应用与挑战（对应思维导图一级分支 6）

“写字”这门手艺一旦离开书法台、走进工厂车间，就换了个名字叫柔顺作业。凡是“既要走到位、又要用对力”的活，都是力/位混合控制的舞台。

最经典的舞台是插孔装配。销钉入孔，横向偏一点就卡死，纵向用力过猛就刮伤孔壁。混合控制的方案是：横向两个方向用位置控制微调对准，纵向进给方向用力控制感知“插到位”的力信号。打磨抛光则是另一个典型：磨头沿曲面切向运动，由位置环负责轨迹；法向保持恒定压力，由力环负责去除量均匀。曲面跟踪任务更像“在球面上写字”——笔尖始终垂直于曲面、压力恒定、位置沿曲面游走，其底层依据就是接触力模型：

$$\displaystyle \mathbf{F}_n = k_e\,\Delta z,\qquad \Delta z = z - z_{surf}$$

1. 法向接触力：$\mathbf{F}_n$ 是末端与工件之间的法向作用力，方向垂直于接触面。

$$\mathbf{F}_n$$

2. 环境刚度：$k_e$ 是工件的等效接触刚度，越硬的工件 $k_e$ 越大。

$$k_e$$

3. 法向侵入量：$\Delta z$ 是末端沿法向进入工件的微小深度，等于当前高度与表面高度之差。

$$\Delta z = z - z_{surf}$$

4. 闭环意义：力控环通过调节侵入量间接控制接触力，实现恒力跟踪曲面。

$$\mathbf{F}_n = k_e\,\Delta z$$

当然，工程远比理论复杂。钢对钢的高刚度接触仍容易激起震荡；软橡胶等未知时变环境难以建立准确模型；频繁的自由/接触切换对实时性要求极高。因此新一代方案正将力/位混合控制与阻抗控制、模型预测控制、强化学习相结合：用学习算法预测环境刚度，用混合控制保障底层安全，形成“智能柔顺”闭环。

从一支钢笔到一个机械臂，变化的是尺度，不变的是那句口诀：该走位的走位，该发力的发力。机器人学会了人类写字时最自然的分工，这正是力/位混合控制诞生四十余年后依然活跃在工业一线的根本原因。

AI 应用场景

Python 演示

# 力/位混合控制 2 自由度仿真: x 方向位置跟踪, y 方向恒力按压import math DT = 0.001        # 仿真步长(秒)STEPS = 3000      # 仿真总步数M = 1.0           # 末端等效质量(kg)KP = 1600.0       # 位置环比例增益KD = 80.0         # 位置环阻尼增益KF_P = 0.8        # 力环比例增益KF_I = 120.0      # 力环积分增益KD_Y = 80.0       # 力通道阻尼增益K_ENV = 6000.0    # 环境(工件)接触刚度(N/m)THRESHOLD = 0.02  # 接触面高度(m)F_DES = 10.0      # 期望接触力(N)  def run():    # 状态变量: x/vx 为位置通道, y/vy 为力通道    x, vx = 0.0, 0.0    y, vy = THRESHOLD, 0.0    y_int = 0.0    pos_err_max = 0.0    f_list = []     for i in range(STEPS):        t = i * DT         # 位置通道: 跟踪 x 方向正弦期望轨迹        x_d = 0.04 * math.sin(2.0 * math.pi * t)        x_dd = -0.04 * (2.0 * math.pi) ** 2 * math.sin(2.0 * math.pi * t)        ax = x_dd + KP * (x_d - x) - KD * vx        vx += ax * DT        x += vx * DT        pos_err_max = max(pos_err_max, abs(x_d - x))         # 力通道: 对接触面施加恒定法向力        f_meas = max(0.0, K_ENV * (y - THRESHOLD))        y_int += (F_DES - f_meas) * DT        ay = KF_P * (F_DES - f_meas) + KF_I * y_int - KD_Y * vy        vy += ay * DT        y += vy * DT        f_list.append(f_meas)     tail = f_list[int(STEPS * 0.8):]    f_avg = sum(tail) / len(tail)    print("x 方向位置跟踪最大误差: %.4f m" % pos_err_max)    print("y 方向接触力稳态均值: %.4f N (期望 10.0 N)" % f_avg)    print("y 方向接触力波动范围: %.4f ~ %.4f N" % (min(tail), max(tail)))  if __name__ == "__main__":    run()

1. x_d = 0.04 * math.sin(2.0 * math.pi * t)位置通道的期望轨迹，相当于“手腕要走出的路线”。
2. ax = x_dd + KP * (x_d - x) - KD * vx位置环 PD 加前馈：用误差修正加速度，让末端跟上正弦轨迹。
3. f_meas = max(0.0, K_ENV * (y - THRESHOLD))接触力模型：环境刚度乘侵入量，压得越深力越大。
4. y_int += (F_DES - f_meas) * DT力误差累计，积分项负责消除稳态误差。
5. ay = KF_P * (F_DES - f_meas) + KF_I * y_int - KD_Y * vy力环 PI 加阻尼：把接触力拉回期望值 10 N。
6. print("x 方向位置跟踪最大误差: %.4f m" % pos_err_max)输出统计验证两个通道“各管各的、互不干扰”。

常见误区

面试题