PONTIFÍCIA UNIVERSIDADE CATÓLICA DO RIO GRANDE DO SUL
FACULDADE DE ENGENHARIA
ENGENHARIA MECÂNICA
4444W-02 SISTEMAS ROBOTIZADOS
Prof. Felipe Kühne
Lista de Exercícios 3 - Cinemática Inversa
1. Determine o centro do punho de um manipulador de seis graus de liberdade, dada a matriz de
cinemática direta H 60 e o parâmetro d 6 abaixo:
−1
0
770 
 0
− 0,87 0
0,5
50 
, d 6 = 100mm
H 60 = 
 − 0,5 0 − 0,87 − 301,6


0
0
1 
 0
2. Considerando que o manipulador da questão anterior possui um punho esférico nas suas três últimas
juntas e sabendo que as três primeiras juntas possuem ângulos correspondentes à seguinte matriz de
rotação:
1 0 0 
3
R 0 = 0 − 1 0  ,
0 0 − 1
calcule a cinemática inversa de orientação.
3. Mostre que a matriz de rotação de um punho esférico resume-se a
c 46
R =  s 46
 0
6
3
− s 46
c 46
0
0
0
1
quando θ 5 = 0 .
4. Dada a matriz de transformação homogênea de um manipulador de seis graus de liberdade e a matriz
de rotação para a cinemática inversa de posição abaixo, determine a solução para a cinemática inversa
de orientação:
−1
0
770 
 0
− 0,87 0
0,5
50 
6

H0 =
 − 0,5 0 − 0,87 − 301,6


0
0
1 
 0
1 0 0 
R = 0 − 1 0 
0 0 − 1
3
0
1
5. O desenho abaixo representa um manipulador esférico com as suas três primeiras juntas. Apresente as
equações que definem a cinemática inversa de posição para este robô.
6. Considerando que o manipulador do exercício anterior possui os seguintes parâmetros de DenavitHartenberg e as seguintes coordenadas para o centro do punho, determine as variáveis das juntas para
a cinemática inversa de posição.
i
ai
αi
di
θi
1
0
π/2
525
θ1 *
2
0
π/2
0
θ2 *
3
0
0
d3 *
0
4
0
-π/2
0
θ4 *
5
0
π/2
0
θ5 *
6
0
0
100
θ6 *
 0 
p c = 750 
525 
7. Para o mesmo manipulador do exercício 5, considere agora um punho esférico anexado ao seu corpo,
formando um manipulador de 6GDL. Para as variáveis das juntas encontradas no exercício anterior,
calcule a cinemática inversa de orientação, dada a seguinte matriz de transformação homogênea total:
0,97 0,26
 0
0
6
H0 = 
0,26 − 0,97

0
 0
0
0 
1 850
0 525

0 1 
2
8. O desenho abaixo representa um manipulador articulado cujo último sistema de coordenadas localizase no centro de um punho esférico ( O4 ). Determine as expressões para a cinemática inversa de
posição deste robô.
9. Para o manipulador do exercício anterior, determine o centro do punho, dada a seguinte matriz de
transformação homogênea e o parâmetro d 6 :
0,5
0,61 1303,5 
 0,61
0,36 − 0,87 0,35 752,59
,
H 60 = 
 0,71
0
− 0,71 998,76


0
0
1 
 0
d 6 = 100mm
10. Para o manipulador do exercício 8, calcule a cinemática inversa de posição, dados os seguinte
parâmetros construtivos do robô (grandezas em milímetros):
a1 = 150 a 2 = 770 a3 = 100 d1 = 525 d 4 = 740
11. Apresente as equações para a cinemática inversa de posição para o manipulador SCARA da figura
abaixo, sabendo que as variáveis das juntas são θ1 , θ 2 e d 3 , e o centro do punho é
p c = [x c
yc
zc ] .
T
3
12. Para o seguinte manipulador esférico, apresente pelo menos uma solução possível para a cinemática
inversa de posição e orientação.
i
ai
αi
di
θi
1
0
π/2
525
θ1 *
2
0
π/2
0
θ2 *
3
0
0
d3 *
0
4
0
-π/2
0
θ4 *
5
0
π/2
0
θ5 *
6
0
0
100
θ6 *
0
0,5

 − 3
