Robotica Cinematica

Cinemática Inversa I

La cinemática inversa más sencilla es la que se aplica al brazo robot tipo SCARA y brazo robot cilíndrico. En el brazo Scara tenemos que resolver los dos ángulos de las articulaciones. En este tipo de brazos el eje Z no interviene en la cinemática inversa porque es un resultado en sí mismo. Desde el punto de vista del cálculo sólo tenemos en cuenta los ejes X e Y, y la longitud de las articulaciones.

El triángulo formado por LongBrazo, LongAntBr e Hipotenusa suele ser del tipo irregular, sobre todo si los dos catetos (LongBrazo y LongAntBr) no son iguales. Para resolver los ángulos de este tipo de triángulo hay que aplicar el Teorema del Coseno.

Observa que en las fórmulas trigonométricas se usa la función arcocoseno "Acos()" y un tipo de arcotangente llamado "Atan2()". No es lo mismo Atan(Y/X) que Atan2(Y, X).

Se diferencia la una de la otra, además de la sintaxis, en que Atan2(), dependiendo de los signos contenidos en los valores X e Y tiene presente el cuadrante en el que se encuentra y puede recorrer los cuatro cuadrantes. Por contrapartida la función Atan() sólo tiene efecto en los cuadrantes 1 y 4. Comprueba que tu lenguaje de programación contiene la función Atan2() porque si no los datos no serán correctos.

En todos los lenguajes de programación las operaciones y los resultados de las funciones trigonométricas son siempre en radianes. Si queremos dar la información de los ángulos en el monitor es más cómodo hacerlo en grados "normales" llamados -grados sexagesimales- (se llama así porque cada grado son 60 minutos y a su vez, cada minuto son 60 segundos). Pasar radianes a grados sexagesimales se hace multiplicando el resultado en radianes por 180/Pi(57.29577951...). Y viceversa, para pasar grados sexagesimales a radianes hay que multiplicar por Pi/180 (0.017453292...). En el código fuente podrás ver ejemplos de estas conversiones según las necesidades del programa.

Los nombres de las variables que verás en el código fuente también están en la imagen que ves aquí arriba para que sepas de dónde viene cada una de las fórmula trigonométrica en las que además viene con el ajuste para situarlo en una zona de la pantalla. La aparente complejidad del programa es debido a que tenemos que situar esos puntos en la pantalla para formar mediante dos líneas el brazo tipo Scara de 2GL (GL= Grado de Libertad). En el monitor el eje X se corresponde con el plano que estamos acostumbrados en matemáticas, pero el eje Y no comienza en la esquina inferior izquierda, sino en la parte superior izquierda. Por esta razón en el código fuente interviene la variable BaseY al comienzo de la cinemática inversa.

.

WindowTitle "Cinematica Inversa para brazo robot SCARA."

Declare Sub InverseK

Declare Sub DibujaBrazo

Screen 19 ' Ventana de 800x600.

Cls

Dim Shared As Double pi, grad, rad, Xaux, Yaux, x, y, AngAntBr, AngBrazo

Dim Shared As Integer BaseY, BaseX, LongBrazo, LongAntBr

Dim Shared As String Tecla

pi = Atn(1)*4

rad = pi/180

grad = 180/pi

BaseX = 400 ' Punto X Base (hombro) Situamos el brazo en pantalla.

BaseY = 300 ' punto Y Base (hombro)

LongBrazo = 150 ' Longitud Brazo. Puedes modificar las longitudes del brazo o antebrazo.

LongAntBr = 150 ' Longitud AnteBrazo.

x=(150) ' Posicion Inicial X. Damos las coordenadas iniciales de la punta del brazo.

y=(150)+BaseY ' Posicion Inicial Y. Se puede modificar los valores que están dentro del paréntesis.

' Procurar que esté dentro del área de trabajo.

Tecla=" "

While Tecla<>Chr(27) And Tecla<>Chr(13) And Tecla<>Chr(255)+"k"

Locate 35,33: Print "Pulsa W S A D para mover el Brazo."

Locate 36,35: Print "Para salir pulsa Esc o Enter."

InverseK

If Tecla <> "" Then ' Sólo cuando se pulsa una tecla dibuja el brazo.

DibujaBrazo

EndIf

Tecla=LCase(InKey)

If Tecla="a" Then x=x-1

If Tecla="d" Then x=x+1

If Tecla="s" Then y=y-1

If Tecla="w" Then y=y+1

Wend

End

Sub InverseK

'----------------------------------

'*******Cinemática Inversa*********

'----------------------------------

Dim As Double LadoA, Alfa, Beta, Gamma, Hipotenusa

LadoA=y-BaseY

Hipotenusa=Sqr((LadoA^2)+(x^2))

Alfa=Atan2(LadoA,x)

Beta= ACos( ((LongBrazo^2)-(LongAntBr^2)+(Hipotenusa^2))/(2*LongBrazo*Hipotenusa) )

AngBrazo= Alfa+Beta ' ANGULO BRAZO(en radianes).

Gamma=ACos( ((LongBrazo^2)+(LongAntBr^2)-(Hipotenusa^2))/(2*LongBrazo*LongAntBr) )

AngAntBr=Gamma-(180*rad) ' ANGULO ANTEBRAZO(en radianes).

If Str(Beta)="-1.#IND" Or Str(Gamma)="-1.#IND" Then

x=Xaux ' En caso de error (fuera de rango) se vuelve a llamar a sí misma

y=Yaux ' y carga con los valores anteriores correctos.

InverseK

EndIf

Xaux=x

Yaux=y

End Sub

Sub DibujaBrazo

'---------------------

' PUNTOS PARA DIBUJAR.

'---------------------

Dim As Double PYa, PYb, PXa, PXb, BrazoPY, BrazoPX, AntBrazoPY, AntBrazoPX

PYa=LongBrazo*-Sin(AngBrazo)

PYb=LongAntBr*-Sin(AngAntBr+AngBrazo)

PXa=LongBrazo*Cos(AngBrazo)

PXb=LongAntBr*Cos(AngAntBr+AngBrazo)

'BRAZO (x,y)

BrazoPY=PYa+BaseY ' Punto de coordenada Y del Brazo.

BrazoPX=PXa+BaseX ' Punto de coordenada X del Brazo.

'ANTEBRAZO (x,y)

AntBrazoPY=PYb+PYa+BaseY ' Punto de coordenada Y del AnteBrazo.

AntBrazoPX=PXb+PXa+BaseX ' Punto de coordenada X del AnteBrazo.

...