Trayectorias

Trayectorias para un Brazo Robot tipo SCARA (2D).

Teniendo la cinemática inversa resuelta podemos pasar al siguiente nivel, que es mover el brazo robot de un punto del espacio a otro de forma autónoma:

[ X0, Y0, Z0 ] ---------> [ X1, Y1, Z1 ]

Necesitamos un algoritmo que nos lleve lineal y uniforme de dicho punto al siguiente.

Trayectorias cartesianas en dos dimensiones.

Para mover un objeto en el espacio de un punto al siguiente en mi caso lo resuelvo con el algoritmo de Bresenham. Es una manera relativamente sencilla, elegante y digital para realizar trayectorias punto por punto y en todas las dimensiones o ejes que necesitemos. Este algoritmo se usa ámpliamente en el mundo de los plotter, máquinas de CNC y gráficos (en un nivel primigenio).

Imagina un plano cartesiano. Queremos ir del punto (3, 4) al (17, 8). Se sobre-entiende que el primer valor es del eje X y después de la coma es el valor del eje Y. Como no podemos estar en zonas intermedias (esto sólo es posible en un mundo analógico), tenemos que ceñirnos a ocupar un cuadradito en cada posición hasta alcanzar el objetivo.

Para ver la prueba que voy a hacer puedes bajarte el programa haciendo clic aquí, o haciendo clic en la imagen de abajo. Usaré un algoritmo de Bresenham 3D, pero sólo usaré los ejes X e Y, al eje Z siempre le introduciré cero porque no voy a utilizar ese eje.

Inicialmente comienza en el punto (0, 0) pero para este ejemplo esa parte la despreciamos. Introducimos los valores (3, 4) y después (17, 8), el algoritmo de Bresenham nos dará una secuencia de números. Cada punto (x, y) que da el algoritmo es un paso que se dirige hacia el siguiente, así hasta completar el recorrido. Puedes comprobar que la penúltima imagen se corresponde con los datos que da el programa (imagen a pie de este párrafo).

En principio no vamos a necesitar comprender cómo funciona internamente el algoritmo de Bresenham. Lo que nos interesa en este apartado es usar el algoritmo para nuestro propósito de trazar trayectorias. Este algoritmo dibuja líneas (esas líneas para nosotros son trayectorias), punto por punto, en cualquier ángulo de manera cartesiana.

Ahora hemos de pasar al siguiente nivel, ya que cada punto (x, y) que nos da el algoritmo se ha de corresponder a dos ángulos que situará el terminal del brazo en dicho punto (x, y). Veamos cómo se hace.

Trayectorias cartesianas para un brazo tipo SCARA.

Tenemos por un lado el algoritmo de Bresenham (2D) para realizar las trayectorias, y por el otro, tenemos resuelto la cinemática Inversa para 2 grados de libertad que aplicaremos al brazo tipo SCARA. Vamos a necesitar hacer trabajar las dos cosas juntas para poder desplazar el brazo al lugar que deseemos dentro del área de trabajo que pueda alcanzar.

Intenta ver la lógica a lo que viene a continuación, porque si no tendrás un verdadero dolor de cabeza. No has de saber ni de trigonometría ni cómo funciona Bresenham, porque ya lo tienes resuelto, sólo has de ver la lógica de por qué cada vez que el algoritmo de Bresenham nos da un punto (x, y) de la trayectoria, hemos de calcular la cinemática inversa para dicho punto.

Pasemos a verlo detalladamente, paso a paso, usando Bresenham y llamando éste a la cinemática inversa.

La imagen a pie de este párrafo es un esquema donde podemos ver cómo Bresenham envía los datos a la cinemática inversa (IK) y éste calcula los dos ángulos para el brazo SCARA. Los dos ángulos te servirán para simular el brazo en el monitor y también para enviarlos a la electrónica de los motores del brazo. Una vez hecho esto, vuelve a Bresenham para obtener el siguiente punto de la trayectoria que estamos recorriendo y vuelve a repetir el proceso, así hasta finalizar la trayectoria.