H 60 = 0
2
1
0

0
0
3
2
0,5
0
0
392,79 

226,78
878,55 

1 
4
UMA BREVE EXPLICAÇÃO SOBRE AS FUNÇÕES ATAN E ATAN2:
A função trigonométrica arctan( y x ) é a função inversa de tan ( y x ) , ou tan −1 ( y x ) , onde y é o cateto
oposto e x é o cateto adjacente de um triângulo retângulo formado por θ .
Uma outra forma de escrever esta função é através do seno e cosseno:
tan (α ) =
 sin (α ) 
sin (α )
→ α = arctan

cos(α )
 cos(α ) 
Se o numerador e o denominador são positivos, o ângulo α encontra-se no primeiro quadrante; se o
numerador é negativo e o denominador é positivo, α encontra-se no segundo quadrante; se ambos
numerador e denominador são positivos, α encontra-se no terceiro quadrante; por fim, se o numerador é
positivo e o denominador é negativo, α encontra-se no quarto quadrante.
No caso de ambos numerador e denominador serem negativos, teremos que:
 − sin (α ) 
 sin (α ) 
 = arctan

 − cos(α ) 
 cos(α ) 
α = arctan
Ou seja, o ângulo na realidade encontra-se no quarto quadrante, mas esta informação é perdida quando do
cálculo do arco-tangente. O ângulo calculado desta forma está incorreto.
A função atan2 leva em consideração então os sinais dos dois argumentos, colocando o resultado no
quadrante correto. Por exemplo, os dois cálculos abaixo utilizam argumentos -1 para o numerador e -1
para o denominador. Fica óbvio que o resultado correto apenas é obtido ao usar a função atan2.
 −1

 −1
= arctan(1)
α = arctan
α = arctan 2(− 1,−1)
= −135 o
= 45 o
Podemos organizar os valores da função atan2 da seguinte forma, conforme o quadrante do ângulo:
arctan( y x )
 x>0
 x < 0, y ≥ 0 arctan( y x ) + π

 x < 0, y < 0 arctan( y x ) − π
arctan 2( y, x ) = 
π 2
 x = 0, y > 0
 x = 0, y < 0
−π 2

 x = 0, y = 0
indefinido
5
RESPOSTAS
1.
p c = d 60 − R 60 k
−1
0  0
 770 
 0



=  50  − 100  − 0,87 0
0,5  0
− 301,6
 − 0,5 0 − 0,87  1
 770 
 0 


=  50  − 100  0,5 
− 301,6
 − 0,87 
 770 
p c =  0 
− 215
−1
2. Como R 60 = R 30 R 36 , pode-se isolar a matriz de rotação do punho esférico: R 36 = (R 30 ) R 60 .
Definiremos também a matriz U , que corresponde genericamente a R 36 . Assim:
−1
( )
= (R )
R 36 = R 30
3 T
0
R 60
R 60
−1
0 
1 0 0  0



0,5 
= 0 − 1 0 − 0,87 0
0 0 1   − 0,5 0 − 0,87 
−1 0 
 0

6
R 3 = 0,87 0 0,5  := U
 0,5 0 0,87 
com
u11 u12
U = u 21 u 22
u 31 u 32
u13 
u 23 
u 33 
A matriz de rotação para um punho esférico é definida pela cinemática direta deste componente:
c 4 c 5 c 6 − s 4 s 6
R =  s 4 c5 c6 + c 4 s 6
 − s5 c6
6
3
− c 4 c5 s 6 − s 4 c 6
− s 4 c5 s 6 + c 4 c 6
s5 s6
c4 s5 
s 4 s5  .
c5 
Uma matriz de rotação é dita ortonormal, já que as linhas e colunas desta matriz possuem módulo
unitário. Assim, se u 33 ≠ ±1 , os elementos u13 , u 23 , u 31 e u 32 serão diferentes de zero.
6
No caso deste exercício, temos que u 33 = cos(θ 5 ) = 0,87 . Assim, como cos 2 θ 5 + sin 2 θ 5 = 1 , pode-se
2
dizer que sin (θ 5 ) = ± 1 − u 33
. Ainda, tan (θ 5 ) = sin (θ 5 ) cos(θ 5 ) , ou seja, θ 5 = arctan(sin (θ 5 ) cos(θ 5 )) .
Então:
 ± 1− u2 
