Dan B. Marghitu, Mechanisms and Robots Analysis with MATLAB®
Подождите немного. Документ загружается.
328 B Programs of Chapter 3: Velocity and Acceleration Analysis
fprintf(’\n’)
vA=[000];aA=[000];
vC=[000];aC=[000];
vB1 = vA + cross(omega1,rB);
aB1 = aA + cross(alpha1,rB) - dot(omega1,omega1)
*
rB;
fprintf(’vB = vB1 = [ %g, %g, %g ] (m/s)\n’, vB1)
fprintf(’aB = aB1 = [ %g, %g, %g ] (m/sˆ2)\n’, aB1)
fprintf(’\n’)
% angular velocity of link 3
omega3z = sym(’omega3z’,’real’); % omega3z unknown
omega3=[00omega3z ];
vB21 = sym(’vB21’,’real’); % vB21 unknown
% vB2B1 parallel to the sliding direction
vB2B1 = [ vB21
*
cos(phi1) vB21
*
sin(phi1) 0 ];
% vB2 = vB3 = vC + omega3 x (rB-rC)
vB3 = vC + cross(omega3,rB-rC);
vB2 = vB3;
eqvB = vB2 - ( vB1 + vB2B1 ); % vB2 = vB1 + vB2B1
eqvBx = eqvB(1); % equation component on x-axis
eqvBy = eqvB(2); % equation component on y-axis
solvB = solve(eqvBx,eqvBy);
omega3zs = eval(solvB.omega3z);
vB21s = eval(solvB.vB21);
Omega3 = [0 0 omega3zs];
VB21 = vB21s
*
[cos(phi1) sin(phi1) 0];
VB3 = vC + cross(Omega3,rB-rC);
% print the equations for calculating
% omega3z and vB21
fprintf(’vB2 = vB1 + vB2B1 => \n’)
qvBx=vpa(eqvBx,6);
fprintf(’x-axis:\n’)
fprintf(’%s=0\n’,char(qvBx))
qvBy=vpa(eqvBy,6);
fprintf(’y-axis:\n’)
fprintf(’%s=0\n’,char(qvBy))
B.3 Inverted Slider-Crank Mechanism 329
fprintf(’=>\n’)
fprintf(’omega3z = %g (rad/s)\n’, omega3zs)
fprintf(’vB21 = %g (m/s)\n\n’, vB21s)
fprintf(’omega3 = [%g, %g, %g](rad/s)\n’, Omega3)
fprintf(’vB2B1 = [ %g, %g, %d ] (m/s)\n’, VB21)
fprintf(’\n’)
fprintf(’vB3 = [ %g, %g, %d ] (m/s)\n’, VB3)
fprintf(’\n’)
% angular acceleration of link 3
alpha3z = sym(’alpha3z’,’real’); % alpha3z unknown
alpha3=[00alpha3z ];
aB21 = sym(’aB21’,’real’); % aB21 unknown
% aB2B1 parallel to the sliding direction
aB2B1 = [ aB21
*
cos(phi1) aB21
*
sin(phi1) 0 ];
% aB2=aB3=aC+alpha3 x rCB-(omega3)ˆ2 rCB
aB3=aC+cross(alpha3,rB-rC)-...
dot(Omega3,Omega3)
*
(rB-rC);
aB2 = aB3;
% aB2B1cor = 2 omega1 x vB2B1
aB2B1cor = 2
*
cross(omega1,VB21);
% aB2 = aB1 + aB2B1 + aB2B1cor
eqaB = aB2 - ( aB1 + aB2B1 + aB2B1cor );
eqaBx = eqaB(1); % equation component on x-axis
eqaBy = eqaB(2); % equation component on y-axis
solaB = solve(eqaBx,eqaBy);
alpha3zs = eval(solaB.alpha3z);
aB21s = eval(solaB.aB21);
Alpha3 = [0 0 alpha3zs];
AB21 = aB21s
*
[cos(phi1) sin(phi1) 0];
AB3=aC+cross(Alpha3,rB-rC)-...
dot(Omega3,Omega3)
*
(rB-rC);
% print the equations for calculating
% alpha3z and aB21
fprintf...
(’aB2B1cor = [%g, %g, %d] (m/sˆ2)\n’, aB2B1cor)
330 B Programs of Chapter 3: Velocity and Acceleration Analysis
fprintf(’\n’)
fprintf...
(’aB2 = aB1 + aB2B1 + aB2B1cor => \n’)
qaBx=vpa(eqaBx,6);
fprintf(’x-axis:\n’)
fprintf(’%s=0\n’,char(qaBx))
qaBy=vpa(eqaBy,6);
fprintf(’y-axis:\n’)
fprintf(’%s=0\n’,char(qaBy))
fprintf(’=>\n’)
fprintf(’alpha3z = %g (rad/sˆ2)\n’, alpha3zs)
fprintf(’aB21 = %g (m/sˆ2)\n’, aB21s)
fprintf(’\n’)
fprintf...
(’alpha3 = [ %g, %g, %g ] (rad/sˆ2)\n’, Alpha3)
fprintf(’aB2B1 = [ %g, %g, %d ] (m/sˆ2)\n’, AB21)
fprintf(’aB3 = [ %g, %g, %d ] (m/sˆ2)\n’, AB3)
fprintf(’\n’)
omega23 = omega2 - Omega3;
alpha23 = alpha2 - Alpha3;
fprintf...
(’omega23 = [ %g, %g, %g ] (rad/s)\n’, omega23)
fprintf...
(’alpha23 = [ %g, %g, %g ] (rad/sˆ2)\n’, alpha23)
% end of program
Results:
phi = 60 (degrees)
rB = [ 0.113535, 0.196648,0](m)
rC = [ 0.15, 0,0](m)
rD = [ 0.175, 0.303109,0](m)
phi3 = 100.505 (degrees)
Velocity and acceleration analysis
omega1 = [0, 0, 3.14159](rad/s)
alpha1 = [0, 0, 0](rad/sˆ2)
vB = vB1 = [ -0.617787, 0.356679, 0 ] (m/s)
aB = aB1 = [ -1.12054, -1.94083, 0 ] (m/sˆ2)
B.4 R-RTR-RTR Mechanism 331
vB2 = vB1 + vB2B1 =>
x-axis:
-.196648
*
omega3z+.617787-.500000
*
vB21=0
y-axis:
-.364655e-1
*
omega3z-.356679-.866025
*
vB21=0
=>
omega3z = 4.69102 (rad/s)
vB21 = -0.609381 (m/s)
omega3 = [0, 0, 4.69102](rad/s)
vB2B1 = [ -0.30469, -0.527739, 0 ] (m/s)
vB3 = [ -0.922477, -0.17106, 0 ] (m/s)
aB2B1cor = [3.31588, -1.91443, 0] (m/sˆ2)
aB2 = aB1 + aB2B1 + aB2B1cor =>
x-axis:
-.196648
*
alpha3z-1.39290-.500000
*
aB21=0
y-axis:
-.364655e-1
*
alpha3z-.472095-.866025
*
aB21=0
=>
alpha3z = -6.38024 (rad/sˆ2)
aB21 = -0.276477 (m/sˆ2)
alpha3=[0,0,-6.38024 ] (rad/sˆ2)
aB2B1 = [ -0.138239, -0.239436, 0 ] (m/sˆ2)
aB3 = [ 2.0571, -4.0947, 0 ] (m/sˆ2)
omega23=[0,0,-1.54942 ] (rad/s)
alpha23=[0,0,6.38024 ] (rad/sˆ2)
B.4 R-RTR-RTR Mechanism
%B4
% Velocity and acceleration analysis
% R-RTR-RTR
clear all; clc; close all
AB = 0.15; AC = 0.10; CD = 0.15; %(m)
DF = 0.40; AG = 0.30; % (m)
332 B Programs of Chapter 3: Velocity and Acceleration Analysis
phi = pi/6 ; % (rad)
xA=0;yA=0;rA=[xAyA0];
xC = 0; yC = AC; rC = [xC yC 0];
xB=AB
*
cos(phi); yB = AB
*
sin(phi); rB = [xB yB 0];
eqnD1=’( xDsol - xC )ˆ2 + ( yDsol - yC )ˆ2 = CDˆ2’;
eqnD2=’(yB-yC)/(xB-xC)=(yDsol-yC)/(xDsol-xC)’;
solD = solve(eqnD1, eqnD2, ’xDsol, yDsol’);
xDpositions = eval(solD.xDsol);
yDpositions = eval(solD.yDsol);
xD1=xDpositions(1); xD2=xDpositions(2);
yD1=yDpositions(1); yD2=yDpositions(2);
if(phi>=0&&phi<=pi/2)||(phi>=3
*
pi/2&&phi<=2
*
pi)
if xD1 <= xC xD=xD1; yD=yD1; else xD=xD2; yD=yD2;
end
else
if xD1 >= xC xD=xD1; yD=yD1; else xD=xD2; yD=yD2;
end
end
rD=[xD yD 0];
phi2=atan((yB-yC)/(xB-xC)); phi3=phi2;
phi4=atan(yD/xD)+pi; phi5=phi4;
xF=xD+DF
*
cos(phi3); yF=yD+ DF
*
sin(phi3);
rF=[xF yF 0];
xG=AG
*
cos(phi5); yG=AG
*
sin(phi5);
rG = [xG yG 0];
fprintf(’Results \n\n’)
fprintf(’phi = phi1 = %g (degrees) \n’,phi
*
180/pi)
fprintf(’rA = [ %g, %g, %g ] (m)\n’, rA)
fprintf(’rC = [ %g, %g, %g ] (m)\n’, rC)
fprintf(’rB = [ %g, %g, %g ] (m)\n’, rB)
fprintf(’rD = [ %g, %g, %g ] (m)\n’, rD)
fprintf...
(’phi2 = phi3 = %g (degrees) \n’,phi2
*
180/pi)
fprintf...
(’phi4 = phi5 = %g (degrees) \n’,phi4
*
180/pi)
fprintf(’rF = [ %g, %g, %g] (m)\n’, rF)
fprintf(’rG = [ %g, %g, %g] (m)\n’, rG)
% Graphic of the mechanism
plot([xA,xB],[yA,yB],’k-o’,[xD,xF],[yD,yF],’b-o’,...
[xA,xG],[yA,yG],’r-o’)
xlabel(’x (m)’), ylabel(’y (m)’),...
title(’positions for \phi = 30 (deg)’),...
B.4 R-RTR-RTR Mechanism 333
text(xA,yA,’ A’), text(xB,yB,’ B’),...
text(xC,yC,’ C’), text(xD,yD,’ D’),...
text(xF,yF,’ F’), text(xG,yG,’ G’),...
axis([-0.3 0.3 -0.1 0.3]), grid
fprintf(’\n’)
fprintf(’Velocity and acceleration analysis \n\n’)
n = 50.;
omega1=[00pi
*
n/30 ]; alpha1 = [000];
fprintf...
(’omega1 = [ %g, %g, %g ] (rad/s)\n’, omega1)
fprintf...
(’alpha1 = [ %g, %g, %g ] (rad/sˆ2)\n’, alpha1)
fprintf(’\n’)
vA=[000];aA=[000];
vB1 = vA + cross(omega1,rB); vB2 = vB1;
aB1 = aA + cross(alpha1,rB) - ...
dot(omega1,omega1)
*
rB;
vB2 = vB1;
aB2 = aB1;
fprintf...
(’vB = vB1 = vB2 = [ %g, %g, %g ] (m/s)\n’,vB1)
fprintf...
(’aB = aB1 = aB2 = [ %g, %g, %g ] (m/sˆ2)\n’,aB1)
fprintf(’\n’)
omega3z=sym(’omega3z’,’real’);
vB32=sym(’vB32’,’real’);
omega3=[00omega3z ];
% omega3z unknown (to be calculated)
% vB32 unknown (to be calculated)
vC=[000];%Cisfixed
% vB3 = vC + omega3 x rCB
% (B3 & C are points on link 3)
vB3 = vC + cross(omega3,rB-rC);
% point B2 is on link 2 and point B3 is on link 3
% vB3 = vB2 + vB3B2
% between the links 2 and 3 there is a
% translational joint B_T
% vB3B2 is the relative velocity of B3 wrt 2
% vB3B2 is parallel to the sliding direcion BC
% vB3B2 is written as a vector
334 B Programs of Chapter 3: Velocity and Acceleration Analysis
vB3B2 = vB32
*
[ cos(phi2) sin(phi2) 0];
% vB3 = vB2 + vB3B2
eqvB = vB3 - vB2 - vB3B2;
% vectorial equation
% the component of the vectorial equation on x-axis
eqvBx = eqvB(1);
% the component of the vectorial equation on y-axis
eqvBy = eqvB(2);
% two equations eqvBx and eqvBy with two unknowns
% solve for omega3z and vB32
solvB = solve(eqvBx,eqvBy);
omega3zs=eval(solvB.omega3z);
vB32s=eval(solvB.vB32);
Omega3 = [0 0 omega3zs]; Omega2 = Omega3;
VB32 = vB32s
*
[cos(phi2) sin(phi2) 0];
% print the equations for calculating
% omega3 and vB32
fprintf...
(’vB3 = vC + omega3 x rCB = vB2 + vB3B2 => \n’)
qvBx=vpa(eqvBx,6);
fprintf(’x-axis: %s = 0 \n’, char(qvBx))
qvBy=vpa(eqvBy,6);
fprintf(’y-axis: %s = 0 \n’, char(qvBy))
fprintf(’=>\n’)
fprintf(’omega3z = %g (rad/s)\n’, omega3zs)
fprintf(’vB32 = %g (m/s)\n’, vB32s)
fprintf(’\n’)
fprintf...
(’omega2=omega3 = [ %g,%g,%g ](rad/s)\n’, Omega3)
fprintf(’vB3B2 = [ %g, %g, %g ] (m/s)\n\n’, VB32)
% Coriolis acceleration
aB3B2cor = 2
*
cross(Omega3,VB32);
alpha3z=sym(’alpha3z’,’real’);
aB32=sym(’aB32’,’real’); % aB32 unknown
alpha3=[00alpha3z ]; % alpha3z unknown
aC=[000];%Cisfixed
% aB3 acceleration of B3
aB3 = aC + cross(alpha3, rB-rC) -...
dot(Omega3, Omega3)
*
(rB-rC);
% aB3B2 relative velocity of B3 wrt 2
B.4 R-RTR-RTR Mechanism 335
% aB3B2 parallel to the sliding direcion BC
aB3B2 = aB32
*
[ cos(phi2) sin(phi2) 0];
% aB3 = aB2 + aB3B2 + aB3B2cor
eqaB = aB3 - aB2 - aB3B2 - aB3B2cor;
% vectorial equation
eqaBx = eqaB(1); % equation component on x-axis
eqaBy = eqaB(2); % equation component on y-axis
solaB = solve(eqaBx,eqaBy);
alpha3zs=eval(solaB.alpha3z);
aB32s=eval(solaB.aB32);
Alpha3 = [0 0 alpha3zs];
Alpha2 = Alpha3;
AB32 = aB32s
*
[cos(phi2) sin(phi2) 0];
% print the equations for calculating
% alpha3 and aB32
fprintf...
(’aB32cor = [ %g, %g, %d ](m/sˆ2)\n’, aB3B2cor)
fprintf(’\n’)
fprintf(’aB3=aC+alpha3xrCB-(omega3.omega3)rCB\n’)
fprintf(’aB3=aB2+aB3B2+aB3B2cor =>\n’)
qaBx=vpa(eqaBx,6);
fprintf(’x-axis:\n’)
fprintf(’%s = 0\n’,char(qaBx))
qaBy=vpa(eqaBy,6);
fprintf(’y-axis:\n’)
fprintf(’%s = 0\n’,char(qaBy))
fprintf(’=>\n’);
fprintf(’alpha3z = %g (rad/s)\n’, alpha3zs)
fprintf(’aB32 = %g (m/s)\n’, aB32s)
fprintf(’\n’)
fprintf...
(’alpha2=alpha3 = [ %g,%g,%g ](rad/sˆ2)\n’, Alpha3)
fprintf(’aB3B2 = [ %g, %g, %g ] (m/sˆ2)\n\n’, AB32)
% vD3 velocity of D3
% D3 & C points on link 3
vD3 = vC + cross(Omega3,rD-rC);
vD4 = vD3;
fprintf(’vD3 = vD4 = [ %g, %g, %g ] (m/s)\n’, vD3)
% aD3 acceleration of D3
aD3 = aC + cross(Alpha3, rD-rC)-...
dot(Omega3, Omega3)
*
(rD-rC);
aD4 = aD3;
336 B Programs of Chapter 3: Velocity and Acceleration Analysis
fprintf(’aD3 = aD4 = [ %g, %g, %g ] (m/sˆ2)\n’, aD3)
fprintf(’\n’)
omega5z = sym(’omega5z’,’real’);
% omega5z unknown
vD54 = sym(’vD54’,’real’);
% vD54 unknown
omega5=[00omega5z ];
% vD5 velocity of D5
% D5 & A points on link 5
vD5 = vA + cross(omega5,rD);
% vD5D4 relative velocity of D5 wrt 4
% vD5D4 parallel to the sliding direcion DE
vD5D4 = vD54
*
[ cos(phi5) sin(phi5) 0];
% vD5 = vD4 + vD5D4
eqvD = vD5 - vD4 - vD5D4;
% vectorial equation
eqvDx = eqvD(1); % component on x-axis
eqvDy = eqvD(2); % component on y-axis
solvD = solve(eqvDx,eqvDy);
omega5zs=eval(solvD.omega5z);
vD54s=eval(solvD.vD54);
Omega5 = [0 0 omega5zs];
Omega4 = Omega5;
VD54 = vD54s
*
[cos(phi5) sin(phi5) 0];
% print the equations for calculating
% omega5 and vD54
fprintf...
(’vD5 = vA + omega5 x rD = vD4 + vD5D4 => \n’)
qvDx=vpa(eqvDx,6);
fprintf(’x-axis: %s = 0 \n’, char(qvDx))
qvDy=vpa(eqvDy,6);
fprintf(’y-axis: %s = 0 \n’, char(qvDy))
fprintf(’=>\n’)
fprintf(’omega5z = %g (rad/s)\n’, omega5zs)
fprintf(’vD54 = %g (m/s)\n’, vD54s)
fprintf(’\n’)
fprintf...
(’omega4=omega5 = [ %g, %g, %g ] (rad/s)\n’, Omega5)
fprintf(’vD5D4 = [ %g, %g, %g ] (m/s)\n’, VD54 )
fprintf(’\n’)
% Coriolis acceleration
aD5D4cor = 2
*
cross(Omega5,VD54);
B.4 R-RTR-RTR Mechanism 337
alpha5z = sym(’alpha5z’,’real’);
aD54 = sym(’aD54’,’real’);
alpha5=[00alpha5z ]; % alpha5z unknown
aE=[000];
% aD5 acceleration of D5
aD5=aE+cross(alpha5,rD)-dot(Omega5,Omega5)
*
rD;
% aD5D4 relative velocity of B5 wrt 4
% aD5D4 parallel to the sliding direcion AD
% aD54 unknown
aD5D4 = aD54
*
[ cos(phi5) sin(phi5) 0];
% aB5 = aB4 + aD5D4+ aD5D4cor
eqaD = aD5 - aD4 - aD5D4 - aD5D4cor;
% vectorial equation
eqaDx = eqaD(1); % component on x-axis
eqaDy = eqaD(2); % component on y-axis
solaD = solve(eqaDx,eqaDy);
alpha5zs = eval(solaD.alpha5z);
aD54s = eval(solaD.aD54);
Alpha5 = [0 0 alpha5zs];
Alpha4 = Alpha5;
AD54 = aD54s
*
[cos(phi5) sin(phi5) 0];
% print the equations for calculating
% alpha5 and aD54
fprintf...
(’aD54cor = [ %g, %g, %g ] (m/sˆ2)\n’, aD5D4cor)
fprintf(’\n’)
fprintf(’aD5=aA+alpha5xrD-(omega5.omega5)rD=\n’)
fprintf(’aD5=aD4+aD5D4+aD5D4cor =>\n’)
qaDx=vpa(eqaDx,6);
fprintf(’x-axis: %s = 0 \n’, char(qaDx))
qaDy=vpa(eqaDy,6);
fprintf(’y-axis: %s = 0 \n’, char(qaDy))
fprintf(’=>\n’)
fprintf(’alpha5z = %g (rad/sˆ2)\n’, alpha5zs)
fprintf(’aD54 = %g (m/sˆ2)\n’, aD54s)
fprintf(’\n’)
fprintf...
(’alpha4=alpha5 = [ %g,%g,%g ] (rad/sˆ2)\n’, Alpha5)
fprintf(’aD54 = [ %g, %g, %g ] (m/sˆ2)\n’, AD54)
% end of program