Exercícios Cinemática inversa

Prévia do material em texto

1 
 
PONTIFÍCIA UNIVERSIDADE CATÓLICA DO RIO GRANDE DO SUL 
FACULDADE DE ENGENHARIA 
ENGENHARIA DE CONTROLE E AUTOMAÇÃO 
 
44646-04 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 60H e o parâmetro 6d abaixo: 
 












−−−
−
−
=
1000
6,30187,005,0
505,0087,0
770010
6
0H , mmd 1006 = 
 
 
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: 










−
−=
100
010
001
3
0R , 
calcule a cinemática inversa de orientação. 
 
 
 
3. Mostre que a matriz de rotação de um punho esférico resume-se a 
 









 −
=
100
0
0
4646
4646
6
3 cs
sc
R 
quando 05 =θ . 
 
 
 
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: 
 












−−
−
−−
=
1000
6,301100
50071,071,0
770071,071,0
6
0H 










−
−=
100
010
001
3
0R 
2 
 
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 Denavit-
Hartenberg e as seguintes coordenadas para o centro do punho, determine as variáveis das juntas para 
a cinemática inversa de posição. 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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: 
 
 












−
=
1000
525097,026,0
850100
0026,097,0
6
0H 
 
 
i ai ααααi di θθθθi 
1 0 pi/2 525 θ1 * 
2 0 pi/2 0 θ2 * 
3 0 0 d3 * 0 
 4 0 -pi/2 0 θ4 * 
5 0 pi/2 0 θ5 * 
6 0 0 100 θ6 * 










=
525
750
0
cp
3 
 
8. O desenho abaixo representa um manipulador articulado cujo último sistema de coordenadas localiza-
se no centro de um punho esférico ( 4O ). 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 6d : 
 












−
−
=
1000
76,99871,0071,0
59,75235,087,036,0
5,130361,05,061,0
6
0H , mmd 1006 = 
 
 
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): 
 
740525100770150 41321 ===== ddaaa 
 
 
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 3d , e o centro do punho é 
[ ]Tccc zyx=cp . 
 
 
4 
 
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 pi/2 525 θ1 * 
2 0 pi/2 0 θ2 * 
3 0 0 d3 * 0 
 4 0 -pi/2 0 θ4 * 
5 0 pi/2 0 θ5 * 
6 0 0 100 θ6 * 














−
=
1000
55,878001
78,2265,02
30
79,3922
35,00
6
0H
5 
 
UMA BREVE EXPLICAÇÃO SOBRE AS FUNÇÕES ATAN E ATAN2: 
 
A função trigonométrica ( )xyarctan é a função inversa de ( )xytan , ou ( )xy1tan − , 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: 
 
( ) ( )( )α
α
α
cos
sin
tan = → 
( )
( )




=
α
α
α
cos
sin
arctan 
 
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: 
 
( )
( )
( )
( )




=





−
−
=
α
α
α
α
α
cos
sin
arctan
cos
sin
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. 
 
( )
o45
1arctan
1
1
arctan
=
=






−
−
=α
 
( )
o135
1,12arctan
−=
−−=α
 
 
Podemos organizar os valores da função atan2 da seguinte forma, conforme o quadrante do ângulo: 
 
( )
( )
( )
( )










==
−<=
>=
−<<
+≥<
>
=
indefinidoyx
yx
yx
xyyx
xyyx
xyx
xy
0,0
20,0
20,0
arctan0,0
arctan0,0
arctan0
,2arctan
pi
pi
pi
pi
 
 
 
6 
 
RESPOSTAS 
 
1. 










−
=










−
−










−
=




















−−
−
−
−










−
=
−=
215
0
770
87,0
5,0
0
100
6,301
50
770
1
0
0
87,005,0
5,0087,0
010
100
6,301
50
770
6
0
6
0
c
c
p
kRdp
 
 
 
2. Como 63
3
0
6
0 RRR = , pode-se isolar a matriz de rotação do punho esférico: ( ) 6013063 RRR −= . 
Definiremos também a matriz U , que corresponde genericamente a 63R . Assim: 
 
( )
( )
UR
RR
RRR
=









 −
=










−−
−
−










−=
=
=
−
:
87,005,0
5,0087,0
010
87,005,0
5,0087,0
010
100
010
001
6
3
6
0
3
0
6
0
13
0
6
3
T
 
com 










=
333231
232221
131211
uuu
uuu
uuu
U
 
 
A matriz de rotação para um punho esférico é definida pela cinemática direta deste componente: 
 










−
+−+
−−−
=
56565
546465464654
546465464654
6
3
csscs
ssccscsscccs
sccssccssccc
R . 
 
Uma matriz de rotação é dita ortonormal, já que as linhas e colunas desta matriz possuem módulo 
unitário. Assim, se 133 ±≠u , os elementos 13u , 23u , 31u e 32u serão diferentes de zero. 
 
7 
 
No caso deste exercício, temos que ( ) 87,0cos 533 == θu . Assim, como 1sincos 5252 =+ θθ , pode-se 
dizer que( ) 2335 1sin u−±=θ . Ainda, ( ) ( ) ( )555 cossintan θθθ = , ou seja, ( ) ( )( )555 cossinarctan θθθ = . 
Então: 





 ±
