一、阻抗控制器（Impedance Controller）
1. 弹簧阻尼系统的例子
Fig. 1 弹簧阻尼系统
如图 1 (a) 所示的弹簧阻尼系统，设弹簧的自然长度为 x_0，当对系统施加外力 F_ext 时，系统动力学方程为： m\ddot{x}=F_{ext}+K(x-x_{0})+D\dot{x} \tag{1}
现在的问题： 给定图 1 (b) 所示的一个物体，质量为 M，物体上有位移传感器可以实时的测量到物体的位置 x，还有一个力传感器可以实时的测量物体受到的外力 F_ext，请问你可以设计控制率使得物体 M 在受到外力 F_ext 时，和图 1 (a) 中的物体 m 的动力学特性（即物体 M 的运动也满足 (1) 式）一样吗？
我们可以设计一个力控制器，力控制器的输出的控制量为力 F，此时物体 M 满足的动力学方程为： M\ddot{x}=F+F_{ext} \tag{2}
将 (1) 式变形为： \ddot{x}=\frac{F_{ext}+K(x-x_{0})+D\dot{x}}{m} \tag{3}
注意，(3) 式中的 F_ext 可以从力传感器中直接测量，x 可以从位置传感器中直接测量，\dot{x} 可以对 x 求微分得到。
将 (3) 式代入 (2) 式，得到控制量 F 为： F=M\frac{F_{ext}+K(x-x_{0})+D\dot{x}}{m}-F_{ext} \tag{4}
此时，我们可以 (4) 式代入 (2) 式，发现： \ddot{x} = \frac{F_{ext}+K(x-x_{0})+D\dot{x}}{m} \tag{5}
即物体 M 的动力学特性和图 1 (a) 中的物体 m 的动力学特性一样。
如果图 1 (a) 中的 F_ext 是屏幕前的你给的，你会感觉到在推一个弹簧阻尼系统。同时，你又以同样的力 F_ext 来推图 1 (b) 中的物体 M，此时系统由 (4) 式的控制率控制，你将会得到和推图 1 (a) 同样的感受。
自动控制让我们在推动不同质量的物体时，得到了相同的感受，同时，系统也得到了同样的动力学响应。另外，一般如果系统是位置控制（我们的很多系统都是纯位置控制），位置控制的优点是位置精度高，缺点是不安全（尤其是在人机交互过程中）。如果我们按照上述图 2 (b) 中所介绍的，将系统虚拟成一个弹簧阻尼系统，就安全很多（因为它有了弹簧阻尼系统的动力学特性）。
2. 统一的阻抗控制器框架
Fig. 2 阻抗控制器控制框图
在图 2 中，x_d 是期望位移，F_ext 是作用在系统的外力（必须可以得到，或者直接通过力传感器，或者通过观测器），x 是系统的真实位移（必须可以得到，直接通过传感器，一般系统都有位置传感器），阻抗控制器的输出为控制力 F。
为了简单直观，不妨设： 系统动力学方程（Plant Dynamics）： F+F_{ext}=m\ddot{x} \tag{6}
即系统只由一个物体 m 组成，外力 F_ext 和控制力直接作用在这个物体 m 上。
期望的系统动力学特性 （Desired System Dynamics): M_{d}\ddot{e}+D_{d}\dot{e}+K_{d}e=F_{ext} \tag{7}
其中 e = x_d - x，M_d, D_d, K_d 为期望的质量系数，阻尼系数，刚度系数。
我们注意到，\ddot{e}=\ddot{x}{d}-\ddot{x}，代入到 (6) 中可以得到： F+F{ext} = m(\ddot{e}+\ddot{x}_{d}) \tag{8}
与 1. 弹簧阻尼系统的例子 中介绍的一样，将 (7) 式变形为： \ddot{e} = \frac{F_{ext}-D_{d}\dot{e}-K_{d}e}{M_{d}} \tag{9}
将 (9) 式代入 (8) 式得到： F=m(\frac{F_{ext}-D_{d}\dot{e}-K_{d}e}{M_{d}}+\ddot{x}{d})-F{ext} \tag{10}
(10) 式中的 \dot{e} 可以通过 \dot{x}-\dot{x}_{d} 计算得到。
将 (10) 式的控制力代入 系统动力学方程(6) 式中，可以得到 期望的系统动力学特性(7) 式。
3. 机械臂关节空间的阻抗控制器
Fig. 3 机械臂关节空间阻抗控制器的控制框图
对于机械臂的关节空间： 关节空间的动力学方程（Plant Dynamics in Joint Space）： \mathbf{\tau} + \mathbf{\tau}_{ext} = \mathbf{M}(\mathbf{q})\mathbf{\ddot{q}}+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{q}}+\mathbf{G(q)} \tag{11}
期望的关节空间动力学特性（Desired Dynamics in Joint Space）： \mathbf{M}{d}\mathbf{\ddot{e}}+\mathbf{D}{d}\mathbf{\dot{e}}+\mathbf{K}{d}\mathbf{e}=\mathbf{\tau}{ext} \tag{12}
其中 e = q_d - q，M_d, D_d, K_d 分别为质量系数矩阵，阻尼系数矩阵，刚度系数矩阵。
与上述的计算同理，将 \ddot{q}=\ddot{q}{d}-\ddot{e} 代入 关节空间的动力学方程(11) 式中，得到： \mathbf{\tau} + \mathbf{\tau}{ext} = \mathbf{M}(\mathbf{q})\mathbf{(\ddot{q}}_{d}-\mathbf{\ddot{e}})+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{q}}+\mathbf{G(q)} \tag{13}
再将 (12) 式变形为 \ddot{e}=\mathbf{M}{d}^{-1}(\mathbf{\tau}{ext} -\mathbf{D}{d}\mathbf{\dot{e}}-\mathbf{K}{d}\mathbf{e}) 代入到 (13) 式中，可以得到阻抗控制器的力矩控制量 \mathbf{\tau} 为： \mathbf{\tau}=\mathbf{M(q)}[\mathbf{\ddot{q}}{d}-\mathbf{M}{d}^{-1}(\mathbf{\tau}{ext}-\mathbf{D}{d}\mathbf{\dot{e}}-\mathbf{K}{d}\mathbf{e})]+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{q}}+\mathbf{G(q)}-\mathbf{\tau}{ext} \tag{14}
在 (14) 式中，M(q), C(q,\dot{q}) 和 G(q) 在系统建模阶段就已经知道，q 和 \mathbf{\tau}{ext} 通过传感器可测量出来，\mathbf{\dot{q}} 可以直接通过微分计算出来，\mathbf{\ddot{q}}{d} 是期望轨迹的二阶导数也是已知的，M_d, D_d 和 K_d 是我们预先设计好的系数矩阵。所以，(14) 式中右侧的所有变量都是已知的。使用 (14) 式的控制量代入 (11) 式中，可以发现系统的动力学特性变成了 (12) 式。
4. 机械臂笛卡尔空间的阻抗控制器
Fig. 4 机械臂笛卡尔空间阻抗控制器的控制框图
对于机械臂的笛卡尔空间，考虑机械臂有 6 个关节自由度（为了雅可比矩阵写起来方便，不然还牵扯雅可比矩阵伪逆）。 笛卡尔空间的动力学方程（Plant Dynamics in Cartesian Space）： \mathbf{M}(\mathbf{q})\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\ddot{x}}-\mathbf{M(q)}\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{J}(q)}\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{x}}+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{x}}+\mathbf{G(q)} = \mathbf{\tau} + \mathbf{J^{T}(q)}\mathbf{F}_{ext} \tag{15}
请参照文末 附录 中对于 (15) 式的推导。
期望的笛卡尔空间动力学特性（Desired Dynamics in Cartesian Space）： \mathbf{M}{d}\mathbf{\ddot{e}}+\mathbf{D}{d}\mathbf{\dot{e}}+\mathbf{K}{d}\mathbf{e}=\mathbf{\tau}{ext} \tag{16}
其中 e = x_d - x，M_d, D_d, K_d 分别为质量系数矩阵，阻尼系数矩阵，刚度系数矩阵。
和上述的计算过程一样，最终可以得到阻抗控制器的输出控制力矩 \mathbf{\tau} 为： \mathbf{\tau} = \mathbf{M}(\mathbf{q})\mathbf{J}^{-1}\mathbf{(q)}[\mathbf{\ddot{x}}{d}-\mathbf{M}{d}^{-1}(\mathbf{F}{ext}-\mathbf{D}{d}\mathbf{\dot{e}}-\mathbf{K}{d}\mathbf{e})-\mathbf{\dot{J}(q)}\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{x}}]+\ \mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{x}}+\mathbf{G(q)}-\mathbf{J^{T}(q)}\mathbf{F}{ext} \tag{17}
5. 阻抗控制器的小结
对于阻抗控制器，需要 系统动力学模型（阻抗控制率就是根据 系统动力学模型 和 期望动力学特性 计算出来的）。对系统的动力学模型的精度要求高，如果动力学模型偏差大，计算出来的控制力或者控制力矩会让系统发生非 期望动力学响应特性，甚至不稳定。
此外，当外力 F_ext 或者外力矩 \tau_{ext} 为 0 时，阻抗控制器会和位置控制器一样，由于 期望动力学特性 的保证（可以参考 (16) 式），实现精确的位置伺服。但是这是理想的情况，事实上，阻抗控制器常常并不能实现精确的位置伺服，因为动力学模型往往不是精确到和系统真实的动力学模型一模一样。所以有时候安全性与精度难以兼得。
我们知道的市面上的机械臂有一些支持阻抗控制器，比如 KUKA 机械臂，它的每个关节都装配了力矩传感器用来估计外力和外力矩。
二、导纳控制器（Admittance Controller）
1. 碰撞和拖拽的例子
Fig. 5 不同控制模式下的系统特性
如图 5 (a) 所示，物体在位置控制模式下，静止在某一个位置。当有外力和这个物体发生碰撞时，或者有外力试图拖拽这个物体，由于位置控制的内环（力矩环）会产生极大的力矩来抗拒偏离当前位置，所以结果是撞也撞不动，拉也拉不动。
但是在人机交互场景中，我们希望在发生碰撞或者拖拽的情况时，被控制的物体可以发生位移，避免硬碰硬的情况。上述的阻抗控制器可以做得到，但是我们也讨论了 阻抗控制器 需要 精确的系统动力学模型。
现在的问题： 有没有一种控制器，不需要精确的系统动力学模型，还能在被控制物体与外界发生碰撞和拖拽时，产生柔顺的位移？
导纳控制器的提出提供了一种方案。
2. 统一的导纳控制器框架
Fig. 6 导纳控制器控制框图
在图 6 中，x_d 是期望位移，F_ext 是作用于系统的外力，x 是系统的当前位置，x_ref 是导纳控制器的输出的参考位移。
简而言之，导纳控制器根据外力 F_ext 和期望位移 x_d 计算出参考位移 x_ref 发送给系统，系统的内部由位置控制器来完成对参考位移 x_ref 的精确伺服，即图 6 中的 Position Control 的虚线框 是一个稳定的位置伺服系统，且独立于导纳控制器。此外，可以发现整个过程不需要系统的动力学模型。
如果我们想要实现图 5 (b) 中的情况，即外力碰撞或者拖拽让物体发生位移，可以如何设计导纳控制器呢？ 一个简单的思路是设计一个比例控制器： K_{d}(x_{ref}-x_{d})=F_{ext} \tag{18}
其中，K_d > 0。这样，当外力 F_ext 发生碰撞或者拖拽时，x_ref 会在 x_d 的位置基础上，沿着 F_ext 的方向发生额外的位移 |\frac{F_{ext}}{K_{d}}|。这样就避免了硬碰硬。
但是，如果物体当前位置为 x_d，突然有一个较大的外力 F_ext 作用在物体上，计算出来的参考位移： x_{ref}=x_{d}+\frac{F_{ext}}{K_{d}} \tag{19}
这样导纳控制器输出给系统的位置控制系统一个较大的阶跃信号，让 图 6 中的 Position Control 去追踪，可以这样做但是不建议，一般有过伺服控制经验的人员都知道，一般给位置伺服系统发送的位置信号都需要进行插值让轨迹变得平滑，不要直接发送大的阶跃信号。
因为系统与外界交互时，外力 F_ext 的大小变化，没法预测，如果解决 (18) 式的比例控制器引入的问题呢？ 重新设计一个丝滑的控制器： M_{d}\ddot{e} + D_{d}\dot{e} + K_{d}e = F_{ext} \tag{20}
其中，e = x_{ref}-x_{d}，M_d, D_d 和 K_d 是我们人为设计的，使得这个二阶系统稳定。
(20) 式的设计方法的优点请看下图 Fig. 7，
Fig. 7 导纳控制器对于不同外力情况下的响应特性
如图 7 (a) 所示，当外力 F_ext = 0 时，导纳控制器输出的参考位移 x_ref 收敛到期望位移 x_d，整个系统退化为纯位置控制。如图 8 (b) 所示，当外力 F_ext 为非零常数 C 时，导纳控制器输出的参考位移 x_ref 收敛到与期望位移 x_d 的稳态误差为 \frac{C}{K_{d}}。当外力 F_ext 剧烈变化时（即存在高频分量），此时 (20) 式可以看作是一个二阶低通滤波器（滤波器的形式为 \frac{1}{M_{d}s^{2}+D_{d}s+K_{d}}，滤波器输入为 F_ext，滤波器输出为 e，这个滤波器是不是和二阶巴特沃斯低通滤波器很像？），可以抑制 F_ext 的高频分量，输出平滑的 e。
值得注意的是，上述的 e 都是平滑连续的（由 (20) 式的二阶微分方程保证的），期望轨迹 x_d 是我们人为插值后输入的，也是平滑连续的，所以参考轨迹 x_{ref}=e+x_{d} 也是平滑连续的。这样，图 6 中的 Position Control 的输入平滑连续，保证了系统的位置伺服的连续稳定。
3. 机械臂关节空间的导纳控制器
Fig. 8 机械臂关节空间导纳控制器的控制框图
关节空间导纳控制器（Admittance Controller in Joint Space）： \mathbf{M}{d}\mathbf{\ddot{e}}+\mathbf{D}{d}\mathbf{\dot{e}}+\mathbf{K}{d}\mathbf{e}=\mathbf{\tau}{ext} \tag{21}
其中，e = q_{ref}-q_{d}，M_d, D_d, K_d 分别是用户设计的参数。
4. 机械臂笛卡尔空间的导纳控制器
Fig. 9 机械臂笛卡尔空间导纳控制器的控制框图
笛卡尔空间导纳控制器（Admittance Controller in Cartesian Space）： \mathbf{M}{d}\mathbf{\ddot{e}}+\mathbf{D}{d}\mathbf{\dot{e}}+\mathbf{K}{d}\mathbf{e}=\mathbf{F}{ext} \tag{22}
其中，e = x_{ref}-x_{d}，M_d, D_d, K_d 分别是用户设计的参数。
5. 导纳控制器的小结
导纳控制器的实现非常的简单，需要系统受到的外力 F_ext 或者外力矩 \tau_{ext}（一般由力传感器直接测量得到），和系统的期望位移 x_d，代入导纳控制器中，就可以计算出参考位置 x_{ref}。
当系统受到的外力 F_ext 或外力矩 \tau_{ext} 为 0 时，整个系统就变成了纯位置控制，此时理论上系统可以实现精确的位置伺服，参考图 7 (a)。
阻抗控制和导纳控制的对比表格如下，
| 控制方式 | 控制对象 | 控制器设计需求 |
|---|---|---|
| 阻抗控制 | 机器人施加的力/力矩 | 精确的系统动力学模型，位置传感器，力传感器 |
| 导纳控制 | 机器人的轨迹调整 | 位置传感器（用于位置伺服控制），力传感器 |
根据上述表格可以看到，阻抗控制需要精确的系统动力学模型，相应地，阻抗控制可以做变阻抗控制（可以当作一种力控策略）等，这里不多说，大家可以去看对应的文献。
附录
笛卡尔空间的动力学方程推导： \mathbf{M}(\mathbf{q})\mathbf{\ddot{q}}+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{q}}+\mathbf{G(q)} = \mathbf{\tau} + \mathbf{J^{T}(q)}\mathbf{F}_{ext} \tag{I}
其中，F_{ext} 是作用在机械臂末端执行器上的外力以及外力矩。 因为 \mathbf{\dot{x}}=\mathbf{J(q)}\mathbf{\dot{q}} \tag{II}
所以 \mathbf{\ddot{x}}=\mathbf{\dot{J}(q)}\mathbf{\dot{q}}+\mathbf{J(q)}\mathbf{\ddot{q}} \tag{III}
其中，\mathbf{\dot{J}(q)}=\frac{d\mathbf{J(q)}}{dt}。
将 (II) 和 (III) 式代入 (I) 式中，得到 \mathbf{M}(\mathbf{q})\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\ddot{x}}-\mathbf{M(q)}\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{J}(q)}\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{x}}+\mathbf{C}(\mathbf{q},\mathbf{\dot{q}})\mathbf{J}^{-1}\mathbf{(q)}\mathbf{\dot{x}}+\mathbf{G(q)} = \mathbf{\tau} + \mathbf{J^{T}(q)}\mathbf{F}_{ext}
