张海涛-2013424064-第四次作业+Baumgarte违约修正

弹簧阻尼四连杆机构动力学分析—Baumgarte违约修正

force=zeros(8,n); F=zeros(1,n);

%% 用欧拉法对四连杆机构进行位置分析，速度分析，加速度分析。动力学方程的左边矩阵，AX=B，求解X A=[M CqT;Cq 0]

for i=1:n

A=[ m2, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0

0, m2, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0

0, 0, (l2^2*m2)/12, 0, 0, 0, 0, 0, 0,

(l2*sin(q(3,i)))/2, -(l2*cos(q(3,i)))/2, -(l2*sin(q(3,i)))/2, (l2*cos(q(3,i)))/2, 0, 0, 0, 0

0, 0, 0, m3, 0, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0

0, 0, 0, 0, m3, 0, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0, 0

0, 0, 0, 0, 0, (l3^2*m3)/12, 0, 0, 0, 0, 0, -(l3*sin(q(6,i)))/2, (l3*cos(q(6,i)))/2, -(l3*sin(q(6,i)))/2, (l3*cos(q(6,i)))/2, 0, 0

0, 0, 0, 0, 0, 0, m4, 0, 0, 0, 0, 0, 0, -1, 0, 1, 0

0, 0, 0, 0, 0, 0, 0, m4, 0, 0, 0, 0, 0, 0, -1, 0, 1

0, 0, 0, 0, 0, 0, 0, 0, (l4^2*m4)/12, 0, 0, 0, 0, -(l4*sin(q(9,i)))/2, (l4*cos(q(9,i)))/2, -(l4*sin(q(9,i)))/2, (l4*cos(q(9,i)))/2

1, 0, (l2*sin(q(3,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

0, 1, -(l2*cos(q(3,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

1, 0, -(l2*sin(q(3,i)))/2, -1, 0, -(l3*sin(q(6,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

0, 1, (l2*cos(q(3,i)))/2, 0, -1, (l3*cos(q(6,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

21

弹簧阻尼四连杆机构动力学分析—Baumgarte违约修正

0, 0, 0, 1, 0, -(l3*sin(q(6,i)))/2, -1, 0, -(l4*sin(q(9,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0

0, 0, 0, 0, 1, (l3*cos(q(6,i)))/2, 0, -1, (l4*cos(q(9,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0

0, 0, 0, 0, 0, 0, 1, 0, -(l4*sin(q(9,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0

0, 0, 0, 0, 0, 0, 0, 1, (l4*cos(q(9,i)))/2, 0, 0, 0, 0, 0, 0, 0, 0];

fs=k*(sqrt((q(7,i)-750)^2+(q(8,i)-0)^2)-L0); %弹簧力

fc=-c*q_v(7,i)*(750-q(7,i))/sqrt((q(7,i)-750)^2+(q(8,i)-0)^2)+c*q_v(8,i)*(q(8,i)-0)/sqrt((q(7,i)-750)^2+(q(8,i)-0)^2); %阻尼力

F(1,i)=fs+fc; %弹簧力加阻尼力 B=[ 0;

-m2*g; M;

0; -m3*g 0;

fs*(750-q(7,i))/sqrt((q(7,i)-750)^2+(q(8,i)-0)^2)+fc*(750-q(7,i))/sqrt((q(7,i)-750)^2+(q(8,i)-0)^2); %四杆质心所受的x轴方向的力

-m4*g-fs*(q(8,i)-0)/sqrt((q(7,i)-750)^2+(q(8,i)-0)^2)-fc*(q(8,i)-0)/sqrt((q(7,i)-750)^2+(q(8,i)-0)^2); %四杆质心所受的y轴方向的力 0;

-(l2*q_v(3,i)^2*cos(q(3,i)))/2-2*a*(q_v(1,i)+q_v(3,i)*l2/2*sin(q(3,i)))-b^2*(q(1,i)-l2/2*cos(q(3,i)));

-(l2*q_v(3,i)^2*sin(q(3,i)))/2-2*a*(q_v(2,i)-q_v(3,i)*l2/2*cos(q(3,i)))-b^2*(q(2,i)-l2/2*sin(q(3,i)));

(l2*cos(q(3,i))*q_v(3,i)^2)/2+(l3*cos(q(6,i))*q_v(6,i)^2)/2-2*a*(q_v(1,i)-q_v(3,i)*l2/2*sin(q(3,i))-q_v(4,i)-q_v(6,i)*l3/2*sin(q(6,i)))-b^2*(q(1,i)+l2/2*cos(q(3,i))-q(4,i)+l3/2*cos(q(6,i)));

(l2*sin(q(3,i))*q_v(3,i)^2)/2+(l3*sin(q(6,i))*q_v(6,i)^2)/2-2*a*(q_v(2,i)+q_v(3,i)*l2/2*cos(q(3,i))-q_v(5,i)+q_v(6,i)*l3/2*cos(q(6,i)))-b^

22

弹簧阻尼四连杆机构动力学分析—Baumgarte违约修正

2*(q(2,i)+l2/2*sin(q(3,i))-q(5,i)+l3/2*sin(q(6,i)));

(l3*cos(q(6,i))*q_v(6,i)^2)/2+(l4*cos(q(9,i))*q_v(9,i)^2)/2-2*a*(q_v(4,i)-q_v(6,i)*l3/2*sin(q(6,i))-q_v(7,i)-q_v(9,i)*l4/2*sin(q(9,i)))-b^2*(q(4,i)+l3/2*cos(q(6,i))-q(7,i)+l4/2*cos(q(9,i)));

(l3*sin(q(6,i))*q_v(6,i)^2)/2+(l4*sin(q(9,i))*q_v(9,i)^2)/2-2*a*(q_v(5,i)+q_v(6,i)*l3/2*cos(q(6,i))-q_v(8,i)+q_v(9,i)*l4/2*cos(q(9,i)))-b^2*(q(5,i)+l3/2*sin(q(6,i))-q(8,i)+l4/2*sin(q(9,i)));

(l4*q_v(9,i)^2*cos(q(9,i)))/2-2*a*(q_v(7,i)-q_v(9,i)*l4/2*sin(q(9,i)))-b^2*(q(7,i)+l4/2*cos(q(9,i))-l5);

(l4*q_v(9,i)^2*sin(q(9,i)))/2-2*a*(q_v(8,i)+q_v(9,i)*l4/2*cos(q(9,i)))-b^2*(q(8,i)+l4/2*sin(q(9,i)));];

temp=A\\B;

q_ac(:,i)=temp(1:9); % 解的前9项为杆的加速度和角加速度 force(:,i)=temp(10:17); % 解的后8项为铰链处的力 if i==n; break end

q_v(:,i+1)=q_v(:,i)+dt*q_ac(:,i); q(:,i+1)=q(:,i)+dt*q_v(:,i); end

h0=figure(14);

set(h0,'name','弹簧阻尼器所受的力'); plot(t,-F(1,:)/1000) hold on

data0=load('force.tab');

plot(data0(:,1),data0(:,2),'r-'); xlabel('时间t/s') ylabel('力/N')

title('弹簧阻尼器所受的力') legend('matlab','adams') %% 二杆质心位移

% 二杆质心X位移变化 h1=figure(1);

set(h1,'name','二杆质心位移'); subplot(311) plot(t,q(1,:),'b-') hold on;

data1=load('gan2_xy.tab'); plot(data1(:,1),data1(:,2),'r-');

23

弹簧阻尼四连杆机构动力学分析—Baumgarte违约修正

xlabel('时间t/s') ylabel('位移/m')

title('二杆质心X位移变化') legend('matlab','adams') % 二杆质心Y位移变化 subplot(312) plot(t,q(2,:),'b-') hold on;

plot(data1(:,1),data1(:,3),'r-'); xlabel('时间t/s') ylabel('位移/m')

title('二杆质心Y位移变化') legend('matlab','adams') % 二杆质心角度变化 subplot(313) plot(t,q(3,:),'b-') hold on;

data0=load('gan234_a.tab'); plot(data0(:,1),data0(:,2),'r-');

xlabel('时间t/s') ylabel('角度rad')

title('二杆质心角度变化') legend('matlab','adams')

%% 三杆质心位移

% 三杆质心X位移变化 h2=figure(2);

set(h2,'name','三杆质心位移'); subplot(311) plot(t,q(4,:),'b-') hold on;

data2=load('gan3_xy.tab'); plot(data2(:,1),data2(:,2),'r-'); xlabel('时间t/s') ylabel('位移/m')

title('三杆质心X位移变化') legend('matlab','adams') % 三杆质心Y位移变化 subplot(312) plot(t,q(5,:),'b-') hold on;

plot(data2(:,1),data2(:,3),'r-'); xlabel('时间t/s') ylabel('位移/m')

24