导纳控制二阶系统仿真

本文参考《Unified Impedance and Admittance Control》

1. 导纳控制原理

这里仅对导纳控制进行简要概述，导纳控制和阻抗控制的区别和联系参考我之前的文章机器人阻抗控制和导纳控制的深度解析

导纳控制由三部分组成，可以分为外环的导纳控制（输入外力输出期望位置）和内环的位置控制（输入期望位置$x_d$，通过改变控制力$F$，完成$x$对$x_d$的跟踪），示意图如下

其中，
$x_0$为最开始不施加任何外力时的参考位置；
$x_d$为导纳控制过程中输出的参考位置，会随着运动的进行更新；
$F$为位置控制器输出的控制力，力是产生运动的根本原因；
$F_{ext}$为外力，在导纳控制中通常是传感器检测到的外力；
$x$为实际运动位置；
$$M_d({\ddot{x}}_d-{\ddot{x}}_0)+D_d({\dot{x}}_d-{\dot{x}}_0)+K_d(x_d-x_0)=$$