Computation of Multibody Kinematics And Dynamics 1.1 Multibody Mechanical Systems. 2D Planar Mechanism. 3D Spatial Mechanism

Transcription

1 Computation of Multibody Kinematics And Dynamics 1.1 Multibody Mechanical Systems D Planar Mechanism D Spatial Mechanism 1

2 Computer-Aided Design (CAD) Mechanics: Statics and dynamics. Dynamics : kinematics and kinetics. Kinematics is the study of motion, i.e., the study of displacement, velocity, and acceleration, regardless of the forces that produce the motion. Kinetics or Dynamics is the study of motion and its relationship with the forces that produce that motion.

3 1. Coordinate Systems A four-bar mechanism with generalized coordinates. 4 Coordinate q [ 1 ] T Constraints ( r l s d ) rl cosls cos1rs cos( 1) ( r l s d ) rlcosdscos 1 degrees of freedom 4 = 1

4 coordinates 1 q constraints rcos dcos scos l 1 rsin dsin ssin 1 dof 1 4

5 q x y x y x y r x1 cos1 r y1 sin1 r d x1 cos1x cos r d y1 sin1 y sin d s x cos x cos d s y sin y sin s x cos l s y sin Cartesian Coordinates 1 coordinates and 11 kinematic constraints T dof

6 Coordinates Systems Generalized Relative Cartesian coordinates coordinates coordinates Number of coordinates Minimum Moderate Large Number of second-order Minimum Moderate Large differential equations Number of algebraic None Moderate Large constraint equations Order of nonlinearity High Moderate Low Derivation of the Hard Moderate Simple equations of motion hard Computational Efficient Efficient Not as efficient efficiency Development of a Difficult Relatively Easy general-purpose difficult computer program 6

7 1. Computation Kinematics A mechanism that is formed from a collection of links or bodies kinematically connected to one another. An open-loop mechanism may contain links with single joint. A closed-loop mechanism is formed from a closed chain, wherein each link is connected to at least two other links of the mechanism. Figure (a) Open-loop mechanism-double pendulum and (b) closed-loop mechanism-four-bar linkage. Single and Multi-Loop Mechanism Figure (a) Single-loop mechanism and (b) multi-loop mechanism. 7

8 High and Low Pair of Kinematic Joint Figure Example of kinematic pairs: (a) revolute joint, (b) translational joint, (c) gear set, (d) cam follower, (e) screw joint, and (f) spherical ball joint. 8

9 Generalized Coordinates generalized coordinates, algebraic constraint equations, l cos l cos l cos d l sin l sin l sin d 1 1 Figure four-bar mechanism. 9

10 .1 Planar Kinematics in Cartesian Coordinates q [,, ] T i xc yc c i i The column vector is the vector of body-fixed coordinates for body in a plane. q x, y, z,,, i c c c 1 i Inertial coordinates Body fixedcoordinates T is the vector of body-fixed coordinates for body in three-dimensional space Position vector at global coordinate X Y r r As P P i i i i x xc cos sin y y c sincos is coordinate transformation matrix. i i 1

11 A constraint equation describing a condition on the vector of coordinates of a system can be expressed as follows: q In some constraint and driving function, the variable time may appear explicitly: Constraint Equation q, t Constraint Jacobian Matrix by differentiating the constraint equations m1 n1 ( q) q q, often denoted as q, q q q+ q q q also denoted as,q q( q ) q, or q q q qq= ( qq ) qq, m1 q q mn 11

12 Kinematically equivalent. Redundant Constraint Figure (a) A double parallel-crank mechanism and (b) its kinematically equivalent. Kinematics of Mechanism Mainly composed of revolute joint and translation joint Quick-return mechanism: (a) schematic presentation and (b) its equivalent representation without showing the actual outlines. 1

13 .1 Planar Kinematics in Cartesian Coordinates Inertial system Χ Y Body fixed system ξ η r r As P P i i i i Coordinate transformation matrix A i cos sin sincos i 1

14 P P r s r s i i j j. Revolute Joint Constraint Model r As r A s ( r,) P P i i i j j j P P P P x cos sin cos sin (,) i i i i i x r j j j j j P P P P yi i sini i cosi yj j sinj j cos j Figure Revolute joint P connecting bodies i and j. 14

15 Constraint Jacobian Matrix ( q) by differentiating the constraint equations q q, Denoteas q q q ( q ) q q q q q= ( q ) q q q q 15

16 Time Derivative of Revolute Joint Constraint P P P P 1 x cos sin x cos sin i i i i i j j j j j P P P P yi i sini i cosi yj j sinj j cosj x i ( i P sin i i P cos i ) i x j ( j P sin j j P cos j ) j y i ( i P cos i i P sin i ) i y j ( j P cos j j P sin j ) j 16

17 Time Derivative of Revolute Constraint P P P P 1 x cos sin x cos sin i i i i i j j j j j P P P P y sin cos y sin cos x i i i i i j j j j j x i ( P i sin i P i cos i ) i x j ( P j sin j P j cos j ) j y i ( P i cos i P i sin i ) i y j ( P j cos j P j sin j ) j ( sin cos ) ( cos sin ) x P P P P i i i i i i i i i i i j y P P P P ( j sinj j cos j) j ( j cosj j sin j) j ( cos sin ) ( sin cos ) y P P P P i i i i i i i i i i i j P P P P ( j cosj j sin j) j ( j sinj j cos j) j or s s p p i i j j 17

18 . Translational Joint Constraint Model Figure Different representations of a translational joint. Translational Joint nd T i P P x P R P R i x j xi xi yi yi P P yi yj P P P Q P P P Q ( xi xj )( yi yi ) ( yi yj )( xi xj ) i j ( i j) Figure A translational joint between bodies i and j. 18

19 Constraint Jacobian Matrix ( q) q q, Denoteas q q q ( q ) q q q q q= ( q ) q q q q 19

20 Time Derivative of Translational Joint Constraint P Q P P P Q P P ( xi xi )( yj yi ) ( yi yi )( xj xi ) i j ( i j) [( x x )( x x ) ( y y )( y y ) [( x x )( y y ) ( y y )( x x ) P Q P Q P Q P Q i i i j i i i j j i i i j i i i j i

21 Driving Link Figure (a) The motion of the slider is controlled in the direction and (b) the motion of point P is controlled in the y direction. x d() t i P y d() t i x 1

22 .1 Kinematic Analysis ground Treat ground as a rigid body 1 1

23 A Matlab Program for Analysis of Kinematics

24 Example of a four-bar linkage mechanism 1 A 1 y 1 OA 8mm AB 6mm BC 18mm OC 18mm B 8 Constraint equations: x 4cos 1 1 y 4sin x y x y x y 1 1 4cos x 1cos 1 1 4sin y 1sin 1 1 1cos x 9cos 1sin y 9sin 9cos 18 9sin 1 constraint from driving link t 1 O C x To solve the 9 equations for 9 unknown q T x 1, y1, 1, x, y,, x, y, 4

25 The Jacobian matrix and the right-side of the velocity equations q 1 1 4sin 1 4cos 1 4sin 1 1sin cos 1 1 cos J 5 1 1sin 1 9sin 6 1 1cos 1 9cos 7 1 9sin 8 1 9cos 9 1 Let J to solve Jq β for the velocity q q 5

26 The right-side of the acceleration equations 9sin 9cos 9sin 1sin 9cos 1cos 1sin 5sin 1cos 5cos 4sin 4cos γ q γ q on for the accelerati solve To J 6

27 The procedure of m-file Initialize and start at t Call nonlinear solver with initial positionto solveqt Determine Jacobian matrix and β with q t to solve q t Determine γ with q t to solve q t Assume q t Δt with intended motion and set it as the new initial position of next iteration Plot the time response of q t, q t and q t Determine the positions of the bars and animate them 7

28 The code of m.file (1) 1. % Set up the time interval and the initial positions of the nine coordinates. T_Int=:.1:;. X=[ 5 pi/ ]; 4. global T 5. Xinit=X; % Do the loop for each time interval 8. for Iter=1:length(T_Int); 9. T=T_Int(Iter); 1. % Determine the displacement at the current time 11. [Xtemp,fval] = fsolve(@constreq4bar,xinit); % Determine the velocity at the current time 14. phi1=xtemp(); phi=xtemp(6); phi=xtemp(9); 15. JacoMatrix=Jaco4bar(phi1,phi,phi); 16. Beta=[ *pi]'; 17. Vtemp=JacoMatrix\Beta; % Determine the acceleration at the current time. dphi1=vtemp(); dphi=vtemp(6); dphi=vtemp(9); 1. Gamma=Gamma4bar(phi1,phi,phi,dphi1,dphi,dphi);. Atemp=JacoMatrix\Gamma;. 4. % Record the results of each iteration 5. X(:,Iter)=Xtemp; V(:,Iter)=Vtemp; A(:,Iter)=Atemp; % Determine the new initial position to solve the equation of the next 8. % iteration and assume that the kinematic motion is with inertia 9. if Iter==1. Xinit=X(:,Iter); 1. else. Xinit=X(:,Iter)+(X(:,Iter)-X(:,Iter-1));. end 4. 5.end 8

29 The code of m.file () 6.% T vs displacement plot for the nine coordinates 7.figure 8.for i=1:9; 9. subplot(9,1,i) 4. plot (T_Int,X(i,:)) 41. set(gca,'xtick',[], 'FontSize', 5) 4.end 4.% Reset the bottom subplot to have xticks 44.set(gca,'xtickMode', 'auto') % T vs velocity plot for the nine coordinates 47.figure 48.for i=1:9; 49. subplot(9,1,i) 5. plot (T_Int,V(i,:)) 51. set(gca,'xtick',[], 'FontSize', 5) 5.end 5.set(gca,'xtickMode', 'auto') % T vs acceleration plot for the nine coordinates 56.figure 57.for i=1:9; 58. subplot(9,1,i) 59. plot (T_Int,A(i,:)) 6. AxeSup=max(A(i,:)); 61. AxeInf=min(A(i,:)); 6. if AxeSup-AxeInf<.1 6. axis([-inf,inf,(axesup+axesup)/-.1 (AxeSup+AxeSup)/+.1]); 64. end 65. set(gca,'xtick',[], 'FontSize', 5) 66.end 67.set(gca,'xtickMode', 'auto') 9

30 The code of m.file () 68.% Determine the positions of the four revolute joints at each iteration 69.Ox=zeros(1,length(T_Int)); 7.Oy=zeros(1,length(T_Int)); 71.Ax=8*cos(X(,:)); 7.Ay=8*sin(X(,:)); 7.Bx=Ax+6*cos(X(6,:)); 74.By=Ay+6*sin(X(6,:)); 75.Cx=18*ones(1,length(T_Int)); 76.Cy=zeros(1,length(T_Int)); % Animation 79.figure 8.for t=1:length(t_int); 81. bar1x=[ox(t) Ax(t)]; 8. bar1y=[oy(t) Ay(t)]; 8. barx=[ax(t) Bx(t)]; 84. bary=[ay(t) By(t)]; 85. barx=[bx(t) Cx(t)]; 86. bary=[by(t) Cy(t)]; plot (bar1x,bar1y,barx,bary,barx,bary); 89. axis([-1,4,-1,]); 9. axis normal M(:,t)=getframe; 9.end

31 Initialization 1. % Set up the time interval and the initial positions of the nine coordinates. T_Int=:.1:;. X=[ 5 pi/ ]; 4. global T 5. Xinit=X; 1. The sentence is notation that is behind symbol %.. Simulation time is set from to with Δt =.1.. Set the appropriate initial positions of the 9 coordinates which are used to solve nonlinear solver. 4. Declare a global variable T which is used to represent the current time t and determine the driving constraint for angular velocity. 1

32 Determine the displacement 1. [Xtemp,fval] = fsolve(@constreq4bar,xinit); 1. Call the nonlinear solver fsolve in which the constraint equations and initial values are necessary. The initial values is mentioned in above script. The constraint equations is written as a function (which can be treated a kind of subroutine in Matlab) as following and named as constreq4bar. The fsolve finds a root of a system of nonlinear equations and adopts the trust-region dogleg algorithm by default. a. function F=constrEq4bar(X) b. c. global T d. e. x1=x(1); y1=x(); phi1=x(); f. x=x(4); y=x(5); phi=x(6); g. x=x(7); y=x(8); phi=x(9); h. i. F=[ -x1+4*cos(phi1); j. -y1+4*sin(phi1); k. x1+4*cos(phi1)-x+1*cos(phi); l. y1+4*sin(phi1)-y+1*sin(phi); m. x+1*cos(phi)-x+9*cos(phi); n. y+1*sin(phi)-y+9*sin(phi); o. x+9*cos(phi)-18; p. y+9*sin(phi); q. phi1-*pi*t-pi/]; The equation of driving constraint is depended on current time T

33 Determine the velocity 14. phi1=xtemp(); phi=xtemp(6); phi=xtemp(9); 15. JacoMatrix=Jaco4bar(phi1,phi,phi); 16. Beta=[ *pi]'; 17. Vtemp=JacoMatrix\Beta; 15. Call the function Jaco4bar to obtain the Jacobian Matrix depended on current values of displacement. 16. Declare the right-side of the velocity equations. 17. Solve linear equation by left matrix division \ roughly the same as J -1 β. The algorithm adopts several methods such as LAPACK, CHOLMOD, and LU. Please find the detail in Matlab Help. a. function JacoMatrix=Jaco4bar(phi1,phi,phi) b. c. JacoMatrix=[ -1-4*sin(phi1) ; d. -1 4*cos(phi1) ; e. 1-4*sin(phi1) -1-1*sin(phi) ; f. 1 4*cos(phi1) -1 1*cos(phi) ; g. 1-1*sin(phi) -1-9*sin(phi); h. 1 1*cos(phi) -1 9*cos(phi); i. 1-9*sin(phi); j. 1 9*cos(phi); k. 1 ];

34 Determine the acceleration. dphi1=vtemp(); dphi=vtemp(6); dphi=vtemp(9); 1. Gamma=Gamma4bar(phi1,phi,phi,dphi1,dphi,dphi);. Atemp=JacoMatrix\Gamma; 1. Call the function Gamma4bar to obtain the right-side of the velocity equations depended on current values of velocity.. Solve linear equation to obtain the current acceleration. a. function Gamma=Gamma4bar(phi1,phi,phi,dphi1,dphi,dphi) b. c. Gamma=[ 4*cos(phi1)*dphi1^; d. 4*sin(phi1)*dphi1^; e. 4*cos(phi1)*dphi1^+1*cos(phi)*dphi^; f. 4*sin(phi1)*dphi1^+1*sin(phi)*dphi^; g. 1*cos(phi)*dphi^+9*cos(phi)*dphi^; h. 1*sin(phi)*dphi^+9*sin(phi)*dphi^; i. 9*cos(phi)*dphi^; j. 9*sin(phi)*dphi^; k. ]; 4

35 Determine next initial positions 9. if Iter==1. Xinit=X(:,Iter); 1. else. Xinit=X(:,Iter)+(X(:,Iter)-X(:,Iter-1));. end 9.~. Predict the next initial positions with assumption of inertia except the first time of the loop. 5

36 Plot time response 7.figure 8.for i=1:9; 9. subplot(9,1,i) 4. plot (T_Int,X(i,:)) 41. set(gca,'xtick',[], 'FontSize', 5) 4.end 4.% Reset the bottom subplot to have xticks 44.set(gca,'xtickMode', 'auto') % T vs velocity plot for the nine coordinates 47.figure 48.for i=1:9; Create a blank figure. 9. Locate the position of subplot in the figure. 4. Plot the nine subplots for the time responses of nine coordinates. 41. Eliminate x-label for time-axis and set the font size of y-label. 44. Resume x-label at bottom because the nine subplots share the same time-axis. 47.~ It is similar to above. 6

37 Animation 69.Ox=zeros(1,length(T_Int)); 7.Oy=zeros(1,length(T_Int)); 71.Ax=8*cos(X(,:)); 7.Ay=8*sin(X(,:)); 7.Bx=Ax+6*cos(X(6,:)); for t=1:length(t_int); 81. bar1x=[ox(t) Ax(t)]; 8. bar1y=[oy(t) Ay(t)]; 8. barx=[ax(t) Bx(t)]; 84. bary=[ay(t) By(t)]; 85. barx=[bx(t) Cx(t)]; 86. bary=[by(t) Cy(t)]; plot (bar1x,bar1y,barx,bary,barx,bary); 89. axis([-1,4,-1,]); 9. axis normal M(:,t)=getframe; 9.end 69. Determine the displacement of revolute joint. 8. Repeat to plot the locations by continue time elapsing. 81. Determine the horizontal location of OA. 88. Plot OA, AB, BC, and OC. 89. Set an appropriate range of axis. 7

38 Time response of displacement x 1 y 1 1 x y x y t 8

39 Time response of velocity x 1 y 1 1 x y x y t 9

40 Time response of acceleration x 1 y 1 1 x y x y t 4

41 Example of a slider-crank mechanism 11 Constraint equations: x y x y x y x y x cos y sin cos x 1cos sin y 1sin 1cos x 1cos y y y x x 1cos 1sin 1sin 1cos constraint from the driving link t AG mm GB mm BO mm To solve the 1 equations for 1 unknown T q x1, y1, 1, x, y,, x, y,, x4, y4, 4 41

42 The Jacobian matrix and the right-side of the velocity equations J 1. 1cos 7, 17 sin 15 1sin, 1 cos 9 sin 5 1 5, 4,, 16, 1, 8, 4 1 6, 4, 6,, 18, 14, 1, 6,, 1, sin 1 cos cos 9 1sin 8 cos 19 y y x x 4

43 The right-side of the acceleration equations 1 1sin 1cos 1sin sin 1cos cos sin cos γ γ cos 1 1sin sin cos 1 where y y x x y y x x 4

44 Animation 44

45 Time response of displacement x y x y x 4 y 4 4 t 45

46 Time response of velocity x y x y x 4 y 4 4 t 46

47 Time response of acceleration x y x y x 4 y 4 4 t 47

48 Kinematic Modeling Ground x., y.,. Revolute joint ,.,.,. A A A A 4 4.,., 1.,. B B B B 1.,.,.,. 1 1 translational joint.,., 1.,., A A B B ,.,.,. C C driving constraint t. Figure Kinematic modeling of a slider-crank mechanism. 48

49 Translation Joint y 1 n d B c x1 c1s1 1 x11c1 B y s c y 1s x1 1c1 x4 d y 1s y c4s4 1s4 n s c 1 1c T x c1 x4 nd1s4 1c4 y1 1s1 y 4 1c y 1s y x 1c x 1s s 4 x1 1c y c4c1 1s4s1 1 y11s1 y41s4 x11c1x41c

50 Kinematic Modeling Ground revolute joints translational joint driving constraint x 1 1 y x x cos 4 4 y y sin 5 4 x cos x 1 cos 6 y sin y 1sin 7 x 1 cos x 8 1 y 1sin y 9 1 ( 1cos )( y 1sin y ) ( x 1cos x )( 1sin ) t Figure Kinematic modeling of a slider-crank mechanism. 5

51 Jacobian Matrix Figure The Jacobian matrix. 51

52 Cont d 5

53 kinematic constraints driving link velocity equations acceleration. Solution Technique ( q) ( d ) ( q, t) q= or qq q ( d ) ( d) ( d) q+ = or q qt q t q ( d ) q= ( d ) q t q q q+ q= or qq ( qq ) qq q q ( d) ( d) ( d) ( d) q ( q ) q q q q q qt tt 5

54 At any given instant t (1) Solve q ( qq= ) d q ( q, t) Solution Technique q=(the right hand side) n equations for n unknowns () Solve ( qq= ) q d q n equations for n unknowns () Solve ( qq= ) q d q q() t ( q, t) q=(the right hand side) ( qq= ) q () t (the right hand side) n equations for n unknowns q () t 54

55 4.1 Planar Rigid Body Dynamics mx f i i xi m y f i i yi n i i i m x fx m y f y n M i q i g i i 55

56 Illustration of Constraint Force Pure rolling of a disk down the slope mg x y f N mgs f mx mgc N my f R I c eqs. for 5 unknowns xy,,, f, N xr constraint equations y R g 1 x gs, y, s, N mgc, f mgs R 56

57 Constraint Force ( q, t), q ( c) q Mq g g T q T q ( c) q q n1 or q, q mn There exists Lagrange Multiplier, T such that g g is that constraint force. The equations of motion can be written as Mq m1 57

58 Generalized coordinate x, Constraint eq. x R, y R x R, y R T M mgs q q mgc q m 1 x m 1 mgs y mgc 1 mr R 1 1 R 1 g 1 5 eqs. for 5 unknowns x gs, y, s, 1 mgs, mgc R 1 1 T mgs The constraint force 1 q mgc R 1 mgrs 1 is the friction force for pure rolling. 58

59 4. Physical meaning of constraint force In Revolute Joint m x 1 f 1 ( ) ( ) x 1 m y fy P P y i i yi xi x i n i m i x i f ( x)i 1 m i y i f ( y)i i i n i ( y i P y i ) 1 (x i P x i ) 59

60 Constraint Force in Translational Joint For a translational joint between i and j, the equation of motion for body i can be written as P Q mx f ( y y ) i i xi i i 1 P Q my f ( x x ) i i yi i i 1 P P Q P P Q n [( x x )( x x ) ( y y )( y y )] i i i j i i i j i i i 1 6

61 4. Formulation of Multi-body Dynamic Systems Mq T q g qq= n m n m linear algebraic equations in unknowns for q and. 61

62 5.1 Euler Angles A DCB c s 1 c s D s c c s s c C B 1 s c 1 6

63 Euler Angles A= c c s c s c s s c c s s s c c c s s s c c c c s s s s c c (5.1) 6

64 Time Derivatives of Euler Angles sin sin cos sin cos sin cos sin sin cos sincos sin cos 1 64

65 5. Bryant Angles 65

66 Bryant Angles A DCB 1 c s c s D c s C 1 B s c 1 1 s1 c 1 s c 1 66

67 Bryant Angles A= cc cs s c1s s1sc c1c s1ss s1c s 1sc1sc s1cc1ss c1c 67

68 Time Derivative of Bryant Angles cos1cos sin 1 cossin cos sin 1 68