🦉 猫头鹰都在卷机械臂逆解了，你确定不关注一下？

【机器人学习】小白学习第15讲：力控制 —— 柔顺、阻抗、混合力/位置控制

一句话核心：拧螺丝、擦玻璃、插孔这类活儿，位置控制会"较劲"到把零件顶坏；正确姿势是让机器人末端表现得像一个弹簧-阻尼-质量系统，或者干脆在某些方向控位置、另一些方向控力。

• 本讲目标：理解为什么位置控制不够；讲透阻抗控制的物理直觉；看懂自然/人工约束与混合力位控制的选择矩阵

一、为什么需要这个

小讲 1：位置控制在装配任务里为什么会滚成死循环

为什么说接触任务会翻车

前面 14 讲都在干一件事：让末端精确到达某个位置。碰到"抛光、去毛刺、机加工、装配"这类任务，这条路会翻车。

举的例子：要用纯位置方式完成轴孔装配，两个零件的相对定位精度必须比零件本身的机械公差高一个数量级。而环境的几何和力学特性，恰恰是最难精确建模的那一半。

误差 → 力 → 更大误差：五步滚成死循环

死循环是怎么形成的：

1. 规划有一点误差 → 末端"多走了一点"；
2. 多走的这一点撞上环境 → 产生接触力；
3. 接触力把末端推离期望轨迹；
4. 位置控制器看到偏差，加大力矩把它推回去；
5. 于是接触力更大…… 直到电机饱和或者零件被顶坏。

常见误区

**反直觉的一点：**明确指出——环境越硬、位置控制越准，这个死循环越容易发生。因为"准"意味着控制器一定会死磕那点偏差。想破局，就必须让机器人在接触时表现出柔顺（compliance）。

交互控制两大类：间接力控制与直接力控制

接触力 h_e是描述交互状态最完整的物理量。据此把交互控制分成两大类：
间接力控制
不闭合力反馈环，通过运动控制间接影响力。包括本讲的柔顺控制和阻抗控制。直接力控制
闭合力反馈环，可以把接触力控制到指定值。包括的力控制和的混合力/位控制。

停下来想一想：为什么"环境越硬 + 位置控制越准"反而越容易出事？为什么本讲讲的间接力控制不闭合力反馈环？

二、核心概念

小讲 2：带接触力的动力学模型，以及被动/主动柔顺这根"广义弹簧"

1. 带接触力的动力学模型

在有接触力时，模型多出一项：

$$B(q)\ddot{q}+C(q,\dot{q})\dot{q}+F\dot{q}+g(q)=u-J^T(q)h_e$$

符号	含义
h_e	末端施加在环境上的接触力/力矩（6×1）。本约定 h 表示"力+力矩"组合
−Jᵀ h_e	末端接触力折算成的关节力矩。就是你在学的力雅可比

2. 柔顺控制：让机器人变成一根弹簧

先看看"本来只想控位置"时会发生什么。用上一讲的操作空间 PD + 重力补偿，在接触后的稳态满足（、9.3）：

$$\tilde{x}=K_P^{-1}{h}_A⟺h_A=K_P\tilde{x}$$

看清楚：偏差和接触力成正比，比例系数就是 K_P。这意味着位置控制器本身就已经是一根"广义弹簧"——K_P是刚度，K_P^-1是柔顺。
被动柔顺
靠机械装置实现。两个刚体之间垫一个六维弹簧：h_s = K dx。K_f平移刚度、K_μ旋转刚度、K_c耦合刚度。
典型器件：RCC（Remote Centre of Compliance，远端柔顺中心）——装在末端和最后一个连杆之间，插入方向刚度高、其余方向很软，插孔时孔壁反力会自动把销"导正"。
缺点：换任务就得换硬件，通用性差。主动柔顺
靠控制软件实现，改几个数就能换任务。
关键改进：把操作空间误差定义在期望坐标系里，这样得到的柔顺关系才不随末端姿态乱变（、9.20）：
h_e = K_P dx→主动刚度；dx = K_P⁻¹ h_e→主动柔顺。
和环境耦合之后，接触力的稳态值：

