fundposori
TRANSCRIPT
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
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
Solución con MATLAB
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)
Solución con MATLAB
>> 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