Una vez terminado el recorrido se espera a la siguiente posición final (otra trayectoria para el brazo). La imagen de abajo es un esquema general del funcionamiento completo.

En la entrada de datos del algoritmo de Bresenham sólo introduciremos la "posición final". La "posición inicial" al comienzo será (0, 0), y cada vez que el algoritmo resuelva todos los punto, la posición inicial tomará el valor de la posición final. Por ejemplo, si ejecuto el algoritmo de Bresenham e introduzco como "posición final" (3, 4), recorrerá todos los valores desde (0, 0) hasta alcanzar el punto (3, 4). Cuando finaliza, la posición (3, 4) pasa a convertirse en la "posición inicial". Así que cuando introduzco otra posición, por ejemplo (17, 8), la "posición inicial" vale (3, 4) y la posición final (17, 8). Una vez llegue a la posición (17, 8) ésta pasa a convertirse en la "posición inicial". Así sucesivamente.

Cálculo para 3 dimensiones con más grados de libertad:

De la misma forma que hacemos trayectorias 2D se puede hacer trayectorias 3D. Lo que cambia es la versatilidad de la cinemática inversa y las dimensiones del algoritmo de Bresenham. Por ejemplo, para hacer trayectorias con un brazo de 5 grados de libertad (más la pinza, pero la pinza no se cuenta como grado de libertad) se puede usar un algoritmo de Bresenham de 6 dimensiones, en las cuales las variables que maneja son los ejes X, Y, Z, distancia-pinzas, y los ángulos cabeceo (pitch) y balanceo (roll); y una cinemática inversa para 4 grados de libertad (el ángulo del balanceo de muñeca no entra dentro de la cinemática inversa, pero éste sería el 5º grado de libertad). Uso esas dos herramientas en los simuladores de brazo robot que se encuentran en esta web para realizar los movimientos automáticos de los brazos robots.

Primero hay que comenzar por lo sencillo, una vez dominas en 2D puedes pasar a 3D. Saltar directamente a 3D es toda una epopeya.

Un programa de ejemplo de traslaciones en 2D para un brazo tipo SCARA:

A continuación pongo un programa escrito en FreeBasic donde podrás ver los procesos y cómo Bresenham está anidado con la cinemática inversa (IK).

Para descargar el código fuente y ejecutable haz clic aquí.


(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 or clic here.
WindowTitle "Ejemplo de trayectorias para brazo robot tipo SCARA."
Screen 19               ' Resolución del monitor: 800x600.
Declare Sub InverseK
Declare Sub Bresenham
Dim        As String  Xin, Yin  ' Nos servirá para entrar las coordenadas.
Dim Shared As Double  x, y      '*Leer abajo de esta página para saber la razón
                                ' de por qué estas variables son tipo Double'. 
                                
Const Pi   = Atn(1) * 4
Const Rad  = Pi  / 180
Const Grad = 180 / Pi
Const BaseX  = 400      ' Punto X Base (hombro)  Aquí situamos el brazo en pantalla. Por
Const BaseY  = 300      ' Punto Y Base (hombro)  defecto está en el centro del monitor.
Const LongBrazo  = 150  ' Longitud Brazo.        Puedes modificar las longitudes tanto
Const LongAntBr  = 150  ' Longitud AnteBrazo.    del brazo como del antebrazo.
x=(150)        ' Posición Inicial X.    Aquí damos las coordenadas iniciales de la punta del brazo.
y=(150)+BaseY  ' Posición Inicial Y.    Se puede modificar los valores que están dentro del paréntesis.
               '                        Procura que esté dentro del área de trabajo.
Do
    Bresenham   ' Llama al algoritmo Bresenham con la Posición Final de las variables x, y.
    
    Color  0,24
    Locate 34,15: Print "Para salir del programa pulsa 2 veces 'Enter' sin introducir ningun
    valor.";
    Color  15,1
    Locate 35,15: Input "Introduce un valor comprendido entre -212 hasta 212 para el eje X:", Xin
    Locate 36,15: Input "Introduce un valor comprendido entre -212 hasta 212 para el eje Y:", Yin
    Color  15,0
    x = Val(Xin)         ' Convierte el valor tipo String a un valor numérico.
    y = Val(Yin) + BaseY ' Necesitamos sumar 'BaseY' para que salga el brazo correctamente en
                         ' el monitor porque la coordenada 'y' (eje Y) comienza en la esquina
                         ' superior izquierda, en vez de la esquina inferior izquierda.

Loop Until Xin = ""      ' Finaliza el programa al pulsar 'Enter' sin haber introducido ningún
                           valor.
End

Sub InverseK  ' Cálculo de la Cinemática Inversa para 2 grados de libertad (Tipo Scara).

    Dim As Double PYa, PYb, PXa, PXb, LadoA, Alfa, Beta, Gamma
    Dim As Double AngAntBr,AngBrazo, BrazoPY, BrazoPX
    Dim As Double AntBrazoPY, AntBrazoPX, 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).
    ' ****** En este punto finaliza la Cinemática Inversa para dos grados de libertad.************
    ' ******** Lo siguiente es el dibujo del brazo tipo Scara con dos simples líneas. ************

    '---------------------------------
    ' Cálculo de los puntos a dibujar.     
    '---------------------------------
    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.
    '----------------
    ' Dibuja Líneas.
    '----------------    
       
    Cls 
    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 el monitor los resultados de la Cinematica Inversa (IK).
    '------------------------------------------------------------------- 
    Locate 1,20:  Print "Angulos  ----> Brazo:"; CInt(AngBrazo*Grad); Chr(248);_
    "  "; "AnteBrazo:"; CInt((AngAntBr*Grad)+180); Chr(248);"  "
    Locate 2,20:  Print "Posicion ---->      ";x;"X"; "  "; y-BaseY;   "Y   "
    '-------------------------------------------------------------------