$$h_e=(I_6+K{K}_P^{-1}{)}^{-1}K\cdot d{x}_{r,d}$$

符号	含义
K	环境的刚度矩阵（通常半正定，因为接触力只出现在某些方向）
K_P	机器人的主动刚度（由控制增益决定）
dx_r,d	期望位姿相对环境自然位置的偏离量 —— 也就是"你多走了多少"

直觉上：**接触力 = 两个弹簧串联后的等效刚度 × 你多走的距离。**谁软谁就变形多、谁硬谁说了算。 的操作建议：环境该让的方向把 K_P取大，机器人该让的方向把 K_P取小。

小讲 3：阻抗控制与导纳控制——给末端"捏"出一个弹簧性格

3. 阻抗控制：本讲最重要的概念

柔顺控制只管"停下来时是什么样"（静态关系）。阻抗控制管的是整个动态过程：撞上去的那一瞬间，机器人怎么反应。

思路：在上讲的操作空间逆动力学控制上，额外加上力测量并补偿（、9.31）：

$$u=B(q)y+n(q,\dot{q})+J^T(q)h_e$$

代入后得到：

$$M_d\ddot{x\stackrel{~}{}}+K_D\dot{x\stackrel{~}{}}+K_P\tilde{x}=h_A$$

提示

这就是那句最重要的话：阻抗控制不直接控制力，而是让末端表现得像一个"质量-弹簧-阻尼"系统。

你给它一个力 h，它就按M_d ẍ̃ + K_D ẋ̃ + K_P x̃ = h这条规律"让"出一个位移 x̃。

·**M_d**决定"惯性感"——你推它，它多重、响应多迟钝；

·**K_D**决定"阻尼感"——你推它，它多黏、会不会来回晃；

·**K_P**决定"软硬"——你推到底，它能让多少。

**生活比喻：**不是"我一定要用 10 N 的力擦玻璃"，而是"我把手变成一根软硬适中的弹簧，玻璃高一点我就退一点，低一点我就进一点"。

特别指出两点：

• **必须测力。**不测力的话，右边会多出一个M_d B_A⁻¹(q)，而操作空间惯量 B_A随构型变，系统又耦合起来了。力传感器通常装在腕部，见。
• +Jᵀh_e这一项让机器人对外部作用无限刚，而−J_A⁻¹ M_d⁻¹ h_A这一项才把它重新"变软"成有限阻抗。两者配合才能得到。

停下来想一想

**阻抗参数不好选：**取 M_d= 100I，K_D= 500I，K_P= 2500I。结果——

·自由运动方向（竖直）：ω_n= 5 rad/s，ζ = 0.5，很稳；

·受约束方向（水平）：环境刚度 k_x= 10³ N/m 时 ω_n≈ 5.9、ζ ≈ 0.42；k_x= 10⁴ N/m 时 ω_n≈ 11.2、ζ 掉到 0.22。

**结论：环境越硬，接触方向越容易振荡。**同一个阻抗参数，在自由方向和接触方向的表现完全不同——这就是阻抗控制的本质困难。

导纳控制（Admittance Control，）：把"运动控制"和"阻抗控制"拆成两层。先用阻抗方程
KaTeX parse error: Unexpected end of input in a macro argument, expected '}' at end of input: …lde{z} = h_{e}

积分出一个"柔顺坐标系"（理想中末端该在哪），再把它交给逆动力学运动控制去执行。好处：运动控制环的增益可以取很大（抗扰动强），阻抗环的增益可以取很软（接触力小），两者互不干扰。稳定性条件：运动控制环的带宽必须大于阻抗环的带宽。

小讲 4：直接力控制、约束运动与自然/人工约束的填表法

图 1：阻抗控制的物理模型。阻抗控制不去直接指定力，而是把末端"捏"成一个质量–弹簧–阻尼系统：KP 是挂在期望位置 xd 上的弹簧（软硬），KD 是并联的阻尼器（黏滞），Md 是末端表现出的惯性。环境从右边压过来的接触力 he 作用在这个系统上，产生偏差 x̃。调阻抗，就是调这三个元件。

