53

CAPTULO 4

4. CINEMTICA INVERSA DO MANIPULADOR ROBOTHRONNo captulo anterior desenvolveu-se o procedimento para determinar a posio e a orientao do efetuador final, dadas as variveis das juntas. Agora ser examinado o procedimento inverso para determinar as variveis das juntas, dada uma posio e orientao desejadas para o dispositivo terminal. O problema da cinemtica inversa importante porque as tarefas de movimentao so naturalmente estabelecidas em termos da posio e da orientao desejadas. Os modelos cinemtico direto e inverso fazem parte do algoritmo de controle do rob industrial. Estes so includos nas malhas de servo controle e utilizados no comando e interpolao de trajetria. O problema da cinemtica inversa mais difcil que o problema da cinemtica direta porque no h uma soluo sistemtica aplicvel a todos os robs [14]. A questo da cinemtica inversa consiste em determinar-se o conjunto de ngulos e/ou deslocamentos das juntas que fornecem a posio e orientao especificadas para o efetuador final. Em geral, o problema da cinemtica inversa pode ser resolvido por vrios mtodos, tais como as transformaes inversas, lgebra de screw, matrizes duais, quartenions duais , mtodo iterativo, aproximaes geomtricas[25], e a soluo desacoplada de Piper [4]. A cinemtica de uma junta do manipulador foi definida em funo dos parmetros de Denavit-Hartenberg como

ci s i Ti 1 ( ai , i , di ,i ) = i 0 0

si c i c i c i s i 0

si s i ci s i c i 0

ai c i ai s di 1

(4.1)

e a cinemtica direta do manipulador foi obtida atravs do produto das matrizes de cada uma das juntas como mostra a equao (4.2)

54

T06 = T01 T12 T23 T34 T45 T56

(4.2)

O resultado deste produto representado pela matriz de transformao homognea cujos elementos so 12 equaes no lineares e por comodidade, os elementos da matriz de orientao sero denominados rij a partir deste ponto, como mostra a equao (4.3)

r11 r12 r r 6 H = T0 = 21 22 r31 r32 0 0

r13 r23 r33 0

px py pz 1

(4.3)

Assumindo que as dimenses do rob, sua posio e orientao so conhecidas, ento a matriz H obtida na anlise da cinemtica direta que d a posio e orientao do efetuador final uma matriz numrica. A matriz de transformao homognea contm o vetor posio e sua orientao representados na equao (4.4) ;

R0n H =T = 06 0

n p0 1

(4.4)

onde

R0n descreve a orientao do efetuador final,n p0 expressa as coordenadas do vetor posio do efetuador final e

0 = [0 0 0] .

A equao (4.2) da cinemtica direta uma seqncia de transformaes homogneas para as articulaes do manipulador, que inicia na base e seque at o efetuador final. O grafo da Figura 40 ilustra esta seqncia transformaes realizadas na equao. 2

1 T 0 T

T

T

3 T 4 T

T Figura 40 Grafo representando as multiplicaes das matrizes homogneas

6

5

55 A matriz de transformao homognea do manipulador, dada pelo termo no lado esquerdo na equao (4.2), representada pela seta verde que segue no sentido anti-horrio do grafo uma matriz numrica que representa a posio e a orientao do efetuador final. O lado direito da mesma equao (4.2), ilustrado pela seta azul que segue no sentido horrio e representa os produtos das matrizes das transformaes homogneas de cada articulao do manipulador e seus elementos so funes das variveis das juntas. O lado esquerdo da equao (4.2) representa o vetor posio que vai da base do rob, cujo sistema coordenado tem a origem O0, at o ponto central do efetuador final, no sistema coordenado com origem O6, representado na Figura 41 .X3

d4X2 Z5 Z3

Z5

d6 O6 X6 Y6 Z6

Y2 Z2

a2Y1

Z1

X1

Z0 d1 O0 Y0 X0

Figura 41 Representao das articulaes do manipulador Robothron Os valores numricos dos parmetros D-H,representados por d1, a2, d4 e d6 podem ser obtidos da Tabela 3.2.

4.1 CINEMTICA INVERSA DESACOPLADAA obteno da cinemtica inversa no tarefa fcil para a maioria dos manipuladores e no pode ser obtida atravs de simples multiplicaes matriciais. No entanto, nos manipuladores com seis graus de liberdade, onde os trs ltimos eixos consecutivos se interceptam num ponto, possvel desacoplar o problema da cinemtica inversa em dois problemas simples [4], conhecidos respectivamente, como cinemtica da posio inversa, e cinemtica da orientao inversa [1]. Os trs ltimos eixos consecutivos

