Cinemática Inversa de Manipuladores Robóticos

SEM0317 - Aula 6
Cinemática Inversa de
Manipuladores Robóticos
Prof. Dr. Marcelo Becker
EESC - USP
Sumário da Aula
• Definições
• Solução Algébrica vs. Geométrica
• Exemplos em Robôs Industriais
• Exercícios Recomendados
• Bibliografia Recomendada
EESC-USP
© M. Becker 2007
2/54
Definições
• Cinemática Inversa:
− Sejam dadas a posição e
orientação desejadas para a
ferramenta, quais as variáveis
de junta do manipulador?
•Aplicação mais importante:
– Geração de Trajetórias...
EESC-USP
© M. Becker 2007
3/54
Definições
Existência de Soluções
sx
ax
sy
ay
sz
az
0
0
Pitch
Roll
Pos.
nx
n
0
TN =  y
nz

0
Yaw
• Dada a matriz:
s
a
n
px 
py  n s a p
=
pz  0 0 0 1 

1
• Deseja-se obter: θ1, θ2, ..., θN
EESC-USP
© M. Becker 2007
4/54
Definições
Existência de Soluções
• Tomando um Robô com 6 GDLs como exemplo:
– 16 elementos na matriz
nx
n
y
0

T6 =
nz

0
sx
ax
sy
ay
sz
az
0
0
px 
py 
pz 

1
• Deseja-se obter: θ1, θ2, ..., θ6
3 equações de rotação
independentes
6 Equações ññ-lineares
e transcendentais
3 equações de posição
Valores triviais
– Sendo N=6, temos 6 incógnitas e 12
6 equações...
EESC-USP
© M. Becker 2007
5/54
Definições
Existência de Soluções
– Número de Soluções:
– Pode haver mais de uma solução ou até mesmo,
nenhuma (Volume de trabalho)
– Pode ser trabalhosa a solução de equações não
lineares...
2 soluções!
EESC-USP
© M. Becker 2007
6/54
Definições
Existência de Soluções
Múltiplas Soluções
• Problema: Escolher uma solução...
Solução mais próxima
Obstáculos
A1
Pesos / Cargas
A2
Volume de Trabalho
Limites das Juntas
EESC-USP
© M. Becker 2007
7/54
Definições
Existência de Soluções
• Para o Robô PUMA 560, há 8 soluções para a
cinemática inversa...
– As 3 primeiras variáveis de junta (θ1,θ2,θ3) definem
posição do braço do robô
– As 3 últimas (θ4,θ5,θ6), a orientação da garra
• 4 soluções para a 3 primeiras juntas:
– Ombro à direita
– Ombro à esquerda
θ4' = θ4 + 180º
θ5' = −θ5
θ6' = θ6 + 180º
EESC-USP
© M. Becker 2007
8/54
Definições
Existência de Soluções
Número de Soluções
Quanto maior o número de parâmetros de DenavitHatenberg não nulos, maior o número de possíveis
soluções para o robô atingir a posição desejada.
Número de Soluções vs. ai ≠ 0 para um robô com 6 juntas de rotação
EESC-USP
ai
Número de Soluções
a1 = a3 = a5= 0
≤4
a3 = a5= 0
≤8
a3 = 0
≤ 16
Todos ai≠ 0
≤ 16
© M. Becker 2007
9/54
Definições
Existência de Soluções
• Como encontrar as soluções?
– Não há algoritmo genérico → Equações ñ lineares
• Deve-se encontrar todas as variáveis de junta!
• Deve-se calcular todas as soluções!
– Duas Classes:
• Analíticas: Closed-form solutions
• Numéricas: Numerical solutions (iterativas)
EESC-USP
© M. Becker 2007
10/54
Definições
Existência de Soluções
• Métodos Analíticos
• Obtêm todas as soluções
• Não trivial...
• Empregados quando um grande número de
parâmetros de Denavit-Hartenberg são nulos!
• Dois métodos:
– ALGÉBRICO
– GEOMÉTRICO
• Métodos Numéricos Iterativos
• Convergem para solução possível
• Estratégias Anti-colisão
• J-1
EESC-USP
© M. Becker 2007
11/54
Definições
Existência de Soluções
• E quando GDL = n (< 6)?
nx
n
0
Tn =  y
nz

