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