The L-E (Torque) Dynamical Model: Inertial Forces Coriolis & Centrifugal Forces Gravitational Forces...
43
The L-E (Torque) Dynamical Model: 1 1 1 n n n i i ij j kj k j i i j k j D qq C qqq hq b q Inert ial Force s Coriolis & Centrifu gal Forces Gravitati onal Forces Frictio nal Forces
-
date post
19-Dec-2015 -
Category
Documents
-
view
223 -
download
1
Transcript of The L-E (Torque) Dynamical Model: Inertial Forces Coriolis & Centrifugal Forces Gravitational Forces...
The L-E (Torque) Dynamical Model:
1 1 1
n n ni
i ij j kj k j i ij k j
D q q C q q q h q b q
Inertial Forces
Coriolis & Centrifugal
Forces
Gravitational Forces Frictional
Forces
Lets Apply the Technique --
Z0
Y0
Cm1
X0
X1
Z1
X2
Y2
Y1
Cm2
Z2
Lets do it for a 2-Link “Manipulator”
Link 1 has a Mass of m1; Link 2 a mass of m2
Before Starting lets define a L-E Algorithm:
Step 1 Apply D-H Algorithm to build Ai matrices and find Fi the “link frame”
Step 2 Set T00=I; i=1; D(q)=0
Step 3 Find ci the Homogenous coordinate of the center of mass of link I WRT Fi
Step 4 Set Fc as the translation of Frame F1 to Cm of i Compute Inertia Tensor Di about Cm wrt Fc
Step 5 Compute: zi-1(q); T0i; ci
bar(q); Di(q)
Before Starting lets define a L-E Algorithm:
Step 6 Compute Special Case of Ji(q)
Step 7Partition Ji and compute D(q) = D(q) + {ATmKA + BTDKB}
Step 8 Set i = i+1 go to step 3 else (i=n+1) set i=1 & continue
Step 9 Compute Ci(q); hi(q) and frictioni
Step 10 Formulate Torquei equation
Step 11 Advance “i” go to step 9 until i>n
We Start with Ai’s
1 1 1 1
1 1 1 11
2 2 2 2
2 2 2 22
0
0
0 0 1 0
0 0 0 1
0
0
0 0 1 0
0 0 0 1
C S l C
S C l SA
C S l C
S C l SA
Not Exactly D-H Legal (unless there is more to the robot than these 2 links!)
So Let’s find 0T2
0T2 = A1*A2
1 2 1 2 1 2 1 2 2 1 2 2 1 2 1 1
1 2 1 2 1 2 1 2 2 1 2 2 1 2 1 102
12 12 2 12 1 1
12 12 2 12 1 102
0
0
0 0 1 0
0 0 0 1
:
0
0
0 0 1 0
0 0 0 1
CC S S C S S C l C C l S S l C
S C C S S S C C l S C l C S l ST
simplified
C S l C l C
S C l S l ST
I’ll Compute Similar Terms back – to – back rather than by the Algorithm
1 01
1
1 1
1 1 1 1
1 1 1 101
1 11
1 1 1 11 1 1
1 1 1 1
20
0
1
01 0 0 0
00 1 0 0
0 0 1 00 0 1 0
0 0 0 1
22000
200 0 1 0 01
K
K
c H T c
l
c c
C S l C
S C l SH T
l Cl
C S l Cl S
c S C l S
C2(bar) Computation:
2 02
2
2 2
12 12 2 12 1 1
12 12 2 12 1 102
2
12 12 2 12 1 12
12 12 2 12 1 1
20
0
1
01 0 0 0
00 1 0 0
0 0 1 00 0 1 0
0 0 0 1
200000 0 1 01
K
K
c H T c
l
c c
C S l C l C
S C l S l SH T
llC S l C l C
c S C l S l S
2 121 1
2 121 1
2
20
l CC
l Sl S
Finding D1
Consider each link a thin cylinder
These are Inertial Tensors with respect to a Fc aligned with the link Frames at the Cm
21 1
1
21 1
22 2
2
22 2
0 0 0
0 012
0 012
0 0 0
0 012
0 012
m lD
m l
m lD
m l
Continuing for Link 1
0 01 1 1 1
1 1 1 121 1
1 1 1 1 1
21 1
[ ]
0 0 00 0
0 0 0 012
0 0 1 0 0 10 0
12
TD R D R
C S C Sm l
D S C S C
m l
Simplifying:
2 22 1 1 1 11 1 1
2 221 1 1 1
1 1 1 1
21 1
21 1 12
21 11 1 1 1
012 12
012 12
0 012
0
012
0 0 1
m l m lS S C
m l m lD S C C
m l
S S Cm l
D S C C
Continuing for Link 2
0 02 2 2 2
12 12 12 1222 2
2 12 12 12 12
22 2
212 12 122
22 22 12 12 12
[ ]
0 0 00 0
0 0 0 012
0 0 1 0 0 10 0
12
0
012
0 0 1
TD R D R
C S C Sm l
D S C S C
m l
S S Cm l
D S C C
Now lets compute the Jacobians
1
1 1
0
1 1
11 1 1
01
0
0
20
02
0 0
cJ q
Z
l S
c l CZ c
q
Finishing J1
1 1
1 1
1
02
020 0
0 0
0 0
1 0
l S
l C
J
Note the 2 column is all zeros – even though Joint 2 is revolute – this is the special case!
Jumping into J2
2 2
2 1 2
0 1
22
01
22
1 12
:
0
0
0
c cJ q q
Z Z
here
cZ c
q
cZ c d
q
This is 4th column of A1
Continuing:
22
01
2 12 2 121 1 1 1
2 12 2 121 1 1 1
0
0
0
2 20 0
0 02 2
1 00 0
cZ c
q
l C l Sl C l S
l S l Cl S l C
And Again:
2 121 1
1 122 12
1 1 1 12
2 12
2 12
20
02
1 00
2
20
l Cl C
l Cc l S
l S l Sq
l S
l C
Summarizing, J2 is:
2 12 2 121 1
2 12 2 121 1
2
2 2
2 20 0
0 0
0 0
1 1
l S l Sl S
l C l Cl C
J
Developing the D(q) Contributions
D(q)I = (Ai)TmiAi + (Bi)TDiBi
Ai is the “Upper half” of the Ji matrix
Bi is the “Lower Half” of the Ji matrix
Di is the Inertial Tensor of Linki defined in the Base space
Building D1
D(q)1 = (A1)Tm1A1 + (B1)TD1B1
Here: 1 1
1 1 1
1
02
020 0
0 0
0 0
1 0
l S
l CA
B
Looking at the 1st Term (Linear Velocity term)
1 1 1 1
1 1 1 11
1 1
1 1 1 11 1
1
2 2 2 2 21 1 1 1 1
1 1
21 1
0 02 2
1 : 0 02 20 1 0 1
02
002 2
20 0 1
0 1
0 0440 00 0
1 01
4
T
st
l S l S
l C l CTerm m
l S
l S l Cl C
m
l S l C lm m
m l
0 0
Looking at the 2nd Term (Angular Velocity term)
Recall that D1 is:
Then:
21 1 12
21 11 1 1 1
0
012
0 0 1
S S Cm l
D S C C
21 1 12
21 11 1 1
21 1
0 0 00 0 1
2 : 0 0 00 0 012
0 0 1 1 0
1 0
0 012
nd
S S Cm l
Term S C C
m l
Putting the 2 terms together, D(q)1 is:
2 2
1 1 1 11
21 1
1 0 1 0
0 0 0 04 12
1 0
0 03
m l m lD q
m l
Building the Full Manipulator D(q)
D(q)man = D(q)1 + D(q)2
Where– D(q)2 = (A2)Tm2A2 + (B2)TD2B2
And recalling (from J2):
2 12 2 121 1
2 2 12 2 121 1
2
2 2
2 20 0
0 0
0 0
1 1
l S l Sl S
l C l CA l C
B
Building the 1st D(q)2 Term:
2 22
2 12 2 121 1
2 12 2 121 1 1 1
2 12 2 122 1 1
2 12 2 12
2 222 2 1 2 21 1 2 2
2 2 22 1 2 2 2
2 20
2 22 2
00 02 2
4 4 2
4 2 4
Tm A A
l S l Sl S
l S l Cl S l C
l C l Cm l C
l S l C
l l l l Cl l l C
ml l l C l
How about term (1,1) details!
2 12 2 12 2 12 2 121 1 1 1 1 1 1 11,1
2 2 2 22 2 2 22 12 1 2 12 1 2 12 1 2 12 11 1 1 1
22 2 2 2 2212 12 1 1 1 1 2 12 1
2 2 2 2
2 24 2 4 2
4
l S l S l C l Cterm l S l S l C l C
l S l l S S l C l l C Cl S l C
lS C l S C l l S S C
12 1
2221 1 2 1 2 1 2 1 1 2 1 2 1
22 2 221 1 2 1 2 1 1 2 1 2 1 1 2
2221 1 2 2
4
4
4
C
ll l l S C C S S C C S S C
ll l l S C C S S C C C S S
ll l l C
similar reasoning for the other terms in the matrix
Building the 2rd D(q)2 Term:
Recall D2:
Then:
212 12 122
22 22 12 12 12
0
012
0 0 1
S S Cm l
D S C C
212 12 122
1 1 22 22 12 12 12
22 2
0 0 00 0 1
0 0 00 0 112
0 0 1 1 1
1 1
1 112
TS S C
m lB D B S C C
m l
Combining the 3 Terms to construct the Full D(q) term:
2 222 2 1 2 2
2 21 1 2 21 1 2 2
2 2 22 1 2 2 2
2 2 2 2 221 1 2 2 2 2 1 2 2 2 2
2 1 1 2 2 2
2 2 22 1 2 2 2 2 2
2 2
1 0 1 14 4 20 0 1 13 12
4 2 4
3 4 12 4 2 12
4 2 12 4
Man
l l l l Cl l l C
m l m lD q m
l l l C l
m l l m l l l l C m lm l l l C m
l l l C m l lm m
22 2
12
m l
Simplifying then D(q) is:
2 2 221 1 2 2 2 1 2 2 2 2
2 1 2 1 2 2
2 22 1 2 2 2 2 2 2
3 3 2 3
2 3 3
man
m l m l m l l C m lm l m l l C
D qm l l C m l m l
NOTE: D(q)man is Square in the number of Joints!
This Completes the Fundamental Steps:
Now we compute the Velocity Coupling Matrix and Gravitation terms:
,( ) ,( )
3
1
1( ) ( ) ( )
2
( )
ikj man ij man kj
k i
nj
i k j kik j i
C q D q D qq q
h g m A q
For the 1st Link
11 12 11 12
1 1 1 11
11 12 21 22
2 2 1 1
1
2
man
here i = 1; j = 1 or 2; k = 1 or 2
we take 'terms' from D(q)
kj
D D D D
q q q qC
D D D D
q q q q
Plugging ‘n Chugging
From Earlier:
THUS:
2 2 221 1 2 2 2 1 2 2 2 2
2 1 2 1 2 2
2 22 1 2 2 2 2 2 2
3 3 2 3
2 3 3
man
m l m l m l l C m lm l m l l C
D qm l l C m l m l
2 2 2 2 22 21 1 2 2 2 1 2 2 2 2 1 1 2 2 2 1
2 1 2 1 2 2 2 1 2 1 2 2
1 1 11
2 2 221 1 2 2 2 1 2 2 2 2
2 1 2 1 2 2
2 2
3 3 2 3 3 3
1
2
3 3 2 3
kj
m l m l m l l C m l m l m l m l lm l m l l C m l m l l C
q q qC
m l m l m l l C m lm l m l l C
q q
22 2 2 2
1
222 22 1 2 2 2 2
1 1
2 3
32 3
C m l
q
m lm l l C m l
q q
P & C cont:
1
1 2 2 21 2 2 2
1 2 2 2
0 00 01
0 022
0 0
11 2
kjC l l m Sl l m S
l l m S
Finding h1:
Given: gravity vector points in –Y0 direction (remembering the model!)
gk =(0, -g0, 0)T
g0 is gravitational constant
In the ‘h’ model Akij is extracted from the
relevant Jacobian matrix Here:
3 2
1 11 1
( )jk j k
k j
h g m A q
Continuing:
2
1 21 2 21 0 1 12 2 12
1
1 1 2 121 0 1 2 1 1
1 2 2 120 2 1 1
2 2
2 2
jj
j
h g m A g m A m A
l C l Ch g m m l C
m m l Cg m l C
looking back to jacobians and substituting:
Note: Only k = 2 has a value for gk which is g0!
Stepping to Link 2
22
2
21 22 11 12
1 1 2 2
21 22 21 22
2 2 2 2
1 2 2 2
1( ) ( )
2
1
2
1 1
2 41
04
kj j jkk
C D q D qq q
D D D D
q q q q
D D D D
q q q q
l l m S
Computing h2
3 2
2 21 2
2 0 2 2 122 0 2 22 ( ) 2
jk j k
k j
h g m A
g m l Ch g m A q
Building “Torque” Models for each Link
In General:
1 1 1
n n ni
i ij j kj k j i ij k j
D q q C q q q h q b q
For Link 1:
The 1st terms:
2nd Terms:
1 11 1 12 21
2 221 2 2 1 2 2
2 1 2 1 2 2 1 2 23 3 3 2
n
j jj
D q q D q D q
m l l l l Cm l m l l C q m q
2 21 1 2 1 1 1 2
11 1 21 1 2 12 2 1 22 21 1
22 1 2 21 2 2 2 2 1 2
22
2 1 2 2 2 1
( )
0 02
2
jk k jk j
C q q q C q C q q C q q C q
m l l Sl l m S q q q
qm l l S q q
Writing the Complete Link 1 Model
2 221 2 2 1 2 2
1 2 1 2 1 2 2 1 2 2
22 1 2 2 12
2 1 2 2 2 1 0 2 1 1 1 1
3 3 3 2
( )2 2 2
m l l l l Cm l m l l C q m q
q m m l Cm l l S q q g m l C b q
And, Finally, For Link 2:
22 2 2 2 2
1 1 1
n n n
j j kj k jj k j
D q q C q q q h q b q
Ist 2 terms:
1st Terms:
2nd Terms:
2 21 1 22 21
2 22 1 2 2 2 2
2 1 23 2 3
n
j jj
D q q D q D q
l l l C m lm q q
22 1 2 2 2 1 2 2 2 1 2 21 1 2 1 2
22 1 2 21
02 2 2
2
m l l S m l l S m l l Sq q q q q
m l l Sq
Finalizing Link 2 Torque Model:
2 222 1 2 2 2 2 2 1 2 2
2 2 1 2 1
0 2 2 122 2
3 2 3 2
( )2
l l l C m l m l l Sm q q q
g m l Cb q
