Date post: | 31-Jul-2018 |
Category: |
Documents |
Upload: | truongnguyet |
View: | 225 times |
Download: | 1 times |
Analytical Mechanics: Link Mechanisms
Shinichi Hirai
Dept. Robotics, Ritsumeikan Univ.
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 1 / 28
Agenda
Agenda
1 Open Link MechanismKinematics of Open Link MechanismDynamics of Open Link Mechanism
2 Closed Link MechanismKinematics of Closed Link MechanismDynamics of Closed Link Mechanism
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 2 / 28
Open Link Mechanism Kinematics of Open Link Mechanism
Kinematics of two link open mechanism
θ1
θ2l1
l2
link 1
link 2
joint 1
joint 2
lc2
lc1
x
y
two link open link mechanismli length of link ilci distance btw. joint i and
the center of mass of link imi mass of link iJi inertia of moment of link i
around its center of massθ1 rotation angle of joint 1θ2 rotation angle of joint 2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 3 / 28
Open Link Mechanism Kinematics of Open Link Mechanism
Kinematics of two link open mechanism
position of the center of mass of link 1:
xc1△=
[xc1yc1
]= lc1
[C1
S1
]position of the center of mass of link 2:
xc2△=
[xc2yc2
]= l1
[C1
S1
]+ lc2
[C1+2
S1+2
]orientation angle of link 1:
θ1
orientation angle of link 2:θ1 + θ2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 4 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Kinetic energy
velocity of the center of mass of link 1:
xc1 = lc1θ1
[−S1
C1
]angular velocity of link 1:
θ1
kinetic energy of link 1:
T1 =1
2m1xT
c1xc1 +1
2J1θ
21
=1
2(m1l
2c1 + J1)θ
21
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 5 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Kinetic energy
velocity of the center of mass of link 2:
xc2 = l1θ1
[−S1
C1
]+ lc2(θ1 + θ2)
[−S1+2
C1+2
]angular velocity of link 2:
θ1 + θ2
kinetic energy of link 2:
T2 =1
2m2xT
c2xc2 +1
2J2(θ1 + θ2)
2
=1
2m2{l21 θ21 + l2c2(θ1 + θ2)
2 + 2l1lc2C2θ1(θ1 + θ2)}+1
2J2(θ1 + θ2)
2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 6 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Kinetic energy
total kinetic energy
T = T1 + T2 =1
2
[θ1 θ2
] [ H11 H12
H21 H22
] [θ1θ2
]where
H11 = J1 +m1l2c1 + J2 +m2(l
21 + l2c2 + 2l1lc2C2)
H22 = J2 +m2l2c2
H12 = H21 = J2 +m2(l2c2 + l1lc2C2)
inertia matrix
H△=
[H11 H12
H21 H22
]Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 7 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Partial derivatives
H11 and H12 = H21 depend on θ2:
∂H11
∂θ2= −2h12,
∂H12
∂θ2=
∂H21
∂θ2= −h12 (h12
△= m2l1lc2S2)
H11 = −2h12θ2, H12 = H21 = −h12θ2∂T
∂θ1= H11θ1 + H12θ2,
∂T
∂θ2= H21θ1 + H22θ2
− d
dt
∂T
∂θ1= −H11θ1 − H11θ1 − H12θ2 − H12θ2
= 2h12θ1θ2 + h12θ22 − H11θ1 − H12θ2
− d
dt
∂T
∂θ2= −H21θ1 − H21θ1 − H22θ2 − H22θ2
= h12θ1θ2 − H21θ1 − H22θ2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 8 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Partial derivatives
H11, H22, and H12 = H21 are independent of θ1
∂T
∂θ1=
1
2
[θ1 θ2
] [ 0 00 0
] [θ1θ2
]= 0
H11 and H12 = H21 depend on θ2
∂T
∂θ2=
1
2
[θ1 θ2
] [ −2h12 −h12−h12 0
] [θ1θ2
]= −h12θ
21 − h12θ1θ2
contribution of kinetic energy:
∂T
∂θ1− d
dt
∂T
∂θ1= 2h12θ1θ2 + h12θ
22 − H11θ1 − H12θ2
∂T
∂θ2− d
dt
∂T
∂θ2= −h12θ
21 − H21θ1 − H22θ2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 9 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Gravitational potential energy
potential energy of link 1:
U1 = m1g yc1 = m1g lc1S1
potential energy of link 2:
U2 = m2g yc2 = m2g (l1S1 + lc2S1+2)
potential energy:U = U1 + U2
partial derivatives w.r.t. joint angles:
∂U
∂θ1= G1 + G12,
∂U
∂θ2= G12
whereG1 = (m1lc1 +m2l1) gC1, G12 = m2lc2 gC1+2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 10 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Work done by actuator torques
work done by τ1 applied to rotational joint 1:
τ1θ1
work done by τ2 applied to rotational joint 2:
τ2θ2
work done by the two actuator torques:
W = τ1θ1 + τ2θ2
partial derivatives w.r.t. joint angles:
∂W
∂θ1= τ1,
∂W
∂θ2= τ2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 11 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Lagrange equations of motion
Lagrangian:L = T − U +W
Lagrange equations of motion
∂L
∂θ1− d
dt
∂L
∂θ1= 0
∂L
∂θ2− d
dt
∂L
∂θ2= 0
let ω1△= θ1 and ω2
△= θ2:
− H11ω1 − H12ω2 + h12ω22 + 2h12ω1ω2 − G1 − G12 + τ1 = 0
− H22ω2 − H12ω1 − h12ω21 − G12 + τ2 = 0
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 12 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
Lagrange equations of motion
canonical form of ordinary differential equations:[θ1θ2
]=
[ω1
ω2
][H11 H12
H21 H22
] [ω1
ω2
]=
[h12ω
22 + 2h12ω1ω2 − G1 − G12 + τ1
−h12ω21 − G12 + τ2
]state variables: joint angles θ1, θ2 and angular velocities ω1, ω2
the inertia matrix is regular −→ 2nd eq. is solvable−→ we can compute ω1 and ω2
θ1, θ2, ω1, ω2 are functions of θ1, θ2, ω1, ω2
⇓we can sketch θ1, θ2, ω1, ω2 using any ODE solver(e.g. Runge-Kutta method)
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 13 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
PD control
τ1 = KP1(θd1 − θ1)− KD1θ1
τ2 = KP2(θd2 − θ2)− KD2θ2
⇓[θ1θ2
]=
[ω1
ω2
][H11 H12
H21 H22
] [ω1
ω2
]=
[· · ·+ KP1(θ
d1 − θ1)− KD1ω1
· · ·+ KP2(θd2 − θ2)− KD2ω2
]
current values of θ1, θ2, ω1, ω2
⇓their time derivatives θ1, θ2, ω1, ω2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 14 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
PI control
τ1 = KP1(θd1 − θ1) + KI1
∫ t
0
{(θd1 − θ1(τ)} dτ
τ2 = KP2(θd2 − θ2) + KI2
∫ t
0
{(θd2 − θ2(τ)} dτ
Introduce additional variables:
ξ1△=
∫ t
0
{(θd1 − θ1(τ)} dτ
ξ2△=
∫ t
0
{(θd2 − θ2(τ)} dτ
ξ1 = θd1 − θ1, τ1 = KP1(θd1 − θ1) + KI1ξ1
ξ2 = θd2 − θ2, τ2 = KP2(θd2 − θ2) + KI2ξ2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 15 / 28
Open Link Mechanism Dynamics of Open Link Mechanism
PI control
⇓[θ1θ2
]=
[ω1
ω2
][H11 H12
H21 H22
] [ω1
ω2
]=
[· · ·+ KP1(θ
d1 − θ1) + KI1ξ1
· · ·+ KP2(θd2 − θ2) + KI2ξ2
]ξ1 = θd1 − θ1
ξ2 = θd2 − θ2
current values of θ1, θ2, ω1, ω2, ξ1, ξ2⇓
their time derivatives θ1, θ2, ω1, ω2, ξ1, ξ2Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 16 / 28
Closed Link Mechanism Kinematics of Closed Link Mechanism
Kinematics of four-link closed mechanism
x
y
O
link 1
link 2
link 3
link 4
θ1
θ3
θ2θ4
tip point
l1
l2
l3
l4
(x1,y1) (x3,y3)
joint 1
joint 2
joint 3
joint 4
joint 5
joint 1, 3: activejoint 2, 4, 5: passive
θ1, θ2, θ3, θ4:rotation angles
τ1, τ3:actuator torques
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 17 / 28
Closed Link Mechanism Kinematics of Closed Link Mechanism
Kinematics of four-link closed mechanism
decomposition of closed link mechanism into open link mechanisms:left arm link 1 and 2right arm link 3 and 4
end point of the left arm:[x1y1
]+ l1
[C1
S1
]+ l2
[C1+2
S1+2
]end point of the right arm:[
x3y3
]+ l3
[C3
C3
]+ l4
[C3+4
S3+4
]two constraints:
X△= l1C1 + l2C1+2 − l3C3 − l4C3+4 + x1 − x3 = 0
Y△= l1S1 + l2S1+2 − l3S3 − l4S3+4 + y1 − y3 = 0
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 18 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Lagrangian
Lagrangian of the closed link mechanism:
L = Lleft + Lright + λxX + λyY ,
Lleft, Lright Lagrangians of the left and right armsλx , λy Lagrange multipliers
Lagrange equations of motion:
∂L
∂θ1− d
dt
∂L
∂ω1= 0
∂L
∂θ2− d
dt
∂L
∂ω2= 0
∂L
∂θ3− d
dt
∂L
∂ω3= 0
∂L
∂θ4− d
dt
∂L
∂ω4= 0
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 19 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Contributions of Lleft
contributions of Lagrangian Lleft to the four Lagrange eqs:
− H11ω1 − H12ω2 + h12ω22 + 2h12ω1ω2 − G1 − G12 + τ1,
− H22ω2 − H21ω1 − h12ω21 − G12,
0,
0
where
H11 = J1 +m1l2c1 + J2 +m2(l
21 + l2c2 + 2l1lc2C2),
H22 = J2 +m2l2c2,
H12 = H21 = J2 +m2(l2c2 + l1lc2C2),
h12 = m2l1lc2S2,
G1 = (m1lc1 +m2l1) gC1, G12 = m2lc2 gC1+2
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 20 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Contributions of Lright
contributions of Lagrangian Lright to the four Lagrange eqs:
0,
0,
− H33ω3 − H34ω4 + h34ω24 + 2h34ω3ω4 − G3 − G34 + τ3,
− H44ω4 − H43ω3 − h34ω23 − G34
where
H33 = J3 +m3l2c3 + J4 +m4(l
23 + l2c4 + 2l3lc4C4),
H44 = J4 +m4l2c4,
H34 = H43 = J4 +m4(l2c4 + l3lc4C4),
h34 = m4l3lc4S4,
G3 = (m3lc3 +m4l3) gC3, G34 = m4lc4 gC3+4
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 21 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Contributions of λxX + λyY
contributions of λxX + λyY to the four Lagrange eqs:
λx(−l1S1 − l2S1+2) + λy (l1C1 + l2C1+2)△= λxJx1 + λyJy1,
λx(−l2S1+2) + λy l2C1+2△= λxJx2 + λyJy2,
λx(l3S3 + l4S3+4) + λy (−l3C3 − l4C3+4)△= −λxJx3 − λyJy3,
λx l4S3+4 + λy (−l4C3+4)△= −λxJx4 − λyJy4,
where
J12 =
[Jx1 Jx2Jy1 Jy2
], J34 =
[Jx3 Jx4Jy3 Jy4
]are Jacobian matrices of the left and right arms
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 22 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Contributions of λxX + λyY
constraint force λ = [λx , λy ]T
equivalent torques around rotational joints 1 and 2:
JT12λ =
[λx(−l1S1 − l2S1+2) + λy (l1C1 + l2C1+2)
λx(−l2S1+2) + λy l2C1+2
]
reaction force −λequivalent torques around rotational joint 3 and 4:
JT34(−λ) =
[λx(l3S3 + l4S3+4) + λy (−l3C3 − l4C3+4)
λx l4S3+4 + λy (−l4C3+4)
]
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 23 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Lagrange equations of motion
H11 H12 0 0 −Jx1 −Jy1H12 H22 0 0 −Jx2 −Jy20 0 H33 H34 Jx3 Jy30 0 H34 H44 Jx4 Jy4
ω1
ω2
ω3
ω4
λx
λy
=
f1f2f3f4
where
f1 = h12ω22 + 2h12ω1ω2 − G1 − G12 + τ1, f2 = −h12ω
21 − G12
f3 = h34ω24 + 2h34ω3ω4 − G3 − G34 + τ3, f4 = −h34ω
23 − G34
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 24 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Equations stabilizing constraints
X + 2νX + ν2X = 0, Y + 2νY + ν2Y = 0
⇓[−Jx1 −Jx2 Jx3 Jx4−Jy1 −Jy2 Jy3 Jy4
]ω1
ω2
ω3
ω4
=
[gxgy
]where
gx = −(Jy1ω
21 + Jy2ω
22 + 2Jy2ω1ω2
)+(Jy3ω
23 + Jy4ω
24 + 2Jy4ω3ω4
)+2ν(Jx1ω1 + Jx2ω2 − Jx3ω3 − Jx4ω4) + ν2X
gy =(Jx1ω
21 + Jx2ω
22 + 2Jx2ω1ω2
)−
(Jx3ω
23 + Jx4ω
24 + 2Jx4ω3ω4
)+2ν(Jy1ω1 + Jy2ω2 − Jy3ω3 − Jy4ω4) + ν2Y
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 25 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Dynamic equations for closed link mechanism
H11 H12 0 0 −Jx1 −Jy1H12 H22 0 0 −Jx2 −Jy20 0 H33 H34 Jx3 Jy30 0 H34 H44 Jx4 Jy4
−Jx1 −Jx2 Jx3 Jx4 0 0−Jy1 −Jy2 Jy3 Jy4 0 0
ω1
ω2
ω3
ω4
λx
λy
=
f1f2f3f4gxgy
the coefficient matrix is regular −→ we can compute ω1 through ω4
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 26 / 28
Closed Link Mechanism Dynamics of Closed Link Mechanism
Dynamic Simulation of a Link Mechanism
Report #4 due date : Jan. 8 (Mon.)
Simulate the control of a four-link closed manipulator. Apply PIcontrol to active joints 1 and 3. Use appropriate values ofgeometrical and physical parameters of the manipulator. Show howposition of joint 5 changes according to time.
x
y
O
link 1
link 2
link 3
link 4
θ1
θ3
θ2θ4
tip point
l1
l2
l3
l4
(x1,y1) (x3,y3)
joint 1
joint 2
joint 3
joint 4
joint 5
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 27 / 28
Summary
Summary
Open link mechanism
inertia matrix depends on joint angles
Lagrange equations of motion of open link mechanism
Closed link mechanism
two open link mechanisms with geometric constraints
synthesized from Lagrange equations of open link mechanisms
Shinichi Hirai (Dept. Robotics, Ritsumeikan Univ.)Analytical Mechanics: Link Mechanisms 28 / 28