CINEMÁTICA INVERSA

CINEMÁTICA INVERSA

Caetano Soares Fajardo ∴

Trabalho sobre cinemática inversa elaborado para disciplina de robótica do curso de engenharia de controle e automação da CEFET-RJ Uned Ni.

Docente: Fabricio Lopes e Silva Ms.c. Dicente: Caetano Soares Fajardo∴

Nova Iguaçu Junho de 2016

Sumário Metodologia .................................................................................................................................3 Problema Proposto .......................................................................................................................3 Objetivos ......................................................................................................................................3 Como o problema será resolvido ..................................................................................................4 Desenvolvimento (Resolução do problema) .................................................................................6 Bibliografia ................................................................................................................................15

Metodologia

Problema Proposto 

Obtenção das equações de cinemática inversa do manipulador antropomórfico com pulso de Euler.



Obtenção das equações de cinemática inversa do manipulador SCARA, cilíndrico ou esférico com pulso RPY.

Objetivos 

Obtenção dos ângulos das juntas do manipulador através de uma trajetória proposta no espaço, com os seguintes intervalos de ângulos das juntas:

1   0    90

(1)

 2   0    90

(2)

 3   0    180

(3)



Apresentar resultados numéricos;



Apresentar resultados gráficos;



Adaptar as transformações com os referenciais de situações reais.

Como o problema será resolvido O problema será resolvido pelo método de manipulação de matrizes inversas descrito por (Craig, 2005). O método de manipulação de matrizes inversas consiste na resolução de incógnitas geradas na igualdade entre o produto de uma dada matriz inversa de transformação homogênea para um referencial com a matriz de transformação homogênea total para o manipulador e o produto entre matrizes de transformação homogêneas a partir do índice adotado para o referencial. Na equação 4 são mostradas as operações algébrica para resolução das incógnitas para o referencial 1 para o manipulador The Ultimation PUMA 560 descritas por (Craig, 2005).

 T   0 1

T  21T  2 23T  3 34T  4 45T  5 56T  6 

1 0

1

(4)

6

Onde

 c1  s  1  0   0

s1 c1 0 0

0 0 1 0

0  r11 0 r21 0 r31  1  0

r12 r22 r32 0

r13 r23 r33 0

Px  Py  1  6T Pz   1

(5)

 s1 Px  c1 Py  d 3

(6)

Px   cos

(7)

Py  sen

(8)

Onde



P

2 x

 Py2



  A tan 2( Py , Px )

(9)

(10)

c1 s  s1c 

d3

sen(  1 ) 

d3

(11)



(12)



d  cos(  1 )   1   3  

(13)

2

d d 32  3    1  A tan 2 , 1  2    



1  A tan 2Py , Px   A tan 2 d 3 , Px2  Py2  d 32

(14)



(15)

Nesta resolução para a articulação 1 do manipulador The Ultimation PUMA 560, as matrizes de transformação homogênea foram calculadas pelo método de Denavit Hatemberg. Na resolução deste problema as matrizes de transformação homogênea não foram e nem serão calculadas pelo método de Denavit Hatemberg, entretanto a resolução das incógnitas será a mesma. Segundo (Craig, 2005), uma equação transcendental pode ser convertida para obtenção do ângulo de uma dada articulação. Na equação é mostrada uma equação transcendental, e a sua resolução é descrita nas equações 16, 17, 18, 19 e 20.

a cos  bsen  c

(16)

a(1  u 2 )  2bu  c(1  u 2 )

(17)

(a  c)u 2  2bu  (c  a)  0

(18)

(19)

b  b2  a2  c2 ac

u

 b  b2  a2  c2  ac 

  2 tan 1 

   

(20)

Para o cálculo da matriz inversa da matriz de transformação, resolve-se numericamente a equação mostrada na equação 21, por exemplo para o cálculo da matriz inversa da matriz de transformação 03T resolve-se a equação 22.

 T 0 C

1



 T 0 3



1

C 1 C

T



1 C  2 C 1

T



1

 

 01T

1

   T  T

 23T

1 1 2

1 0 1

1

(21)

(22)

Desenvolvimento (Resolução do problema) Primeiro, resolvem-se as equações de cinemática inversa mostradas na equação 23.

 T 0 1

1 0

(23)

T  61T

6

Onde

 C1  S  1  0   0

S1 C1 0 0

0  l1C1   r11 0 l1 S1  r21 1 0  r31  0 1  0

r12 r22 r32 0