0
sx
sy
sz
ax
ay
az
0
0
px 
py 
pz 

1
n
s
a
fç de n parâmetros!!
• E se a ferramenta for definida por 6 GDL?
– Encontrar a solução “mais próxima” possível da
desejada...
EESC-USP
© M. Becker 2007
12/54
Definições
Existência de Soluções
• E se a ferramenta for definida por 6 GDL?
– Encontrar a solução “mais próxima” possível da
desejada...
•
Estratégia:
1.
2.
Dada 0Tn obter 0Tn*,de modo que seja descrito
com os n parâmetros de junta do manipulador e
seja próximo
próximo de 0Tn .
Aplicar a cinemática inversa a 0Tn* para encontrar
as n variáveis de junta.
Ficar atento com o volume de trabalho do
manipulador
EESC-USP
© M. Becker 2007
13/54
Sumário da Aula
• Definições
• Solução Algébrica vs. Geométrica
• Exemplos em Robôs Industriais
• Exercícios Recomendados
• Bibliografia Recomendada
EESC-USP
© M. Becker 2007
14/54
Solução Algébrica
• Para o mecanismo planar de 3 links...
Parâmetros de Denavit-Hartenberg
θ2
L1
L2
Junta
Y3
θ3
Y0
x3
αi-1
ai-1
di
θi
1
0º
0
0
θ1
2
0º
L1
0
θ2
3
0º
L2
0
θ3
L3
θ1
X0
EESC-USP
− sθ i
 cθ i
 sθ cα
cθ i cα i −1
i
i −1
i −1

Ti =
 sθ i sα i −1 cθ i sα i −1

0
 0
© M. Becker 2007
0
− sα i −1
cα i −1
0

− sα i −1d i 
cα i −1d i 

1

ai −1
15/54
Solução Algébrica
− sθ i
 cθ i
 sθ cα
cθ i cα i −1
i
i −1
i −1

Ti =
 sθ i sα i −1 cθ i sα i −1

0
 0
c123
s
0
T3 =  123
 0

 0
- s123
c123
0
0
EESC-USP
0
− sα i −1
cα i −1
0

− sα i −1d i 
cα i −1d i 

1

ai −1
0
0
1
2
T3 = T1 . T2 . T3
0 L1 .c1 + L2 .c12 
0 L1 .s1 + L2 .s12 

1
0

0
1

© M. Becker 2007
16/54
Solução Algébrica
• Definindo a posição e orientação do 3º link...
cφ = c123
φ
sφ = s123
Y0
x = L1c1 + L2c(x,y)
12
y = L1s1 + L2s12
X0
EESC-USP
© M. Becker 2007
c123
s
0
T3 =  123
 0

 0
- s123
c123
0
0
cφ
s
0
T3 =  φ
0

0
0 L1 .c1 + L2 .c12 
0 L1 .s1 + L2 .s12 

1
0

0
1

- sφ
cφ
0
0
0 x
0 y 
1 0

0 1
17/54
Solução Algébrica
• Assim obtemos as equações algébricas...
x = L1c1 + L2c12  2
2
2
2
x
+
y
=
L
+
L

