跳到主要内容机器人阻抗控制器与导纳控制器原理及实现 | 极客日志C++AI算法
机器人阻抗控制器与导纳控制器原理及实现
机器人常用的两种控制器：阻抗控制器和导纳控制器。阻抗控制器通过力矩输出模拟期望的动力学特性，依赖精确的系统动力学模型，适合高精度位置伺服；导纳控制器根据外力调整参考轨迹，无需精确模型且更安全，适合人机交互场景。两者各有优劣，阻抗控制可实现变阻抗策略，导纳控制则更易于实现柔顺交互。文章详细推导了关节空间和笛卡尔空间下的控制公式，并对比了两种控制方式的差异。
MqEngine1 浏览 机器人的阻抗控制器和导纳控制器
一、阻抗控制器（Impedance Controller）
1. 弹簧阻尼系统的例子
Fig. 1 弹簧阻尼系统
如图 (a) 所示的弹簧阻尼系统，设弹簧的自然长度为 $x_0$，当对系统施加外力 $F_{ext}$ 时，系统动力学方程为：
$$m\ddot{x} = F_{ext} + K(x - x_0) + D\dot{x} \tag{1}$$
现在的问题： 给定如图 (b) 所示的一个物体，质量为 $M$，物体上有位移传感器可以实时的测量到物体的位置 $x$，还有一个力传感器可以实时的测量物体受到的外力 $F_{ext}$，请问你可以设计控制率使得物体 $M$ 在受到外力 $F_{ext}$ 时，和图 (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$ 的动力学特性和图 (a) 中的物体 $m$ 的动力学特性一样。
如果图 (a) 中的 $F_{ext}$ 是屏幕前的你给的，你会感觉到在推一个弹簧阻尼系统。同时，你又以同样的力 $F_{ext}$ 来推图 (b) 中的物体 $M$，此时系统由式 (4) 的控制率控制，你将会得到和推图 (a) 同样的感受。
自动控制让我们在推动不同质量的物体时，得到了相同的感受，同时，系统也得到了同样的动力学响应。另外，一般如果系统是位置控制（我们的很多系统都是纯位置控制），位置控制的优点是位置精度高，缺点是不安全（尤其是在人机交互过程中）。如果我们按照上述介绍，将系统虚拟成一个弹簧阻尼系统，就安全很多（因为它有了弹簧阻尼系统的动力学特性）。
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_de = 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}$$
与 中介绍的一样，将式 (7) 变形为：
$$\ddot{e} = \frac{F_{ext} - D_d\dot{e} - K_de}{M_d} \tag{9}$$
微信扫一扫，关注极客日志
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
相关免费在线工具
- 加密/解密文本
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
- RSA密钥对生成器
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
- Mermaid 预览与可视化编辑
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
- Base64 字符串编码/解码
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
- Base64 文件转换器
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
- Markdown 转 HTML
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML 转 Markdown 互为补充。 在线工具，Markdown 转 HTML在线工具，online
1. 弹簧阻尼系统的例子
将式 (9) 代入式 (8) 得到：
$$F = m(\frac{F_{ext} - D_d\dot{e} - K_de}{M_d} + \ddot{x}d) - F{ext} \tag{10}$$
式 (10) 中的 $\dot{e}$ 可以通过 $\dot{x} - \dot{x}_d$ 计算得到。
将式 (10) 式的控制力代入 系统动力学方程 (6) 式中，可以得到 期望的系统动力学特性 (7) 式。
3. 机械臂关节空间的阻抗控制器
关节空间的动力学方程（Plant Dynamics in Joint Space）：
$$\boldsymbol{\tau} + \boldsymbol{\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} = \boldsymbol{\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) 式中，得到：
$$\boldsymbol{\tau} + \boldsymbol{\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}(\boldsymbol{\tau}{ext} - \mathbf{D}_d\mathbf{\dot{e}} - \mathbf{K}_d\mathbf{e})$ 代入到式 (13) 式中，可以得到阻抗控制器的力矩控制量 $\boldsymbol{\tau}$ 为：
$$\boldsymbol{\tau} = \mathbf{M(q)}[\mathbf{\ddot{q}}_d - \mathbf{M}d^{-1}(\boldsymbol{\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)} - \boldsymbol{\tau}{ext} \tag{14}$$
在式 (14) 式中，$\mathbf{M(q)}, \mathbf{C(q,\dot{q})}$ 和 $\mathbf{G(q)}$ 在系统建模阶段就已经知道，$\mathbf{q}$ 和 $\boldsymbol{\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)} = \boldsymbol{\tau} + \mathbf{J^{T}(q)}\mathbf{F}_{ext} \tag{15}$$
期望的笛卡尔空间动力学特性（Desired Dynamics in Cartesian Space）：
$$\mathbf{M}_d\mathbf{\ddot{e}} + \mathbf{D}_d\mathbf{\dot{e}} + \mathbf{K}d\mathbf{e} = \boldsymbol{\tau}{ext} \tag{16}$$
其中 $\mathbf{e} = \mathbf{x}_d - \mathbf{x}$，$\mathbf{M}_d, \mathbf{D}_d, \mathbf{K}_d$ 分别为质量系数矩阵，阻尼系数矩阵，刚度系数矩阵。
和上述的计算过程一样，最终可以得到阻抗控制器的输出控制力矩 $\boldsymbol{\tau}$ 为：
$$\boldsymbol{\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. 碰撞和拖拽的例子
如图 (a) 所示，物体在位置控制模式下，静止在某一个位置。当有外力和这个物体发生碰撞时，或者有外力试图拖拽这个物体，由于位置控制的内环（力矩环）会产生极大的力矩来抗拒偏离当前位置，所以结果是撞也撞不动，拉也拉不动。
但是在人机交互场景中，我们希望在发生碰撞或者拖拽的情况时，被控制的物体可以发生位移，避免硬碰硬的情况。上述的阻抗控制器可以做得到，但是我们也讨论了 阻抗控制器 需要 精确的系统动力学模型。
现在的问题： 有没有一种控制器，不需要精确的系统动力学模型，还能在被控制物体与外界发生碰撞和拖拽时，产生柔顺的位移？
2. 统一的导纳控制器框架
在图 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_de = F_{ext} \tag{20}$$
其中，$e = x_{ref} - x_d$，$M_d, D_d$ 和 $K_d$ 是我们人为设计的，使得这个二阶系统稳定。
式 (20) 的设计方法的优点请参看下图 Fig. 7。
Fig. 7 导纳控制器对于不同外力情况下的响应特性
如图 (a) 所示，当外力 $F_{ext} = 0$ 时，导纳控制器输出的参考位移 $x_{ref}$ 收敛到期望位移 $x_d$，整个系统退化为纯位置控制。如图 (b) 所示，当外力 $F_{ext}$ 为非零常数 $C$ 时，导纳控制器输出的参考位移 $x_{ref}$ 收敛到与期望位移 $x_d$ 的稳态误差为 $\frac{C}{K_d}$。当外力 $F_{ext}$ 剧烈变化时（即存在高频分量），此时式 (20) 可以看作是一个二阶低通滤波器（滤波器的形式为 $\frac{1}{M_ds^2 + D_ds + K_d}$，滤波器输入为 $F_{ext}$，滤波器输出为 $e$，这个滤波器是不是和二阶巴特沃斯低通滤波器很像？），可以抑制 $F_{ext}$ 的高频分量，输出平滑的 $e$。
值得注意的是，上述的 $e$ 都是平滑连续的（由式 (20) 式的二阶微分方程保证的），期望轨迹 $x_d$ 是我们人为插值后输入的，也是平滑连续的，所以参考轨迹 $x_{ref} = e + x_d$ 也是平滑连续的。这样，图 6 中的 Position Control 的输入平滑连续，保证了系统的位置伺服的连续稳定。
3. 机械臂关节空间的导纳控制器
关节空间导纳控制器（Admittance Controller in Joint Space）：
$$\mathbf{M}_d\mathbf{\ddot{e}} + \mathbf{D}_d\mathbf{\dot{e}} + \mathbf{K}d\mathbf{e} = \boldsymbol{\tau}{ext} \tag{21}$$
其中，$\mathbf{e} = \mathbf{q}_{ref} - \mathbf{q}_d$，$\mathbf{M}_d, \mathbf{D}_d, \mathbf{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}$$
其中，$\mathbf{e} = \mathbf{x}_{ref} - \mathbf{x}_d$，$\mathbf{M}_d, \mathbf{D}_d, \mathbf{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)} = \boldsymbol{\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)} = \boldsymbol{\tau} + \mathbf{J^{T}(q)}\mathbf{F}_{ext}$$