4. 力控制：直接把力控到指定值

阻抗控制只能"间接"影响力。要精确指定"用 10 N 擦"，得闭合力反馈环。

常见误区

为什么力控制里一般不用 D（微分）？：力测量通常噪声很大，微分会把噪声放大到没法用。所以镇定作用靠速度阻尼项来提供。力控制律里往往是"力测量 + 速度测量"，必要时再加位置测量。

三种实现（都基于已有的逆动力学位置控制）：
9.4.1 内环位置
力控制器算出一个位置参考x_F= C_F(f_d− f_e) 送给位置环。
C_F必须含积分（PI），否则稳态时 f_e达不到 f_d，而且环境自然位置 x_r会一直影响结果。
代价：闭环变三阶。9.4.2 内环速度
把位置反馈环断开，x_F就变成速度参考，位置是速度的积分。
好处：力控制器用纯 P 就能做到稳态无差，而且闭环只有二阶，设计简单。
代价：没有积分作用，对未建模动态没有抑制能力。9.4.3 并联力/位控制
在上一条基础上并联一个位置作用：位置项 K_Px̃ 与力项 K_PC_F(f_d−f_e) 直接相加。
效果：不受约束的方向能走到 x_d；受约束的方向把 x_d当成一个额外扰动，牺牲位置去换力。

5. 约束运动：有些方向根本由不得你

的核心事实：**不可能在同一个方向上同时任意指定位姿和力。**环境说不许动，你就只能在那个方向上施力；环境说不许施力，你就只能在那儿动。

**刚性环境：**约束写成代数方程：

$$\varphi (q)=0\text{，}{J}_{\varphi }(q)\dot{q}=0\text{，}{h}_e=S_f(q)\lambda \text{，}{S}_f=J^{-T}(q)J_{\varphi }^T(q)$$

符号	含义
ϕ(q) = 0	完整约束（holonomic），m 个，m < n
J_ϕ	约束雅可比，m×6
S_f	力选择矩阵：把 m 个独立"约束力幅值" λ 映射成 6 维末端力 he
R(S_f)	力控子空间，维数 m
S_v	速度选择矩阵，满足S_fᵀ S_v = O；R(Sv) 是速度控子空间，维数 6−m

直觉上：6 个自由度被切成两半——m 个归"力"（环境定位置，你定力），6−m 个归"运动"（环境无力，你定位置）。S_f和 S_v就是把这 6 维空间劈开的那把刀，通常称为选择矩阵（selection matrices）。

停下来想一想

特意纠正的一个混淆：力和速度之间的关系叫互易性（reciprocity），h_eᵀ v_e = 0，不是正交！因为力和速度属于不同的向量空间（量纲都不同），谈"正交"没有意义。物理含义是：在刚性无摩擦接触下，约束力对一切满足约束的位移不做功。

**柔顺环境：**此时"力"和"变形"通过柔顺矩阵 C 联系，等效刚度：

$$K^{\prime }=S_f(S_f^{T}C{S}_f{)}^{-1}{S}_f^T\text{，}{C}^{\prime }=(I_6-P_v)C$$

6. 自然约束与人工约束

把上面的抽象说法变成"填表"操作。先建立一个约束坐标系O_c–x_cy_cz_c，让它的轴正好贴着任务几何。
自然约束（Natural）
由任务几何直接决定，不是你选的。
· 速度约束：这个方向不许你动（如墙面不许你穿过去）；
· 力约束：这个方向不许你施力（如光滑墙面切向无力）。人工约束（Artificial）
由你的控制策略决定的参考值。
只能指定那些不受自然约束的量。
两组约束互补：每个自由度恰有一个归自然、一个归人工，合起来覆盖全部 6 个变量。
给了三个典型案例，这里列出两个：

