导纳控制 admittance control

简单记录一下导纳控制。
对于一个质量块$m$，其位移记为$x$，我们期望它按照预期保持在$x_0$。质量块受到环境的外力，记为$F_{ext}$。施加的控制的力记为$F$。
导纳控制希望通过控制使得将外力转化为相对期望位移的偏差位移。

$$m\ddot{x}=F+F_{ext}$$$$e=x-x_0，M_d\ddot{e}+D_d\dot{e}+K_{d}e=F_{ext}$$对于导纳控制，其后接一个$PD$位置控制器$$F=k_p(x_d-x)-k_d\dot{x}$$$$m\ddot{x}+k_d\dot{x}+k_p(x-x_d)=F_{ext}$$$$M_d({\ddot{x}}_d-{\ddot{x}}_0)+D_d({\dot{x}}_d-{\dot{x}}_0)+K_d(x_d-x_0)=F_{ext}$$

为更好地理解，我们进行$Matlab$仿真。
先将上式转化成状态空间模型。
取$x_1=x$，$x_2=\dot{x}$，$x_3=x_d$，$x_4={\dot{x}}_d$。
取期望轨迹$x_0=t$，则
$${\dot{x}}_1=x_2$$$${\dot{x}}_2=-\frac{k_p}{m}{x}_1-\frac{k_d}{m}{x}_2+\frac{k_p}{m}{x}_3+\frac{1}{m}{F}_{ext}$$$${\dot{x}}_3=x_4$$$${\dot{x}}_4=-\frac{K_d}{M_d}{x}_3-\frac{D_d}{M_d}{x}_4+\frac{D_d+K_{d}t}{M_d}+\frac{1}{M_d}{F}_{ext}$$
取$m=1$，$F_{ext}=0.1sin(t)$
我们先将Mass-Damper-Spring与PD的参数设为1

function xdot = mass_env(t, x);
xdot(1)=x(2);
xdot(2)=[-1 -1 1 0]*x+0.1*sin(t);
xdot(3)=x(4);
xdot(4)=[0 0 -1 -1]*x+1+t+0.1*sin(t);
xdot=xdot';

t0=0;tf=100;step=0.001;
x0(1:4) = [0 0 0 0]';
[t x] = ode45('mass_env',[t0:step:tf], x0);
plot(t, x(:,1));

下图分别为质量块的位置，和与期望位置的差值。

下面，我们考虑用导纳控制进行力的控制。
记期望力$F_d$
$$F=k_p(x_d-x)-k_d\dot{x}$$$$m\ddot{x}+k_d\dot{x}+k_p(x-x_d)=F_{ext}$$$$M_d{\ddot{x}}_d+D_d{\dot{x}}_d+K_d{x}_d=F_d-F-F_{ext}$$

为更好地理解，我们进行$Matlab$仿真。
先将上式转化成状态空间模型。
取$x_1=x$，$x_2=\dot{x}$，$x_3=x_d$，$x_4={\dot{x}}_d$，则
$${\dot{x}}_1=x_2$$$${\dot{x}}_2=-\frac{k_p}{m}{x}_1-\frac{k_d}{m}{x}_2+\frac{k_p}{m}{x}_3+\frac{1}{m}{F}_{ext}$$$${\dot{x}}_3=x_4$$$${\dot{x}}_4=\frac{k_p}{M_d}{x}_1+\frac{k_d}{M_d}{x}_2-\frac{K_d+k_p}{M_d}{x}_3-\frac{D_d}{M_d}{x}_4+\frac{1}{M_d}(F_d-F_{ext})$$
取$m=1$，$F_{ext}=0.1sin(t+0.1)$，$F_d=5sin(t)$
我们先将Mass-Damper-Spring与PD的参数设为1

function xdot = mass_env1(t, x);
xdot(1)=x(2);
xdot(2)=[-1 -1 1 0]*x+0.1*sin(t+0.1);
xdot(3)=x(4);
xdot(4)=[1 1 -2 -1]*x+1+5*sin(t)-sin(t+0.1);
xdot=xdot';

t0=0;tf=100;step=0.001;
x0(1:4) = [0 0 0 0]';
[t x] = ode45('mass_env1',[t0:step:tf], x0);
plot(t, x(:,3)-x(:,1)-x(:,2));

此时，$F_d$与$F+F_{ext}$的差如下图

我们提高$K_d$的值到$99$，观察到误差缩小，如下图