1
2 + 2L1L2c2
y = L1s1 + L2s12 
• Lembrando que:
c12 = c1c2 − s1s2
θ
=
atan2(s
,
c
)
2
2
2
s = c s +s c
12
1 2
x2 + y2 − L21 − L22
c2 =
2L1L2
1 2
2
2
s2 = ± 1 − c
EESC-USP
© M. Becker 2007
18/54
Solução Algébrica
• Dessa forma, para encontrar θ1:
x = k1c1 − k2s1
k1 = L1 + L2c2
y = k1s1 + k2c1
k2 = L2s2
• Sendo:
2
1
2
2
r = + k +k
k1 = r.cos γ = r.cγ
γ = atan2(k2 , k1 )
k2 = r.sin γ = r.sγ
EESC-USP
© M. Becker 2007
19/54
Solução Algébrica
• Reescrevendo as equações:
x = k1c1 − k2s1
y = k1s1 + k2c1
• Então:
x
= cos( γ + θ1 )
r
y
= sin( γ + θ1 )
r
EESC-USP
x
= cγ c1 − sγ s1
r
y
= cγ s1 + sγ c1
r
y x
( γ + θ1 ) = atan2 ,  = atan2(y, x )
r r
θ1 = atan2(y, x ) − atan2(k2 , k1 )
© M. Becker 2007
20/54
Solução Algébrica
• Sendo:
θ1 = atan2(y, x ) − atan2(k2 , k1 )
θ2 = atan2(s2 , c2 )
• Obtém-se:
θ1 + θ2 + θ3 = atan2(sφ , cφ ) = φ
EESC-USP
© M. Becker 2007
21/54
Solução Algébrica
• Obs.: uso de Redução Polinomial para resolver
equações transcendentais
θ
u = tan( )
2
1 − u2
cosθ =
1 + u2
2u
senθ =
1 + u2
Apêndice A: Livro J.J. Craig
EESC-USP
© M. Becker 2007
22/54
Solução Geométrica
• Para o mesmo mecanismo planar de 3 links...
– Aplicando a lei dos co-senos no ∆ABC:
x2 + y 2 = L21 + L22 - 2L1L2cos(180º −θ2 )
Y0
cos(180º −θ2 ) = −cos(θ2 )
B
θ2
L2
C
y
θ3
L1
A
θ1 β ψ
x
EESC-USP
© M. Becker 2007
x2 + y2 − L21 − L22
c2 =
2L1L2
X0
23/54
Solução Geométrica
• Para os ângulos β e ψ...
β = atan2(y, x)
cos ψ =
x2 + y2 + L21 − L22
2L1 x2 + y 2
Lei dos co-senos!
• Assim:
θ1 = β ± ψ
EESC-USP
θ1 + θ2 + θ3 = φ
© M. Becker 2007
24/54
Sumário da Aula
• Definições
• Solução Algébrica vs. Geométrica
• Exemplos em Robôs Industriais
• Exercícios Recomendados
• Bibliografia Recomendada
EESC-USP
© M. Becker 2007
25/54
Exemplo 1
Robô PUMA 560
• Manipulador com 6 GDLs
nx
n
0
T6 =  y
nz

0
sx
sy
sz
ax
ay
az
0
0
px 
py 
pz 

1
0
T6 = 0T1 (θ1 ).1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
[ T (θ )]
-1 0
0
1
1
. T6 =1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
EESC-USP
© M. Becker 2007
26/54
Exemplo 1
Robô PUMA 560
[ T (θ )]
. T6 =1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
 c1 s1
- s c
 1 1
0 0

0 0
0 0 nx
0 0 ny
.
1 0 nz

0 1 0
-1 0
0
1
1
EESC-USP
sx
ax
sy
sz
ay
az
0
0
© M. Becker 2007
px 
py  1
= T6
pz 

1
27/54
Exemplo 1
Robô PUMA 560
nx = c23 .(c4 .c5 .c6 − s4 .s6 ) − s23 .s5 .s6
1
 1nx
1
ny
1

T6 = 1
 nz

 0
1
ny = −s4 .c5 .c6 − c4 .s6
nz = −s23 .(c4 .c5 .c6 − s4 .s6 ) − c23 .s5 .c6
1
1
sx = -c23 .(c4 .c5 .s6 − s4 .c6 ) + s23 .s5 .s6
1
sy = s4 .c5 .s6 − c4 .c6
1
sz = s23 .(c4 .c5 .s6 + s4 .c6 ) + c23 .s5 .s6
1
ax = −c23 .c4 .s5 − s23 .c5
1
ay = s4 .s5
1
az = s23 .c4 .s5 − c23 .c5
EESC-USP
© M. Becker 2007
1
sx
1
ax
1
sy
1
ay
sz
0
1
az
0
1
1
px = −s23 .d4 + c23 .a3 + c2 .a2
1
py = d3
1
pz = -c23 .d4 − s23 .a3 − s2 .a2
px 

