simulaÇÃo da cinemÁtica inversa de robÔs trr em fortran 90

UFBA

UNIVERSIDADE FEDERAL DA BAHIA

SIMULAÇÃO DA CINEMÁTICA INVERSA DE ROBÔS TRR EM

FORTRAN 90

PAULO VINICIUS LISBOA PEREIRA

FILIPE MAROCCI DE ALCANTARA

SALVADOR - BA

JUNHO DE 2012

Introdução

Motivação

Para a consolidação de um setor industrial solido muitos países incentivaram a indústria

a automatizar sua produção. Um dos setores industriais que mais impulsionou o avanço

na tecnologia foi o automobilístico. A necessidade de baratear a produção e torna-la

mais rápida fez com que empresários do setor e governantes injetassem recursos

humanos e financeiros com pesquisa e desenvolvimento de sistemas mais eficazes para

a produção neste setor.

Tendo em vista esta conjuntura descreveremos a aplicação de algumas destas

tecnologias na área de computação.

Objetivos

Este trabalho possui como objeto geral a utilização da linguagem Fortran 90 para

calculo da cinemática inversa de manipuladores robóticos com a primeira junta

torcional, junta da base, e segunda e terceira junta rotacional (TRR).

Objetivos específicos

Sabendo-se da necessidade de manipulares robóticos em ambientes confinados. Este

trabalho visa à especificação da trajetória que o órgão terminal ira descrever.

Desenvolveremos a trajetória retilínea do órgão terminal e explanaremos a possibilidade

de trajetória mais complexas como curvas lisas, curvas que não se cruzam, através da

parametrização de suas coordenadas.

A cinemática do manipulador TRR

será descrita por uma via mais

intuitiva. Este trabalho não fará

menções a uma descrição algébrica

com o artifício da mudança de base

ou das transformações lineares. Mas

sim, relacionará a cinemática do

manipulador por um viés

geométrico. Esta representação é

mais didática, contudo perde com

relação à algébrica quando tratamos

de manipuladores mais complexos,

manipuladores que possuam maiores graus de liberdade.

Desenvolvimento

Cinemática de manipuladores

A cinemática descreve os movimentos dos corpos sem levar em conta as forças que

geraram estes movimentos. Em manipuladores robóticos esta irá descrever a posição e a

velocidade do seu órgão efetuador e dos seus ligamentos. A posição do órgão terminal

de um manipulador depende dos deslocamentos angulares de suas juntas. Dividimos a

cinemática de manipuladores em cinemática direta e inversa.

De maneira geral, com a cinemática direta de um manipulador robótico se obtém,

através dos ângulos das juntas, a posição do órgão terminal deste manipulador com

relação a um sistema de coordenadas cartesianas, que comumente possui o eixo de

intersecção das abscissas, das ordenas e das contas encontra-se na sua base. A

cinemática direta é pouco intuitiva para o planejamento de trajetórias, muitas vezes

demasiadas complexas, possivelmente até impossíveis para este fim. Uma possível

solução para este problema seria a possibilidade de trabalhar com coordenadas

cartesianas, forma mais intuitiva de planejar trajetórias. Porém, haveria uma

necessidade de relaciona-las com os ângulos das juntas para um possível controle do

manipulador. Tem-se em mente que o controle é possível apenas com a posição angular

das juntas. Resolvendo o problema da cinemática inversa, ou seja, obtenção das

coordenadas angulares através das coordenadas cartesianas pode-se planejar as

trajetórias de forma mais intuitiva e usa-las como fator de conversão para obter as

coordenadas angulares.

Cinemática direta aplicada ao manipulador TRR.

Definimos como d = a2 cos θ2 + a3 cos(θ2 + θ3) , a projeção

dos elos 2 e 3 sobre o plano xy. Então respectivamente o

seno e o cosseno do ângulo da junta torcidal são as

componentes x e y do órgão terminal.

X = d cos θ1= [a2 cos θ2 + a3 cos(θ2 + θ3 )]cos θ1

Y = d sen θ1= [a2 cos θ2 + a3 cos(θ2 + θ3 )] senθ1

Z = a1 + a2 sen θ2 + a3 sen (θ2+ θ3)

Cinemática inversa aplicada ao manipular TRR

Podemos encontrar a expressão para encontrar o ângulo da

segunda junta rotacional aplicando a lei dos cossenos para o

triângulo formado pelo centro da junta 1, chamada de R na

figura, o centro da junta 2, ponto S, e o ponto P, ponto onde

fica situado órgão terminal do robô. Definimos a distância r como √ . Dai

tiramos que:

[ ]

+2 cos[θ3 ]

Por consequência temos:

θ3 [

]

Para o ângulo da primeira junta rotacional vamos definir dois ângulos que facilitaram o

seu calculo. Temos o ângulo formado pela tangente da diferença do primeiro elo com a

hipotenusa r. Assim definimos:

Como √ , temos:

√

Definimos o segundo ângulo auxiliar como:

Podendo ser mais facilmente visualizado na imagem no inicio da pagina. Ainda com o

auxilio da imagem acima definimos o ângulo da primeira

junta rotacional como:

O ângulo da junta torcidal é definido como o arco, cuja

tangente é a coordenada Y sobre X. Matematicamente,

temos:

1=

Conclusão

A utilização de um software é de vital importância para um controle dinâmico e em

tempo ótimo de um manipulador robótico. A linguagem de programação Fortran

atendeu a tal propósito devido a suas infinidades de funções, que facilitaram a conclusão

do trabalho.

Anexo

!

! Definindo as constantes que são usadas no programa e nas subrotinas.

!

module constantes

implicit none

save

real :: Pi=14159

end module constantes

!

program control_rob

!

! Receber valores das coordenadas cartesianas e retornar com os angulos das juntas.

! Simular uma tragetória proxima a uma reta através de variações infinitesimais dos

parametros.

!

! Primeira parte - declaração de variaveis

!

implicit none

real :: posx,posy,posz,ang_tor,ang_rot1,ang_rot2,incr,pas_motor

real:: t

real,dimension (6):: pos

real,dimension (3):: elo

!

!______________________________________________________________________

_____________

! Segunda parte - Definição de parametros. Ler os valores iniciais e finais.

!

25 write (*,*) "Informe o tamanho dos elos em centimetros."

write (*,*) " Como no exemplo digite o valor do elo um, dois e tres separado por

espaço em branco."

write (*,*) " Exemplo: val01 val02 val03"

read (*,*,err=25)elo

29 write (*,*) "Informe em radianos o passo do motor"

read (*,*,err=29)pas_motor

!

32 write (*,*) "Informe os valores iniciais e em seguida os valores finais das

coordenadas x,y e z."

write (*,*) " Como no exemplo: 99 99 99 99 99 99."

read (*,*, err=32)pos

!______________________________________________________________________

_____________

!

! Terceira parte - controle da tragetória, determinação dos angulos.

! Criar matriz para quardar resultados.

!

call f_incr (incr,pas_motor,elo(1),elo(2),elo(3))

!

t=0

!

! Laço para calcular a reta paramétrica,

! a cinematica inversa e alocar o resultado na matriz.

!

do while(t<=1)

!

! Chamar subrotina parametric

call parametric(pos(1),pos(2),pos(3),pos(4),pos(5),pos(6),posx,posy,posz,t)

! Chamar a surotina cinematc

call calc_cinemat(posx,posy,posz,elo(1),elo(2),elo(3),ang_tor,ang_rot1,ang_rot2)

write(*,*)"angulo tocidal: ",ang_tor

write(*,*)"angulo rotacional 1: ",ang_rot1

write(*,*)"angulo rotacional 2: ",ang_rot2

!

! Atualizar parametro t

!

t=t+incr

end do

stop

end program control_rob

!

! Função incremento - O valor do incremento esta relacionado com o passo do motor.

Temos que ter em vista que

! variações de angulos menores que o passo do

motor não desempenham melhora na precisão do

! movimento, apenas criam um maior custo

computacional.

!

SUBROUTINE f_incr (incr,pas_mot,elo1,elo2,elo3)

use constantes

real,intent(in) :: elo1,elo2,elo3

real,intent(out) :: incr

real :: del_xy,del_z

!

! A minima variação se dá quando ha o movimento do angulo mais proximo da

extremidade.

! Atribuimos a variavel incr a menor variação.

!

del_xy=(elo2+elo3)-(elo2+elo3*Cos(pas_mot))

del_z=(elo1+elo2+elo3)-(elo1+elo2+elo3*Sin((Pi/2)-pas_mot))

!

incr=min(del_xy,del_z)

return

end subroutine f_incr

!

! Subrotina retas parametricas

!

SUBROUTINE

parametric(posx_in,posy_in,posz_in,posx_fin,posy_fin,posz_fin,posx_par,posy_par,pos

z_par,t)

real,intent(in):: posx_in,posy_in,posz_in,posx_fin,posy_fin,posz_fin

real,intent(out)::posx_par,posy_par,posz_par

real,intent(inout):: t

!

!

posx_par= posx_in+t*(posx_fin - posx_in)

!

posy_par= posy_in+t*(posy_fin - posy_in)

!

posz_par= posz_in+t*(posz_fin - posz_in)

return

END SUBROUTINE parametric

!

! Subrotina para calcular a cinemática inversa.

!

SUBROUTINE

calc_cinemat(posx_par,posy_par,posz_par,elo1,elo2,elo3,ang_tor,ang_rot1,ang_rot2)

real,intent(in) :: posx_par,posy_par,posz_par,elo1,elo2,elo3

real,intent(out) :: ang_tor,ang_rot1,ang_rot2

real :: alfa,beta

!

!

ang_tor = atan(posy_par/posx_par)

!

ang_rot2 = acos(((posx_par**2)+(posy_par**2)+((posz_par - elo1)**2)-(elo2**2)-

(elo3**2))/(2*elo1*elo2))

!

alfa= atan((posz_par-elo1)/(sqrt((posx_par**2)+(posy_par**2))))

beta= atan(elo3*sin(ang_rot2)/( elo2 + elo3*cos(ang_rot2)))

!

ang_rot1 = alfa - beta

return

END SUBROUTINE calc_cinemat