End Sub

Sub Bresenham  ' El Algoritmo de Bresenham sirve para trazar punto a punto la trayectoria.
    Dim    As Integer   Incrd1, Incrd2, IncrXold, IncrXnew, IncrYold, IncrYnew  
    Dim    As Integer   DistX, DistY, NumPixel, Decision, Bucle
    Dim    As Integer   Xnew, Ynew
    Static As Integer   Xold, Yold
    Xnew = x
    Ynew = y
    DistX = Abs(Xnew - Xold)
    DistY = Abs(Ynew - Yold)

    If DistX >= DistY Then
       NumPixel = DistX + 1
       Decision = (2 * DistY) - DistX
       Incrd1 = DistY * 2
       Incrd2 = (DistY - DistX) * 2
       IncrXold = 1
       IncrXnew = 1
       IncrYold = 0
       IncrYnew = 1
    Else
       NumPixel = DistY + 1
       Decision = (2 * DistX) - DistY
       Incrd1 = DistX * 2
       Incrd2 = (DistX - DistY) * 2
       IncrXold = 0
       IncrXnew = 1
       IncrYold = 1
       IncrYnew = 1
  End If

  If Xold > Xnew Then
     IncrXold = -IncrXold
     IncrXnew = -IncrXnew
  End If
  If Yold > Ynew Then
     IncrYold = -IncrYold
     IncrYnew = -IncrYnew
  End If
  x = Xold
  y = Yold
  For Bucle = 1 To NumPixel

     InverseK   ' Llama a la Cinemática Inversa para calcular los ángulos en cada punto (x,y)
     ' que va trazando Bresenham. Los valores están almacenados en las variables
                   ' globales 'x' e 'y'
     Sleep 10   ' Hacemos una pausa para poder visualizar el recorrido.
     If Decision < 0 Then
        Decision = Decision + Incrd1
        x = x + IncrXold
        y = y + IncrYold
     Else
        Decision = Decision + Incrd2
        x = x + IncrXnew
        y = y + IncrYnew
     End If
  Next Bucle

  Xold = Xnew
  Yold = Ynew
End Sub

* La razón de que las variable x e y sean "Double" en vez de "Integer" (sólo se guardan en esas variables valores enteros) es porque participan en el cálculo de la cinemática inversa con otras variables que son "Double". Por tanto todas han de ser del mismo tipo para no errar en los cálculos.