NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING
Transcript of NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING
NMT EE 589 & UNM ME 482/582
ROBOT ENGINEERING
Dr. Stephen Bruder NMT EE 589 & UNM ME 482/582
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Given that we wish to find the path 𝑞 (𝑡 (in joint space) which minimizes the energy of the system
where the Lagrangian is (7.15)
From Calculus of Variations we know that the optimal path satisfies (7.16)
1min ( ) min ( , ) ndq q q dq dq К υ
1( , ) ( ) ( )
2
Tq q q D q q q υ
( ) ( )0
d
q dt q
Slide 2 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Let's further examine the terms on the lhs of equation (7.16). First expand equation (7.15) using (7.17) (7.18)
Applying equation (7.16), in scalar form we get, (7.19)
11 12 1 1
1
1 2
1 1( ) [ ]
2 2
n
T
n
n n nn n
D D D q
q D q q q q
D D D q
1 1
1( )
2
n n
ij i ji jD q q q
( ) ( )0, 1, ,
k k
dk n
q dt q
Slide 3 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
For k = 1,...,n:
Starting with the second term on the lhs of equation (7.19) (7.20)
(since D is symmetric and the last equals 0) (7.21) (7.22) (7.23)
1 1
( ) 1 1 ( )( ) ( )
2 2
n n
kj j ik ij ik
qD q q D q q
q q
υ
1( )
n
kj jjD q q
1
( )( )
n
kj jjk
d dD q q
dt q dt
1 1
( ) ( )n n
kj j kj jj j
dD q q D q q
dt
1 1 1
( )( )
n n n kj
kj j i jj j ii
D qD q q q q
q
Slide 4 / 21
1 21 11 2
( ) ( ) ( )( )
n n kj kj kj
kj j n jj jn
D D DD q q q q q q
q q q
1 1
1( , ) ( ) ( )
2
n n
ij i ji jq q D q q q q
υ
0 ki ikD D
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Next, the first term on the lhs of equation (7.19) suggests that (7.24)
Finally, substituting these results (equations (7.23) and (7.24)) back into equation (7.19) yields (7.25)
Hence,
1 1
( )( ) 1 ( )
2
n n ij
i jj ik k k
D q qq q
q q q
υ
1 1
2 2
1 2 1 2[ ( ) ( ) ( )] [ ( , ) ( , ) ( , )] ( ) 0k k kn k k kn k
n n
q q
q qD q D q D q C q q C q q C q q G q
q q
1 1 1
( ) ( )1 ( )( ) 0
2
n n n kj ij
kj j i jj j ii k k
D q D q qD q q q q
q q q
υ
Slide 5 / 21
1 1
1( , ) ( ) ( )
2
n n
ij i ji jq q D q q q q
υ
(7.26)
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Recalling Eqn. 7.25 (7.25)
we could also write the resulting Matrix/Vector eqn as:
This representation provides better clarity regarding the inherent structure of the equations:
○ D(.) is only a fn of the qi’s and is multiplied only by 𝑞 ,
○ C(.) (as shown above) is only a fn of the qi’s and is multiplied by 𝑞𝑖 𝑞𝑗 type terms, and
○ The G(.) vector is only a function of the qi’s
1 1 1
2 1 2
1 2[ ( ) ( ) ( )] ( ) ( ) 0k k kn k
n n n
q q q
q q qD q D q D q C q G q
q q q
1 1 1
( ) ( )1 ( )( ) 0
2
n n n kj ij
kj j i jj j ii k k
D q D q qD q q q q
q q q
υ
Slide 6 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Collecting rows corresponding to k = 1,...,n, and rewriting in matrix form gives: (7.26)
In the general case wherein an external torque (𝜏 ) is applied to the system, the rhs of equation (7.26) becomes non-zero. Specifically, (7.27)
In our robotics problems, 𝜏 is the vector of joint torques generated by the actuator at each joint.
Equation (7.27) represents a very generalized form of a dynamic model, which is valid for any robot with rigid links (and rigid joints).
○ We could also add other dissipative terms such as friction
( ) ( , ) ( )D q q C q q q G q
( ) ( , ) ( ) 0D q q C q q q G q
Slide 7 / 21
( ) ( ) ( ) 0i jD q q C q q q G q
OR
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.5 The Equations of Motion
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Summary:
The path 𝑞 (𝑡 which satisfies (7.27) was obtained by imposing (7.28)
where ℒ ≐ К − 𝛖, is the difference between the kinetic energy and potential energy.
( ) ( )d
q dt q
Slide 8 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.6 A Step by Step Procedure to Construct the
Dynamic Model
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 1: Inertia Matrices
First, compute the inertia matrix of each link, preferably in a coordinate system located at the link's center of mass (frame {𝑖}, i = 1,...,n) of each link.
○ For the purpose of this course, the inertia matrices 𝑖, i = 1,...,n will always be given!
Step 2: Forward Kinematics
Construct the D-H table, and thus the forward kinematic relationships 𝑇1
0 , 𝑇20 , … , 𝑇𝑛
0 . Transform these homogeneous matrices to link center of mass coordinates:
○ From these we can get 𝑃0 𝑐𝑖, 𝑧 0𝑖 , and , i = 1,...,n.
0 0
1 2 1 2( , , , ) ( , , , ) , 1, ,i i
i
c i i i cT q q q T q q q T i n
0
iR
Slide 9 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.6 A Step by Step Procedure to Construct the
Dynamic Model
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 3: Compute Velocities
a) Construct the 𝑣 𝑐𝑖0
(i.e., 𝑣 𝑐𝑖 expressed in terms of frame {0}), which can be
obtained by directly differentiating 𝑃0 𝑐𝑖.
b) Construct the 𝜔0 𝑖, i = 1,...,n, which can be obtained by recalling that then transform this vector to frame {𝑖} by .
Step 4: Kinetic & Potential Energy
Develop the expressions for the kinetic energy К = К𝑖𝑖=1
𝑛
, where
and potential energy 𝛖 = 𝛖𝑖𝑖=1
𝑛
0 0 00
1 21 2 ii iz z z
0 0( )i i
T
iR
1( )
2 i i i i
T T
i c c i iv v m К
0( )i
T
i c iq g P mυ
Slide 10 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.6 A Step by Step Procedure to Construct the
Dynamic Model
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 5: Euler-Lagrange Eqn
Apply the scalar Euler-Lagrange equations to the Lagrangian ℒ ≐ К − 𝛖, i = 1,...,n to develop (7.30)
Step 6: Generalized Form
Collect the terms and rewrite the expressions generated from Step 5 in the desired matrix form: (7.31)
○ This is a differential equation which governs the motion of the robot, described in terms of the generalized coordinates 𝑞 (𝑡 .
( ) ( ), 1, ,k
k k
dk n
q dt q
( ) ( , ) ( )D q q C q q q G q
Slide 11 / 21
( ) ( ) ( )i jD q q C q q q G q
OR
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
A First Dynamic Model Example:
A two link RR manipulator.
The Denavit-Hartenberg table:
D-H
params. i di
1 0 0 0
2 0 2 0
i-1 i-1
ai
d i
1( )t
2 ( )t
Slide 12 / 21
Handout:
Mathematica File: RR_bot
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 1: Inertia Matrices
Assume the inertia matrices are computed in the link center of mass coordinate frames {𝑐1} and {𝑐2}, respectively, as
1 2
1 1 2 2
1 2
0 0 0 0 0 0
0 0 ; 0 0
0 0 0 0
c cI I
I I
Model the links as slender rods
Slide 13 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 2: Forward Kinematics
From the D-H table,
Hence,
1 1 2 2
1 1 2 20 1
1 2
C S 0 0 C S 0 2
S C 0 0 S C 0 0
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
T T
12 12 1
12 12 10 0 1
2 1 2
C S 0 2C
S C 0 2S
0 0 1 0
0 0 0 1
T T T
Slide 14 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
From the diagram
hence
and
1 2
1 2
1 0 0 1 1 0 0 2
0 1 0 0 0 1 0 0
0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
c cT T
1 1 1
1 1 1
1 1 10 0 1 0
1
C S 0 C
S C 0 S
0 0 1 0
0 0 0 1
c cT T T T
2 2 2
12 12 1 12
0 0 2 012 12 1 12
2
C S 0 2 C C
S C 0 2 S S
0 0 1 0
0 0 0 1
c cT T T T
Slide 15 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 3: Compute Velocities
Compute the linear velocities for each center of mass (𝑣 𝑐𝑖, i = 1,2), with
respect to frame {0}:
1 1
1
0 0
1
C
( ) S
0
c c
d dv P
dt dt
2 2
1 12
0 0
1 12
2(C C )
( ) 2(S S )
0
c c
d dv P
dt dt
2
1 1 12 12
1 1 12 12
2 S S
2 C C
0
cv
1
1 1
1 1
S
C
0
cv
Slide 16 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Compute the angular velocity for each link 𝜔0 𝑖 (in terms of frame {0}),
Then, compute , the angular velocity of each link coordinatized in the same frame in which 𝑖 is computed (i.e., center of mass)
○ Link#1:
○ Link#2:
0 0( )i i
T
iR
00
11 1 1
0
0
1
z
1 1
1 1
0 0
1 1 1 1
1
C S 0 0 0
( ) S C 0 0 0
0 0 1 1
T
TR
00 0
22 1 2 2
1 1 2
0 0 0
0 0 0
1
z
2 2
0 0
2
1 2
0
( ) 0TR
Slide 17 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 4: Kinetic & Potential Energy
Noting that gravity acts in the negative 𝑦 0 direction gives:
where 𝑔 ≐ −9.81 m/s2. The potential energy is:
[0 0]Tg g
2
1 12
0
2 2 2 1 12 2 1 12
2 C C
[0 0] 2 S S 2 (S S )
0
T
cm g P m g m g
υ
1
1
0
1 1 1 1 1 1
C
[0 0] S S
0
T
cm g P m g m g
υ
Slide 18 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
The kinetic energy is:
1 1 1 11 1 1
1 1
1 1 1 1 1 1 1 1 1
1 1
2
1 1 1
1( )
2
S 0 0 0 01
[ S C 0] C [0 0 ] 0 0 02
0 0 0
1( )
2
T T
c cm v v
m I
I
m I
К
1 12 1 12 2
2 1 12 1 12 2 1 12 1 12 2 1 12 1 12 2 1 2 2
2 1 2
(S S ) S 0 0 0 01
4 [ (S S ) S (C C ) C 0] (C C ) C [0 0 ] 0 0 02
0 0 0
m I
I
2 2 2 22 2 1
1( )
2
T T
c cm v v К
2 2 2
2 1 2 2 2 1 2 1 2 2
1[ ( ) 4 (2(1 C ) 2(1 C ) )]
2I m
Slide 19 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 5: Euler-Lagrange Eqn
Apply the Euler-Lagrange equations.
2 2 2 2
1 1 2 1 12 1 1 1 2 1 2 2 2 1 2 1 2 2
12 S 4 (S S ) ( ) ( ) 4 2(1 C ) 2(1 C )
2m g m g m I I m
1 2 1 2 К К υ υ
1
1 1
2
1 1 1 12 2 2 2 1 2 2 2 2
1 2 2 1 2 1 2 2 2 2
2 8 4
8 1 4 1
d
dt
g c m c c m m s m s
m c m I I c m I
2
2 2
2
12 2 2 2 1 2 2 2 1 2 2 22 4 4 1 4
d
dt
gc m m s c m I m I
Slide 20 / 21
© 2011, Dr. Stephen Bruder
7. Robot Dynamics 7.7 A First Dynamic Model Example
Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering
Step 6: Generalized Form
Rewriting in the generalized matrix form
gives rise to
Optionally, in the alternative generalized form
( ) ( , ) ( )D q q C q q q G q
1 2 2 1 2 2 2 2 1 1 2 1 12 12 2 2 2 2 2 2 1
2 2 2 2 2 2 12 21 2 2 1 2
8 (1 C ) 4 (1 C ) C 2 (C C )8 S 4 S
4 (1 C ) 4 2 C4 S 0
m m I I m I gm gmm m
m I m I gmm
1 2 2 1 2 2 2 2 2
2 2 2 2 2 1
8 (1 C ) 4 (1 C )
4 (1 C ) 4
m m I I m I
m I m I
1 1
2 2 2 2 1 1 2 1 12 1
1 2
2 2 2 12 2
2 2
0 8 S 4 S C 2 (C C )
4 S 0 0 2 C
m m gm gm
m gm
( ) ( ) ( )i jD q q C q q q G q
Slide 21 / 21