1
py 
1
pz 

1 
1
28/54
Exemplo 1
Robô PUMA 560
1
• Assim:
 c1 s1
- s c
 1 1
0 0

0 0
py = d3 = −s1px + c1py
0 0 nx
0 0 ny
.
1 0 nz

0 1 0
sx
ax
sy
ay
sz
0
az
0
px   1nx

py   1ny
= 1
pz   nz
 
1   0
1
sx
1
ax
1
sy
1
ay
sz
0
1
az
0
1
px 

1
py 
1
pz 

1 
1
• Faz-se a substituição trigonométrica:
px = ρ.cosφ = ρ.cφ
py = ρ.senφ = ρ.sφ
EESC-USP
onde
© M. Becker 2007
ρ = px2 + py2
φ = atan2(py , px )
29/54
Exemplo 1
Robô PUMA 560
d3 = −s1px + c1py
px = ρ.cosφ = ρ.cφ
Obtém-se:
py = ρ.senφ = ρ.sφ
EESC-USP
© M. Becker 2007
d3
c1sφ − s1cφ =
ρ
30/54
Exemplo 1
Robô PUMA 560
d3
c1sφ − s1cφ =
ρ
• Lembrando a fórmula da diferença de ângulos:
2
d3
d3
sen(φ − θ1 ) =
cos(φ − θ1 ) = ± 1 − 2
ρ
ρ
2 
d
d
φ − θ1 = atan2 3 ,± 1 − 32 
ρ

ρ


(
)
(
2
x
2
y
2
3
θ1 = atan2 py , px - atan2 d3,± p + p − d
EESC-USP
© M. Becker 2007
)
31/54
Exemplo 1
Robô PUMA 560
nx = c23 .(c4 .c5 .c6 − s4 .s6 ) − s23 .s5 .s6
1
 1nx
1
ny
1

T6 = 1
 nz

 0
1
ny = −s4 .c5 .c6 − c4 .s6
nz = −s23 .(c4 .c5 .c6 − s4 .s6 ) − c23 .s5 .c6
1
1
sx = -c23 .(c4 .c5 .s6 − s4 .c6 ) + s23 .s5 .s6
1
sy = s4 .c5 .s6 − c4 .c6
1
sz = s23 .(c4 .c5 .s6 + s4 .c6 ) + c23 .s5 .s6
1
ax = −c23 .c4 .s5 − s23 .c5
1
ay = s4 .s5
1
az = s23 .c4 .s5 − c23 .c5
EESC-USP

© M. Becker 2007
1
sx
1
ax
1
sy
1
ay
sz
0
1
az
0
1
1
px = −s23 .d4 + c23 .a3 + c2 .a2
1
py = d3
1
pz = -c23 .d4 − s23 .a3 − s2 .a2
px 

1
py 
1
pz 

1 
1
32/54
Exemplo 1
Robô PUMA 560
• Agora:
 c1 s1
- s c
 1 1
0 0

0 0
0 0 nx
0 0 ny
.
1 0 nz

0 1 0
• Assim:
sx
ax
sy
ay
sz
0
az
0
px   1nx

py   1ny
= 1
pz   nz
 
1   0
1
sx
1
ax
1
sy
1
ay
sz
0
1
az
0
1
px 

1
py 
1
pz 