33 
θ 5 = arctan


u 33


 ± 1 − 0,87 2
= arctan

0,87


 = arctan ± 0,5 

 0,87 

θ5 = ±π 6
Assim, temos duas possíveis soluções para θ 5 . Analisando os outros elementos de R 36 , vemos que o valor
de θ 5 vai influenciar também nos valores de θ 4 e θ 6 . Ou seja, dependendo de qual solução escolhermos
para θ 5 , teremos valores diferentes para θ 4 e θ 6 .
Resolvendo θ 4 e θ 6 para sin (θ 5 ) positivo, tem-se que:
u
tan (θ 4 ) = 23
u13
=
sin (θ 4 )sin (θ 5 ) sin (θ 4 )
=
cos(θ 4 )sin (θ 5 ) cos(θ 4 )
u 
 − 0,5 
θ 4 = arctan 23  = arctan

 0 
 u13 
θ 4 = −π 2
tan (θ 6 ) =
u 32
− u 31
=
sin (θ 5 )sin (θ 6 )
− (− sin (θ 5 ) cos(θ 6 ))
=
sin (θ 5 )sin (θ 6 ) sin (θ 6 )
=
sin (θ 5 ) cos(θ 6 ) cos(θ 6 )
 u 32 
 0 
 = arctan

 − 0,5 
 − u 31 
θ 6 = arctan
θ6 = 0
Agora, se considerarmos a solução para sin (θ 5 ) negativo, temos que:
− u 23
− u 32
tan (θ 4 ) =
tan (θ 6 ) =
− u13
u 31
=
− (sin (θ 4 )(− sin (θ 5 )))
− (cos(θ 4 )(− sin (θ 5 )))
=
− (− sin (θ 5 )sin (θ 6 ))
− (− sin (θ 5 )) cos(θ 6 )
=
sin (θ 4 )sin (θ 5 ) sin (θ 4 )
=
cos(θ 4 ) sin (θ 5 ) cos(θ 4 )
=
sin (θ 5 )sin (θ 6 ) sin (θ 6 )
=
sin (θ 5 ) cos(θ 6 ) cos(θ 6 )
 − u 23 
0,5 
 = arctan

 0 
 − u13 
θ 4 = arctan
θ4 = π 2
 − u 32
 u 31
θ 6 = arctan

 0 
 = arctan

 0,5 

θ6 = 0
7
Logo, temos dois conjuntos de ângulos que satisfazem a solução da cinemática inversa de orientação:
sin (θ 5 )
θ 4 = − π
2


positivo: θ 5 = π
6

=
0
θ
 6
3. Como θ 5 = 0 , cos(θ 5 ) = 1 e sin (θ 5 ) = 0
colunas é unitário), temos que:
c 4 c 6
6
R 3 =  s 4 c6

sin (θ 5 )
θ 4 = π
2


negativo: θ 5 = − π
6

=
0
θ
 6
