Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy...
Transcript of Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy...
![Page 1: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/1.jpg)
Robot Dynamics
Hesheng WangDept. of Automation
Shanghai Jiao Tong University
![Page 2: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/2.jpg)
What is Robot Dynamics?
• Robot dynamics studies the relation between robot motion and forces and moments acting on the robot.
1
x
l1
l2
2
1
2
![Page 3: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/3.jpg)
The velocity v can be determined from the cross product of and rp . Here rp is a vector from any point on the axis of rotation to P.
v = x rp = x r
The direction of v is determined by the right-hand rule.
Rotation about a Fixed Axis
![Page 4: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/4.jpg)
The acceleration of P can also be defined by differentiating the velocity.
a = dv/dt = d/dt x rP + x drP/dt= x rP + x ( x rP)
It can be shown that this equation reduces to
a = a x r – w2r = at + an
Rotation about a Fixed Axis(continued)
The magnitude of the acceleration vector is a = (at)2 + (an)2
Normal/centripetal accelerationTangent accel
![Page 5: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/5.jpg)
Rotation of a Vector• Consider rotation of a vector about a
axis.
Point P is rotating about axis u.r is the position vector of point P.
u
Pr
rotation of speed the:
uω :lcotiyangular ve The
rωrr
dtd
:ectorposition v of change of rate The
![Page 6: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/6.jpg)
Rotation of a Frame
• Consider a frame B rotating about an unit vector u.
A frame reference the w.r.t.B frame of
axes theof vectorsldirectionaunit :,, BBB kji u
xByB
zB
Frame A
BBBBBB dtd
dtd
dtd kωkjωjiωi , ,
RωR
kωjωiωkjiR
:B frame ofmatrix rotation theof derivative The
BBBBBBdtd
![Page 7: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/7.jpg)
General Motion• A general motion can be considered a
combination of a translation with a point and motion about the point.
A with slate that tranframeA :frame reference fixedA :
111 zyxAzyxO ooo
y1
x0
z0
y0
x1
z1B
A
O
rωVV AB
)( rωωrαaa AB
The velocity relation:
The acceleration relation:
![Page 8: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/8.jpg)
Introduction to Dynamics
![Page 9: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/9.jpg)
Newton’s Laws of Motion
onaccelerati the:forcenet the:
mass the:
aF
aFm
m
Fa
F’F
First Law: A particle originally at rest, or moving in a straight line at constant velocity, will remain in this state if the resultant force acting on the particle is zero.
Second Law: If the resultant force on the particle is not zero, the particle experiences an acceleration in the same direction as the resultant force. This acceleration has a magnitude proportional to the resultant force.
Third Law: Mutual forces of action and reaction between two particles are equal, opposite, and collinear.
![Page 10: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/10.jpg)
ExampleFind the accelerations of the ball
and the wedge.
m1
m2
Frictionless surface
N
m1gm2gN
R
Solution:(1) Draw the free-body diagram of the particles(2) Apply Newton’s 2nd Law
For the ball
x
y
(2) cos(1) sin
11
1
gmNymNxm
b
b
For the wedge
(3) sin2 Nxm w
![Page 11: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/11.jpg)
Example (continued)• Consider acceleration relationship between
the ball and the wedge
aw
ab
ab/w
wedge the.r.t. w ball theof velocity relative :
/
/
wb
wbwb
aaaa
Note that the relative velocity is along the surface of the wedgeFrom the diagram, we have
(5) sin(4) cos
/
/
wbb
Wbwb
ayaxx
From eqs. (1)-(5), we can solve the five unknowns, i.e. the acceleration
![Page 12: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/12.jpg)
Linear Momentum• Linear momentum: product of mass and velocity:
– It is a vector, in the same direction as velocity
• Principle of Linear Momentum: The rate of change of the linear momentum of a particle is equal to the result force acting on the particle
VL m
LVaF dtdm
:Law 2nd sNewton' Fromm
m/sKg : UnitSI - V
mV
![Page 13: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/13.jpg)
Angular Momentum• r: the position vector of a particle w.r.t. a
reference point O.• V: the velocity of the particle• m: the mass of the particle• The angular momentum of the particle
about reference O:
V
r
O
VrH mo
s
o
/mKg isunit Itsrule hand-right by the determined isdirection Its
. and both lar toperpendicu vector a is
2
rVH
![Page 14: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/14.jpg)
Principle of Angular MomentumConsider a force F acting on the particle V
r
O
F
oo
o mm
MFrH
FrVrVrH
Principle of angular momentum: The rate of change of the angular momentum of particle about a fixed point O is equal to the resultant moment of forces acting on the particle about point O
Differentiating the angular momentum
![Page 15: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/15.jpg)
Dynamics of a System of ParticlesConsider a system of n particles.
eij
f2
f1 fi
fj
m1
m2
mi
mj
eji
fi: the external force exerted on particle ieij: the internal force exerted on particle i
by particle jmi: mass of particle i.ri: position vector of particle i
iiii dtdmm rVL :momentumLinear
i
i
mmr
rc :mass ofcenter The Cci Mm VrL Total mass
Velocity of center of massThe linear momentum of a system of particles is
equal to the product of the total mass and the velocity of the center of mass
![Page 16: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/16.jpg)
Dynamics of a System of ParticlesDifferentiating the linear momentum
ei
f2
f1 fi
fj
m1
m2
mi
mj
ej
i j
ijii
ii dtdm )(2
2
efrL
i
icC MM faVL ac: acceleration of center of mass
The rate of change the linear momentum of a system of particles is equal to the resultant of all EXTERNAL forces acting on the particles
i
ii j
ij fLe 0
The center of mass of the system moves as if all the forces and masses are concentrated at the center of mass
Equation of motion ofCenter of mass
![Page 17: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/17.jpg)
Angular Momentum of a System of Particles
Similarly, by differentiating the angular momentum
ei
f2
f1 fi
fj
m1
m2
mi
mj
ej
O
i
Oiio mdtd MVrH
The resultant moment of all EXTERNAL forces acting on the system about O
The reference point must be a fixed point. However, the center of mass of the system can be the reference point even when it is moving
![Page 18: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/18.jpg)
Example:Example: Calculate the angular acceleration of the massless link
m2
m1
l1
l2
O
(2) Consider the angular momentum about O m1g
Rx
Ry
m2g
SolutionConsider the two particles and the link as a system
coscos
00
)(00
dt
1122222
211 glmglmlmlm
doo MH
222
211
2211 cos)(lmlm
lmlm
(1) Analyze the external forces
![Page 19: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/19.jpg)
Linear and Angular Momentums for Rigid Body
• Since a rigid body can be considered as a system with infinite number of particles, the linear momentum
CmVL Center of mass
CV
body theoflocity angular ve :C.about matrix tensor Inertia :
ωI
IωH C
•The angular momentum about the center of mass ω
![Page 20: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/20.jpg)
Newton’s Equation and Euler’s Equation
• A general motion can be considered a combination of a translation with the center of mass and motion about the center of mass
C with slates that tranframeA :frame reference fixedA :
111 zyxCzyxO ooo
icm fFa
iiiAA
CCA n
dtd frIωωωIMH )(
The equation of translation:
The equation of motion about C:
y1
x0
z0
y0
x1
z1
C
O fi
f1ni
ri
Newton’s equation
Euler’s equation
![Page 21: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/21.jpg)
• The angular velocity is with respect to the translating frame.
• The inertia tensor matrix is with respect to the translating frame, so it will change its value with rotation of the body.
• The force is the resultant of the EXTERNAL force
• The moment is the resultant moment of the EXTERNAL forces and moments.
Newton’s Equation and Euler’s Equation (Cont’)
![Page 22: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/22.jpg)
Example• Derive the dynamic equation of the 2 DOF
manipulator. Here, the masses of links 1 and 2 are m1 and m2 respectively. Assume that the mass is uniformly distributed over the link.
2
Solution:
(1) Analyzing forces acting on the links
m2g
Rx
Ry
1
x0
l1
l2
2
1
2
1Rx
Ry
Nx
Nym1g
O
![Page 23: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/23.jpg)
Example (continued)(2) Dynamics of link 1. As
link 1 is rotating about O,
1O oI M
(1) sincoscos2
111111
111 lRlRlgmI xyO
1Rx
Ry
Nx
Nym1g
1
Moment of inertia about O Resultant moment about O
![Page 24: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/24.jpg)
Example(3) Dynamics of link 2. As link 2 is in a general plane motion
2
2
22222
2
CCC
CmMωIωωI
Fa
2
m2g
Rx
RyC2
BAcceleration of C2: BnCBtCBC //2
aaaa
1
...111
111
12
11
12
11
clsl
slcl
BtBnB
aaa
Bna
Bta
21
BnC /a
BtC /a
12122
212
/ )(2 s
clBnC a
1212
212
/ )(2 c
slBtC a
122
212
12212
12
1111
122
212
12212
12
1111
)(2
)(2
)(
)(2
)(2
)(2
slclscl
clslcslC
a
Newton’sequation: (3)
(2) 22
2
2
2gmRam
Ramyyc
xxc
![Page 25: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/25.jpg)
Example (continued)Consider Euler’s equation. As the mass in uniformly distributed and the link is
symmetric, the inertia tensor matrix is diagonal.
122
122
2212
22
21212
22
22
00
00
000000
00
00
000000
cRlsRlII
I
II
I
yxzz
yyxx
zz
yyxx
(4) 22
)( 122
122
2212 cRlsRlI yxzz
From (2) and (3), we can solve Rx and Ry. Substituting Rx and Ry in eqs. (1) and (4) leads to the dynamic equation of the robot arm.
![Page 26: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/26.jpg)
Formulation of Robot Dynamics
![Page 27: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/27.jpg)
Recursive Newton-Euler FormulationWe consider manipulators with revolute joints only.
i
Joint i
Joint i-1
Link i-1 zizi-1
ai-1
1i xi-1
yi-1
Oi-1
xi
yi
si-1Ci: centerof mass
Oi
ri
Angular velocity relation between link i-1 and i:
1/1 iiii ωωω
(1) 1 iiii zωω
Differentiating (1) (2) 11 iiiiiii zωzωω
Relative angular velocity if link I to link i-1
![Page 28: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/28.jpg)
Recursive Newton-Euler Equation (Cont’)
• Consider velocity and acceleration of Oi.
1-ii1/1-i1
i
O toO of velocity realtive :O of velocity:
O of velocity:
ii
ii
VVV
1/1 iiii VVV
As the relative motion of Oi w.r.t. Oi-1 is a motion about Oi-1,
111/ iiii sωV (3) 111 iiii sωVV
Accelerationat Oi: (4) )( 111111
1
iiiiii
RnRtiisωωsωa
aaaa
Acceleration at the center of mass: (5) )(
/
iiiiii
OCiC iii
rωωrωa
aaa
![Page 29: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/29.jpg)
Forward Equations
From (1), (2), (4) and (5), we can recursively calculate the angular velocity and acceleration of the links, and the acceleration at the center of mass.
The initial conditions:
0 ,0 0 ,0
0,iWhen
0000
aVωω
i=0
Calculate i+1 from (1)
Calculate i+1 from (2).
Calculate ai+1 from (3)
Calculate aci+1 from (4)
i=n-1 ENDi=i+1No YES
(1) 1 iiii zωω
(2) 11 iiiiiii zωzωω
(3) )( 111111 iiiiiii sωωsωaa
(4) )( 11 iiiiiiCi rωωrωaa
![Page 30: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/30.jpg)
Dynamic equation of robot link
Derive the dynamics by applying Newton-Euler equations to link i.
(1) Draw the free-body diagram of link i. Assume that the links are rigidly connected. Cut link i from the arm:
gmm
iiici i
00
1ffa
ni -fi+1
-ni+1
fi
si
ri
mig
Oi
Ci
Oi+1
fi: the force acting on link i by link i-1ni: the moment applied on link i by link i-1
Applying Newton’s Law
(7) 00
1
gmm
iicii i
faf
![Page 31: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/31.jpg)
Dynamics of robot link (Cont’)Applying the Euler’s equation
11 )( iiiiiiiiiiii frsfrnnωIωωI
ni -fi+1
-ni+1
fi
si
ri
mig
Oi
Ci
Oi+1
(8) )(
11
iiiiiiiiiiii frsfrnωIωωIn
Eqs. (7) and (8) give the recursive backward equation for calculating the interaction force and momentRelation between ni and joint torque
iTii nz
as actuator produces torque about the joint axis only
![Page 32: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/32.jpg)
Backward CalculationInitial conditions:
links ofnumber the:0 ,0 :link)last (for thek iWhen 1k1
kk nf
i=K
Calculate fi from (7)
Calculate ni from (8)
i=i-1
i=0 ENDYESNO
![Page 33: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/33.jpg)
Lagrange Formulation of Robot Dynamics
• Lagrange formulation is an analytical method for deriving the robot dynamics. It is based on the energy and work principle
• Energy of Mechanical Systems– Kinetic energy: energy due to motion of a
particle or body– Potential energy: due to gravitational forces,
deformation of mechanical systems, etc.
![Page 34: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/34.jpg)
Kinetic Energy• A particle (body) has kinetic energy when it moves.
Kinetic energy is always greater than zero
Iωω
rr
Tc
T
mV
dm
21
21
21K :body rigid aFor
2V
2
21K :particle aFor mV
2
21K :particles of system aFor iiVm
V
C
ω
Vc
I: inertia tensor
![Page 35: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/35.jpg)
Potential Energy• We here consider the gravitational potential
energy only.
cmgz U:body rigid aFor
z
mgz U :particle aFor
igzim U:particles of system aFor
z: the height of the particle w.r.t. a reference level
The height of the center of mass
Potential energy is a value relative to the reference. It could be positive, zero and negative
![Page 36: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/36.jpg)
WorkWhen a particle underwent a displacement r
under a constant force f, the work done by the force on the particle is
– Work is a scalar– It could be positive, zero and negative– SI unit: Nm
r
frf TW
![Page 37: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/37.jpg)
Work done by time-varying forceConsider the work done by a
time-varying force on a particle that moved from one position to another.
f(t)
Position 1
Position 2
V2
V1
K 21
21
)(
21
22
2
1
2
1
2
1
mVmV
dmddtdm
dtW T
VVrV
rf
Work-energy principle: The work done by a force acting on particle is equal to the change of its kinetic energy
![Page 38: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/38.jpg)
Conservative force and Non-Conservative force
• The force associated to the potential energy is called conservative force. A force that is not associated with the potential energy is called non-conservative force.
• The work done by conservative force (gravity force)
• The work done by non-conservative forces
UzzmgWg )( 12
mg z1
z2
)( UKUKWKWW
n
gn
The work done by non-conservative forces is equal to the change of total energy
![Page 39: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/39.jpg)
Conservation of Energy• If no non-conservative force acting on a system
(a particle, or a system of particles, or a rigid body), does not do any work, the total energy of the system is conserved.
constantUK
![Page 40: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/40.jpg)
Lagrange Equation• Generalized coordinates q: A set of parameters
for representing the configuration (position & orientation) of a system.– q must specify the configuration uniquely– Once the values of q are fixed, the system cannot
move.– The choice of q is not unique.
• Degree of freedom (DOF): The dimension of the generalized coordinates vector q is called degrees of freedom of the system
![Page 41: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/41.jpg)
Examples
1
x0
l1
l2
2
1
2
21q DOF =2
(x,y)
yx
q DOF=4
![Page 42: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/42.jpg)
Generalized ForceConsider the work done by non-conservative
forces under a differential displacement of the system
qqf
r
scoordinate dgeneralize theof change aldifferenti a todue ofpoint action
at thent displaceme aldifferenti :i
i
fi:W work aldifferenti The
iTiW rf
The generalized force F of the system is given by
vectorscoordinate dgeneralize: )()(
q q
F W
![Page 43: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/43.jpg)
ExampleCalculate the generalized force of
the 2 DOF arm
1
x0
l1
l2
2
1
2f
T21,motion aldifferentiFor q
21 :sCoordinate dGeneralize q
The differential work
point end theofmotion aldifferenti the:2211
xxf
TW
qq )(Jx qfqqqqf
T
T
JJW))((
)( 21
2211
当前无法显示此图像。
Generalized Force:
![Page 44: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/44.jpg)
Lagrange Equationq: the generalized coordinates of a systemK: Kinetic energy of the systemU: the potential energy of the systemF: the generalized forces of the systemDefine L=K-U: Called LagrangianThe dynamics of the system is given by
Fqq
LL
dtd
![Page 45: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/45.jpg)
Derivation of Robot Dynamics using Lagrange Equation
1) Choose the generalized coordinates q (usually use the kinematics parameters defined by the D-H method)
2) Identify the non-conservative forces that are exerted at the system and do work
3) Calculate the kinetic energy K and the potential energy U, and then L=K-U
4) Calculate the partial derivatives5) Calculate the generalized force F.6) Apply the Lagrange equation.
![Page 46: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/46.jpg)
Example 1Example: Denote the mass of link i by mi. The mass is
uniformly distributed over the link. Derive the dynamics of the 2 DOF arm.
1
x
l1
l2
2
1
2Solution
21 :scoordiante dGeneralize )1( q
21 , : workdo that forces veconservati-Non )2( (3) The kinetic energy:
211 2
1K O,about rotates 1link oI22
222 22 2
121K motion, plane generalin 2link cc IVm
c2
Centerof mass
y
)()(4
2
2211221
221
222
121
222
122
11
122
11
222
2
2
cllllyxV
slsly
clclxccc
c
cPosition ofthe Center Of mass
![Page 47: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/47.jpg)
Example 1 (continued)The total kinetic energy
2
2 21 2 2 1 1 2 1 2 2 1 1 2
2 22 2 1 2
1 1( ) ( )2 2
1 1 ( )( )8 2
o
c
K K K I m l m l l c
m l I
The potential energy (assume y=0 is the reference
)2
(2 12
21121
112211 slslgmslgmgymgymU cc
(4) Calculate the partial derivatives
))(41()
21()( 21
2222122121
2120
12
cIlmcllmlmIL
))(41(
21
2122212212
22
cIlmcllmL
)2
(2 12
21121
11
1clclgmclgmL
)(21
2 2112212122
22
sllmclgmL
![Page 48: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/48.jpg)
Example 1 (continued)5) The generalized forces:
2211 W
21 F
6) Apply the Lagrange equation: Fqq
LL
dtd
21
12222
12212
122211212212212
2
1
222222122
222
2212222222122
222
2121
5.05.05.0)5.0()5.0(25.05.025.0
5.025.025.0
gclmsllmgclmgclmmsllm
IlmcllmIlmcllmIlmcllmIlmlmI
cc
cco
![Page 49: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/49.jpg)
Example 2Example 2: The moment of inertia of the first link is I1. The mass
of link 2 is m2. The mass is concentrated at the endpoint. An external force f acts at the endpoint. Derive the dynamics of the arm.
0003009020001
22
111
l
dai iiii
x0
y0z0
z1
x1
y1
z2
x2y2 z3
x3y3
Solution: (1) Generalized coordinates
Use the D-H method to assign frames and select the joint angles as the generalized coordinates
21qGeneralized coordinates:
f
l2
![Page 50: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/50.jpg)
Example 2 (continued)(2) No-conservative forces that do work:
(3) Kinetic energy, potential energy and Lagrangian
2111 2
1 :1Link IK 2
22 21 :2Link VmK
To find the velocity V of the endpoint, we need to solve the forward kinematics
100001000000
1111
10 cs
scT
100000010000
22
22
21
cs
scT
100001000010
001 2
32
lT
f force external theand ,, :inputsjoint 21
Kinetic energy:
![Page 51: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/51.jpg)
Example 2 (continued)
Forward kinematics:
10000000
2212121
12121
21
10
20
cscsscsssccc
TTT
10000 2222
2121212121212121
32
20
30
slcscslcsscscclssccc
TTT
22
22
21
22
22
2
22212212
lclVslcslccl
dtd
V
The potential energy (Assuming that U=0 when z0=0).
22211 sglmghmU
2221122
222
21
22
2221 2
1)(21 sglmghmlmclmIUKL
Lagrangian:
![Page 52: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/52.jpg)
Example 2 (continued)(4) Calculate the partial derivatives
122
2221
1)(
clmIL
2222
2
lmL
01
L
2222
12222
22sin
21 cglmlmL
![Page 53: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/53.jpg)
Example 2 (continued)(5) Generalized forces:
fqJFxf )( 21
2211TTW
x: the position of the endpoint
22212212
slcslccl
x
22
212212212212
22212212
0)(
clsslcclsclcsl
slcslccl
qqxqJ
(6) Applying the Lagrange equation leads to the dynamics:
fqJ )(
02sin5.02sin5.0
00
21
2222
12222
212222
2
122
22
2221
T
cglmlmlm
lmclmI
![Page 54: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/54.jpg)
Structure of Robot Dynamics• The dynamics of the 2 DOF manipulator:
21
)(
122212221121
),(
212212
2212212
2
1
)(
222222122
222
2212222222122
222
2121
5.05.0)5.0(
5.0)5.0(
25.05.025.05.025.025.0
qqq
q
GC
H
cc
cco
gclmgclmgclmm
sllmsllm
IlmcllmIlmcllmIlmcllmIlmlmI
positionjoint on the depending r,manipulato theofmatrix Intertia :)(qH matrix symmetric )()( qq HHT
forces Coriolis and lcentrifuga the:),( qq Cjoint. a ofcity joint velo theof square on the depend that Terms :force Centrigual
joints. twoof citiesjoint velo theof
product on the depend that Terms:force Coriolis
forcegravity The :)(qG
![Page 55: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/55.jpg)
Structure of Robot Dynamics
• The centrifugal and Coriolis term can be re-written as
2
1
),(
212212
212212
)(21
22212
2221222212
212212
2212212
0)5.0(5.0)5.0(5.00
05.05.0
21
5.0)5.0(),(
qqSqH
qqC
sllmsllm
sllmsllmsllm
sllmsllm
0),( ,any For ),(),(
i.e. matrix, symmetric-skew a is ),(
T
xqqSxx
qqSqqSqqS
T
![Page 56: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/56.jpg)
Structure of Robot Dynamics
• In general, the dynamics of a robot manipulator has the following form:
τqGqqqSqHqqH )()),()(21()(
matrix. inertial definite-positive and Symmetric :)(qH
matrix symmetric-skew a :),( qqS
0energy kinetic theis )(21
qqHq T
nT R ,0),( xxqqSx
Inertial force Centrifugal and Coriolis forces Gravity Joint inputs
![Page 57: Robot Dynamics-new.ppt [兼容模式] - SJTU · 2015. 10. 22. · R c l R s I l I I I I I x y zz yy xx zz yy xx (4) 2 2 ( ) 12 2 12 2 2 1 2 2 R c l R s l I zz x y From (2) and (3),](https://reader035.fdocuments.net/reader035/viewer/2022071606/614295edd9e4dc11f47f24bc/html5/thumbnails/57.jpg)
Linear Parameterization of Robot Dynamics
The dynamics of the 2 DOF arm:
21
122212221121
212212
2212212
2
1
222222122
222
2212222222122
222
2121
5.05.0)5.0(
5.0)5.0(
25.05.025.05.025.025.0
gclmgclmgclmm
sllmsllm
IlmcllmIlmcllmIlmcllmIlmlmI
cc
cco
225214
21232
2222
2222
21211
5.0)5.0(
5.025.0
25.0
lmlmm
llmIlm
IlmlmIc
co
DefineThe parameters depend on mass, length, moment of inertia. They are the physical parameters
21
54321
),,,(
122111221
12122122122100
)5.0(2)2(
β
qqqqY
gcθsθcθθgcgcθθθsθθcθθ
Parametervector
The result can be generalized to n DOF robot manipulator