机器人与环境的物理交互是实现复杂操作任务的核心挑战。传统的位置控制在自由空间运动时表现优异，但在接触任务中往往力不从心——过高的位置增益可能导致过大的接触力，甚至损坏机器人或环境。本章系统介绍阻抗控制理论，这是一种优雅地统一位置与力控制的框架，通过调节机器人的动态行为来实现柔顺且稳定的物理交互。我们将深入探讨不同控制模式的数学基础、稳定性分析以及工程实现中的关键问题。
在机器人控制领域，存在三种基本的控制范式，每种都有其独特的优势和适用场景。
位置控制（Position Control） 是最直观的控制方式，控制器的目标是让机器人末端执行器达到期望位置 $\mathbf{x}_d$：
\[\boldsymbol{\tau} = \mathbf{K}_p(\mathbf{x}_d - \mathbf{x}) + \mathbf{K}_d(\dot{\mathbf{x}}_d - \dot{\mathbf{x}}) + \mathbf{g}(\mathbf{q})\]其中 $\mathbf{K}_p$ 和 $\mathbf{K}_d$ 分别是位置和速度增益矩阵，$\mathbf{g}(\mathbf{q})$ 是重力补偿项。这种控制方式在自由空间运动中效果excellent，但在接触环境中会产生问题：微小的位置误差会导致巨大的接触力。
力控制（Force Control） 直接控制机器人与环境的接触力 $\mathbf{F}_d$：
\[\boldsymbol{\tau} = \mathbf{J}^T(\mathbf{F}_d + \mathbf{K}_f \int (\mathbf{F}_d - \mathbf{F}) dt)\]其中 $\mathbf{J}$ 是雅可比矩阵，$\mathbf{K}_f$ 是力控制增益。纯力控制在接触任务中表现出色，但无法控制运动方向上的位置。
阻抗控制（Impedance Control） 则是一种更加灵活的方法，它不直接控制位置或力，而是控制位置与力之间的动态关系：
\[\mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{C}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{F}_{ext}\]这里 $\mathbf{M}d$、$\mathbf{C}_d$、$\mathbf{K}_d$ 分别是期望的惯性、阻尼和刚度矩阵，$\mathbf{F}{ext}$ 是外部作用力。
场景类型 位置控制 力控制 阻抗控制
--------------------------------------------
自由空间运动 优秀 差 良好
硬接触 差 优秀 良好
软接触 一般 良好 优秀
装配任务 差 一般 优秀
人机协作 危险 安全 最安全
计算复杂度 低 中 高
传感器需求 编码器 力传感器 两者皆需
考虑一个n自由度机械臂的动力学方程：
\[\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}^T\mathbf{F}_{ext}\]在阻抗控制中，我们希望末端执行器表现出期望的阻抗特性。通过逆动力学控制，可以设计控制律：
\[\boldsymbol{\tau} = \mathbf{M}(\mathbf{q})\mathbf{J}^{-1}[\ddot{\mathbf{x}}_d - \dot{\mathbf{J}}\dot{\mathbf{q}}] + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) - \mathbf{J}^T\mathbf{F}_{cmd}\]其中 $\mathbf{F}_{cmd}$ 是根据阻抗控制律计算的虚拟力：
\[\mathbf{F}_{cmd} = \mathbf{M}_d^{-1}[\mathbf{K}_d(\mathbf{x}_d - \mathbf{x}) + \mathbf{C}_d(\dot{\mathbf{x}}_d - \dot{\mathbf{x}}) - \mathbf{F}_{ext}]\]笛卡尔阻抗控制直接在任务空间定义期望的动态行为。对于6维任务空间（3维位置+3维姿态），期望阻抗可以表示为：
\[\begin{bmatrix} \mathbf{F} \\ \boldsymbol{\tau}_e \end{bmatrix} = \begin{bmatrix} \mathbf{K}_p & \mathbf{0} \\ \mathbf{0} & \mathbf{K}_o \end{bmatrix} \begin{bmatrix} \Delta \mathbf{p} \\ \Delta \boldsymbol{\phi} \end{bmatrix} + \begin{bmatrix} \mathbf{D}_p & \mathbf{0} \\ \mathbf{0} & \mathbf{D}_o \end{bmatrix} \begin{bmatrix} \Delta \dot{\mathbf{p}} \\ \Delta \boldsymbol{\omega} \end{bmatrix}\]其中 $\Delta \mathbf{p}$ 是位置误差，$\Delta \boldsymbol{\phi}$ 是姿态误差（可用轴角表示），$\mathbf{K}_p$、$\mathbf{K}_o$ 是位置和姿态刚度，$\mathbf{D}_p$、$\mathbf{D}_o$ 是相应的阻尼矩阵。
姿态误差的计算需要特别注意，常用的方法是通过四元数：
\[\Delta \boldsymbol{\phi} = 2 \cdot \text{vec}(\mathbf{q}_d \otimes \mathbf{q}^{-1})\]其中 $\text{vec}(\cdot)$ 提取四元数的向量部分。
关节阻抗控制在关节空间定义期望行为：
\[\boldsymbol{\tau} = \mathbf{K}_q(\mathbf{q}_d - \mathbf{q}) + \mathbf{D}_q(\dot{\mathbf{q}}_d - \dot{\mathbf{q}}) + \mathbf{g}(\mathbf{q})\]关节阻抗与笛卡尔阻抗之间通过雅可比矩阵相关联：
\[\mathbf{K}_x = (\mathbf{J}^{-T} \mathbf{K}_q \mathbf{J}^{-1})|_{\mathbf{q}=\mathbf{q}_0}\]这个关系表明，关节刚度通过运动学配置影响笛卡尔刚度，在奇异配置附近，笛卡尔刚度会显著降低。
对于冗余机器人（n > 6），可以利用零空间调节刚度椭球的形状而不改变末端位置：
\[\boldsymbol{\tau} = \mathbf{J}^T \mathbf{F}_{task} + (\mathbf{I} - \mathbf{J}^T \mathbf{J}^{T\dagger}) \boldsymbol{\tau}_{null}\]其中 $\mathbf{J}^{T\dagger} = (\mathbf{J}\mathbf{J}^T)^{-1}\mathbf{J}$ 是雅可比的伪逆，$\boldsymbol{\tau}_{null}$ 是零空间力矩，可用于：
当机器人与环境接触时，系统动力学变为：
机器人 环境
↓ ↓
M_r ẍ + C_r ẋ + K_r x = F_c
↑
F_c = K_e(x - x_e) + C_e ẋ
闭环系统的稳定性取决于机器人阻抗 $Z_r(s)$ 与环境阻抗 $Z_e(s)$ 的相互作用：
\(Z_r(s) = M_r s^2 + C_r s + K_r\) \(Z_e(s) = C_e s + K_e\)
Hogan的稳定性准则指出，对于稳定的接触：
稳定性可通过Nyquist图分析，避免 $Z_r(j\omega) + Z_e(j\omega) = 0$ 的情况。
为保证被动性，阻抗参数必须满足：
\[\mathbf{M}_d > 0, \quad \mathbf{K}_d \geq 0, \quad \mathbf{C}_d \geq 2\sqrt{\mathbf{M}_d \mathbf{K}_d}\]最后一个条件确保系统是过阻尼或临界阻尼的。在实际实现中，通常选择：
\[\mathbf{C}_d = 2\zeta\sqrt{\mathbf{M}_d \mathbf{K}_d}\]其中 $\zeta \in [0.7, 1.0]$ 是阻尼比。
Mason提出的选择矩阵 $\mathbf{S}$ 允许在不同方向上分别进行力控制和位置控制：
\[\boldsymbol{\tau} = \mathbf{J}^T[\mathbf{S}\mathbf{F}_d + (\mathbf{I} - \mathbf{S})\mathbf{F}_p]\]其中：
例如，对于平面抛光任务：
S = diag(1, 1, 0, 1, 1, 1)
↑ ↑ ↑ ↑ ↑ ↑
x y z α β γ
表示z方向力控制，其他方向位置控制。
De Schutter和Bruyninckx提出的约束框架通过定义约束雅可比 $\mathbf{J}_c$ 来描述任务：
\[\mathbf{J}_c \dot{\mathbf{x}} = \mathbf{0}\]控制律设计为：
\[\boldsymbol{\tau} = \mathbf{J}^T_c \boldsymbol{\lambda} + (\mathbf{I} - \mathbf{J}^T_c \mathbf{J}^{T\dagger}_c)\mathbf{J}^T \mathbf{F}_{motion}\]其中 $\boldsymbol{\lambda}$ 是约束力，$\mathbf{F}_{motion}$ 控制约束允许的运动。
在力/位置控制模式切换时，需要注意：
切换逻辑示例：
if (contact_detected && F_normal > F_threshold):
S[2] = 0 # 切换到z方向力控制
integral_force_z = 0 # 重置积分器
F_d[2] = F_normal # 设置期望力为当前接触力
德国宇航中心（DLR）的轻型机器人系列（LWR-III/IV、KUKA LBR iiwa）是阻抗控制领域的标杆。这些机器人专为安全人机协作设计，面临的核心挑战包括：
DLR机器人的独特设计：
关节传感器配置：
┌─────────────────────────┐
│ 电机侧编码器（19位） │ ← 电机位置 θ_m
├─────────────────────────┤
│ 谐波减速器（100:1） │ ← 弹性元件 K_joint
├─────────────────────────┤
│ 连杆侧编码器（12位） │ ← 连杆位置 θ_l
├─────────────────────────┤
│ 关节力矩传感器 │ ← 力矩测量 τ_sensor
└─────────────────────────┘
双编码器设计允许直接测量关节弹性变形： \(\tau_{elastic} = K_{joint}(\theta_m / N - \theta_l)\)
其中 $N$ 是减速比。
DLR采用分层控制架构：
第1层：关节力矩控制（3kHz）
τ_cmd = τ_model + K_τ(τ_d - τ_sensor) + D_τ(τ̇_d - τ̇_sensor)
第2层：笛卡尔阻抗控制（1kHz）
F_d = M_d(ẍ_d - ẍ) + C_d(ẋ_d - ẋ) + K_d(x_d - x) + F_ext
τ_d = J^T F_d + null_space_torque
第3层：任务规划（100Hz）
振动抑制：通过状态反馈抑制弹性关节振动 \(\tau_{motor} = \tau_{desired} + K_{damp}(\dot{\theta}_m - \dot{\theta}_l)\)
碰撞检测：基于动量观测器 \(r = K_I \int (\tau_{ext} - \tau_{threshold}) dt\) 当 $|r| > r_{max}$ 时触发安全反应。
可变刚度控制：根据任务阶段自适应调节
装配接近阶段：K = 1000 N/m
接触阶段： K = 200 N/m
插入阶段： K = 50 N/m
DLR LBR iiwa的典型性能：
传统阻抗控制使用整数阶微分，分数阶控制引入非整数阶微分算子：
\[M_d s^2 + C_d s^{\alpha} + K_d = Z_d(s), \quad 0 < \alpha < 2\]分数阶微分的Caputo定义： \(D^{\alpha} f(t) = \frac{1}{\Gamma(n-\alpha)} \int_0^t \frac{f^{(n)}(\tau)}{(t-\tau)^{\alpha-n+1}} d\tau\)
其中 $n-1 < \alpha < n$，$\Gamma(\cdot)$ 是伽马函数。
优势：
实现方法： Oustaloup近似将分数阶算子离散化： \(s^{\alpha} \approx K \prod_{k=-N}^{N} \frac{s + \omega_k'}{s + \omega_k}\)
其中频率点按几何级数分布。
自适应阻抗根据环境特性和任务需求实时调节参数：
基于力跟踪误差的自适应律： \(\dot{K}_d = -\gamma_K \mathbf{e}_F \mathbf{e}_x^T\) \(\dot{C}_d = -\gamma_C \mathbf{e}_F \dot{\mathbf{e}}_x^T\)
其中 $\mathbf{e}_F = \mathbf{F}_d - \mathbf{F}$ 是力误差，$\mathbf{e}_x = \mathbf{x}_d - \mathbf{x}$ 是位置误差。
基于强化学习的阻抗优化： 使用策略梯度方法优化阻抗参数：
状态 s = [e_x, ė_x, F_ext, Ḟ_ext]
动作 a = [ΔK_d, ΔC_d, ΔM_d]
奖励 r = -w_1||e_F||² - w_2||e_x||² - w_3||a||²
人类手臂阻抗调节机制：
仿生实现： \(K_{arm} = K_{base} + \alpha \cdot co\_contraction + \beta \cdot reflex\_gain\)
本章系统介绍了机器人阻抗控制的理论基础与工程实现。关键要点包括：
核心公式汇总：
阻抗控制基本方程： \(\mathbf{M}_d(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d) + \mathbf{C}_d(\dot{\mathbf{x}} - \dot{\mathbf{x}}_d) + \mathbf{K}_d(\mathbf{x} - \mathbf{x}_d) = \mathbf{F}_{ext}\)
稳定性条件： \(\mathbf{C}_d \geq 2\sqrt{\mathbf{M}_d \mathbf{K}_d}\)
笛卡尔-关节刚度映射： \(\mathbf{K}_x = \mathbf{J}^{-T} \mathbf{K}_q \mathbf{J}^{-1}\)
练习10.1 一个2自由度平面机械臂，连杆长度均为0.5m，当前配置为 $q_1 = 30°$，$q_2 = 60°$。如果关节刚度为 $\mathbf{K}_q = \text{diag}(100, 50)$ Nm/rad，计算末端笛卡尔刚度矩阵。
Hint: 先计算雅可比矩阵，然后应用刚度变换公式。
练习10.2 设计一个阻抗控制器，使机器人末端在接触刚度为 $K_e = 10000$ N/m 的环境时保持稳定。期望接触力为10N，选择合适的阻抗参数。
Hint: 考虑阻抗比 $K_d/K_e$ 应该远小于1。
练习10.3 实现一个平面抛光任务的混合力/位置控制。机器人需要在xy平面内跟踪轨迹，同时在z方向保持5N的接触力。写出选择矩阵和控制律。
Hint: z方向力控制，xy方向位置控制。
练习10.4 推导七自由度机械臂利用零空间调节末端刚度椭球方向的控制律。假设主任务是保持末端位置不变，次任务是最大化某个方向的刚度。
Hint: 使用梯度投影法在零空间优化刚度指标。
练习10.5 分析分数阶阻抗 $C_d s^{1.5}$ 相比整数阶阻尼 $C_d s$ 的频率响应特性。绘制Bode图并讨论物理意义。
| Hint: 计算 $ | s^{1.5} | $ 和 $\angle s^{1.5}$ 在 $s = j\omega$ 时的值。 |
练习10.6（开放题）设计一个自适应阻抗控制器用于未知刚度表面的接触任务。控制器应能在线估计环境刚度并相应调整机器人阻抗。
Hint: 使用递归最小二乘(RLS)估计环境参数。
练习10.7 证明在接触过程中，如果机器人阻抗 $Z_r(s)$ 和环境阻抗 $Z_e(s)$ 都是严格正实的(SPR)，则闭环系统是稳定的。
Hint: 使用Passivity定理和Lyapunov方法。
练习10.8 为双臂协调操作设计阻抗控制策略。两个机械臂需要协同搬运一个刚性物体，如何分配内力和运动控制？
Hint: 定义虚拟刚度连接两臂，使用主从或对称控制架构。
错误：K_d = [1000, 500; 200, 800] # 非对称！
正确：K_d = [1000, 350; 350, 800] # 对称正定
if (det(J*J') < 0.001): # 接近奇异
use damped_inverse # 使用阻尼逆
K_joint = 100 Nm/rad # 关节空间
K_cart = 100 N/m # 笛卡尔空间
# 不能直接比较！
本章完成。阻抗控制是现代机器人实现安全、柔顺交互的核心技术。掌握其原理与实现细节，对于开发下一代智能机器人系统至关重要。