任务	自然约束（环境说了算）	人工约束（你来指定）	m
平面上滑动	żc、ωx、ωy、fx、fy、μz	fz、μx、μy、ẋc、ẏc、ωz	3
圆柱销插孔	ẋc、ẏc、ωx、ωy、fz、μz	fx、fy、μx、μy、żc、ωz	4
摇曲柄	ẋc、żc、ωx、ωy、fy、μz	fx、fz、μx、μy、ẏc、ωz	4

插孔任务里，人工约束中ż_c ≠ 0表示"往里插"，其余（f_x、f_y、μ_x、μ_y、ω_z）通常取 0——径向力和力矩给 0，就是"我不用力顶孔壁，让孔壁自己把我导向正确位置"。这正是 RCC 被动柔顺装置的思路，只不过现在用控制实现。

7. 混合力/运动控制

有了 S_f和 S_v，就可以把 6 维任务"劈"成两半，各用各的控制律。先把动力学写在你已经熟悉的末端形式：

$$B_e(q){\dot{v}}_e+n_e(q,\dot{q})={\gamma }_e-h_e\text{，}{B}_e=J^{-T}B{J}^{-1}$$

然后用逆动力学 + 按子空间分解的输入：

$$\alpha =S_v{\alpha }_{\nu }+C^{\prime }{S}_f{f}_{\lambda }⟹\dot{\nu }={\alpha }_{\nu }\text{，}\ddot{\lambda }=f_{\lambda }$$

看结果：**速度通道和力通道被彻底解耦，变成两个互不干扰的小问题。**然后各自配一个线性控制律（、9.81）：

$$\begin{array}{r}{\alpha }_{\nu }={\dot{\nu }}_d+K_{P\nu }({\nu }_d-\nu )+K_{I\nu }\int ({\nu }_d-\nu )d\varsigma ｜\text{速度通道：}PI\\ f_{\lambda }={\ddot{\lambda }}_d+K_{D\lambda }({\dot{\lambda }}_d-\dot{\lambda })+K_{P\lambda }({\lambda }_d-\lambda )｜\text{力通道：}PD\end{array}$$

刚性环境下更简单：直接用f_λ = λ_d，或加 P / 加 I 反馈（–9.94），其中 I 项专门对付常值偏置扰动。

停下来想一想：稳态时阻抗方程退化成哪个式子，从而和主动柔顺控制完全一致？插孔任务里力控子空间 R(S_f) 的维数 m = 4，那么留给速度控制的是哪几个自由度？

三、关键公式

小讲 5：阻抗公式链、三个矩阵的"手感"，以及它的稳态退化

阻抗控制的完整公式链

$$\begin{array}{r}\text{阻抗控制（}/9.31/9.32\text{）：}\\ u=B(q)y+n(q,\dot{q})+J^T(q)h_e\\ y=J_A^{-1}(q)M_d^{-1}[M_d{\ddot{x}}_d+K_D\dot{x\stackrel{~}{}}+K_P\tilde{x}-M_d{\dot{J}}_A\dot{q}-h_A]\\ ⟹M_d\ddot{x\stackrel{~}{}}+K_D\dot{x\stackrel{~}{}}+K_P\tilde{x}=h_A\end{array}$$

Md、KD、KP 分别管什么手感

符号	含义
M_d	期望惯性矩阵（正定对角）——你希望末端"感觉起来多重"
K_D	期望阻尼矩阵——你希望末端"感觉起来多黏"
K_P	期望刚度矩阵——你希望末端"感觉起来多硬"
h_A	等效接触力（在解析雅可比意义下的表达）
x̃	操作空间位姿误差x_d − x_e

直觉上：**这三个矩阵就是你给机器人"捏"出来的性格。**想让它擦玻璃时温柔一点，就把 K_P调小、K_D调大；想让它钻孔时顶得住，就把 K_P调大。

稳态时退化：阻抗控制的静态行为就是柔顺

提示

一个重要的退化关系：稳态时（ẍ̃ = 0, ẋ̃ = 0），阻抗方程退化成K_P x̃ = h_A—— 这跟主动柔顺的关系完全一样。 也是这么说的：静态时，阻抗控制的表现和主动柔顺控制一致；阻抗控制多的，是那一段"撞上去的过程"。