r13 r23 r33 0

Px  Py  1  6T Pz   1

(24)

C1 PX  S1 Py  l1C1  l 2  l3C2

(25)

 S1 Px  C1 Py  l1 S1  l3 S 2

(26)

Pelo método geométrico descrito por (Craig, 2005), o ângulo da junta do referencial 2 para o manipulador antropomórfico em questão é mostrado na equação 29.

cos 2  

Px2  Py2  l12  l 22

(27)

2l1l 2

sen 2    1  cos 2 

(28)

 2  A tan 2sen 2 , cos 2 

(29)

Para o ângulo da junta do referencial 1, a equação 51.

 T 0 2

1 0

(30)

T  26T

6

Onde

 C12  S  12  0   0

S12 C12 0 0

0  l1C12  l 2 C 2   r11 0 l1 S12  l 2 S 2  r21  r31 1 0  0 1  0

r12 r22 r32 0

r13 r23 r33 0

Px  Py  2  6T Pz   1

(31)

C12 Px  S12 Py  l1C12  l2 C2  l3

(32)

 S12 P x C12 Py  l1 S12  l 2 S 2  0

(33)

C12 Px  l1   S12 Py  l2 C2  l3

(34)

12  1   2

(35)

S12 l1  Px   C12 Py  l 2 S 2

(36)

S12 

 l 2 S 2  C12 Py

l1  Px 

(37)

cos1 cos 2   sen1 sen 2 Px  sen( 1) cos 2   cos1 sen 2 Py  l 2 cos 2   l3 cosa  b  cosa cosb  sena senb sena  b  sena cosb  senbcosa 

 C1 Px  S1 Py  l1C1  l 2  l3 

 C1 Py  l1 S1  S1 Px  l3 

   C 2 

   S 2 

C1 Px  S1 Py  l1C1  l 2  l3C2

C1 

l3C2  l 2  S1 Py

(41)

(42)

(43)

(44)

Px  l1 

l3C2  l 2  S1 Py

Px  l1 

Py  l1 S1  S1 Px  l3 S 2

 Py2  Py l2 S1   l1  Px   l3 S 2  l3C 2   Px  l1   Px  l1  Px  l1    Py

l2 Px  l1  Px  l1  2  Py    l1  Px   Px  l1    

l3 S 2  l3C 2 S1 

(40)



(45)

(46)

(47)

sen1   cos1   1

(48)

cos(1 ) 2  1  sen(1 ) 2

(49)

cos(1 )  1  sen(1 ) 2

(50)

1  A tan 2(sen(1 ), cos(1 ))

(51)

2

2

(38)

(39)

 T 0 3

3 4 5 T T T  4 5 6

 cos 1   2   3   sen 1   2   3   0  0



 

1 0

sen  1   2   3 cos  1   2   3 0 0



(52)

T  34T 45T 56T

6

 



 

 

   



  

0  l 2 cos  2   3  l 3 cos  3  l 1 cos  1   2   3 0 l 2 sen  2   3  l 3 sen  3  l 1 sen  1   2   3 1 0 0 1









 

 

cos 1   2   3 Px  sen 1   2   3 Py  l 2 cos  2   3  l3 cos  3  l1 cos 1   2   3  0





(53)

(54)

 sen1   2   3 Px  cos1   2   3 Py  l 2 sen 2   3   l3  3  (55)  l1 sen1   2   3   0

cos1   2   3 Px  l1   sen1   2   3 Py  l2 cos 2   3   l3 cos 3 

(56)

sen1   2   3 l1  Px   cos1   2   3 Py  l2 sen 2   3   l3 sen 3  (57)

1   2  a

sena   3  

  

  

sen a cos  3  sen  3 cos a 

 l3 sen 3   l 2 sen 3   cosa3Py

(58)

(59)

l1  Px 

 

      l1  Px 

   

 l 3 sen  3  l 2 sen  3  cos a cos  3  sen a sen  3 Py

(60)

sena   3 l1  Px   cosa   3 Py  l2 sen 2   3   l3 sen 3  (61)

sena cos 3   cosa sen 3 l1  Px   cosa cos 3   sena sen 3 Py  l 2 sen 2 cos 3   cos 2 sen 3   l3 sen 3 

(62)

sen 3 cosa l1  Px   sena Py  l 2 cos 2   l3  

(63)

 sena  cos 3 l1  Px   cosa  cos 3 Py  l 2 sen 2  cos 3 