56 interceptados num ponto constituem o chamado punho esfrico, que difcil de ser implementado mecanicamente. Desta forma a soluo da cinemtica inversa segue trs passos: A decomposio do manipulador no punho; A determinao dos ngulos das trs primeiras juntas; A determinao dos trs ngulos do efetuador final.

A decomposio do manipulador no punho o passo mais importante deste procedimento que se inicia com a localizao do centro do punho [26]. Na Figura 42, o vetor p que determina a posio do efetuador final conhecido e d6 tambm; j o vetor p w que representa a posio do centro do punho do manipulador pode ser obtido atravs da soma vetorial de p e o vetor de orientao do punho. d6X3 X2 Y2 Z2 Z3 Z4 Z5

X6 pw Y6

Z6

Y1

Z1

X1

p

Z0 Y0 X0

Figura 42 - Representao grfica dos vetores de posio do punho e efetuador final Desta forma, o vetor p w e a matriz de orientao do sistema do centro do punho possibilitam determinar as variveis das juntas encarregadas da posio do punho [26].

4.2 SOLUO DA CINEMTICA INVERSA PARA 1Ps multiplicando-se o lado esquerdo da equao (4.2) obtm-se a equao (4.5).

T06 T56

( )

1

= T01 T12 T23 T34 T45

(4.5)

57 As operaes realizadas pela equao (4.5) so ilustradas pelo grafo da Figura 43 ilustra onde a seta verde, no sentido anti-horrio representa o produto do lado esquerdo e a seta azul no sentido horrio, representa o lado direito desta equao. 1 T 0 T 6 5 T 2 T 3 T 4 T

T Figura 43 Grafo representativo da equao (4.5) O produto da multiplicao das matrizes do lado esquerdo da equao (4.5) apresentado na equao (4.6).

r11c6 r12 s6 r c r s 6 6 1 T0 (T5 ) = 21 6 22 6 r31c6 r32 s6 0

r11s6 + r12 c6 r21s6 + r22 c6 r31s6 + r32 c6 0

r13 r23 r33 0

px r13 d 6 p y r23 d 6 pz r33d 6 1

(4.6)

A quarta coluna da equao (4.6) o vetor p w , cujos elementos so valores numricos e referem-se s coordenadas do centro do punho do manipulador. Esta posio a origem O5 do sistema coordenado localizada no centro do punho. A Figura 44 exibe a vista superior do modelo do manipulador com as coordenadas numricas (O5)x e (O5)y desta origem. O5 d4 Y0 a2 (O5)y

1X0 (O5)x

Figura 44 Vista superior do modelo como punho desacoplado O produto do lado direito da equao (4.5) est na equao B1 do Apndice B e as

58 trs primeiras linhas da quarta coluna da equao B1, mostradas na equao (4.7) , tambm representam as coordenadas da posio do centro do punho em funo das variveis das trs primeiras juntas. Igualando os elementos da quarta coluna da matriz da equao (4.6) aos elementos da quarta coluna da matriz da equao B1,obtm-se:

( O5 ) a c c + d c s x 4 1 23 2 1 2 ( O5 ) y = a2 s1c2 + d 4 s1s23 ( O5 ) z d1 + a2 s2 d 4 c23

(4.7)

Para isolar a varivel da primeira junta, o ngulo q1, divide-se (O5)y por (O5)x no lado esquerdo da equao (4.7) e as mesmas linhas no lado direito, como mostra a equao (4.8)

1 = atan2

r23d 6 d y r13 d 6 d x

(4.8)

Deduzida a equao do ngulo da articulao da base do manipulador calcula-se o seu valor numrico.

4.3 SOLUO DA CINEMTICA INVERSA PARA 3 E 2Com a deduo do ngulo de rotao da base, a obteno dos ngulos das articulaes do cotovelo e do ombro do manipulador podem ser obtidas a partir de uma anlise da Figura 45. O vetor

p1w

que parte da origem O1 e segue at o centro do punho O5 possibilita

deduzir equaes que relacionam os ngulos desejados.

59

x3 z2

x2

O5

3

P1w

x1

