彻底搞懂阻抗控制、导纳控制、力位混合控制

本人在学习的机械臂力控时，始终觉得这三个概念太过抽象，不能很好理解，读了很多博客和文献，仍然感觉没有参透，今天在读李正义博士的毕业论文《机器人与环境间力/位置控制技术研究与应用》时豁然开朗，感觉逐渐理解了一切，因此写下这篇博客，帮助和我一样的萌新们。

我们为什么需要柔顺控制？

普通的位置控制，是控制机器人末端到指定的地点，这种控制是说一不二的，让机械臂向前运动1cm，就是运动1cm，没有商量的余地。这种控制在涉及到接触作业时就会产生麻烦，因为现实环境是充满各种误差的，位置上很小的误差，经过大刚度的放大会产生很大的接触力。例如，一个作业需要机器人向前1cm接触到前方固定的玻璃板，我们假设机械臂的刚度无限大。然而，因为各种误差，钢板与机器人的实际距离其实是0.9cm，可机器人还是会到1cm的位置，这时你最好希望这块玻璃不太贵，因为它以及在劫难逃了。

被动柔顺控制

有同学会说，那好办啊，我在机械臂末端加一个六自由度的弹簧不久解决了？让机械臂末端具有有限的刚度，位置误差就只会产生比较小的力，不久迎刃而解了吗？
这种方法就叫被动柔顺控制。通过弹簧，阻尼等机构，让机器人拥有顺从自然环境的能力。但这样做有一个巨大的问题，就是精度低。机械臂拥有复杂的结构，有非线性的摩擦力、传动间隙等等，导致期望刚度无法精确获得。这样，就无法精确地由位置信息得到接触力信息或由力信息得到位置信息了。

阻抗控制

于是，一个天才的想法诞生了，那就是主动柔顺控制。主动柔顺控制又分为阻抗控制、导纳控制和混合力位控制。

阻抗控制，严谨地说，就是把控制器等效为阻抗系统，输入位置输出力，机器人等效为导纳系统，输入力输出位置。所以，阻抗控制需要我们能够获取位置信息，并且能控制机器人的关节力矩。
依然用我们刚才那个碰玻璃的例子，采用阻抗控制的话，我们并不指定机器人末端的位置，而是让机器人末端产生一个力$F_r$，这个力$F_r$是这么算的：
$$F_r=K_d\left(X_r-X\right)+B_d\left({\dot{X}}_r-\dot{X}\right)+M_d({\ddot{X}}_r-\ddot{X})$$
其中，$X$，$\dot{X}$，$\ddot{X}$表示编码器得到的关节位置计算出的末端执行器实际位置；$X_r$，$\dot{X_r}$，$\ddot{X_r}$表示参考位置（1m，0m/s,0m/s2）；$K_d,B_d,M_d$是用户自己设定的阻抗参数。要注意，这三个参数并不代表机器人模型的刚度阻尼质量，而是用户自己设定的力和位置之间的关系。
这么做的意义在哪里呢？假设，想运动1cm的机械臂撞到了0.9cm处的玻璃，此时玻璃会对机器人产生一个末端接触力$F_e$，这个力抵消会$F_r$的作用，让机械臂逐渐不再往前运动。平衡状态下，$F_r$=$F_e$,机器人也会停在0.9cm多一点点的位置。其$X$与$X_r$始终有一段距离，用来产生力$F_r$。
问题解决了！我们的玻璃保住了，我们也能通过下式
$$M_d\left(\ddot{X}-{\ddot{X}}_d\right)+B_d\left(\dot{X}-{\dot{X}}_d\right)+K_d\left(X-X_d\right)=-F_e$$
精确地知道机器人的位置和末端受力。而阻抗控制的用户们要做的，就是仔细选择$K_d,B_d,M_d$的值，使其呈现想要的特性。
当然 ，阻抗控制不止这一种用法。更多时候，我们想让机械臂与墙维持一个恒定的力$F_d$，即跟踪期望力$F_d$，这时，用$E=F_e-F_d$来代替$F_e$，就可以实现力的跟踪。
这里的推导省略了关节空间和笛卡尔空间的转换。借用论文里的图来呈现整个过程：

导纳控制

导纳控制是阻抗控制的反过程，把控制器等效为导纳系统，输入力输出位置，机器人等效为阻抗系统，输入位置输出力。但它们的目的都是一样的，即不希望把玻璃撞破。导纳控制需要我们能获取力信息（因此常需要在末端加力传感器），且能控制机器人的关节位置（这个基本都可以）。
导纳控制的公式与阻抗控制相同，不过未知量不同，如下：
$$F_e=K_d\left(X_r-X\right)+B_d\left({\dot{X}}_r-\dot{X}\right)+M_d({\ddot{X}}_r-\ddot{X})$$
在导纳控制中，已知量为接触力$F_e$，未知量为$X$。注意这里的$X$既是真实位置$X$，又是发送给机器人的期望位置$X_d$，也就是说在导纳控制里我们假设位置控制是没有误差的，指哪打哪。通过上述公式计算出$X$之后发送给机器人，就实现了柔顺控制。
依然以机器人撞玻璃为例，初始时$F_e$为0，故给机器人发送的$X$等于$X_r$也就是1cm。运动过程中，机械臂碰到玻璃，$F_e$增大，$X$随之减小，直到最后停在玻璃前一点点的位置。控制图如下：

注1：图中的Ke是位置控制环需要的，和导纳控制无关。
注2：本节出现了三个位置变量，$X_d$，$X_r$,$X$，第一个是期望位置，即发送给机械臂的控制命令，也就是想让机器人运动到哪。第二个是参考位置，也就是一切完美零误差时的位置，本例指1cm。第三个是实际位置，真实位置。

力位混合控制

其实，上两种控制已经能比较好的完成普通柔顺控制了。但是在很多情况下，我们需要同时对力和位置有高精度的控制，例如机械臂沿着某个面进行抛光作业，这时，既需要在垂直面方向上进行力控制，又需要在切面方向进行位置控制。这就需要用到力位混合控制。
需要注意，虽然阻抗控制和导纳控制一个是控制关节力矩，一个是控制关节位置，本质上都属于力控制，因为它们的最终效果是通过调节$K_d,B_d,M_d$让机械臂和接触面维持一个合适的力。
力位控制比较简单，将需要力控和需要位控的方向解耦，分别给予控制就好啦。在下图中，S为对角阵，元素为0和1，力控的方向，S相应位置为1，位置控制的方向，相应位置为0，$\bar{S}$取反，如下图示意: