Professional Documents
Culture Documents
Mutibody Kinematics & Dynamics
Mutibody Kinematics & Dynamics
View range
Tandem or opposite
Wipe angle
Location of pivots
Timing of wipers
Wiping velocity
The force acting on the wipers
1
Kinematics
Kinematics
Deal with the way things move
Kinematic analysis
Determine
Position, displacement, rotation, speed, velocity,
acceleration
Provide
Geometry dimensions of the mechanism
Operation range
Dynamic analysis
Power capacity, stability, member load
requirements
Linkage
Frame
Links rigid body
Joint
Primary joint (full joint)
Mechanism Joint
Simple link
Complex link
Point of interest
Actuator
Simple Link
Simple Link
(with point of
interest)
Complex Link
Pin Joint
Revolute Joint
5
Kinematic Diagram
Slider Joint
Translation Joint
Cam Joint
Gear Joint
1.7 MOBILITY
10
One degree of freedom actually if pivoted links are the same size
11
12
13
14
s : short link
l : long link
p , q : intermediate link
15
16
17
18
19
20
r4
r2
r3
r3
r5
X1
r1
r4
(1) r 1 r 2 r 3 r 4 0
r1s1 r2c 2 r3c3 5.3
r c r s r s 3.2 0
1 1 2 2 3 3
r1 3,1 30,r2 4.9,r3 3.3
Y
r1
X
r3
r2
r 1 r 2 r 3 0
0.5c1 1.75c 2 1
0.5s 1.75s y 0
2
for1 2 ,solve for1and ymax
for1 2 ,solve for1and ymin
22
23
24
r1
X
r2
r4
r3
r 1 r 2 r 3 r 4 0
2 r 2 3r 3 4 r 4 0
eqs forunknowns3 and 4
25
26
X
r1
r4
r3
r2
r 1 r 2 r 3 0
0
1r 1 2 r 2 0
5
eqs for1and 2
r r 1 r 4
r 1r 1 2 r 4
27
Y
r1
r2
X
r3
r 1 r 2 r 3 0
solve forand
1r 1 2 r 2 r2 0
vc 2
1 5rad min,r 2
vs 2
eqs forunknowns2 and v
28
r2
r1
Y
r3
X
r 1 r 2 r 3 0
1
r2 6, 2 340 r 3
3
eqs forunknownsr1and 1
1r 1 2 r 2 r 2 0
8c
r 2 2
8s 2
eqs forunknowns1and 2
29
Y
r2
r1
r3
r4
r 1 r 2 r 3 r 4 0
1r 1 2 r 2 3r 3 0
given 1 find 2 and 3
30
31
X
r1
r2
r4
r3
r 1 r 2 r 3 r 4 0
1r 1 2 r 2 3r 3 0
given 1 find 2 and 3
1 1r 1 2 r 2 2 2 r 2 3r 3 3 3r 3 0
solve for2 and 3
32
r5
rp
rp r 1 r 2 r 3 r 5
rp 1r 1 2 r 2 3 r 3 r 5
rp 1 1r 1 2 r 2 2 2 r 2 3 r 3 r 5 3 3 r 3 r 5
33
Y
X
r1
r2
r3
r 1 r 2 r 3 0
0
1r 1 2 r 2 0
v
solve for2 and v
1r 1 1 1r 1 2 r 2 2 2 r 2 0
a
35
2D Planar Mechanism
3D Spatial Mechanism
36
4 Coordinate
q [1 2 3 ]T
3 Constraints
Generalized Coordinates
1
q 2
3
2 constraints
Cartesian Coordinates
12 coordinates and 11 kinematic constraints
q x1 y1 1 x2 y2 2 x3 y3 3
r
x1 cos 1 0
2
r
y1 sin 1 0
2
r
d
x1 cos 1 x2 cos 2 0
2
2
r
d
y1 sin 1 y2 sin 2 0
2
2
d
s
x2 cos 2 x3 cos 3 0
2
2
d
s
y2 sin 2 y3 sin 3 0
2
2
s
x3 cos 3 l 0
2
s
y3 sin 3 0
2
dof 9 8 1
40
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.
41
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.
42
Generalized Coordinates
3 generalized coordinates,
2 algebraic constraint equations,
ri P ri Ai siP
Coordinate transformation matrix
cos sin
Ai
sin
cos
44
Constraint Equation
( q ) 0
also denoted as
q+
q0
q
q
q q (q q )q q 0
q q = (q q )q q
45
Redundant Constraint
Kinematically equivalent.
Figure (a) A double parallel-crank mechanism and (b) its kinematically equivalent.
Kinematics of Mechanism
Mainly composed of revolute joint and translation joint
46
( r ,2)
P
P
P
P
i cos i y j j sin j
j cos j
yi i sin i +
0
47
48
sip i2 s jp j2
49
Translational Joint
niT d 0
P
P
x
i
j
P
R
P
R
xi xi yi yi P
0
P
yi y j
( xiP x Pj )( yiP yiQ ) ( yiP y Pj )( xiP xQj ) 0
0
0
i j (i j )
0
0
i j (i j )
g = -2[(xiP - xiQ )(xi - x j ) + ( yiP - yiQ )( yi - y j )f -[(xiP - xiQ )( yi - y j ) - ( yiP - yiQ )(xi - x j )fi2
51
Driving Link
xi d (t ) 0
yiP d (t ) 0
52
OA 80mm
AB 260mm
y1 40 sin 1 0
BC 180mm
OC 180mm
1
O
x3 90 cos 3 180 0
y3 90 sin 3 0
driving link
1 2 t 2 0
1 1 0 40sin
2 0 1 40 cos
3 1 0 40sin
4 0 1 40 cos
J 5 0 0
0
6 0 0
0
7 0 0
0
8 0 0
0
9 0 0
1
0 0
0
0 0
0
1 0 130sin 2
0 1 130 cos 2
1 0 130sin 2
0 1 130 cos 2
0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
0 0
0
1 0 90sin 3
0 1 90 cos 3
1 0 90sin 3
0 1 90 cos 3
0 0
0
0
0
0
0
0
0
0
0
2
Acceleration equations
2
40 cos
2
40
sin
2
2
50 cos 130 cos 2 2
2 130 sin 2
50
sin
2
2
2
130 cos 2 2 90 cos 3 3 2
2 90 sin 2
130
sin
2
2
3
3
90 cos 3 3
2
90
sin
3
3
57
58
59
Initialization
1. % Set up the time interval and the initial positions of the nine coordinates
2. T_Int=0:0.01:2;
3. X0=[0 50 pi/2
125.86 132.55 0.2531
215.86 82.55 4.3026];
4. global T
5. Xinit=X0;
1.
2.
3.
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.
60
10. 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(2); phi1=X(3);
f. x2=X(4); y2=X(5); phi2=X(6);
g. x3=X(7); y3=X(8); phi3=X(9);
h.
i. F=[ -x1+40*cos(phi1);
j.
-y1+40*sin(phi1);
k.
x1+40*cos(phi1)-x2+130*cos(phi2);
l.
y1+40*sin(phi1)-y2+130*sin(phi2);
m.
x2+130*cos(phi2)-x3+90*cos(phi3);
n.
y2+130*sin(phi2)-y3+90*sin(phi3);
o.
x3+90*cos(phi3)-180;
p.
y3+90*sin(phi3);
q.
phi1-2*pi*T-pi/2];
61
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,phi2,phi3)
b.
c. JacoMatrix=[
-1 0 -40*sin(phi1)
0 0 0
d.
0 -1 40*cos(phi1)
0 0 0
e.
1 0 -40*sin(phi1)
-1 0 -130*sin(phi2)
f.
0 1 40*cos(phi1)
0 -1 130*cos(phi2)
g.
0 0 0
1 0 -130*sin(phi2)
h.
0 0 0
0 1 130*cos(phi2)
i.
0 0 0
0 0 0
j.
0 0 0
0 0 0
k.
0 0 1
0 0 0
0 0 0;
0 0 0;
0 0 0;
0 0 0;
-1 0 -90*sin(phi3);
0 -1 90*cos(phi3);
1 0 -90*sin(phi3);
0 1 90*cos(phi3);
0 0 0];
62
21. Call the function Gamma4bar to obtain the right-side of the velocity equations depended on
current values of velocity.
22. Solve linear equation to obtain the current acceleration.
a. function Gamma=Gamma4bar(phi1,phi2,phi3,dphi1,dphi2,dphi3)
b.
c. Gamma=[ 40*cos(phi1)*dphi1^2;
d.
40*sin(phi1)*dphi1^2;
e.
40*cos(phi1)*dphi1^2+130*cos(phi2)*dphi2^2;
f.
40*sin(phi1)*dphi1^2+130*sin(phi2)*dphi2^2;
g.
130*cos(phi2)*dphi2^2+90*cos(phi3)*dphi3^2;
h.
130*sin(phi2)*dphi2^2+90*sin(phi3)*dphi3^2;
i.
90*cos(phi3)*dphi3^2;
j.
90*sin(phi3)*dphi3^2;
k.
0];
63
if Iter==1
Xinit=X(:,Iter);
else
Xinit=X(:,Iter)+(X(:,Iter)-X(:,Iter-1));
end
29.~33. Predict the next initial positions with assumption of inertia except the first time of the loop.
64
44. Resume x-label at bottom because the nine subplots share the same time-axis.
47.~ It is similar to above.
65
Animation
69.Ox=zeros(1,length(T_Int));
70.Oy=zeros(1,length(T_Int));
71.Ax=80*cos(X(3,:));
72.Ay=80*sin(X(3,:));
73.Bx=Ax+260*cos(X(6,:));
74.
80.for
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.end
t=1:length(T_Int);
bar1x=[Ox(t) Ax(t)];
bar1y=[Oy(t) Ay(t)];
bar2x=[Ax(t) Bx(t)];
bar2y=[Ay(t) By(t)];
bar3x=[Bx(t) Cx(t)];
bar3y=[By(t) Cy(t)];
plot (bar1x,bar1y,bar2x,bar2y,bar3x,bar3y);
axis([-120,400,-120,200]);
axis normal
M(:,t)=getframe;
66
x2
y
x3
y
t
67
x 2
y
x 3
y
t
68
x2
y
x3
y
t
69
11 Constraint equations:
x1 0
y1 0
1 0
x4 x3 200 cos 3 0
G
C
y4 y3 200 sin 3 0
x3 300 cos 3 x2 100 cos 2 0
1
x
AG 200mm
x2 100 cos 2 x1 0
GB 300mm
y2 100 cos 2 y1 0
BO 200mm
4 1 0
driving link
2 5.76 1.2t 0
To solve the 12 equations for 12 unknown qT x1 , y1 , 1, x2 , y2 , 2 , x3 , y3 , 3 , x4 , y4 , 4
70
Jacobian matrix
1 , 2 , 3 , 6 , 10 , 14 , 18 , 22 , 26 , 34 , 36 1
19 300 cos 3
4 , 8 , 12 , 16 , 20 , 24 , 35 1
28 100 sin 4
5 200 sin 3
29 100 cos 4
9 200 cos 3
31 28
13 , 23 100 sin 2
32 29
33 100cos4 x4 x1 sin 4 y4 y1
15 300 sin 3
17 , 27 100 cos 2
1
2
3
4
5
6
J
7
8
9
10
11
12
1
0
0
0
0
0
0
20
0
28
0
0
0
2
0
0
0
0
0
0
24
29
0
0
0
0
3
0
0
0
0
0
0
0
34
0
0
0
0
0
0
12
0
22
0
0
0
0
0
0
0
0
0
0
16
0
26
0
0
0
0
0
0
0
0
13
17
23
27
0
0
36
0
0
0
4
0
14
0
0
0
0
0
0
0
0
0
0
8
0
18
0
0
0
0
0
0
0
0
5
9
15
19
0
0
0
0
0
0
0
0
6
0
0
0
0
0
31
0
0
0
0
0
0
10
0
0
0
0
32
0
0
0
0
0
0
0
0
0
0
0
33
35
0
0
0
0
0
0
0
0
0
0
0
1.2
71
The right-side
of the acceleration equations
0
2
200
cos
3 3
2
200 sin
3 3
2
2
2
2
2 2
100 sin 22
10
2
where 10 200 cos4 x1 x4 4 200 sin 4 y1 y 4 4 4 100 sin 4 x1 x4 100 cos4 y1 y4
72
x3
y
x4
y
73
x 3
y
x 4
y
74
x3
y
x4
y
75
Kinematic Modeling
Ground
4A 0.0,
3B 300.0, 3B 0.0,
2B 100.0, 2B 0.0
20 100.0, 20 0.0,
10 0.0,
10 0.0
translational joint
4A 0.0,
10 0.0
driving link
Translation Joint
y1
x1 100c1 x4
d
y
y
100
s
1
1
4
100c4 y1 100s1 y4 x1 100c1 x4 100s4
100s4
x1
y1 100c4
x c s1 100 x1 100c1
B 1 1
0 y 100s
y
s
1
1 1
1
1
c s4 0 100s4
n 4
100 100c
s
4
4
4
x 100c1 x4
nT d 100 s4 100c4 1
y1 100s1 y4
77
Kinematic Modeling
Ground
1 x1 0.0
2 y1 0.0
3 1 0.0
revolute joints
4 x4 x3 200 cos 3 0
5 y4 y3 200sin 3 0
6 x3 300 cos 3 x2 100 cos 2 0
7 y3 300sin 3 y2 100sin 2 0
8 x2 100 cos 2 x1 0
9 y2 100sin 2 y1 0
translational joint
10 (100cos 4 )( y1 100sin 1 y4 )
( x1 100cos 1 x4 )(100sin 4 ) 0
11 4 1 0
driving constraint
12 2 5.76 1.2t 0
Jacobian Matrix
79
Contd
30
80
acceleration
q = 0orq q 0
q
( d )
q+
= 0orq ( d ) q t ( d ) 0
q
t
q
0
( d ) q =
(d )
q
t
q + q = 0orq q (q q ) q q 0
q
q
q ( d ) q (q ( d ) q) q q 2qt ( d ) q tt ( d ) 0
81
Solution Technique
At any given instant t
(1) Solve q (q)q = 0
q d (q, t )q = (the right hand side)
ii ni
m 0 0 x f x
0 m 0 y f
y
0 0 n i
M i qi = gi
83
Constraint Force
n1
(q, t ) 0,
q 0orq q 0, q
q
mn
m1
x
y
mgs f mx
mgc N my
mgc N 0
f R I c
4 eqs. for 5 unknowns x, y, , f , N
x R 0
constraint equation
y R 0
2
2g
1
x gs , y 0,
s ,N mgc , f mgs
3
3R
3
85
Generalized coordinate x,
Constraint eq. x R 0 y R 0
x R 0
mgs
q q
mgc
0 0
0
1 0 x
m 0
mgs
0 m
0
0 1 y
mgc
1
2
0
0
mR
R
0
0
2
1 0
R
0 0
1 0
0
0 1
0
0 0 2
2
2g
1
3 eqs. for 3 unknowns x gs , y 0,
s , mgs
3
3R
3
1
mgs
1 0
T
R 0
mgRs
3
31
is the friction force for pure rolling.
86
0
1 fy
P
P
0 0 i ( yi yi ) ( xi xi )
n i
mi xi = f ( x)i - l1
mi yi = f ( y)i - l2
87
mi xi f xi ( yiP yiQ )1
mi yi f yi ( xiP xiQ )1
88
89
M OA 0.08kg
I OA 4.27 105 kg m 2
AB 0.26m
M AB 0.26kg
I AB 1.46 103 kg m 2
BC 0.18m
M BC 0.18kg
I BO 4.86 104 kg m 2
OC 0.18m
g 9.8 m s2
B
A
T 0.1N m
C
O
90
I OA 4.27 105 kg m 2
g 9.8 m s2
M AB 0.26kg
I AB 1.46 103 kg m 2
M BC 0.18kg
I BO 4.86 104 kg m 2
T 0.1N m
0
0
0.08
0
0.08
0
0
4.27 10 5
0
0
0
0
M 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0.26
0
0
0
0.26
0
0
0 1.46 10 3
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0.18 0
0
0
0.18
0
4
0
0
4.86 10
0.08 9.8
0
.
1
g 0.26 9.8
0
.
18
9
.
8
91
J 9 9
1 0 0.04 sin
0 1 0.04 cos
0 0.04 sin
1
0
1
0.04 cos
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1
0
0
0
0
0
0
1 0 0.13 sin 2
0 1 0.13 cos 2
1
0 0.13 sin 2
0
1
0.13 cos 2
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
1 0 0.09 sin 3
0 1 0.09 cos 3
1
0 0.09 sin 3
0
1
0.09 cos 3
0
0
0
0.04 cos
2
0.04 sin
2
2
2
2
0.05 sin 0.13 sin 2 2
0.13 cos 2 2 2 0.09 cos 3 32
2 0.09 sin 2
0
.
13
sin
2
2
3
3
0.09 cos 3 3
2
0.09 sin 3 3
92
Computation
M
The objective is to solve the differenti al equation 99
J 98
J T98 q g
for qt .
O88
M J T98 q g
In numerial computing, the first step is to solve
J
O
98
88
for q091 and 081 with initial position q0 and velocity q 0.
q0 t q t
Integrate
q t , and use the q t and q t
q
0
x1
y
1
q
q
Let z 3 , z
x1 q
q
y1
3
94
Solution array
[T,Z] = ode45(@Func4Bar,[0:0.005:2],Z0);
column vector of
time points
function dz=Func4Bar(t,z)
global L1 L2 L3 L4 torque gravity
phi1=z(3); phi2=z(6); phi3=z(9);
dphi1=z(12); dphi2=z(15); dphi3=z(18);
M=diag([L1 L1 L1^3/12 L2 L2 L2^3/12 L3 L3 L3^3/12]);
J=[ -1 0 -0.5*L1*sin(phi1)
0 -1 0.5*L1*cos(phi1)
1 0 -0.5*L1*sin(phi1)
0 1 0.5*L1*cos(phi1)
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
0 0 0
-1 0 -0.5*L2*sin(phi2)
0 -1 0.5*L2*cos(phi2)
1 0 -0.5*L2*sin(phi2)
0 1 0.5*L2*cos(phi2)
0 0 0
0 0 0
0 0 0;
0 0 0;
0 0 0;
0 0 0;
-1 0 -0.5*L3*sin(phi3);
0 -1 0.5*L3*cos(phi3);
1 0 -0.5*L3*sin(phi3);
0 1 0.5*L3*cos(phi3)];
95
0 0 0
0 0 0
-1 0 -0.5*L2*sin(phi2)
0 -1 0.5*L2*cos(phi2)
1 0 -0.5*L2*sin(phi2)
0 1 0.5*L2*cos(phi2)
0 0 0
0 0 0
0 0 0;
0 0 0;
0 0 0;
0 0 0;
-1 0 -0.5*L3*sin(phi3);
0 -1 0.5*L3*cos(phi3);
1 0 -0.5*L3*sin(phi3);
0 1 0.5*L3*cos(phi3)];
gamma=[ 0.5*L1*cos(phi1)*dphi1^2;
0.5*L1*sin(phi1)*dphi1^2;
0.5*L1*cos(phi1)*dphi1^2+0.5*L2*cos(phi2)*dphi2^2;
0.5*L1*sin(phi1)*dphi1^2+0.5*L2*sin(phi2)*dphi2^2;
0.5*L2*cos(phi2)*dphi2^2+0.5*L3*cos(phi3)*dphi3^2;
0.5*L2*sin(phi2)*dphi2^2+0.5*L3*sin(phi3)*dphi3^2;
0.5*L3*cos(phi3)*dphi3^2;
0.5*L3*sin(phi3)*dphi3^2];
g=[0 gravity*L1 torque
0 gravity*L2 0
0 gravity*L3 0]';
Matrix=[M J';
J zeros(size(J,1),size(J,1))];
d2q=Matrix\[g;gamma];
dz=[z(10:18,:); d2q(1:9,:)];
96
x2
y
x3
y
t
97
x 2
y
x 3
y
t
98
x2
y
x3
y
t
99
Time response of
1
103
3
4
5
6
8
t
100
A slider-crank mechanism
B
T 0.1N m
G
A
C
O
AC 0.1m
M AC 0.1kg
AG 0.2 m
M AB 0.5kg
I AB 1.04 102 kg m 2
GB 0.3m
M BO 0.2 kg
I BO 6.67 104 kg m 2
BO 0.2 m
g 9.8 m s2
101
x3
y
x4
y
102
x 3
y
x 4
y
103
x3
y
x4
y
104
Time response of
1
103
3
4
5
6
7
8
10
11
t
105
c c s c s c s s c c s s
A = s c c c s s s c c c c s
s s
s c
c
A DCB
c
D s
0
s
c
0
0
1 0
0 C 0 c
0 s
1
0
c s 0
s B s c 0
0
c
0 1
106
sin cos
cos
cos 0
sin 0
0
1
107
Bryant Angles
A DCB
0
1 0
c2
D 0 c1 s1 C 0
0 s1 c1
s2
c2 c3
A = c1s3 s1s2 c3
s1s3 c1s2 c3
0 s2
c3
1 0 B s3
0
0 c2
s3 0
c3 0
0
1
c2 s3
c1c3 s1s2 s3
s1c3 c1s2 s3
s2
s1c2
c1c2
108
cos 2 sin 3
sin 2
sin 3
cos 3
0
0 1
0 2
1 3
109