Tema III: Cinemàtica de la Posició.eia.udg.es/~forest/cinematica_inversa.pdf3 1 Sistemes de...

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



( )( )

+++−−−++−−−+++−+

=

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





Robot Manipulador de Stanford




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









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









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




Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.

Cinemàtica Inversa1 Sistemes de Coordenades



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





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.




H

zzzz

yyyy

xxxxR

H

R

HR

pasnpasnpasn

AAAAAAAAAAAA

T

=

=

1000100034333231

24232211

14131211

4






• 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.




H

HRR

HR pRT

=

10 0 0

1ˆ ; 1ˆ ; 1ˆ0ˆˆˆˆ ; 0ˆˆˆˆ ; 0ˆˆˆˆ

===

=⋅⇒⊥=⋅⇒⊥=⋅⇒⊥

aonononoaoanana






6. Quan la solució existeix no sempre és única:• Configuració braç dret, braç esquerre

• Configuracions redundants




5





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.




No l’estudiarem





Solució Geométria del (θ-r)1 Sistemes de Coordenades



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





atan2(s,c)

• Problema de la tan-1:




( ) ( ) ( )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





Solució Geométrica del (θ1- θ2)1 Sistemes de Coordenades



( )( )

+=

=−=

→

−−=

−−=

++=

−−+=

+=

→

−

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





Solució Simbólica del (θ-r)1 Sistemes de Coordenades



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





Solució Simbólica del (θ1- θ2)1 Sistemes de Coordenades



−=

=→

+−=→

=

+−+

=

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





Solució Simbólica del Mitsubishi RV-M1

• Assumirem que la posició d’entrada està expressada utilitzant el vector configuració




}( )( )

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





Solució Simbólica del Mitsubishi RV-M11 Sistemes de Coordenades



( )( ) ),(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








( )( )

( ) ( )( ) ( )

( ) ( ) ( )( )( ) ( ) ( )( )

( ) ( ) ( ) ( )( )( ) ( )( ) ( )( )( ) ( )( ) ( )

( ) ( )( ) ( )( ) ( )( ) ( )

( ) ( ) ( ) ( )( )( ) ( )( ) ( )( )

( ) ( ) ( ) ( )( )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

+−

−

−

+

+

++=++

+=

++=+

+++=+

++−=−⇒

++=−+=

++

+=

++=+

++=

+−+=+⇒

++=−+=

⇒

++=+−=

⇒

+=+=







3 Cinemàtica inversa 322344 qqqq −−=

( ) ( )( )TTq

wwwqwwwe 6545654 ln5

ππ =⇒=

10








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^^ ^





Desacoplament cinemàtic del manipulador de Stanford1 Sistemes de Coordenades



y1

gx

gy

D

22yx gg +

( )

( ) 22

22

22

222

dggD

dggD

yx

yx

−+=

−+=Caculem:•(θ1,.. θ3) Geometricament•(θ4,.. θ6) Simbolicament

x1

d2

11








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








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





Desacoplament cinemàtic del manipulador de Stanford

• La solució de (θ4,.. θ6) la realitzariem Simbolicament, no ho farem donat que surt molt complicada.







Tema III: Cinemàtica de la Posició.Tema III: Cinemàtica de la Posició.1 Sistemes de Coordenades



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

Date post:	22-Jul-2020
Category:	Documents
Upload:	others
View:	0 times
Download:	0 times

Tema III: Cinemàtica de la Posició.eia.udg.es/~forest/cinematica_inversa.pdf3 1 Sistemes de...

Documents