O1 z1 2Figura 45 Representao grfica das origens das articulaes do ombro e punho O grafo da Figura 46 mostra de forma grfica as operaes necessrias para a deduo das equaes dos ngulos de cotovelo e ombro do manipulador. A seta verde indica os valores numricos at agora calculados e a seta em azul indica o que ainda precisa ser calculado. 1 T 0 T 6 T 2 T 3 T 4 T

5 T Figura 46 Grafo das operaes de determinao dos ngulos do cotovelo e ombro A equao (4.9) representa o conjunto de transformaes envolvidas no grafo da Figura 4.7

(T )1 0

1

T06 T56

( )

1

= T12 T23 T34 T45

(4.9)

Efetuando-se a multiplicao das matrizes da equao (4.9), cujo produto pode ser observado na equao B2 do apndice B e tomando-se os trs primeiros elementos da quarta coluna, obtm-se as equaes (4.10), referente ao lado esquerdo, e (4.11) referente ao lado direito da equao (4.9)

60

( O5 ) 1 x px c1 + p y s1 d 6 ( r13c1 + r23 s1 ) O5 = pz d1 d 6 r33 ( 1 )y px s1 d y c1 d 6 ( r13 s1 r23c1 ) 5 ( O1 ) z a2 c2 + d 4 s23 Lado direito da equao (4.9) = a2 s2 d 4 c23 0 se:

(4.10)

A equao (4.11) proveniente do vetor de posio obtido da equao B3 do apndice B. (4.11)

Igualando-se as trs primeiras linhas da quarta coluna das equaes (4.10) e (4.11), obtm-

O5 ( 1 ) x d x c 1 + d y s1 d 6 ( r13c1 + r23 s1 ) a2 c2 + d 4 s23 O5 = d z d1 d 6 r33 ( 1 )y = a2 s2 d 4 c23 5 d x s1 d y c1 d 6 ( r13 s1 r23c1 ) 0 O1 ) ( z

(4.12)

Elevando-se ao quadrado os elementos componentes da primeira segunda linhas da equao (4.12) , elimina-se a varivel de junta desconhecida 2. Direcionando-se a equao para a forma da lei dos co-senos, como mostra a equao (4.13) , obtm-se:

(O )Isolando sen(3),

5 1 x

2

+ ( O15 )

2y

= ( a2 c2 + d 4 s23 ) + ( a2 s2 d 4 c23 )2

2

(4.13)

s3 =Aplicando-se a lei dos senos

(O )

5 2 1 x

+ O15

( )

2 y

2 2 a2 d 4

2a2 d 4 2 v 2(4.14)

c3 = 1 s32Logo, a determinao do ngulo 3

3 = atan2( s3 , c3 )como mostra a equao (4.15) .

Conhecida a varivel de junta 3, adota-se procedimento semelhante para a obteno de 2,

O15 O5 1

( ) ( )

x

= a2 + d 4 s3 d 4 c3 y

d 4 c3 c2 a2 + d 4 s3 s2

(4.15)

61 Isolando-se cos(2) na equao (4.16)

c2 =

( a2 + d 4 s3 ) ( ( O15 ) x ) d 4c3 ( O15 ) y2 2 a2 + d 4 + 2a2 d 4 s3

(4.16)

De modo anlogo isola-se sen(2) na equao (4.17)

s2 =

( a2 + d 4 s3 )

((O ) ) + d c (O )5 1 y 4 3

5 1 x

(4.17)

2 2 a2 + d 4 + 2a2 d 4 s3

Dividindo (4.17) por (4.16) , isolando o argumento e aplicando a funo ATAN2

2 = atan2( s 2 , c2 )

(4.18)

At aqui foram determinados dois dos trs passos do procedimento para a obteno da cinemtica inversa. Estabeleceu-se a posio do centro do punho e os trs ngulos do antebrao do manipulador. A partir deste ponto ser o terceiro passo que a determinao dos ngulos do punho do manipulador.

4.4 SOLUO DA CINEMTICA INVERSA PARA 5A determinao da cinemtica de orientao do punho pode ser obtida por diferentes caminhos. Retomando o estudo da equao (4.2) , observa-se que as trs primeiras matrizes de transformao T01 T12 T23 so conhecidas, logo pr-multiplicando a a inversa deste produto pela matriz homognea obtm-se como resultado a equao (4.19)

(

)

(T

1 0

T12 T23 ) T06 = T34 T45 T561

(4.19)

Os termos do lado esquerdo da equao so valores numricos e os termos do lado direito dependem das variveis das juntas do punho. O grafo da Figura 47 ilustra este procedimento, onde seta verde no sentido anti-horrio representa o lado esquerdo de (4.19) e a seta azul no sentido horrio, representa as transformaes do lado direito cujas variveis de junta ainda permanecem incgnitas .

62

T 1 T 0 T 6

2

T

3 T 4 T

T

5

Figura 47 Grafo representativo das transformaes do punho. As multiplicaes do lado esquerdo e do lado direito da equao (4.19) , podem ser observadas nas equaes B6 e B7,respectivamente, no apndice B. Igualando os elementos de equaes B6 e B7 e extraindo-se as coordenadas da terceira coluna da equao B6 obtm-se

r13c1c23 r23 s 1 c23 + r33 s 23 Lado _ esquerdo(1,3) = r13 s 1 r23c11 r13c1s 23 r23 s 1 s 23 + r33c23 equao B7.

(4.20)

De modo anlogo, a equao (4.21) representa a terceira coluna do lado direito da

c4 s 5 Lado _ direito(1,3) = s 4 s 5 , c5 Igualando-se as equaes (4.20) e (4.21) , como mostra a equao 4.22) ,