1 
1
c1px + s1py = 1px = −s23 .d4 + c23 .a3 + c2 .a2
− pz =1pz = c23 .d4 + s23 .a3 + s2 .a2
EESC-USP
© M. Becker 2007
33/54
Exemplo 1
Robô PUMA 560
• Logo:
• Onde:
− s3 .d4 + c3 .a3 = K
K=
2
x
2
y
2
z
2
2
2
3
2
3
2
4
p +p +p −a −a −d −d
2a2
• Seguindo o mesmo método de solução
trigonométrica:
(
θ3 = atan2(a3, d4 ) - atan2 K,± a32 + d42 − K 2
EESC-USP
© M. Becker 2007
)
34/54
Exemplo 1
Robô PUMA 560
• Para obter θ2:
0
T6 = 0T1 (θ1 ).1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
[ T (θ )]
-1 0
0
3
2
. T6 =3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
s1c23 - s23 - a2c3  nx
 c1c23
- c s
 n
s
s
c
a
s
1 23
23
2 3   y
 1 23
.
 - s1
c1
0
- d3  nz


0
0
1  0
 0
EESC-USP
© M. Becker 2007
sx
ax
sy
sz
ay
az
0
0
px 
py  3
= T6
pz 

1
35/54
Exemplo 1
Robô PUMA 560
3
nx = c4 .c5 .c6 − s4 .s6
 3nx
3
ny
3

T6 = 3
 nz

 0
3
ny = s5 .c6
3
nz = −s4 .c5 .c6 − c4 .s6
3
sx = −c4 .c5 .s6 − s4 .c6
3
sy = -s5 .s6
3
3
sx
3
ax
3
sy
3
ay
sz
0
3
3
az
0
px 

3
py 
3
pz 

1 
3
sz = s4 .c5 .s6 − c4 .c6
3
ax = −c4 .s5
3
ay = c5
3
3
px = a3
3
py = d4
3
az = s4 .s5
EESC-USP
© M. Becker 2007
pz = 0
?
36/54
Exemplo 1
Robô PUMA 560
• Assim:
s1c23 - s23 - a2c3  nx
 c1c23
- c s
 n
s
s
c
a
s
1
23
1
23
23
2
3

.  y
 - s1
c1
0
- d3  nz


0
0
0
1

 0
sx
sy
ax
ay
sz
0
az
0
px   3nx

py   3ny
= 3
pz   nz
 
1   0
3
sx
3
sy
3
sz
0
3
3
ax
ay
3
az
0
px 

3
py 
3
pz 

1 
3
c1c23px + s1c23py − s23pz − a2c3 = a3
- c1s23px − s1s23py − c23pz + a2s3 = d4
EESC-USP
© M. Becker 2007
37/54
Exemplo 1
Robô PUMA 560
• Resolvendo o sistema de equações:
c1c23px + s1c23py − s23pz − a2c3 = a3
- c1s23px − s1s23py − c23pz + a2s3 = d4
(-a3 - a2c3 )pz + (c1px + s1py )(a2s3 − d4 )
s23 =
pz2 + (c1px + s1py )2
c23 =
(a2s3 - d4 )pz − (c1px + s1py )(a2c3 + a3 )
EESC-USP
pz2 + (c1px + s1py )2
© M. Becker 2007
38/54
Exemplo 1
Robô PUMA 560
• Como os denominadores são iguais e positivos:
(-a3 - a2c3 )pz + (c1px + s1py )(a2s3 − d4 )
s23 =
pz2 + (c1px + s1py )2
c23 =
(a2s3 - d4 )pz − (c1px + s1py )(a2c3 + a3 )
pz2 + (c1px + s1py )2
θ23 = atan2[(-a3 - a2c3 )pz − (c1px + s1py )(d4 - a2s3 ),...
(a2s3 - d4 )pz − (c1px + s1py )(a2c3 + a3 )]
EESC-USP
© M. Becker 2007
39/54
Exemplo 1
Robô PUMA 560
θ23 = atan2[(-a3 - a2c3 )pz − (c1px + s1py )(d4 - a2s3 ),...
(a2s3 - d4 )pz − (c1px + s1py )(a2c3 + a3 )]
θ2
θ2 = θ23 − θ3
• Assim:
θ3
z
n
EESC-USP
θ1
s
a
y
© M. Becker 2007
x
40/54
Exemplo 1
Robô PUMA 560
3
nx = c4 .c5 .c6 − s4 .s6
 3nx
