tema iii: cinemàtica de la posició.eia.udg.es/~forest/cinematica_inversa.pdf3 1 sistemes de...
TRANSCRIPT
1
1 Objectives
2 State of the Art
3 Proposal
4 Methodology
5 Results
6 Conclusions
• Contributions
• Features
• Publications
• Further Work
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.
Robot Mitsubishi RV-M1
Tema III: Cinemàtica de la Posició.1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
( )( )
+++−−−++−−−+++−+
=
1000122233234523452345234
2223323451234151523415152341
2223323451234151523415152341
dsasacdcsscscacasdsssccscsscccscacasdcsccssccssccc
THR
x0
l1
z0
x1
y1z1
z2
x2y2
z3
x3y3
z4
y4
x4
y5
x5
Z5 x6
y6
Z6
y0
l2l3
l4
1 Objectives
2 State of the Art
3 Proposal
4 Methodology
5 Results
6 Conclusions
• Contributions
• Features
• Publications
• Further Work
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.
Robot Manipulador de Stanford
Tema III: Cinemàtica de la Posició.1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
000l4q66
l300q303
090º00q44
-90º90º00q55
90º
0
Home
90º0l2q22
90º0l1q11
αiaidiθiGDLL
2
1 Objectives
2 State of the Art
3 Proposal
4 Methodology
5 Results
6 Conclusions
• Contributions
• Features
• Publications
• Further Work
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.
Robot Manipulador de Stanford
Tema III: Cinemàtica de la Posició.1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
6
6
66
665
65
5
55
554
54
4
44
443
43
3
3
2
32
2
2
22
221
21
1
1
11
110
10
1
1
1000100
0000
100000100000
100000100000
1000100
00100001
1000010
0000
1000010
0000
10000
······
−
=
−
=
−
=
=
−
=
−
=
−
−
=
−
−
dcssc
Acssc
Acssc
Ad
A
dcssc
Ad
cssc
Adcssacsccscassscc
A
i
iii
iiiiiii
iiiiiiii
ii
ααθθαθαθθθαθαθ
1 Objectives
2 State of the Art
3 Proposal
4 Methodology
5 Results
6 Conclusions
• Contributions
• Features
• Publications
• Further Work
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.
Robot Manipulador de Stanford
Tema III: Cinemàtica de la Posició.1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
RTH
nx=c1∙c2∙(c4∙c5∙c6+s4∙s6)+s1∙(s4∙c5∙c6-c4∙s6)+c1∙s2∙s5∙c6ny=s1∙c2∙(c4∙c5∙c6+s4∙s6)-c1∙(s4∙c5∙c6-c4∙s6)+s1∙s2∙s5∙c6nz=s2∙(c4∙c5∙c6+s4∙s6)-c2∙s5∙c6
ox=c1∙c2∙(-c4∙c5∙s6+s4∙c6)+s1∙(-s4∙c5∙s6-c4∙c6)-c1∙s2∙s5∙s6oy=s1∙c2∙(-c4∙c5∙s6+s4∙ c6)-c1∙(- s4∙ c5∙ s6-c4∙ c6)- s1∙ s2∙s5∙ s6oz=s2∙(-c4∙ c5∙ s6+ s4∙ c6)+c2∙s5∙ s6
ax= c1∙ c2∙ c4∙ s5+ s1∙ s4∙ s5- c1∙ s2∙ c5ay= s1∙ c2∙ c4∙ s5- c1∙ s4∙s5- s1∙ s2∙ c5az= s2∙ c4∙ s5+ c2∙ c5
px= c1∙ c2∙ c4∙ s5∙d6+ s1∙ s4∙ s5∙ d6- c1∙ s2∙ c5∙ d6+ c1∙ s2∙d3+ s1∙d2py= s1∙ c2∙ c4∙ s5∙d6- c1∙ s4∙ s5∙ d6- s1∙ s2∙ c5∙ d6+ s1∙ s2∙d3- c1∙d2pz= s2∙c4∙ s5∙ d6+ c2∙ c5∙ d6- c2∙d3+d1
3
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Cinemàtica Inversa1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Problema cinemàtic invers
Donada una determinada posició (x,y,z) i una determinada orientació (RPY) obtenir el valor de les variables d’unió (q1, q2,..., qn) que situen el manipulador en la configuració desitjada.
Problema cinemàtic
invers(q1, q2,..., qn)T (x,y,z,θ,φ,α)T
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Propietats Generals de la Solució
1. Sigui P≡espai accessible (P⊂ℜ3) i p la posició a assolir, p∉P⇒no ∃Solució.
2. Sigui R el conjunt d’orientacíons possibles, i (θ,φ,α) l’orientació RPY a assolir, (θ,φ,α) ∉R⇒no ∃ Solució.Notar que no totes les orientacions són possibles degut a:• Limitacions mecàniques a les articulacions• Falta d’algun dels 3 G.D.LL.
3. Igualar la matriu simbólica a la matriu númerica del braçproporciona 12 equacions:
Per un robot de n G.D.LL. Tenim 12 equacions i n incognites.
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
H
zzzz
yyyy
xxxxR
H
R
HR
pasnpasnpasn
AAAAAAAAAAAA
T
=
=
1000100034333231
24232211
14131211
4
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Propietats Generals de la Solució
• Les 12 eq. no són linialment independents:
sigui: donat que RRH és una matriu de rotació
s’acompleix que:
Aquestes 6 eq. mostren depèndencies entre les 12 anteriors, reduint el nº d’eq. independents a 6 amb n incógnites.
4. Manipulació en 3D ⇒ n G.D.LL≥6 (condició necessària però no suficient).
5. No sempre existeix una solució simbólica.
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
H
HRR
HR pRT
=
10 0 0
1ˆ ; 1ˆ ; 1ˆ0ˆˆˆˆ ; 0ˆˆˆˆ ; 0ˆˆˆˆ
===
=⋅⇒⊥=⋅⇒⊥=⋅⇒⊥
aonononoaoanana
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Propietats Generals de la Solució
6. Quan la solució existeix no sempre és única:• Configuració braç dret, braç esquerre
• Configuracions redundants
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
5
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Métodes de Resolució
1. Solució Geométrica.• Només aplicable a robots de pocs G.D.LL (3 o menys).• Aplicable als 3 primers G.D.LL d’alguns robots complexes.• La solució és aplicable en temps real.• No és genérica. Depén del manipulador.
2. Solució Simbólica.• No sempre es pot trobar.• La solució és aplicable en temps real.• No és genérica. Depén del manipulador.
3. Desacoplament cinemàtic• Métode Híbrid entre els 2 anteriors.
4. Solució numérica iterativa.• Genérica. Depén dels paràmetres DH.• Lenta. No aplicable en temps real.
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
No l’estudiarem
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Geométria del (θ-r)1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
y0z0
x1y1
z1
x2
y2
z2
θ1 d2
x0
d1
b1b2
1
1
12110
12110
1000001
·0·0
−−−
−
=dcdcssdsc
THR
θ1
12 ·sdpx =
12 ·cdpy −=
x0
y0
222
11
1
1
12
12 tan··
yx
y
x
y
x
ppq
pp
qcs
cdsd
pp
+=
−=⇒==
−−
Cal utilitzar lafunció atan2!!
6
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
atan2(s,c)
• Problema de la tan-1:
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
( ) ( ) ( )21211 tantan/
2tan
2θθθθππ
=≠∃⇒≤≤− − t
III
III IV
sc s<0
c>0s<0c>0
s>0c>0
s>0c<0
s<0c<0
1θ
2θ
Solució:
( )
+
=⇒
−−<
=⇒
−−=
=⇒
−−>
=
−
−
π
π
)·sgn(tan),(2tan,
0
2)·sgn(),(2tan
,0
tan),(2tan,
0
,2tan
1
1
scscsa
IIIQIIQc
scsaIVQIQ
c
cscsa
IVQIQc
csa
= −
cscsa 1tan),(2tan
= −
cscsa 1tan),(2tan
π+
= −
cscsa 1tan),(2tan
π−
= −
cscsa 1tan),(2tan
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Geométrica del (θ1- θ2)1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
( )( )
+=
=−=
→
−−=
−−=
++=
−−+=
+=
→
−
22122
1
1
21
22
21
21
2
21
22
21
2
2
2212
22
12
2112
22
12
222
2
·cos,·sin2tan,2tan
··2cos
··2)cos(
)·cos(··2
)·cos(··2
θθα
βαβθ
θ
θ
θ
θπ
lllappaq
llllr
llllr
llllr
llllr
ppr
q
xy
yx
θ1
x0
y0
θ2
θ1
py
px
α
β
r
l 1
l2
a
b
c
φ
y
x
)·cos(··2222 φbabac −+=
Teorema de Cosinus
l 2·cos θ 2
7
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del (θ-r)1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
y0z0
x1y1
z1
x2
y2
z2
θ1 d2
x0
d1
b1b2
( ) ( ) ( ) 222
22
21
21
22
212
212
222
11
2
1
12110
12110
···
),(2tan
coneguda NuméricaMatriu
1000
SimbólicaMatriu
1000001
·0·0
yxyx
yx
H
zzzz
yyyy
xxxxR
HR
ppddcsdcdsdppq
aaaq
pasnpasnpasn
dcdcssdsc
T
+=⇒=+=−+=+⇒
−=⇒
=
−−−
−
=
θ
4444 84444 7644444 844444 76
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del (θ1- θ2)1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
−=
=→
+−=→
=
+−+
=
1122
122
2211
2
111221212
1112212120
),(2tan
)·,·(2tan
coneguda NuméricaMatriu
1000
SimbólicaMatriu
10000010
00
θθθ
θ
θ
xy
yxxy
H
zzzz
yyyy
xxxxR
HR
nnaq
aapaapaq
pasnpasnpasn
sasacscacasc
T
4444 84444 76444444 8444444 76
a1
y0z0
θ1
x0
y1z1
θ2
x1
y2
z2
x2
a2
8
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del Mitsubishi RV-M1
• Assumirem que la posició d’entrada està expressada utilitzant el vector configuració
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
}( )( )
44444 844444 76Simbòlic ióConfiguracVector
numéric ióConfigurac
Vector
234
2341
2341
1222332345
2223323451
2223323451
6
5
4
3
2
1
5
5
5
−
+++−
++++
=
=
ce
sse
sce
dsasacdcacasdscacasdc
wwwwww
W
q
q
q
π
π
π
( )( )
+++−−−++−−−+++−+
=
1000122233234523452345234
2223323451234151523415152341
2223323451234151523415152341
dsasacdcsscscacasdsssccscsscccscacasdcsccssccssccc
THR
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del Mitsubishi RV-M11 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
( )( ) ),(2tan 121
22233234512
22233234511 wwaqcacasdswcacasdcw
=⇒
++=++=
( )
−−±=⇒
−−=
−+++=
+++++=
++++=
⇒
+=−++=−+
−+=⇒+
=+
+−+
+
+
++
23
22
23
22
211
323
22
23
22
21
3
232232
22
32
22
1
223223232
22
22
22
232
232
32
22
1
222332
22
22
232
3222332
22
22
232
32
22
1
22233
2
123453
22233
1
23452111
6514123422
12
12345141
2cos
2)cos(
)cos(2
)(2)()(
22
,2tan)(5
aaaabb
qaa
aabbq
qqqaaaabb
ssccaascascabb
sasasasacacacacabb
sasab
dcdwcaca
bsdwswc
wwswcaqcssewswcq
44 344 21
444 8444 76
π
9
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del Mitsubishi RV-M11 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
( )( )
( ) ( )( ) ( )
( ) ( ) ( )( )( ) ( ) ( )( )
( ) ( ) ( ) ( )( )( ) ( )( ) ( )( )( ) ( )( ) ( )
( ) ( )( ) ( )( ) ( )( ) ( )
( ) ( ) ( ) ( )( )( ) ( )( ) ( )( )
( ) ( ) ( ) ( )( )2331233,13322332
233
2233
13322332
22
332
2331332233
22
2332233332233
22
33223333133
22332332
23322331
233
2233
23312332
22
332
2332331233
22333322
33233
23323322
2331233
22332332
23322331
22323232
22323231
222332
222331
2tan
obtenim equacions ambduesafegint
obtenim equacions ambduesafegint
bsabacabsabacaaqsaacabsabaca
s
ssaacabsabaca
sacacacasabaca
ssacacasabsasacacsabssacacab
saacabsabaca
c
csaacabsabaca
sacasacsabsa
ssaacacacabacasacacsabssacacab
sacsscabcassccab
sasabcacab
+−
−
−
+
+
++=++
+=
++=+
+++=+
++−=−⇒
++=−+=
++
+=
++=+
++=
+−+=+⇒
++=−+=
⇒
++=+−=
⇒
+=+=
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del Mitsubishi RV-M11 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa 322344 qqqq −−=
( ) ( )( )TTq
wwwqwwwe 6545654 ln5
ππ =⇒=
10
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Solució Simbólica del Mitsubishi RV-M11 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
322344 qqqq −−=
( )( )Twwwq 6545 lnπ=
),(2tan 121 wwaq =
−−±=
+−
23
22
23
22
211
3 2cos
aaaabb
q
( )65141234 ,2tan wwswcaq −+=
( ) ( ) ( ) ( )( )2331233,13322332 2tan bsabacabsabacaaq +− ++=
x0
z0
y0
n
oa
d6
p̂
^^
^
g=p-d6·a^^ ^
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Desacoplament cinemàtic del manipulador de Stanford1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
y1
gx
gy
D
22yx gg +
( )
( ) 22
22
22
222
dggD
dggD
yx
yx
−+=
−+=Caculem:•(θ1,.. θ3) Geometricament•(θ4,.. θ6) Simbolicament
x1
d2
11
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Desacoplament cinemàtic del manipulador de Stanford1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
D
x0
l1
z0
y0
z1
z2
y2
z3x3
y3
x1
y1
x2
d3
gz-l1 αz1
θ2
( )
( ) 2213
221
23
Dlgd
Dlgd
z
z
+−=
+−=
x0
l1
z0
y0
z1
z2
y2
z3x3
y3
x1
y1
x2
d3
gz-l1 α
D=d3cosα
d3sinα
z1
θ2
( )2
,2tan 12π
αθ +−=
444 8444 76Dlga z
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Desacoplament cinemàtic del manipulador de Stanford1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
x1
y1
θ1
x0
y0
d2·c1
θ1
D·s1
θ1
D·c1 d2·s1
gx
gy
( ) ( )( )
( ) ( )( )
−−
+
+
+−+=
+
+−+=
−−
+=
−−=
−=−−=
22
2
22
22
2
22
22
1
22
22
22
22
1
22
22
1
2
11
121
121
··,2tan
:anglel'calcular podemfinalment
:obtenim (eq.3) la a arat substituin
··:obtenim (eq.2) la at substituin
(eq.3) ·
: trobem(eq.1) la de
(eq.2) ··(eq.1) ··
dD
gDgd
dDd
dDgDggdDa
dDd
dDgDggdDs
dD
gDgdc
dcDg
s
cdsDgsdcDg
xyxxy
xxy
xy
x
y
x
θ
12
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.
Desacoplament cinemàtic del manipulador de Stanford
• La solució de (θ4,.. θ6) la realitzariem Simbolicament, no ho farem donat que surt molt complicada.
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.1 Sistemes de Coordenades
2 Cinemàtica directa
3 Cinemàtica inversa
In terpo lator+
Sample
[ ]321 ,, XXX ′′′
[ ]nΧΧΧΧ ,..,,, 321
[ ]nQQQQ ,..,,, 321 Sequen cing
)(QM
),( QQV &
)(QG
[ ]rQQQQ ,..,,, 321
],[ 21 qqQreal =
Simulation
3D
],[ 21 tiqqQ tt =
[ ]rQQQQ &&&&&&&& ,. . ,,, 321
[ ]rQQQQ &&&& ,. . ,,, 321
SecurityControl
],[ 21 ttt qqQ &&& =
],[ 21 ttt qqQ &&&&&& =
Vel ocitiesCa lc ulaton
iω
[ ]21 , CC PP
iv iv& iω&
Forc es&
TorquesCa lc ulation
if
in
[ ]21,PP
eNeF
[ ]111PRA =
[ ]222 PRA =
21 AATHR ⋅=
1−H
RT
21 , AA
c ommand
. actuatorsK Vτ =
PositionSensorModel
1 2[ , ]perceivedQ q q=
[ ]21 ,RR
t t
q 1 q2
Q i= [ qi1 ,qi2]
1n
1n
Qi=[ qi1,q i2]
t t
q1 q 21r
1r
Qi=[ qi1,q i2]
t t
q1 q 2
1
r
1 r
. . .. .
t t
q1 q 2
r 1r
....Qi=[ qi1,q i2].. .. ..
1
13
2X
t
Y 1
32
t
1 n2
X
t
Y 1
t
i2
in
A A1 2,
KineticsEquation
],[ 21 qqQrea l &&& =
1 2[ , ]percei vedQ q q=& & &
Veloci tySensorModel
DE F IN E CA RT ES IAN P AT H1 =P ATH ( X’1,X ’2 , X’ 2 )DE F IN E RO BO T PA TH 2= PA TH (Q ’1, Q’2 , Q’ 3 )
MO VE S P ATH 1 M OVE PA TH 1 MO VE P AT H2
D EF I NE C ART ES I AN P AT H1 =P AT H( X’1 ,X’ 2, X ’2)D EF I NE R OBOT P AT H2=P ATH( Q’ 1,Q’ 2, Q’ 3 )
M OVE S P AT H1 MOV E P AT H1 M OVE PA TH2
],[ 21 ttt qqQ &&&&&& =STOP
Interpolator+
Sample
Programming Language
Kinematics
Direct Kinetics
Kinematics
Inverse Kin etics
Actuato rs Taco
met
er
Enco
der
Co ntrol
],[ 21 ttt qqQ &&& =
DIAGRAMA DE BLOCS D’UN ROBOT INDUSTRIAL