=








−±
=








−±
=
87,0
5,0
arctan
87,0
87,01
arctan
1
arctan
2
33
2
33
5
u
uθ
 
 
65
piθ ±= 
 
Assim, temos duas possíveis soluções para 5θ . Analisando os outros elementos de 63R , 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 ( )5sin θ positivo, tem-se que: 
( )
( ) ( )
( ) ( )
( )
( )4
4
54
54
13
23
4
cos
sin
sincos
sinsin
tan
θ
θ
θθ
θθ
θ
==
=
u
u
 
 





 −
=





=
0
5,0
arctanarctan
13
23
4
u
uθ 
 
24
piθ −= 
( )
( ) ( )
( ) ( )( )
( ) ( )
( ) ( )
( )
( )6
6
65
65
65
65
31
32
6
cos
sin
cossin
sinsin
cossin
sinsin
tan
θ
θ
θθ
θθ
θθ
θθ
θ
==
−−
=
−
=
u
u
 
 






−
=





−
=
5,0
0
arctanarctan
31
32
6
u
uθ 
 
piθ =6 
 
 
Agora, se considerarmos a solução para ( )5sin θ negativo, temos que: 
( )
( ) ( )( )( )
( ) ( )( )( )
( ) ( )
( ) ( )
( )
( )4
4
54
54
54
54
13
23
4
cos
sin
sincos
sinsin
sincos
sinsin
tan
θ
θ
θθ
θθ
θθ
θθ
θ
==
−−
−−
=
−
−
=
u
u
 
 






=





−
−
=
0
5,0
arctanarctan
13
23
4
u
uθ 
 
24
piθ = 
( )
( ) ( )( )
( )( ) ( )
( ) ( )
( ) ( )
( )
( )6
6
65
65
65
65
31
32
6
cos
sin
cossin
sinsin
cossin
sinsin
tan
θ
θ
θθ
θθ
θθ
θθ
θ
==
−−
−−
=
−
=
u
u
 
 






=




 −
=
5,0
0
arctanarctan
31
32
6
u
uθ 
 
06 =θ 
 
8 
 
Logo, temos dois conjuntos de ângulos que satisfazem a solução da cinemática inversa de orientação: 
 
( )5sin θ positivo: 







=
=
−=
0
6
2
6
5
4
θ
piθ
piθ
 ( )5sin θ negativo: 







=
−=
=
0
6
2
6
5
4
θ
piθ
piθ
 
 
 
3. Como 05 =θ , ( ) 1cos 5 =θ e ( ) 0sin 5 =θ e a matriz de rotação é ortonormal (o módulo das linhas e 
colunas é unitário), temos que: 










+−+
−−−
=
100
0
0
64646464
64646464
6
3 ccsssccs
csscsscc
R . 
 
Utilizando as seguintes identidades trigonométricas: 
 
( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( )646464
646464
sincossinsincos
cossinsincoscos
θθθθθθ
θθθθθθ
+=+
+=−
 
 
vemos que a matriz 63R se torna: 









 −
=
100
0
0
4646
4646
6
3 cs
sc
R , 
onde ( )6446 cos θθ +=c e ( )6446 sin θθ +=s . 
 
 
4. 
( )
( )
UR
RR
RRR
=










−
−−
=










−
−
−−










−=
=
=
−
:
100
071,071,0
071,071,0
100
071,071,0
071,071,0
100
010
001
6
3
6
0
3
0
6
0
13
0
6
3
T
 
 
A matriz de rotação para um punho esférico é definida pela cinemática direta deste componente: 
 










−
+−+
−−−
=
56565
546465464654
546465464654
6
3
csscs
ssccscsscccs
sccssccssccc
R . 
 
9 
 
Nota-se que, neste caso, ( ) 1cos 533 == θu . Como a matriz é ortogonal e possui linhas e colunas com 
módulo unitário, os elementos 13u , 23u , 31u e 32u serão nulos (se ( ) 1cos 5 =θ , ( ) 0sin 5 =θ ). Substituindo-
se estes valores na matriz acima, tem-se que: 
 










+−+
−−−
=
100
0
0
64646464
64646464
6
3 ccsssccs
csscsscc
R . 
 
Utilizando as seguintes identidades trigonométricas: 
 
( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( )646464
646464
sincossinsincos
cossinsincoscos
θθθθθθ
θθθθθθ
+=+
+=−
 
 
vemos que a matriz 63R se torna: 









 −
=
100
0
0
4646
4646
6
3 cs
sc
R , 
 
onde ( )6446 cos θθ +=c e ( )6446 sin θθ +=s . 
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 05 =θ graus implica em os eixos 3z e 5z 
estarem alinhados, como na figura ao lado. 
Assim, o efeito em termos de rotação em torno 
de 3z e 5z é o mesmo. Analisando 11u e 21u 
de 63R , temos que: 
( )
( )
( )64
64
11
21
64
cos
sin
tan
θθ
θθ
θθ
+
+
=
=+
u
u
 
 






=+
11
21
64 arctan
u
uθθ 
No caso deste exercício, 






−
=+
71,0
71,0
arctan64 θθ 
 
4
3
64
piθθ =+ 
 
Assim, qualquer combinação de valores de forma que 4
3
64
piθθ =+ é uma solução válida. 
10 
 
A solução completa para a cinemática inversa de orientação é, então: 
 




−=+
=
4
0
64
5
piθθ
θ
 
 
 
 
5. Observe as definições dos sistemas de coordenadas e dos parâmetros de Denavit-Hartenberg. Isto é 
essencial na determinação das equações. Sendo [ ]Tzyx ppp=cp as coordenadas do centro do 
punho, tem-se: 






=
x
y
p
p
arctan1θ , 2
arctan2
piθ +





=
r
s
 e 223 srd += , 
onde: 
1dps z −= e 
22
yx ppr += . 
 
 
6. o901 =θ o902 =θ mmd 7503 = . 
 
 
7. A matriz de rotação do punho esférico é calculada com ( ) 6013063 RRR −= , que para este exercício é 
 









 −
=
100
026,097,0
097,026,0
6
3R 
 
Note que neste caso a matriz de rotação possui o termo 133 =u , então, ( ) 1cos 5 =θ , ou o05 =θ . De acordo 
com o exercício 3, o formato de 63R reduz-se a: 
 









 −
=
100
0
0
4646
4646
6
3 cs
sc
R
 
 
Assim, apenas a soma 64 θθ + pode ser determinada. 
 
o75
arctan
11
21
64
=






=+
u
uθθ
 
 
8. Respostas omitidas. 
 
11 
 
9. 










=










−
−










=
−=
76,1069
59,717
5,1242
71,0
35,0
61,0
100
76,998
59,752
5,1303
6
06
6
0
c
c
p
kRdp d
 
 
 
 
 
10. o301 =θ , o452 =θ , o453 =θ , o04 =θ , o455 −=θ , o06 =θ . 
 
 
 
11. 
22
yx ppr +=
D
aa
aar
:
2
cos 2
2
2
1
2
2
2
1
2
2 =
−−
=θ
2
2 1sin D−±=θ








−±
=
D
D2
2
1
arctanθ
αφθ −=1 





=
x
y
p
p
arctgφ 





+
=
221
22
cos
sin
θ
θ
α
aa
a
arctg
zpdd −= 13
12 
 
4
3
255,353
55,353
arctan
2
arctan
2
2
piθ
pi
piθ
=
+





=
+





=
r
s
6
19,306
78,176
arctan
arctan
1
1
piθ
θ
=






=








=
y
x
p
p










=












−









=
























−
−










=
−=
55,878
78,176
19,306
0
5,0
2
3
100
55,878
78,226
79,392
1
0
0
001
5,02
30
2
35,00
100
55,878
78,226
79,392
6
0
6
0
c
c
p
kRdp
500
250000
55,35355,353
3
22
22
3
=
=
+=
+=
d
rsd
12. 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
Com 61 piθ = , 432 piθ = e 5003 =d , temos que a matriz de rotação correspondente às três primeiras 
variáveis das juntas é: 
 












−−
−
=
























−














−
=
=
2
202
2
4
2
2
3
4
2
61,05,061,0
100
010
001
010
2
202
2
2
202
2
010
2
305,0
5,002
3
3
0
3
2
2
1
1
0
3
0
R
RRRR
 
 
( )












−
=














−














−
−
−
=
=
−
2
202
2
010
2
202
2
001
5,02
30
2
35,00
2
2
4
261,0
02
35,0
2
2
4
261,0
6
0
13
0
6
3 RRR
 
13 
 
Tendo 63R , podemos resolver a cinemática inversa de orientação. 
 












−
=










=
2
202
2
010
2
202
2
333231
232221
131211
uuu
uuu
uuu
U 
 
 
( ) ( )
4
1arctan
22
22
arctan
22
221
arctan
1
arctan
5
2
33
2
33
5
piθ
θ
±=
±=







 ±
=










−±
=








−±
=
u
u
 
 
 
 
Resolvendo 4θ e 6θ para ( )5sin θ positivo: 
 
 
piθ
θ
=








−
=





=
4
13
23
4 22
0
arctanarctan
u
u
 
 
piθ
θ
=








−
=





−
=
6
31
32
6 22
0
arctanarctan
u
u
 
 
 
 
Agora, se considerarmos a solução para ( )5sin θ negativo: 
 
 
0
22
0
arctanarctan
4
13
23
4
=








=





−
−
=
θ
θ
u
u
 
 
0
22
0
arctanarctan
6
31
32
6
=








=




 −
=
θ
θ
u
u
 
 
 
 
 
Temos então duas soluções possíveis, dependendo do valor de 5θ : 
 
 










=
=
=
=
=
=
piθ
piθ
piθ
piθ
piθ
6
5
4
3
2
1
4
500
4
3
6
d
 
 










=
−
=
=
=
=
=
0
4
0
500
4
3
6
6
5
4
3
2
1
θ
piθ
θ
piθ
piθ
d