停下来想一想：想让末端擦玻璃时更软，该把 K_P调大还是调小？三个矩阵取 M_d= 100、K_D= 500、K_P= 2500 时，自由运动方向的 ω_n和 ζ 各是多少？

小讲 6：两张图——阻抗的物理模型，与插孔任务里的选择矩阵

图 2：混合力/位控制的选择矩阵示意（插孔任务，对应与表 9.2）。孔轴方向 zc 自由，所以控速度 żc；径向 xc、yc 被孔壁卡死，位置由不得你，所以改控力并令 fx、fy ≈ 0，让孔壁反力自动把销导正。

图 2：插孔任务里 Sf 与 Sv 怎么分工

停下来想一想：图 1 里 K_P、K_D、M_d三个元件分别挂在什么位置？图 2 里为什么 z_c方向控速度、x_c/y_c方向改控力？

四、例题 / 动手

例 1：机器人刚度 vs 环境刚度，谁说了算？

例 9.1 的一维版本。销沿 x 方向顶一个弹性平面，环境自然位置x_r = 1.0 m，控制器给的期望位置x_d = 1.1 m（即"多走了" 0.1 m）。

$$\begin{array}{r}\text{稳态接触力：}f=(k_{Px}{k}_x/(k_{Px}+k_x))\cdot (x_d-x_r)\\ \text{稳态位置：}{x}_e=(k_{Px}{x}_d+k_x{x}_r)/(k_{Px}+k_x)\end{array}$$

情形 A —— 机器人硬（k_Px= 10⁵N/m，k_x= 10³N/m，比值 100 ≫ 1）：

$$\begin{array}{r}{x}_e=(10^5\times 1.1+10^3\times 1.0)/1.01\times 10^5=111000/101000\approx 1.099m\\ f=(10^5\times 10^3/1.01\times 10^5)\times 0.1\approx 990\times 0.1\approx 99N\end{array}$$

末端几乎走到了 x_d，接触力几乎完全由环境的弹簧产生（k_x× 0.1 = 100 N）。机器人赢了。

情形 B —— 环境硬（k_Px= 100 N/m，k_x= 10⁴N/m，比值 0.01 ≪ 1）：

$$\begin{array}{r}{x}_e=(100\times 1.1+10^4\times 1.0)/10100=10110/10100\approx 1.001m\\ f=(100\times 10^4/10100)\times 0.1\approx 99\times 0.1\approx 9.9N\end{array}$$

末端几乎停在环境的自然位置 x_r，接触力几乎完全由机器人自己的弹簧产生（k_Px× 0.1 = 10 N）。环境赢了。

提示

**工程含义：**想让接触力小、又不至于把零件顶坏？把机器人做软（K_P 取小）。想让定位准、环境该让路？把机器人做硬（K_P 取大）。这就是"主动柔顺"的全部操作——改一个数，就能切换机器人的"软硬性格"。

例 2：用阻抗方程反推"手感"

假设末端等效为一维阻抗，M_d = 100，K_D = 500，K_P = 2500。

**第 1 步 —— 自由运动方向：**没有接触力，方程是M_d ẍ̃ + K_D ẋ̃ + K_P x̃ = 0。对比标准二阶型：

$$\begin{array}{r}{\omega }_n=\sqrt{K_P/M_d}=\sqrt{2500/100}=5rad/s\\ \zeta =K_D/(2\sqrt{M_d{K}_P})=500/(2\times \sqrt{250000})=500/1000=0.5\end{array}$$

和给出的 ω_ny= 5、ζ_y= 0.5 完全一致。

**第 2 步 —— 接触方向：**环境弹簧 k_x与 K_P并联，于是刚度变成K_P + k_x：

$$\begin{array}{r}{k}_x=10^3\text{：}{\omega }_n=\sqrt{3500/100}\approx 5.9\text{，}\zeta =500/(2\sqrt{350000})\approx 0.42\\ k_x=10^4\text{：}{\omega }_n=\sqrt{12500/100}\approx 11.2\text{，}\zeta =500/(2\sqrt{1250000})\approx 0.22\end{array}$$

