Cinemática Inversa I

Brazo Robot SCARA.

La cinemática inversa más sencilla es la que se aplica al brazo robot tipo SCARA y al 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.

(Si no ves la imagen, haz clic en el botón -Actualizar- de tu navegador)

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 utiliza 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 tiene la función Atan2() porque si no los datos no serán correctos. Más información haciendo clic en el gráfico a pie de este párrafo te llevará a la Wikipedia(En).

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 los grados que estamos acostumbrados, llamados grados sexagesimales. Se llama así porque cada grado son 60 minutos y a su vez, cada minuto son 60 segundos; esto es herencia de las culturas sumeria y babilónica porque tenían como base aritmética el 60 en vez de 10 que usamos nosotros; hoy lo seguimos utilizando para calcular tiempos, ángulos y coordenadas. 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 verás ejemplos de estas conversiones según las necesidades del programa.

Los nombres de las variables 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 está 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.

Te animo a que experimentes y puedas comprobar que tu Brazo Robot tipo Scara puede por ejemplo moverse en línea recta. Aquí se muestra para dos grados de libertad (2GL). Si necesitas que el brazo Scara tenga un ángulo de muñeca o quieres mover 3 grados de libertad pasa al siguiente capítulo.

Nota importante: El programa está preparado para representar el brazo en la pantalla, eso significa que si sacas los datos de los ángulos al mundo real, por ejemplo a unos servos, no serán coincidentes. En la pantalla estoy situando el brazo en un lugar de ésta (BaseX y BaseY) y además el eje Y está invertido.

El código está escrito en FreeBasic. Si usas otro lenguaje de programación, traducir "Basic" y con las variables declaradas en "tipo", es lo más sencillo del mundo.

Descargar código fuente y ejecutable.

(Si usas antivirus Avast has de añadir una exclusión para poder ejecutarlo. Para analizar este o cualquier otro archivo puedes hacer clic aquí)
The translation could modify the code. Use the code without translating or download the program by clicking the link above.
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.

   Cls                                 ' Borra líneas anteriores.        

   PSet   (BaseX,      BaseY     ), 0  ' Punto inicial.

   Line  -(BrazoPX,    BrazoPY   ), 4  ' Dibuja línea del brazo.

   Line  -(AntBrazoPX, AntBrazoPY), 9  ' Dibuja línea del antebrazo.

   '-----------------------------------------------------------------    
   'Muestra en pantalla los resultados de la Cinemática Inversa (IK).      
   '----------------------------------------------------------------- 
   Locate 1,30:  Print "Angulos  ----> Brazo:"; CInt(AngBrazo*grad); Chr(248);_
                 "  "; "AnteBrazo:"; CInt((AngAntBr*grad)+180); Chr(248);"  "
   Locate 2,30:  Print "Posicion ---->      ";x;"X"; "  "; y-BaseY;   "Y   "
End Sub