机器人阻抗控制器与导纳控制器原理

我们注意到，$\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}$$

其中 $\mathbf{e}=\mathbf{q}{d}-\mathbf{q}$，$\mathbf{M}{d}$，$\mathbf{D}{d}$，$\mathbf{K}{d}$ 分别为质量系数矩阵，阻尼系数矩阵，刚度系数矩阵。

与上述的计算同理，将 $\mathbf{\ddot{q}}=\mathbf{\ddot{q}}_{d}-\mathbf{\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) 式变形为 $\mathbf{\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) 式中，$\mathbf{M(q)}$，$\mathbf{C(q,\dot{q})}$ 和 $\mathbf{G(q)}$ 在系统建模阶段就已经知道，$\mathbf{q}$ 和 $\mathbf{\tau}{ext}$ 通过传感器可测量出来，$\mathbf{\dot{q}}$ 可以直接通过微分计算出来，$\mathbf{\ddot{q}}{d}$ 是期望轨迹的二阶导数也是已知的，$\mathbf{M}{d}$，$\mathbf{D}{d}$ 和 $\mathbf{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}$$

其中 $\mathbf{e}=\mathbf{x}{d}-\mathbf{x}$，$\mathbf{M}{d}$，$\mathbf{D}{d}$，$\mathbf{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 机械臂，它的每个关节都装配了力矩传感器用来估计外力和外力矩。其它的市面上的机械臂，比如 UR，我就没有听说过支持阻抗控制器。

二、导纳控制器（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) 式的设计方法的优点请参看下图图 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}$$

其中，$\mathbf{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}$$