e a matriz de rotação é ortonormal (o módulo das linhas e
− s4 s6
+ c4 s6
0
− c 4 s 6 − s 4 c6
− s 4 s 6 + c 4 c6
0
0
0 .
1
Utilizando as seguintes identidades trigonométricas:
cos(θ 4 ) cos(θ 6 ) − sin (θ 4 ) sin (θ 6 ) = cos(θ 4 + θ 6 )
cos(θ 4 )sin (θ 6 ) + sin (θ 4 ) cos(θ 6 ) = sin (θ 4 + θ 6 )
vemos que a matriz R 36 se torna:
c 46
R =  s 46
 0
= sin (θ 4 + θ 6 ) .
6
3
onde c 46 = cos(θ 4 + θ 6 ) e s 46
− s 46
c 46
0
0
0 ,
1
4.
−1
( )
= (R )
R 36 = R 30
3 T
0
R 60
R 60
1 0
= 0 − 1
0 0
− 0,71
6
R 3 =  0,71
 0,5
0 − 0,71 − 0,71 0 
0 − 0,71 0,71
0 
1  0
0
− 1
− 0,71 0
− 0,71 0 := U
0
1
A matriz de rotação para um punho esférico é definida pela cinemática direta deste componente:
c 4 c 5 c 6 − s 4 s 6
R =  s 4 c5 c6 + c 4 s 6
 − s5 c6
6
3
− c 4 c5 s 6 − s 4 c 6
− s 4 c5 s 6 + c 4 c 6
s5 s6
c4 s5 
s 4 s5  .
c5 
8
Nota-se que, neste caso, u 33 = cos(θ 5 ) = 1 . Como a matriz é ortogonal e possui linhas e colunas com
módulo unitário, os elementos u13 , u 23 , u 31 e u 32 serão nulos (se cos(θ 5 ) = 1 , sin (θ 5 ) = 0 ). Substituindose estes valores na matriz acima, tem-se que:
c 4 c 6 − s 4 s 6
R =  s 4 c6 + c 4 s 6

0
− c 4 s 6 − s 4 c6
− s 4 s 6 + c 4 c6
0
6
3
0
0 .
1
Utilizando as seguintes identidades trigonométricas:
cos(θ 4 ) cos(θ 6 ) − sin (θ 4 ) sin (θ 6 ) = cos(θ 4 + θ 6 )
cos(θ 4 )sin (θ 6 ) + sin (θ 4 ) cos(θ 6 ) = sin (θ 4 + θ 6 )
vemos que a matriz R 36 se torna:
c 46
R =  s 46
 0
− s 46
c 46
0
6
3
0
0 ,
1
onde c 46 = cos(θ 4 + θ 6 ) e s 46 = sin (θ 4 + θ 6 ) .
Por este motivo, não é possível o cálculo dos
ângulos θ 4 e θ 6 individualmente, mas
apenas a soma deles.
O significado físico disto é de simples
entendimento, visto que um ângulo na junta 5
de θ 5 = 0 graus implica em os eixos z 3 e z 5
estarem alinhados, como na figura ao lado.
Assim, o efeito em termos de rotação em torno
de z 3 e z 5 é o mesmo. Analisando u11 e u 21
de R 36 , temos que:
tan (θ 4 + θ 6 ) =
=
u 21
u11
sin (θ 4 + θ 6 )
cos(θ 4 + θ 6 )
 u 21 

 u11 
θ 4 + θ 6 = arctan
No caso deste exercício,
 0,71 

 − 0,71 
θ 4 + θ 6 = arctan
θ4 + θ6 = −π 4
Assim, qualquer combinação de valores de forma que θ 4 + θ 6 = − π
4
é uma solução válida.
9
A solução completa para a cinemática inversa de orientação é, então:
θ5 = 0


θ 4 + θ 6 = − π 4
5. Observe as definições dos sistemas de coordenadas e dos parâmetros de Denavit-Hartenberg. Isto é
T
essencial na determinação das equações. Sendo p c = p x p y p z as coordenadas do centro do
punho, tem-se:
 py 
s π
θ 1 = arctan  , θ 2 = arctan  +
e d3 = r 2 + s 2 ,
r 2
 px 
onde:
[
p x2 + p 2y .
s = p z − d1 e r =
6. θ 1 = 90 o
]
θ 2 = 90 o d 3 = 750mm .
−1
( )
7. A matriz de rotação do punho esférico é calculada com R 36 = R 30
R 60 , que para este exercício é
0,26 − 0,97 0
R = 0,97 0,26 0
0
1
 0
6
3
Note que neste caso a matriz de rotação possui o termo u 33 = 1 , então, cos(θ 5 ) = 1 , ou θ 5 = 0 o . De acordo
com o exercício 3, o formato de R 63 reduz-se a:
c 46
R =  s 46
 0
6
3
− s 46
c 46
0
0
0
1
Assim, apenas a soma θ 4 + θ 6 pode ser determinada.
 u 21 

 u11 
θ 4 + θ 6 = arctan
= 75 o
8. Respostas omitidas.
10
9.
p c = d 60 − d 6 R 60 k
1303,5 
 0,61 


= 752,59 − 100  0,35 
998,76
− 0,71
 1242,5 
p c =  717,59 
1069,76
10. θ 1 = 30 o , θ 2 = 45 o , θ 3 = 45 o , θ 4 = 0 o , θ 5 = −45 o , θ 6 = 0 o .
11.
p x2 + p 2y
r=
r 2 − a12 − a22
cos θ 2 =
=: D
2a12 a22
sin θ 2 = ± 1 − D 2
 ± 1− D2

D

θ 2 = arctan
θ1 = φ − α
 py 

 px 
φ = arctg 





a2 sin θ 2 

 a1 + a2 cos θ 2 
α = arctg 
d 3 = d1 − p z
11
p c = d 60 − R 60k
12.
0
0,5

392,79 

=  226,78 − 100 0 − 3
2
1
878,55 
0


 3 
392,79 
 2
=  226,78 − 100  0,5 


878,55 
 0 
306,19 
p c = 176,78 
878,55
 px 


p
 y
 306,19 
= arctan 

 176,78 
s π
r 2
 353,55  π
= arctan 
+
 353,55  2
θ 1 = arctan 
θ1 =
3 
2  0 

0,5  0
0  1 


d3 = s 2 + r 2
θ 2 = arctan   +
π
θ2 =
6
= 353,55 2 + 353,55 2
= 250000
d 3 = 500
3π
4
Com θ 1 = π 6 , θ 2 = 3π 4 e d 3 = 500 , temos que a matriz de rotação correspondente às três primeiras
variáveis das juntas é:
R 30 = R 10 R 12 R 32
 3
 2

=  0,5
 0



 − 0,61
3
R 0 = − 2
4

 2
2

−1
( )
R 36 = R 30
0,5   − 2

2
 2
−
3
0
2 
2
1
0  0
 

0
0,5
− 3
0
2
0
0
1
2 
2  1 0 0 
2  0 1 0 

2 
0  0 0 1


0,61 
2 
4
2 
2
R 60
− 0,61 − 2

4

−
3
=  0,5
2

2
 0,61
4
2  0
0,5
2 

0  0 − 3
2
0
2  1
2  
3  
2  2
2

0,5  =  0

0   2
  2

0 − 2 
2
1
0 
2 
0
2 
12
Tendo R 63 , podemos resolver a cinemática inversa de orientação.
u11 u12
U = u 21 u 22
u 31 u 32
 ± 1− u2
33
θ 5 = arctan

u 33

u13   2
2
u 23  =  0

u 33   2
 2
0 − 2 
2
1
0 
2 
0
2 
2 


 = arctan ± 1 − 2 2  = arctan ± 2 2  = arctan(± 1)


 22 

2 2







(
)
θ5 = ±π 4
Resolvendo θ 4 e θ 6 para sin (θ 5 ) positivo:
 0 
 u 23 

 = arctan
− 2 2
 u13 


 0 
 u 32 

 = arctan
− 2 2
 − u 31 


θ 4 = arctan
θ 6 = arctan
θ4 = π
θ6 = π
Agora, se considerarmos a solução para sin (θ 5 ) negativo:
 0
 − u 23 
 = arctan
 2
 − u13 

θ 4 = arctan


2 
 0
 − u 32 
 = arctan
 2
 u 31 

θ 6 = arctan


2 
θ6 = 0
θ4 = 0
Temos então duas soluções possíveis, dependendo do valor de θ 5 :
 θ1 = π
6

θ 2 = 3π
4

d 3 = 500

 θ4 = π
θ = π
4
 5
 θ 6 = π
 θ1 = π
6

 θ 2 = 3π
4

d 3 = 500

 θ4 = 0
θ = − π
4
 5
 θ 6 = 0
13