3
ny
3

T6 = 3
 nz

 0
3
ny = s5 .c6
3
nz = −s4 .c5 .c6 − c4 .s6
3
sx = −c4 .c5 .s6 − s4 .c6
3
sy = -s5 .s6
3
3
sx
3
ax
3
sy
3
ay
sz
0
3
3
az
0
px 

3
py 
3
pz 

1 
3
sz = s4 .c5 .s6 − c4 .c6
3
3
3
3
ax = −c4 .s5
px = a3
3
ay = c5
py = d4
3
az = s4 .s5
EESC-USP
© M. Becker 2007
pz = 0



41/54
Exemplo 1
Robô PUMA 560
• Assim, para obter θ4:
s1c23 - s23 - a2c3  nx
 c1c23
- c s
 n
s
s
c
a
s
1
23
1
23
23
2
3

.  y
 - s1
c1
0
- d3  nz


0
0
0
1

 0
sx
sy
ax
ay
sz
0
az
0
px   3nx

py   3ny
= 3
pz   nz
 
1   0
3
sx
3
sy
3
sz
0
3
3
ax
ay
3
az
0
px 

3
py 
3
pz 

1 
3
c1c23 ax + s1c23ay − s23 az = −c4 s5
- s1 ax + c1 ay = s4 s5
EESC-USP
© M. Becker 2007
42/54
Exemplo 1
Robô PUMA 560
c1c23 ax + s1c23ay − s23 az = −c4 s5
- s1 ax + c1 ay = s4 s5
• Resolvendo o sistema de equações para θ5 ≠ 0:
θ4 = atan2[(-ax s1 + ay c1 ), (-ax c1c23 - ay s1c23 + az s23 )]
• Se θ5 = 0 → posição singular do manipulador...
EESC-USP
© M. Becker 2007
43/54
Exemplo 1
Robô PUMA 560
• Para obter θ5:
0
T6 = 0T1 (θ1 ).1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
[ T (θ )]
-1 0
0
4
 c1c23c4 + s1s4

- c1s23s4 + s1c4

- c1s23

0

4
. T6 = 4T5 (θ5 ).5 T6 (θ 6 )
s1c23c4 − c1s4
- s23c4
- s1c23s4 − c1c4
- s23s4
- s1s23
- c23
0
0
EESC-USP
- a2c3c4 + d3s4 − a3c4  nx

a2c3s4 + d3c4 + a3s4  ny
.

nz
a2s3 - d4

1
 0
© M. Becker 2007
sx
ax
sy
ay
sz
az
0
0
px 
py  4
= T6
pz 

1
44/54
Exemplo 1
Robô PUMA 560
4
nx = c5 .c6
 4nx
4
ny
4

T6 = 4
 nz

 0
4
ny = s6
4
nz = s5 .c6
4
4
sx = −c5 .s6
4
sx
4
ax
4
sy
4
ay
sz
0
4
4
az
0
px 

4
py 
4
pz 

1 
4
sy = c6
4
sz = −s5 .s6
4
ax = −s5
4
4
ay = 0
4
4

py = 0
4
az = c5
EESC-USP
px = 0
© M. Becker 2007
pz = 0



45/54
Exemplo 1
Robô PUMA 560
• Para θ5:
 c1c23c4 + s1s4

- c1s23s4 + s1c4

- c1s23

0

s1c23c4 − c1s4
- s23c4
- s1c23s4 − c1c4
- s23s4
- s1s23
- c23
0
0
- a2c3c4 + d3s4 − a3c4  nx

a2c3s4 + d3c4 + a3s4  ny
.
 nz
a2s3 - d4

1
 0
sx
ax
sy
ay
sz
az
0
0
px 
py  4
= T6
pz 