sen 3  

 sena  cos 3 l1  Px   cosa  cos 3 Py  l 2 sen 2  cos 3 

(64)

cosa l1  Px   sena Py  l 2 cos 2  l3

           cosa cos   sena sen  Px  l    sena cos   cosa sen   Py 3  1   3  3  3               l  cos  cos   sen sen    l cos  2  2   3  3  3  2  3

       sen3  sena Px  l1  cosa Py  l2sen2   l3 cos3 

cos 3 cosa  Px  l1  sena Py  l2 cos 2

(66)

 

(67)

          

      

cos  cosa  Px  l  sena Py  l cos   l 3 1 2 2 3  sena cos  l  Px  cosa cos  Py  l sen  cos  3 1 3 2 2 3 0  cosa  l  Px  sena Py  l cos   l 1 2 2 3



cos 3 cosa Px  l1   sena Py  l 2 cos 2   l 3   cosa Px  l1 

(65)

(68)

  sena l1  Px   cosa Py  l s sen 2    cos 3   0   cosa l  P   sena P  l cos   l  1 x y 2 2 3  

cos 3   0

3 

 2

rad ,

3 rad 2

(69)

(70)

Na equação 70, foi constatado que o ângulo  3 possui dois valores possíveis, o que prova uma condição de singularidade para este ângulo, portanto na programação do programa que fará a cinemática inversa será adotado o intervalo 0   3  1,4923rad , isto é, o ângulo máximo de

 3 menos 5% para que o manipulador tenha uma margem de segurança de 5% para que não

danifique a estrutura do manipulador, desta forma não podendo trabalhar no intervalo de

0   3  rad .

T 1 06T  45T 56T

(71)

1234  1   2   3   4

(72)

123  1   2   3

(73)

 234   2   3   4

(74)

 34   3   4

(75)

0 4

Adotou-se a notação de:

 cos1234   sen1234   0  0 

 

sen  1234 cos  1234 0 0

 





   

 

0  l1 cos  1234  l 3 cos  34  l 2  234 0 l1 sen  1234  l 3 sen  34  l 2 sen  234



1 0



0 1

  r11  r21  r   31   0

r12 r22 r32

r13 r23 r33

0

0

Px  Py  4 5   5T 6T Pz 1

 

1   2   3  c

(77)

2  3  d

(78)

cosc cos 4   senc sen 4 Px  senc  cos 4   cosc sen 4 Py  l1 cosc  cos 4   senc sen 4   l3 cos 3  cos 4   sen 3 sen 4   l 2 cosd  cos 4   send sen 4   0  senc  cos 4   cosc sen 4 Px  cosc  cos 4   senc sen 4 Py

 l1 senc  cos 4   cosc sen 4   l3 sen 3  cos 4   sen 4  cos 3   l 2 send  cos 4   cosd sen 4   0

(76)

(79)

(80)

sen 4  cosc Px  Py senc   l1 cosc   l3 cos 3  l 2 cosd  

(81)

cos 4 cosc Px  senc Py  l1 cosc   l3 cos 3   l 2 cosd 

(82)

cos 4  senc Px  cosc Py  l1 senc   l3 sen 3   l 2 send   0

 sen 4  senc Px  cosc Py  l1 senc   l3 sen 3   l 2 send   0

sen 4  

 cos 4  senc Px  cosc Py  l1 senc   l3 sen 3   l 2 send 

(83)

 cosc Px  Py senc   l1 cosc   l3 cos 3   l 2 cosd 

cos 4 cosc Px  senc Py  l1 cosc   l3 cos 3   l 2 cosd  

 sen 4  senc Px  cosc Py  l1 senc   l3 sen 3   l 2 send 

  cos 4  senc Px  cosc Py  l1 senc   l3 sen 3   l 2 send   cosc Px  Py senc   l1 cosc   l3 cos 3   l 2 cosd 

(84)

(85)

*

 senc P  cosc P  l senc   l sen   l send   cos cosc P  senc P  l cosc   l cos   l cosd  x

4

y

1

x

3

y

3

1

2

3

3

2

cos 4   0

 4  a tan 20  

(86)

 2

(87)

rad

Na equação 87, foi constatado que o ângulo  4 possui dois valores possíveis, o que prova uma condição de singularidade para este ângulo, portanto na programação do programa que fará a cinemática inversa será adotado o intervalo 0   4  1,4923rad , isto é, o ângulo máximo de

 4 menos 5% para que o manipulador tenha uma margem de segurança de 5% para que não danifique a estrutura do manipulador, desta forma não podendo trabalhar no intervalo de

