dynamicsimulation.m

此程序很好的解决了六自由度平台的仿真

clear

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%q1=0;
%q2=0;
%q3=0;
%q4=0;
%q5=0;
%q6=0;
%l1=5;
%l2=4;
%l3=2;
%h1=0;
%h2=-0.5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%TranMarix=[cos(q2*pi/180)*cos(q3*pi/180),-cos(q1*pi/180)*sin(q3*pi/180)+sin(q1*pi/180)*sin(q2*pi/180)*cos(q3*pi/180),sin(q1*pi/180)*sin(q3*pi/180)+cos(q1*pi/180)*sin(q2*pi/180)*cos(q3*pi/180),q4;
	%     cos(q2*pi/180)*sin(q3*pi/180),cos(q1*pi/180)*cos(q3*pi/180)+sin(q1*pi/180)*sin(q2*pi/180)*sin(q3*pi/180),-sin(q1*pi/180)*cos(q3*pi/180)+cos(q1*pi/180)*sin(q2*pi/180)*sin(q3*pi/180),q5;
		%-sin(q2*pi/180),sin(q1*pi/180)*cos(q2*pi/180),cos(q1*pi/180)*cos(q2*pi/180),q6;
		%0,0,0,1];
  %  A=[(2*l1+l3)*cos(30*pi/180)/3,-(l1-l3)*cos(30*pi/180)/3,-(l1+2*l3)*cos(30*pi/180)/3,-(l1+2*l3)*cos(30*pi/180)/3,-(l1-l3)*cos(30*pi/180)/3,(2*l1+l3)*cos(30*pi/180)/3;
%		l3/2,(l1+l3)/2,l1/2,-l1/2,-(l1+l3)/2,-l3/2;
	%	h1,h1,h1,h1,h1,h1;
	%	1,1,1,1,1,1];
%    B=[(l1+2*l3)*cos(30*pi/180)/3,(l1-l3)*cos(30*pi/180)/3,-(2*l1+l3)*cos(30*pi/180)/3,-(2*l1+l3)*cos(30*pi/180)/3,(l1-l3)*cos(30*pi/180)/3,(l1+2*l3)*cos(30*pi/180)/3;
%	    l1/2,(l1+l3)/2,l3/2,-l3/2,-(l1+l3)/2,-l1/2;
%		h2,h2,h2,h2,h2,h2;
%		1,1,1,1,1,1];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%C=T*A;
%D=C-B;