1
• Assim:
ax (c1c23c4 + s1s4 ) + ay (s1c23c4 + c1s4 ) − az (s23c4 ) = −s5
ax ( −c1s23 ) + ay ( −s1s23 ) + az (-c23 ) = c5
EESC-USP
© M. Becker 2007
46/54
Exemplo 1
Robô PUMA 560
• Assim:
θ5 = atan2(s5 , c5 )
• Onde:
s5 = -ax (c1c23c4 + s1s4 ) − ay (s1c23c4 + c1s4 ) + az (s23c4 )
c5 = ax ( −c1s23 ) + ay ( −s1s23 ) + az (-c23 )
EESC-USP
© M. Becker 2007
47/54
Exemplo 1
Robô PUMA 560
• Para obter θ6:
0
T6 = 0T1 (θ1 ).1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
[ T (θ )]
-1 0
0
5
 c1c23c4 + s1s4

- c1s23s4 + s1c4

- c1s23

0

5
. T6 =5T6 (θ 6 )
s1c23c4 − c1s4
- s23c4
- s1c23s4 − c1c4
- s23s4
- s1s23
- c23
0
0
EESC-USP
- a2c3c4 + d3s4 − a3c4  nx

a2c3s4 + d3c4 + a3s4  ny
.

nz
a2s3 - d4

1
 0
© M. Becker 2007
sx
ax
sy
ay
sz
az
0
0
px 
py  5
= T6
pz 

1
48/54
Exemplo 1
Robô PUMA 560
4
nx = c5 .c6
 4nx
4
ny
4

T6 = 4
 nz

 0
4
ny = s6
4
nz = s5 .c6
4
4
sx = −c5 .s6
4
sx
4
ax
4
sy
4
ay
sz
0
4
4
az
0
px 

4
py 
4
pz 

1 
4
sy = c6
4
sz = −s5 .s6
4
ax = −s5
4
4
ay = 0
az = c5
4



EESC-USP
px = 0
4
py = 0
4
© M. Becker 2007
pz = 0



49/54
Exemplo 1
Robô PUMA 560
• Para obter θ5 e θ6:
0
T6 = 0T1 (θ1 ).1T2 (θ2 ).2T3 (θ3 ).3T4 (θ 4 ).4 T5 (θ5 ).5 T6 (θ 6 )
[ T (θ )]
-1 0
0
4
 c1c23c4 + s1s4

- c1s23s4 + s1c4

- c1s23

0

4
. T6 = 4T5 (θ5 ).5 T6 (θ 6 )
s1c23c4 − c1s4
- s23c4
- s1c23s4 − c1c4
- s23s4
- s1s23
- c23
0
0
EESC-USP
- a2c3c4 + d3s4 − a3c4  nx

a2c3s4 + d3c4 + a3s4  ny
.

nz
a2s3 - d4

1
 0
© M. Becker 2007
sx
ax
sy
ay
sz
az
0
0
px 
py  4
= T6
pz 

1
50/54
Sumário da Aula
• Definições
• Solução Algébrica vs. Geométrica
• Exemplos em Robôs Industriais
• Exercícios Recomendados
• Bibliografia Recomendada
EESC-USP
© M. Becker 2007
51/54
Exercícios Recomendados
• Exercícios Recomendados:
– Todos do Livro do Craig (2005): pp. 128-134
EESC-USP
© M. Becker 2007
52/54
Sumário da Aula
• Definições
• Solução Algébrica vs. Geométrica
• Exemplos em Robôs Industriais
• Exercícios Recomendados
• Bibliografia Recomendada
EESC-USP
© M. Becker 2007
53/54
Bibliografia Recomendada
• Craig, J.C., 2005, Introduction to Robotics:
Mechanics and Control, 3rd Edition, Pearson
Education Inc., ISBN 0-201-54361-3
• Paul, R. P., 1981, Robot Manipulators. Mathematics,
Programming and Control, The MIT Press.
• Fu, K.S., Gonzales, R.C., and Lee, C.S.G., 1987,
Robotics: Control, Sensing, Vision, and Intelligence,
McGraw-Hill Int. Editions, ISBN 0-07-100421-1.
• Corke, P., Robotics Toolbox for MatLab (Release 7).
EESC-USP
© M. Becker 2007
54/54