(4.21)

r13c1c23 r23 s 1 c23 + r33 s 23 c4 s 5 = s s r13 s 1 r23c11 4 5 r13c1s 23 r23 s 1 s 23 + r33c23 c5 Observa-se que o elemento da terceira linha permite isolar cos(5), como segue

(4.22)

c5 = r13c1s 23 r23 s 1 s 23 + r33c23Aplicando a lei dos senos, tem-se:2 s5 = 1 c5

(4.23)

(4.24)

Dividindo equao (4.24) por (4.23) , isolando-se o argumento, obtm-se:

632 5 = atan2 s5 , 1 c5

(

)

(4.25)

Substituindo a equao (4.23) em (4.24) , e esta em (4.25) , obtm-se (4.26)2 5 = atan2 r13 sin 1 r23 cos 1 , 1 ( r13 sin 1 r23 cos 1 )

(4.26)

4.5 SOLUO DA CINEMTICA INVERSA PARA 4Conhecido o ngulo 5, a varivel 4 da junta 4 pode ser isolada a partir da primeira e segunda linhas da equao (4.22) , como mostra a equao (4.27)

c4 = s 4=

r13c1c23 r23 s 1 c23 + r33 s 23 s5 r13 s 1 r23c11 s5 4 = atan2 ( s4 , c4 )

(4.27)

Estas equaes so vlidas somente para s50. (4.28)

Quando s5=0 o rob apresenta uma singularidade.

4.6 SOLUO DA CINEMTICA INVERSA PARA 6Com dois dos trs ngulos do punho calculados, o ngulo 6 pode ser obtido da equao (4.29)

(T

1 0

T12 T23 T34 ) T06 = T45 T561

(4.29)

O grafo da Figura 48 representa a equao (4.29)

64 2

1 T 0 T 6

T

T

3 T 4 T 5

T Figura 48 Grafo representativo das transformaes necessrias para obter 6 A equao B8 do apndice B apresenta o lado esquerdo e a equao B9 apresenta o lado direito da equao (4.29) . Igualando o elemento da terceira linha e primeira coluna da equao B8 e B9, obtm-se:

r11 ( s 1 c4 c1c23 s 4 ) r21 ( c1c4 s 1 c3 s 4 ) r31s 23 s 4 = s 6 r13 ( s 1 c4 c1c23 s 4 ) r23 ( c1c4 s 1 c3 s 4 ) r33 s 23 s 4 = c6 6 = ATAN 2( s 6 , c6 )Logo,

(4.30)

(4.31)

6 = ATAN 2 ( r11 ( s 1 c4 c1c23 s 4 ) r21 ( c1c4 s 1 c23 s 4 ) r31s 23 s 4 ,r13 ( s 1 c4 c1c23 s 4 ) r23 ( c1c4 s 1 c3 s 4 ) r33 s 23 s 4 )

(4.32)

Com a cinemtica direta possvel determinar a posio e orientao do efetuador final, j a forma inversa permite calcular os ngulos das juntas a partir da posio e orientao do mesmo efetuador final.

4.7 RVORE DE SOLUESComo pode ser constatado pelas equaes (4.8) , (4.14) e (4.25) , podem haver duas solues para 1, 3, e para 5 significando que o manipulador pode alcanar oito diferentes localizaes especificadas pela matriz de transformao homognea. Porm algumas desta oito diferentes posies so impraticveis devido as restries de movimento da estrutura de paralelogramo do manipulador.