0   4  rad .

 T  T  T  T  T 4 5

1 3 4

1 2 3

1 1 2

1 0 1

1 0

T  56T

6

(88)

 cos 12345   sen  12345   0  0 

 

sen  12345 cos  12345 0 0

 

 

 







0  l 2 cos  2345  l1 cos  12345  l 3 cos  345 0 l 2 sen  2345  l1 sen  12345  l 3 sen  345 1 0 0 1









  r11

 r21  r   31  0

r12 r22 r32

r13 r23 r33

0

0

Px  Py  5   6T Pz  1 

cos12345 Px  sen12345 Py  l2 cos 2345   l1 cos12345   l3 cos 345   0

 sen12345 Px  cos12345 Py  l2 sen 2345   l1 sen12345 l3 sen 345   0

(92)

m   2345

(93)

n   345

(94)

cos 5 cosk Px  senk Py  l 2 cosm   l1 cosk   l3 cosn 

 sen 5  senk Px  cosk Py  l 2 senm   l1 senk   l3 senn   0

 senk  cos 5   sen 5  cosk Px  cosk  cos 5   senk sen 5 Py  l 2 senm  cos 5   cosm sen 5   l1 senk  cos 5   cosk sen 5   l3 senn  cos 5   cosn sen 5   0

(90)

(91)

k  12345

cosk  cos 5   senk sen 5 Px  senk cos 5   sen 5  cosk Py  l 2 cosm  cos 5   senm sen 5   l1 cosk  cos 5   senk sen 5   l3 cosn  cos 5   senn sen 5   0

(89)

(95)

(96)

(97)

sen 5  cosk Px  senk Py  l 2 cosm   l1 cosk   l3 cosn 

 cos 5  senk Px  cosk Py  l 2 senm   l1 senk   l3 senn   0

sen 5  

(98)

 cos 5  senk Px  cosk Py  l 2 senm  l1 senk   l3 senn 

(99)

 cosk Px  senk Py  l 2 cosm  l1 cosk   l3 cosn 

 5  a tan 2( sen 5 , cos 5 ) 

 2

rad

(100)

Na equação 100, foi constatado que o ângulo  5 possui dois valores possíveis, o que prova uma condição de singularidade para este ângulo, portanto na programação do programa que fará a cinemática inversa será adotado o intervalo 0   5  1,4923rad , isto é, o ângulo máximo de  5 menos 5% para que o manipulador tenha uma margem de segurança de 5% para que não danifique a estrutura do manipulador, desta forma não podendo trabalhar no intervalo de

0   5  rad . cos1 r11  sen1 r21  cos 2345  cos 6 

(101)

 sen1 r11  cos1 r21  sen 2345  cos 6 

(102)

r11 

cos 2345  cos 6   sen1 r21 cos1 

r21 

sen 2345  cos 6   sen1 r21 cos1 

(103)

(104)

cos1234 r11  sen1234 r21  l1 cos1234   l3 cos 34   l 2 cos 234   cos 5 cos 6 

(105)

r11 

cos 2345  cos 6  sen 2345  cos 6   tg 1   tg 1 r11 cos1  cos1 

r11 1  tg 1  

cos 2345  cos 6  sen 2345  cos 6   tg 1  cos1  cos1 

(106)

(107)

 cos 2345  cos 6  sen 2345  cos 6   1  cos1234   tg 1  cos1  cos1    1  tg 1   sen 2345  cos 6     sen1234    cos  1  

(108)

 cos 2345  cos 6  sen 2345  cos 6   1   tg 1   tg 1  cos1  cos1    1  tg 1   l1 cos1234   l3 cos 34   l 2 cos 234   cos 5  cos 6 

  cos1234  cos 2345  cos1 1   tg 1 sen 2345  cos1 1           1  tg cos1 1         cos 6   sen1234  cos1 1 sen 2345      1      sen1234 tg 1  cos 1  cos 2345     1  1    sen1234 tg 1  cos 1 sen 2345   cos 5    1  tg 1     l1 cos1234   l3 cos 34   l 2 cos 234 



(109)



Bibliografia Craig, J. J. (2005). Introduction to robotics mechanics and control. Saddle River, NJ, United States of America: Pearson Education International. Niku, S. B. (2013). Introdução a robótica. Rio de Janeiro: LTC.