导纳控制matlab仿真

 

 clc;
clear;
%%  导纳控制仿真程序M*xedd+B*xed+Kxe=Fe

%例如单自由度
t=0;
xe=0;
xed=0;
xedd=0;

xe_last=0;
xed_last=0;
xedd_last=0;
x_last=0;
dt=0.002;
B=100;
M=2;
K=1000000;
i=1;
while t<10
    Fe=sin(t);
    xedd=(Fe-B*xed_last-K*xe_last)/M;
    xed=xed_last+xedd*dt;
    xe=xe_last+xed*dt;
    x=x_last+xe;
    t=t+dt;
   
    x_out(i,1)=x;
    Fe_out(i,1)=Fe;
   i=i+1;
   xedd_last=xedd;
   xed_last=xed;
   xe_last=xe;
    x_last=x;  
end

subplot(2,1,1);
plot(Fe_out);
hold on;
subplot(2,1,2);
plot(x_out);

2.当刚度K=1000000时，发现力很跟手；

2.当刚减小时K=100时，不跟手，力方向时，输出位置没有减小

大家可以调节参数K，B，M测试效果

当手动拖动末端力传感器时，调节导纳的参数。（当期望过程碰到外力，需要稍微修改 x=x_last+xe为x=x_desire+xe）。