65 Na Figura 49 apresentado o grafo das oito solues possveis para a obteno da cinemtica inversa. Matriz de Transformao Homognea

+1 +3 +2 -3 -2 +3 +2

-1 -3 -2

+5

-5

+5

-5

+5

-5

+5

-5

4 6

4 6

4 6

4 6

4 6

4 6

4 6

4 6

Figura 49 rvore de solues para a cinemtica inversa A comprovao dos resultados apresentados acima so mostrados nas simulaes implementadas no programa Matlab.

4.8 SIMULAES DA CINEMTICA INVERSAA primeira simulao implementada no Matlab parte de valores iniciais, calcula a cinemtica direta apresentando a matriz de transformao homognea (Tabela 4.1) e com os valores desta calcula a cinemtica inversa, determina o erro resultante e apresenta os resultados na Tabela 4.2 .

66 Tabela 4.1 ngulos das juntas e correspondente matriz de transformao homognea ngulos 0 q1 q2 q3 q4 q5 q6 0 0 0 0 0 -0.0000 -1.0000 -0.0000 0 MTH -0.0000 1.0000 0.0000 -1.0000 0 -0.0000 -0.0000 0 530.0000 0.0000 750.0000 1.0000

Tabela 4.2 Simulao da cinemtica direta e inversa e o erro correspondente Cinemtica Direta 0 0 1,0x10-015 * 0 0 0 0 Inversa 0,8668 0 0 0 0 0 erro -0.8668 0 0 0 0 0

Como pode ser observado o erro resultante da ordem de 10-015 que praticamente nulo. Com a finalidade de comprovar a validade das equaes foi novamente calculada a cinemtica direta a partir dos valores da cinemtica inversa da Tabela 4.2 e so apresentados na Tabela 4.3 Tabela 4.3 Simulao da cinemtica direta a partir da inversa calculada ngulos q1 0,8668 q2 q3 q4 q5 q6 0 0 0 0 0 -0.0000 -1.0000 -0.0000 0 MTH -0.0000 1.0000 0.0000 -1.0000 0 -0.0000 -0.0000 0 530.0000 0.0000 750.0000 1.0000

67 Tabela 4.4 Simulao da cinemtica direta a partir da inversa calculada Cinemtica Direta 0,8668 0 1,0x10-015

Inversa 0,8668 0 0 0 0 0

erro 0 0 0 0 0 0

*

0 0 0 0

Nesta segunda simulao o erro nulo. A segunda simulao implementada exibe os grficos (Figura 50) das trajetrias das trs primeiras juntas geradas a partir de uma trajetria retilnea entre os pontos [-100, -100, 1130] a [1140, 100, 380] do espao cartesiano.

200 J unta 1 (graus ) 150 100 50 0

0

0.5

1

1.5

2

2.5 Tempo (s)

3

3.5

4

4.5

5

200 J unta 2 (graus )

150

100

50

0

0.5

1

1.5

2

2.5 Tempo (s)

3

3.5

4

4.5

5

-100 -125 J unta 3 (graus ) -150 -175 -200 -225 -250 0 0.5 1 1.5 2 2.5 Tempo (s) 3 3.5 4 4.5 5

Figura 50 - Trajetrias das juntas da base

68 Para validar a cinemtica inversa a partir dos ngulos de Euler, foram atribudos valores iniciais para os ngulos das juntas e calculou- se os ngulos de Euler no Matllab. Na seqncia, forma determinados os valores dos ngulos das juntas.

=[ 45

0

0

0

0

0]

0.7071 -0.7071 MTH = 0.0000 0 [

0.0000 0.7071 -0.0000 0.7071 -1.0000 -0.0000 0 090.0000

374.7666 374.7666 750.0000 1.0000

] = [45.0000

-90.0000]

Cinemtica inversa Clculo da matriz de transformao homognea mthinveuler = 0.7071 0.0000 0.7071 374.7666 0.7071 374.7666 0 0.0000 1.0000 0.0000 0.0000] -0.7071 -0.0000 0 0 0.0000

0.0000 -1.0000 -0.0000 750.0000 Aplicando a transformao inversa obteve-se

=[ 45.0000

0.0000

Neste captulo foi obtida a cinemtica inversa a partir da matriz de transformao homognea. As simulaes comprovaram a validade das equaes deduzidas.