同样和一致。环境越硬，接触方向越"欠阻尼"——撞上去更容易来回振荡。

**动手想一想：**如果接触方向振荡太厉害，你该调哪个参数？

提示：ζ = K_D/ (2√(M_d(K_P+k_x)))。环境刚度 k_x你改不了，所以要么加大 K_D（多给点阻尼），要么减小 K_P（软一点），要么加大 M_d（但响应会变慢）。 正是这么说的：越想把接触力压小（K_P越小），系统抗扰动能力就越差——这就是阻抗控制的核心取舍。

停下来想一想：情形 A 里机器人比环境硬 100 倍，接触力约 99 N 是由谁的刚度乘出来的？接触方向上环境刚度 k_x变大时，ζ 为什么会掉下去？

五、常见误区

柔顺只描述静态，阻抗描述整个过程

常见误区

误区 1：以为"柔顺"和"阻抗"是一回事。

柔顺只描述静态关系h = K_P x̃，回答"停稳之后力多大"。阻抗描述完整的动态关系M_d ẍ̃ + K_D ẋ̃ + K_P x̃ = h，回答"撞上去的整个过程怎么走"。前者是后者的稳态特例。

阻抗控制不闭合力反馈环

常见误区

误区 2：以为阻抗控制"能精确控制力"。

不能。 把它归为间接力控制——它是不闭合力反馈环的。虽然公式里出现了 h，那是为了测力补偿，不是力反馈。要精确指定"用 10 N"，得用的直接力控制。

互易性不等于正交

常见误区

误区 3：把互易性当成正交。

特意纠正：h_eᵀ v_e = 0叫互易（reciprocity），不是正交。力和速度属于不同向量空间（量纲都不一样），"正交"在这里没有定义。它的物理含义是刚性无摩擦接触下约束力不做功。

接触任务里"准"不是美德

常见误区

误区 4：以为位置环越准越好。

本讲开头的死循环说明：**环境越硬 + 位置控制越准 = 越容易出事。**接触任务里"准"不是美德，"该软的时候能软"才是。

停下来想一想：h_e^Tv_e= 0 的物理含义是什么，为什么不能叫"正交"？要精确指定"用 10 N 擦玻璃"，该用本讲哪一节的方案？

七、本讲小结

停下来想一想：插孔任务的人工约束里，f_x、f_y、μ_x、μ_y通常取什么值？这样取是为了让谁把销"导正"？

要点回顾

1. **为什么需要力控制：**接触任务下，纯位置控制会陷入"误差→力→更大误差→更大力"的死循环，直到电机饱和或零件损坏。接触力 h_e才是描述交互状态最完整的量。
2. **柔顺控制：**位置控制本身就是一根广义弹簧（h_A = K_P x̃）。被动柔顺靠 RCC 这类机械装置，插入方向硬、其余方向软，缺点是换任务要换硬件；主动柔顺靠改 K_P，改软件即可。
3. 阻抗控制（本讲核心）：M_d ẍ̃ + K_D ẋ̃ + K_P x̃ = h_A。**不直接控力，而是给末端"捏"出一个质量–弹簧–阻尼性格。**M_d定惯性感、K_D定阻尼感、K_P定软硬。稳态时退化为柔顺关系。
4. **直接力控制：**内环位置时 C_F必须含积分才能稳态无差；内环速度时位置环断开、x_F变成速度参考，纯 P 即可无差；并联力/位控制则把位置作用和力作用直接相加。
5. 约束运动与混合控制：刚性环境用选择矩阵 S_f（力控子空间，维数 m）和 S_v（速控子空间，维数 6−m）把 6 维空间劈开，S_fᵀ S_v = O；任务描述用自然约束（任务几何决定）+人工约束（你指定），两者互补且覆盖全部变量。

**一句话总结：**接触任务里，该硬的方向控位置，该软的方向控力；而"软得有多自然"，就靠给末端装一个虚拟的弹簧–阻尼–质量系统——这就是阻抗控制。