robot.m

这是一个两关节机械手的数学模型

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This Function is a model of two link robot manipulator.
% m1 is the mass of arm1;
% m2 is the mass of arm2;
% l1 is  the length of arm1;
% l2 is the length of arm2;
% g is the acceleration of gravity;
% t1k is the torque on arm1 at time k;
% t2k is the torque on arm2 at time k;
% angl1k is the angle of arm1 under t1 at time k;
% angl2k is the angle of arm2 under t2 at time k;
% velo1k is the velocity of arm1 at time k;
% velo2k is the velocity of arm2 at time k;
% acce1k is the accelerity of arm1 at time k;
% acce2k is the accelerity of arm2 at time k;
% angl1k1 is the angle of arm1 under t1 at time k+1;
% angl2k1 is the angle of arm2 under t2 at time k+1;
% velo1k1 is the velocity of arm1 at time k+1;
% velo2k1 is the velocity of arm2 at time k+1;
% acce1k1 is the accelerity of arm1 at time k+1;
% acce2k1 is the accelerity of arm2 at time k+1;
% t is the sampling period. 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [angl1k1,angl2k1,velo1k1,velo2k1,acce1k1,acce2k1]=robot(t1k,t2k,angl1k,angl2k,velo1k,velo2k,acce1k,acce2k,t)

%%%%%%%%%%%%%%%%%%%
m1=4;
m2=2;
l1=1;
l2=0.5;
g=9.8;
%%%%%%%%%%%%%%%%%%

T=[t1k t2k]';

M=[m1*l1*l1+m2*(l1*l1+l2*l2+2*l1*l2*cos(angl2k))  m2*l2*l2+m2*l1*l2*cos(angl2k)
   m2*l2*l2+m2*l1*l2*cos(angl2k)                                       m2*l2*l2];

V=[-m2*l1*l2*sin(angl2k)*velo2k*velo2k-2*m2*l1*l2*sin(angl2k)*velo1k*velo2k
   m2*l1*l2*sin(angl2k)*velo1k*velo1k];

G=[m2*l2*g*cos(angl1k+angl2k)+(m1+m2)*l1*g*cos(angl1k)
   m2*l2*g*cos(angl1k+angl2k)];

ACCE=inv(M)*(T-V-G);
%%%%%%%%%%%%%%%%%%
acce1k1=ACCE(1);
acce2k1=ACCE(2);

velo1k1=velo1k+acce1k*t;
velo2k1=velo2k+acce2k*t;

angl1k1=angl1k+velo1k*t;
angl2k1=angl2k+velo2k*t;