背景描述

阻抗控制是一种常见的柔顺控制、力控方法，其核心思想是让机器人表现出类似于一个带阻尼弹簧的特性。让机械臂与外界交互时能保持柔顺性，拓展机械臂的使用场景。

公式推导

一般情形

通常而言，我们可以将机器人的动力学方程写成如下形式

$$\tau +{\tau }_{ext}=M(q)\ddot{q}+C(q,\dot{q})+g(q)$$

其中，$M$为机器人的质量矩阵，$C$为机器人的Coriolis矩阵，$g$为重力向量，$\tau$为机器人的力向量，$q$为机器人的关节角度，$\dot{q}$为机器人的关节角速度，${\tau }_{ext}$为机器人的外力。

而期望的外界阻抗表现如下

$${\tau }_{ext}=K_d(q-q_d)+D_d(\dot{q}-{\dot{q}}_d)+M_d(q)(\ddot{q}-{\ddot{q}}_d)$$

其中，$K$为刚度系数，$D$是阻尼系数。假设，$M_d=M$, 代入动力学方程得到如下方程，反应了外部力矩和电机角度形成了一种阻尼弹簧的效果。

$$\tau =K(q_d-q)+D({\dot{q}}_d-\dot{q})+M(q){\ddot{q}}_d+C(q,\dot{q})+g(q)$$

单电机情况

通常情况下，我们会现在单电机上进行力控实验。此时，可以忽略Coriolis力，如果末端不加负载，也可以不考虑重力。这是最简单的情形。

但是，通常情况下，电机会有一定的摩擦力和粘滞力，我们用$D(\dot{q})$来表示。

需要指出的是函数$D()$满足如下条件

1. 单调递增

2. $D(0)=0$

这里我们可以将动力学方程简化为

$$\tau +{\tau }_{ext}-D\dot{q}=M\ddot{q}$$

如果我们希望表现为如下形式

$${\tau }_{ext}=M_d\ddot{q}+D_d\dot{q}+K_{d}q$$

故可以得到控制律如下

$$\tau =(M-M_d)\ddot{q}+(D-D_d)\dot{q}-K_{d}q$$

通常，我们并不能得到精准的$D$, 所以，我们可以直接设定$D_d=0$通过调整$\tau$的速度增益，直到振动，在每个振幅上都能既不收敛，也不发散，则可以辨识出$D$。此处为了简洁，假设$D$就是个常数即可。

实际算法

设电机输出力矩为${\tau }_i$，外载荷力矩为${\tau }_e$

已知在仅驱动电机和仅施加外载荷时，两个力矩和角度的关系如下

$$\begin{gathered} {\tau }_i=M_i\ddot{q}+D_i\dot{q} \\ {\tau }_e=M_e\ddot{q}+D_e\dot{q} \end{gathered}$$

故整体的动力学方程为

$${\tau }_i+{\tau }_e=(M_i+M_e)\ddot{q}+(D_i+D_e)\dot{q}$$

而我们期望的角度对外力矩的响应如下

$${\tau }_e=M_d{\ddot{q}}_d+D_d{\dot{q}}_d+K_d{q}_d$$

接下来设计控制律

1.(简单粗暴)

具体而言，令

$$\begin{gathered} {\tau }_i=(k_e-1){\tau }_e+(M_i-k_e\ast M_d)\ddot{q}+(D_i-k_e\ast D_d)\dot{q}-k_e\ast K_{d}q \\ {k}_e>1 \end{gathered}$$

代入动力学方程有

$${\tau }_e=\frac{(M_e+k_e{M}_d)}{k_e}\ddot{q}+\frac{(D_e+k_e{D}_d)}{k_e}\dot{q}+K_{d}q$$

当$k_e$充分大时，即便$M_e,D_e$估计误差较大，也可以控制误差范围。

但是，这种方法，当力传感器波动较大时，会有比较明显的抖颤。