Upload
others
View
14
Download
0
Embed Size (px)
Citation preview
QUICK-RETURN MECHANISM SOLUTION BY THE NEWTON-RAPHSON METHOD
Loop Equations
24
4 2 2 4
2 1
0
0ii i
O K KO O O
f se a e a e
1 4 2 2 1
2 4 2 2
cos cos 0
sin sin 0
f s a a
f s a
1st loop
Loop Equations
54
4 4
2
4 5 1 6
0
0ii i i
O L LM MN NO
f a e a e e a a e
3 4 4 5 5 1 6
4 4 4 5 5
cos cos 0
sin sin 0
f a a a a
f a a
2nd loop
Newton Raphson Method
Position Equations
1 4 2 2 1
2 4 2 2
cos cos 0
sin sin 0
f s a a
f s a
0 J f J f
4
5
1 1 1 1
4 5
12 2 2 2
4 5 2
33 3 3 3
4 5 4
4 4 4 4
4 5
k
k
k
k
k k k k
k k k k
sk k k k
k k k k
f f f f
s
ff f f f
s f
ff f f f
s f
f f f f
s
3 4 4 5 5 1 6
4 4 4 5 5
cos cos 0
sin sin 0
f a a a a
f a a
4
5
4 4 4 2 2 1
4 4 4 2 2
4 4 5 5 4 4 5 5 1 6
4 4 5 5 4 4 5 5
sin 0 cos 0 cos cos
cos 0 sin 0 sin sin
sin sin 0 0 cos cos
cos cos 0 1 sin sin
k
k
k
k
k k k k k
k k k k k
k k k ks
k k k k k
s s a a
s s a
a a a a a a
a a a a
Velocity Equations
11 4 4 4 2 2 2
22 4 4 4 2 2 2
33 4 4 4 5 5 5
44 4 4 4 5 5 5
cos sin sin 0
sin cos cos 0
sin sin 0
cos cos 0
s
s
fv V s a
t
fv V s a
t
fv a a
t
fv a a V
t
4 44 2 2 2
4 4 5 2 2 2
4 4 5 5
4 4 5 5
sin 0 cos 0sin
cos 0 sin 0 cos
0sin sin 0 0
0cos cos 0 1
s
sa
s a
Va a
Va a
Acceleration Equations
1 4 4 4 4 4
2 2
4 4 2 2 2 2 2 2
2 4 4 4 4 4
2 2
4 4 2 2 2 2 2 2
2 2
3 4 4 4 5 5 5 4 4 4 5 5 5
4 4 4 4 4
cos sin 2 sin
cos sin cos 0
sin cos 2 cos
sin cos sin 0
sin sin cos cos 0
cos
s s
s s
A a s V
s a a
A a s V
s a a
A a a a a
A a
2 2
4 4 5 5 5 5 5 5sin cos sin 0a a a a
4 44
4 4 5
4 4 5 5
4 4 5 5
2 2
4 4 4 4 2 2 2 2 2 2
2 2
4 4 4 4 2 2 2 2 2 2
2 2
4 4 4 5 5
sin 0 cos 0
cos 0 sin 0
sin sin 0 0
cos cos 0 1
2 sin cos sin cos
2 cos sin cos sin
cos c
s
s
s
s
s
aa a
aa a
V s a a
V s a a
a a
5
2 2
4 4 4 5 5 5
os
sin sina a
%CLEAR: Variables and command window in MATLAB clc,clear % INPUT: PHYSICAL PARAMETERS of MECHANISM (meter) a1=40;a2=15;a4=70;a5=30;a6=22; % INPUT: Maximum Iteration Number Nmax Nmax=100; % INPUT: INITIAL GUESS VALUES for th4, th5, s and to respectively x=[30*pi/180,60*pi/180,40,20]; % INPUT: ERROR TOLERANCE xe=0.001*abs(x); % INPUT: SYSTEM INPUTS (th2,w2,al2) dth=5*pi/180; th2=0*pi/180:dth:180*pi/180; w2=-20.944*ones(1,length(th2)); al2=0*ones(1,length(th2)); %---------------------------------------------- xe=transpose(abs(xe)); kerr=1; %If kerr=1, results are not converged
MATLAB PROGRAM
for k=1:1:length(th2); for n=1:Nmax %---------------------------------------------- %Assign initial guess to unknowns th4(k)=x(1);th5(k)=x(2); s(k)=x(3);to(k)=x(4); % INPUT: JACOBIAN Matrix J=zeros(4,4); J(1,1)=-s(k)*sin(th4(k));J(1,3)=cos(th4(k)); J(2,1)=s(k)*cos(th4(k));J(2,3)=sin(th4(k)); J(3,1)=-a4*sin(th4(k));J(3,2)=a5*sin(th5(k)); J(4,1)=a4*cos(th4(k));J(4,2)=-a5*cos(th5(k));J(4,4)=1; % INPUT: Function f f=zeros(4,1); f(1,1)=-(s(k)*cos(th4(k))-a2*cos(th2(k))-a1); f(2,1)=-(s(k)*sin(th4(k))-a2*sin(th2(k))); f(3,1)=-(a4*cos(th4(k))-a5*cos(th5(k))-a1-a6); f(4,1)=-(a4*sin(th4(k))-a5*sin(th5(k))+to(k)); %---------------------------------------------- eps=inv(J)*f;x=x+transpose(eps);
if abs(eps)<xe kerr=0;break end end if kerr==1 'Error nr' end th4(k)=x(1);th5(k)=x(2); s(k)=x(3);to(k)=x(4); %---velocity--------------------------- fv(1,1)=-w2(k)*a2.*sin(th2(k)); fv(2,1)=w2(k)*a2.*cos(th2(k)); fv(3,1)=0; fv(4,1)=0; vel=inv(J)*fv; w4(k)=vel(1);w5(k)=vel(2); Vs(k)=vel(3);Vt(k)=vel(4);
%---acceleration--------------------------- fa(1,1)=-al2(k)*a2*sin(th2(k))-w2(k)^2*a2*cos(th2(k))+2*w4(k)*Vs(k)*sin(th4(k))+w4(k)^2*s(k)*cos(th4(k)); fa(2,1)=al2(k)*a2*cos(th2(k))-w2(k)^2*a2*sin(th2(k))-2*w4(k)*Vs(k)*cos(th4(k))+w4(k)^2*s(k)*sin(th4(k)); fa(3,1)=w4(k)^2*a4*cos(th4(k))-a5*w5(k)^2*cos(th5(k)); fa(4,1)=w4(k)^2*a4*sin(th4(k))-a5*w5(k)^2*sin(th5(k)); acc=inv(J)*fa; al4(k)=acc(1);al5(k)=acc(2); as(k)=acc(3);at(k)=acc(4); End % Angle: radian --> degree th2d=th2*180/pi; th4d=th4*180/pi; th5d=th5*180/pi; %--------Plots--------------- figure(1), subplot(4,3,1),plot(th2d,th4d,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('\theta_4 (^o)'),xlim([0 180]) subplot(4,3,2),plot(th2d,w4,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('\omega_4 (r/s)'),xlim([0 180]) subplot(4,3,3),plot(th2d,al4,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('\alpha_4 (r/s^2)'),xlim([0 180])
subplot(4,3,4),plot(th2d,th5d,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('\theta_5 (^o)'),xlim([0 180]) subplot(4,3,5),plot(th2d,w5,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('\omega_5 (r/s)'),xlim([0 180]) subplot(4,3,6),plot(th2d,al5,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('\alpha_5 (r/s^2)'),xlim([0 180]) subplot(4,3,7),plot(th2d,s,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('s (cm)'),xlim([0 180]) subplot(4,3,8),plot(th2d,Vs,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('V_s (cm/s)'),xlim([0 180]) subplot(4,3,9),plot(th2d,as,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('a_s (cm/s^2)'),xlim([0 180]) subplot(4,3,10),plot(th2d,to,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('t (cm)'),xlim([0 180]) subplot(4,3,11),plot(th2d,Vt,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('V_t (cm/s)'),xlim([0 180]) subplot(4,3,12),plot(th2d,at,'r','linewidth',2),xlabel('\theta_2 (^o)'),ylabel('a_t (cm/s^2)'),xlim([0 180])
%%-------Writing position values to file ------------------ knm=[transpose(th2d) transpose(th4d) transpose(th5d) transpose(s) transpose(to)]; str = 'th2 th4 th5 s t '; %# A string fName = 'konum.txt'; %# A file name fid = fopen(fName,'w'); %# Open the file if fid ~= -1 fprintf(fid,'%s\r\n',str); %# Print the string fclose(fid); %# Close the file end dlmwrite(fName,knm,'-append',... %# Print the matrix 'delimiter','\t', 'newline','pc'); %%-------Writing velocity values to file ------------------ hz=[transpose(th2d) transpose(w4) transpose(w5) transpose(Vs) transpose(Vt)]; str = 'th2 w4 w5 Vs Vt '; %# A string fName = 'hız.txt'; %# A file name fid = fopen(fName,'w'); %# Open the file if fid ~= -1 fprintf(fid,'%s\r\n',str); %# Print the string fclose(fid); %# Close the file end dlmwrite(fName,hz,'-append',... %# Print the matrix 'delimiter','\t', 'newline','pc');
%%-------Writing acceleration values to file ------------------ ivme=[transpose(th2d) transpose(al4) transpose(al5) transpose(as) transpose(at)]; str = 'th2 al4 al5 as at '; %# A string fName = 'ivme.txt'; %# A file name fid = fopen(fName,'w'); %# Open the file if fid ~= -1 fprintf(fid,'%s\r\n',str); %# Print the string fclose(fid); %# Close the file end dlmwrite(fName,hz,'-append',... %# Print the matrix 'delimiter','\t',... 'newline','pc');