87361021differential-drive-robot.rar_Wheeled Mobile Robot_differ

function u=Dynamics(Tr,Tl,v,omega,theta) m=10; % The robot mass Ra=0.05; % The radius of the wheels a=0.05; % The distance between the center of mass and wheels axis L = 0.5; % The lateral distance of the wheels to the center J=5; % The robot mass moment of inertia M=[m 0 m*a*sin(theta); 0 m -m*a*cos(theta); m*a*sin(theta) -m*a*cos(theta) J]; V=[0 0 m*a*omega*cos(theta); 0 0 m*a*omega*sin(theta); 0 0 0]; S=[cos(theta) -a*sin(theta); sin(theta) a*cos(theta); 0 1]; B=[cos(theta)/Ra cos(theta)/Ra; sin(theta)/Ra sin(theta)/Ra; L/Ra -L/Ra]; Sdot=[-omega*sin(theta) -a*omega*cos(theta); omega*cos(theta) -a*omega*sin(theta); 0 0]; Mh=S'*M*S; Vh=S'*M*Sdot+S'*V'*S; Bh=S'*B; T=[Tr;Tl]; ve=[v;omega]; vdot=inv(Mh)*(T-Vh*ve); u=[vdot(1);vdot(2)];