Fundposori

Fundamentos de la posición y orientación

Representación en el plano

A

Ax

Ay

PA

xA p

yA p

A

Ax

Ay

PA

Coordenadas cartesianas Coordenadas polares

yA

xA

A

p

pP

2yA2

xA pp

)p/parctan( xA

yA

Más información

http://en.wikipedia.org/wiki/Polar_coordinate_system

Representación en el plano (I)

A

Ax

Ay

PB

ORGBAP

yB p

Traslación de un sistema de coordenadas

B

ORGBA

yP

ORGBA

xP

xB p

xA p

yA p

ORGBA

ORGBA

ORGBA

y

x

P

PP

yB

xB

B

p

pP

yB

ORGBA

xB

ORGBA

yB

xB

ORGBA

ORGBA

BORGB

AA

p

p

p

p

y

x

y

x

P

P

P

PPPP

By

Bx

Representación en el plano (II)

A

Ax

Ay

Rotación de un sistema de coordenadas

xA p

yA p

yB

xB

B

p

pP

PB

yB p

B

xB p

By

Bx

)cos( )sen(

)sen(- )cos(

RAB

)cos(p )sen(p

)sen(p- )cos(p

p

p

)cos( )sen(

)sen(- )cos(

yB

xB

yB

xB

yB

xB

BA

PRP AB

Representación en el plano (III)

A

Ax

Ay

Rotación y traslación de un sistema de coordenadas

xA p

yA p

yB

xB

B

p

pPPB

yB p

B

xB p

By

Bx

)cos(p )sen(p

)sen(p- )cos(p

yB

xB

yB

xB

B'

P

'B

ORGBA

yP

ORGBA

XP

)cos(p )sen(p

)sen(p- )cos(p

yB

xB

yB

xB

ORGBA

ORGBA

BORGB

AA

y

xAB

P

PPRPP

ORGBA

ORGBA

ORGBA

y

x

P

PP

Conversión de coordenadas locales a globales

A

Ax

Ay

Aplicación a robótica móvil

4.1

5.2PB

2

B

1

By

Bx

8

3ORGBA yP

4ORGBA xP

2.5

1.4

p

p

)82cos( )81sen(

)82sen(- )8cos( 1

3

4

yA

xA

BORGB

AA

PRPP A

B

2

1BP

Conversión de coordenadas locales a globales (II)

A

Ax

AyAplicación a un brazo articulado

PB

2

B

1

By

Bx

l1=2

8

l2=3)8(sen32

ORGBA

yP

)8(cos3ORGBA xP

3.5

9.2

p

p

)82cos( )81sen(

)82sen(- )81cos(

)8(sen32

)8(cos3

yA

xA

BORGB

AA

PRPP A

B

2.9

5.3

Representación en el espacio

A

Ay

Az

PA

yA p

zA

yA

xA

A

p

p

p

P

Ax

zA p

xA p

Coordenadas cartesianas

zA

A

p

P

Coordenadas cilíndricas

PA

Coordenadas esféricas

Representación en el espacio (II)

A

Ay

Az

PB

yB p

Ax

zA p

Traslación de un sistema de coordenadas

ORGBA

zP

ORGBA

yP

ORGBA

xP

By

Bx

Bz

xB p

zB p

ORGBAP

yA p

xA p

zB

ORGBA

yB

ORGBA

xB

ORGBA

zB

yB

xB

ORGBA

ORGBA

ORGBA

BORGB

AA

p

p

p

p

p

p

z

y

x

z

y

x

P

P

P

P

P

P

PPP

B

Representación en el espacio (III)

100

0)cos()(

0)()cos(

)(

sen

sen

RZ

Rotaciones sobre sistema de coordenadas cartesianas globales

)cos()(sen0

)(sen)cos(0

001

)(

XR

)cos(0)(sen-

010

)(sen0)cos(

)(

YR

Ax

Az

Ay

A

BBz By

Bx

Ax

Az

Ay

A

Bz

Bx

By

B

Ax

Ay

A

Bz

Bx

By

B

Az

PRP AB

BA

Representación en el espacio (IV)

Existen 12 combinaciones de rotación independientes

)(),(),( ZYX RRR

)(),(),( XZY RRR

)(),(),( YXZ RRR

)(),(),( XYZ RRR

)(),(),( ZXY RRR

)(),(),( YZX RRR

)(),(),( XYX RRR

)(),(),( YZY RRR

)(),(),( ZXZ RRR

)(),(),( XZX RRR

)(),(),( YXY RRR

)(),(),( ZYZ RRR

Ax

Az

Ay

A

BBz By

Bx

Ax

Az

Ay

A

Bz

Bx

By

B

Ax

Ay

A

Bz

Bx

By

B

Az

Representación en el espacio (V)Ángulos XYZ ó Ángulos RPY

)()()(,, XYZXYZAB RRRR

Orden de rotación: X,Y,Z

)cos()(sen0

)(sen)cos(0

001

)cos(0)(sen-

010

)(sen0)cos(

100

0)cos()(

0)()cos(

,,

sen

sen

RXYZAB

)()()()()(

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

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

,,

ccscs

sccssccssscs

sscsccsssccc

RXYZAB

Toolbox de Robótica: rpy2tr(, , )

Ángulos de balanceo (Roll), inclinación (Pitch) y orientación (Yall)

Ángulos RPY

)()()()()(

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

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

,,

ccscs

sccssccssscs

sscsccsssccc

RXYZAB

Toolbox de Robótica: rpy2tr(, , )

Ángulos RPY como producto de rotaciones

% Rotación RPY% (gamma, beta, alpha)

TB=rpy2tr(pi/6,pi/4,pi/5)*TA;

Las rotaciones se realizanalrededor de los ejes originales o globales

)cos()(sen0

)(sen)cos(0

001

)cos(0)(sen-

010

)(sen0)cos(

100

0)cos()(

0)()cos(

,,

sen

sen

RXYZAB

Representación en el espacio (VI)

Combinación de traslación y rotación

Variación angularw(rad/s)

LateralFrontal

Superior

PRPPRPP XYZAB

AB

BORGB

ABORGB

AA ,,

Giróscopo adjunto a un vehículo (, , )

Representación en el espacio (VII)

Combinación de traslación y rotación

Ax

Az

Ay

A

Bx

Bz

By

B

5

4

3

ORGBA

ORGBA

ORGBA

z

y

x

P

P

P

4

3

1

p

p

p

zB

yB

xB

8/

3/

6/

PRPPRPP XYZAB

AB

BORGB

ABORGB

AA ,,

Representación en el espacio (VIII)

Programa en Matlab

>> porg=[3 4 5 0]'porg = 3 4 5 0

>> pb=[-1 3 4 1]'pb = -1 3 4 1

>> porg+rpy2tr(pi/6,pi/3,pi/8)*pbans = 6.2810 6.0064 8.3481 1.0000

3.8

6

28.6

,, BORGB

AA PRPP XYZAB

Representación en el espacio (IX)

Ax

Az

Ay

8/

A2l

¿Posición del punto que representa el extremo del eslabón después de rotarprimero pi/2 en Y y después pi/6 en Z?

El punto está en:

76.0

84.1

0

8/sen2

8/cos2

0

p

p

p

zB

yB

xB

B

P

Después de las dos rotaciones:

)2/cos(0)2/(sen-

010

)2/(sen0)2/cos(

100

0)6/cos()6/(

0)6/()6/cos(

,

sen

sen

RYZAB

Representación en el espacio (X)

Ax

Az

Ay

8/

A2l

0.0000

1.9829

0.2611-

, BA PRP YZAB

>> pb=[0 2*cos(pi/8) 2*sin(pi/8) 1]‘>> pa=rotz(pi/6)*roty(pi/2)*pbpa = -0.2611 1.9829 0.0000 1.0000

Solución con MATLAB

Representación en el espacio (XI)

Ax

Az

Ay

A Después de rotar primero pi/2 en Y y después pi/6 en Z, se conoce que la

posición del extremo del robot es la que se muestra.

¿De donde partió?

0.0000

1.9829

0.2611-

p

p

p

zA

yA

xA

AP

Después de las dos rotaciones:

Psen

sen

P

TT

AB

100

0)6/cos()6/(

0)6/()6/cos(

)2/cos(0)2/(sen-

010

)2/(sen0)2/cos(

98.1

26.0

1RRTLa matriz R es ortogonal cuando:

Representación en el espacio (XII)

Ax

Az

Ay

A

97.1

26.0


pa = -0.2611 1.9829 0.0000 1.0000

>> pbb=roty(pi/2)'*rotz(pi/6)'*pa

Usando la traspuesta

Usando la inversa

>> pbb=inv(roty(pi/2))*inv(rotz(pi/6))*pa

pbb = 0.0000 1.8478 0.7654 1.0000

0.7654

1.8478

0.0000

, A1B PRP YZAB

Ángulos de Rotación

Ángulos (, , ) a partir de la matriz de rotación RPY

)()()()()(

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

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

,,

333231

232221

131211

ccscs

sccssccssscs

sscsccsssccc

rrr

rrr

rrr

RXYZAB

)( 31rarcsen

)90(0)( 0 c Solución degenerada

Equivalencia entre la matriz de rotación y parámetros de Euler

)arctan(33

32

r

r

)arctan(11

21

r

r

Representación en el espacio (XI)

Representación matricial de traslación y rotación

333231

232221

131211

,,

rrr

rrr

rrr

RXYZAB

ORGBA

ORGBA

ORGBA

ORGBA

z

y

x

P

P

P

P

44

ORGBA

1000

,,

x

XYZABA

B

PRT

Matriz de transformación homogénea

1000

100

010

001

ORGBA

ORGBA

ORGBA

z

y

x

AB

P

P

P

T

1000

0

0

0

333231

232221

131211

rrr

rrr

rrr

TAB

Traslación Rotación

% Se define una matriz % de transformación hogénea

TA=[ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];

% Visualización de la matrizframe(TA,'b',1);

Definición de un cuadro de referencia origen

Traslación de un sólido en el espacio

% Se define matriz de transformación hogénea

TA=[1 0 0 0; 0 1 0 0 ; 0 0 1 0; 0 0 0 1];

% Traslación en xyz

TB=transl(1,2,3)*TA;

1 0 0

0 1 0( , , )

0 0 1

0 0 0 1

x

y

z

q

qtransl x y z

q

Rotación de un sólido en el espacio

% Se define matriz de % transformación hogénea

TA=[1 0 0 0; 0 1 0 0 ; 0 0 1 0; 0 0 0 1];

% Rotación en y 45 grados

TB=roty(pi/4)*TA;

0 0

0 1 0 0( )

0 0

0 0 0 1

c s

rotys c

Matriz rotación

)cos(0)(sen-

010

)(sen0)cos(

)(

YR

Rotación y traslación de un sólido en el espacio

% Rotación en y 45 grados% y traslación xyz

TB=transl(1,2,3)*roty(pi/4)*TA;

frame(TA,'b',1.5);frame(TB,'r',1.5);axis([-1 3 -1 1 -1 5])

0

0 1 0

0

0 0 0 1

x

y

z

c s q

qT

s c q

Representación en el espacio (XII)

Inversión de la transformada

TXYZABXYZ

ABXYZ

BA RRR ,,,,,, 1

1 TT AB

BA

1000

,,,, ORGBAPRR

TT

XYZAB

TXYZ

ABB

A

Toolbox de Robótica: trinv(T)

Representación en el espacio (XIII)

Matriz de transformación homogénea

PRPPRPP XYZAB

AB

BORGB

ABORGB

AA ,,

11000

,,

11

BORGB

ABA PPRPT

P XYZABA

B

Ax

Az A

Ay

Bx Cx

By

Cy

BzCz

B CPC

PA

111

CCA PT

PTT

P AC

BC

AB

Representación en el espacio (XIV)

Ecuaciones de transformadas

Ax

Az A

AyBx

Cx

By

Cy

Bz

Cz

B

C

TTTT CO

AC

BO

AB

Oy

OOz

Ox

TTT BO

AB

AO TTT C

OAC

AO

TTTT CO

AC

AB

BO

1

Aplicación a brazos articulados

Determinar la posición con respecto a la base

2x

2z

2y 2

4

3

1

p

p

p

z3

y3

x3

3P

33y

3z

3x

l1=2

l2=3

01y

1x1z

11

23

1000

0100

00

00

11

11

01

cs

sc

T

0

0

0

ORG10P

100

0

0

11

11

01

cs

sc

R

1000

0100

00

20

22

22

12

cs

sc

T

0

0

2

ORG21P

100

02

0

2

22

12

cs

sc

R

1000

0100

00

30

33

33

23

cs

sc

T

0

0

3

ORG32P

100

0

0

33

33

23

cs

sc

R

111

303

323

12

01

0 PT

PTTT

P

Solución utilizando Matlab

4

3

1

p

p

p

z3

y3

x3

3P

33y

3z

3x

l1=2

l2=3

1

3

111

303

323

12

01

0 PT

PTTT

P

9/

8/

5/

3

2

1

>> P3=[-1 3 4 1]'P3 = -1 3 4 1

>> t=[pi/5 -pi/8 -pi/9]'t = 0.6283 -0.3927 -0.3491

>> T01=[cos(t(1,1)) -sin(t(1,1)) 0 0;sin(t(1,1)) cos(t(1,1)) 0 0; 0 0 1 0; 0 0 0 1]>> T12=[cos(t(2,1)) -sin(t(2,1)) 0 2;sin(t(2,1)) cos(t(2,1)) 0 0; 0 0 1 0; 0 0 0 1] >> T23=[cos(t(3,1)) -sin(t(3,1)) 0 3;sin(t(3,1)) cos(t(3,1)) 0 0; 0 0 1 0; 0 0 0 1]>> P0=T01*T12*T23*P3

P0 = 3.8812 4.9698 4.0000 1.0000

2

Compararse con el robot Scara

Aplicación a brazos articulados (II)

Aplicación a robótica móvil

A

Ax

Ay

4.1

5.2PB

2

B

1

By

Bx

8

3ORGBA yP

4ORGBA xP

2

1BP

1

0

2.5

1.4

1

0

2

1

1000

0

3

4

100

0)cos()(

0)()cos(

11

BA

sen

sen

PT

P AB

Aplicación a robótica móvil (II)


>> pb=[1 2 0 1]‘>> TBA=[cos(pi/8) -sin(pi/8) 0 4; sin(pi/8) cos(pi/8) 0 3; 0 0 1 0; 0 0 0 1]>> pa=TBA*pbpa = 4.1585 5.2304 0 1.0000

Ángulos de Euler Z-Y-X

Ángulos RPY: Se rota relativo al sistema {A}

Ángulos Euler: Se rota relativo al sistema {B} móvil

Ax

Ay

A

Bz

Bx

By

B

Az

Bz

Bx

By Bz

Bx

By

)cos()(sen0

)(sen)cos(0

001

)cos(0)(sen-

010

)(sen0)cos(

100

0)cos()(

0)()cos(

,,'''

sen

sen

R ZYXAB

Solución equivalente a los Ángulos RPY rpy2tr(, , )

Ángulos de Euler Z-Y-Zecho offdisp('Muestra de Ángulos de Euler Z-Y-Z')clear allTA=[1 0 0 0; 0 1 0 0 ; 0 0 1 0; 0 0 0 1];TB=eul2tr(45,0,0);frame(TA,'b',1);frame(TB,'r',1);

( , , )AB zyz

c c c s s c c s s c c s

R s c c c s s c s c c s s

s c s s c

)arctan(13

23

r

r )arccos( 33r

)()()()()(

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

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

,,

333231

232221

131211

'''

csscs

ssccscsscccs

sccssccssccc

rrr

rrr

rrr

R ZYZAB

)arctan(31

32

r

r

Equivalencia entre la matriz de rotación y parámetros de Euler

Ángulos de Rotación

0)( s Solución degenerada

Ángulos ( , , ) a partir de la matriz de rotación Z-Y-Z

Consideraciones computacionales

PRRRP DCD

BC

AB

A

PRP DAD

A 63 multiplicacionesy 42 sumas

PRRP CBC

AB

A

PRP BAB

A 27 multiplicaciones

y 18 sumas

Método A

Método B

Fuente: Craig, J.: Robótica. Tercera edición. 2006

Fundposori

Technology

Transcript of Fundposori