CINEMÁTICA DIRECTA Y CINEMÁTICA INVERSA DE UN ROBOT DELTA ANGULAR Y DE UN ROBOT DELTA LINEAL

En esta primera entrada estudiaremos la cinemática directa de un robot delta angular y escribiremos código en python para la simulación.

El Problema

Se tiene el robot delta angular de la figura 1 (adjunto al final del post), se desea resolver el problema cinemático directo y el problema cinemático

Delta_angular

Figura 1: Robot delta angular

El problema cinemático directo

Comencemos resolviendo el problema cinemático directo. Esto es, obtener las ecuaciones que determinan la posición de la herramienta o plataforma a partir de las coordenadas articulares del robot, en este caso los tres ángulos entre la base y cada uno de los brazos superiores.

Entendiendo la geometría

Antes de comenzar detengámonos en analizar la geometría del robot delta de la figura 2.

Delta_angular_acotada

Figura 2: Robot Acotado

 

En la parte superior tenemos una placa de forma triangular que se encuentra fija, a este elemento lo llamaremos base y fijaremos un sistema de referencia en el punto B el cual se ubica en el centro del triángulo base. El sistema de referencia de la base viene dado por los vectores X_B, Y_B y Z_B. Llamaremos a los brazos 1, 2 y 3, el eslabón superior tiene una longitud L_s y el eslabón inferior una longitud de L_i para cada brazo. Consideraremos los ángulos de rotación entre la base y los eslabones superiores \theta_1, \theta_2 y \theta_3. Denotaremos H al punto donde se ubica la herramienta o plataforma cuya ubicación queremos obtener, ubicamos allí un sistema de referencia de coordenadas X_H, Y_H y Z_H que por la configuración de las barras inferiores, las cuales forman un paralelogramo, no rotarán respecto a al sistema de referencia de la base B. Finalmente llamaremos A_i, B_i y C_i (i=1,2,3) a las articulaciones.

tabla1

Tabla 1: Parámetros del robot

Grados de libertad

Podemos calcular el número de grados de libertad usando la fórmula de Grübler:

NGDL = \lambda (n - j - 1) + \sum_{i=1}^{j} f_i    (1)

Donde:
\lambda: Grados de libertad del espacio de trabajo (3 en el plano, 6 en el espacio).
n: Número de eslabones (incluyendo la base).
j: Número de articulaciones.
f_i: Grados de libertad de la articulación i.

Aplicando la ecuación:
NGDL = 6 (17-21-1) + 21 = -9

Lo cual es incorrecto,para hacer que la fórmula funcione debemos tratar las barras paralelas como un solo eslabón por brazo.

NGDL = 6 (14-15-1) + 15 = 3

Planteamiento geométrico

Si observamos de nuevo la figura 2 podemos establecer la siguiente igualdad vectorial

^{B}\vec{A_i}+^B\vec{L_{si}}+^B\vec{L_{ii}} = ^B\vec{P_P} + ^B_P\vec{R} ^P\vec{P_i}    (2)

Como no existe rotación entre los sistemas en B y en H tenemos que ^B_P\vec{R}=1 quedando entonces

^{B}\vec{A_i}+^B\vec{L_{si}}+^B\vec{L_{ii}} = ^B\vec{P_P} + ^P\vec{C_i}    (3)

^B\vec{L_{ii}}= ^B\vec{P_P} + ^P\vec{C_i}-^B\vec{L_{si}} -^{B}\vec{A_i}    (4)

El vector ^B\vec{P_P} determina la posición de la herramienta respecto a B y tiene coordenadas

^B\vec{P_P} = \left[ X_H, Y_H, Z_H \right]

Para determinar ^{B}\vec{A_i} detallemos la figura 3.

Base

Figura 3: Base

Con el uso de trigonometría podemos determinar las coordenadas de los ^{B}\vec{A_i}

^{B}\vec{A_1} = [0, -{{\sqrt{3}}\over{6}} S_B, 0]

^{B}\vec{A_2} = [{{1}\over{4}} S_B, {{\sqrt{3}}\over{12}} S_B, 0]

^{B}\vec{A_3} = [ -{{1}\over{4}} S_B, {{\sqrt{3}}\over{12}} S_B, 0]

Para determinar los vectores ^P\vec{C_i} lo hacemos desde la representación de la plataforma móvil (figura 4).

 

Herramienta

Figura 4: Plataforma móvil

Entonces las coordenadas de los ^P\vec{C_i} serán:

^P\vec{C_1} = [0, -{{\sqrt{3}}\over{3}} S_H, 0]

^P\vec{C_2} = [{{S_H}\over{2}}, {{\sqrt{3}}\over{6}} S_H, 0]

^P\vec{C_3} = [-{{S_H}\over{2}}, {{\sqrt{3}}\over{6}} S_H, 0]

Para determinar ^B\vec{L_{si}} debemos tomar en cuenta que tenemos su módulo L_s y su ángulo con respecto a la plataforma \theta_i

brazo.png

Figura 5: Eslabón superior

^B\vec{L_{s1}} = [0, -L_s cos(\theta_1), -L_s sin(\theta_1)]

^B\vec{L_{s2}} = [{{\sqrt{3}}\over{2}} L_s cos(\theta_2), {{1}\over{2}} L_s cos(\theta_2), -L_s sin(\theta_2)]

^B\vec{L_{s3}} = [-{{\sqrt{3}}\over{2}} L_s cos(\theta_3), {{1}\over{2}} L_s cos(\theta_3), {{1}\over{2}} L_s sin(\theta_3)]

Sustituyendo en la ecuación (4) obtenemos:

ecuaciones

No conocemos el valor de ^B\vec{L_{ii}} pero sabemos que su magnitud es L_i, así que tomamos la norma de las expresiones anteriores obteniendo

ecuaciones_mod.png

Para resolver este sistema llevaremos las ecuaciones anteriores a la forma

r^2 = (x -a_x)^2+(y-a_y)^2+(z-a_z)^2

Que corresponde a la representación espacial de una esfera. Siendo entonces nuestro problema la intersección de las 3 esferas.

esf.png

tabla_const.png

Tabla 2: Constantes a, b y c

Código en Python para la solución interactiva del problema

Se he escrito el siguiente código en Python 3 para la solución del problema cinemático directo de manera interactiva. Además se reprsenta gráficamente la solución.

Si quieres ejecutar y experimentar con el código interactivo puedes copiar el código y ejecutarlo en un Jupyter Notebook en https://cloud.sagemath.com/ seleccionando el kernel Anaconda Python 3.

from scipy.optimize import root
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from ipywidgets import interact, FloatSlider, interactive
from IPython.display import display
%matplotlib inline
#plt.style.use('default') # descomentar en /uncomment in: matplotlib 1.5.1


def interact(Sh,Sb,Ls,Li,theta1,theta2,theta3):
f = np.zeros(3)
a = np.zeros(3)
b = np.zeros(3)
c = np.zeros(3)
p = np.zeros(3)
a[0] = 0
a[1] = -np.sqrt(3)*Sh/3+Ls*np.cos(theta1)-np.sqrt(3)*Sb/6
a[2] = Ls*np.sin(theta1)
b[0] = Sh/2-(np.sqrt(3)/2)*Ls*np.cos(theta2)-Sb/4
b[1] = (1/6)*np.sqrt(3)*Sh-Ls*np.cos(theta2)/2-(1/12)*np.sqrt(3)*Sb
b[2] = Ls*np.sin(theta2)
c[0] = -Sh/2+np.sqrt(3)*Ls*np.cos(theta3)/2+Sb/4
c[1] = (1/6)*np.sqrt(3)*Sh-Ls*np.cos(theta3)/2-(1/12)*np.sqrt(3)*Sb
c[2] = Ls*np.sin(theta3)


def f(x):
return [
(x[0] + a[0])**2 + (x[1] + a[1])**2 + (x[2] + a[2])**2 - Li**2,
(x[0] + b[0])**2 + (x[1] + b[1])**2 + (x[2] + b[2])**2 - Li**2,
(x[0] + c[0])**2 + (x[1] + c[1])**2 + (x[2] + c[2])**2 - Li**2
]
def jac(x):
return np.array([[2*(a[0]+x[0]),2*(a[1]+x[1]),2*(a[1]+x[1])],
[2*(b[0]+x[0]),2*(b[1]+x[1]),2*(b[1]+x[1])],
[2*(c[0]+x[0]),2*(c[1]+x[1]),2*(c[1]+x[1])]])

sol = root(f, [0.0, 0.0, -0.5], jac=jac)
fig = plt.figure(figsize=(10,10))
ax = fig.gca(projection='3d',aspect=1,xlim=[-1,1],ylim=[-1,1])
ax.scatter(sol.x[0],sol.x[1],sol.x[2])
ax.plot([0,0,0,sol.x[0],sol.x[0]],
[0,-np.sqrt(3)/6*Sb,-np.sqrt(3)/6*Sb-Ls*np.cos(theta1),sol.x[1]-np.sqrt(3)/3*Sh,sol.x[1]],
[0,0,-Ls*np.sin(theta1),sol.x[2],sol.x[2]],'r')
ax.plot([0,Sb/4,(1/4)*Sb+np.sqrt(3)*Ls*np.cos(theta2)/2,sol.x[0]+Sh/2,sol.x[0]],
[0,np.sqrt(3)/12*Sb,np.sqrt(3)*Sb/12+Ls*np.cos(theta2)/2,sol.x[1]+np.sqrt(3)*Sh/6,sol.x[1]],
[0,0,-Ls*np.sin(theta2),sol.x[2],sol.x[2]],'g')
ax.plot([0,-(1/4)*Sb,-Sb/4-np.sqrt(3)*Ls*np.cos(theta3)/2,sol.x[0]-Sh/2,sol.x[0]],
[0,np.sqrt(3)*Sb/12,np.sqrt(3)*Sb/12+Ls*np.cos(theta3)/2,sol.x[1]+np.sqrt(3)*Sh/6,sol.x[1]],
[0,0,-Ls*np.sin(theta3),sol.x[2],sol.x[2]],'k')
print("xp=",sol.x[0]," yp=",sol.x[1]," zp=",sol.x[2])

theta1_slider = FloatSlider(min=0, max=np.pi/2, step=0.1, value=np.pi/6, description='Angulo Theta1')
theta2_slider = FloatSlider(min=0, max=np.pi/2, step=0.1, value=np.pi/6, description='Angulo Theta2')
theta3_slider = FloatSlider(min=0, max=np.pi/2, step=0.1, value=np.pi/6, description='Angulo Theta3')
Sh_slider = FloatSlider(min=0.05, max=1, step=0.05, value=0.1, description='Lado triángulo herramienta')
Sb_slider = FloatSlider(min=0.05, max=1, step=0.05, value=0.1, description='Lado triángulo base')
Ls_slider = FloatSlider(min=0.1, max=2, step=0.1, value=0.2, description='Long. eslabón superior')
Li_slider = FloatSlider(min=0.1, max=2, step=0.1, value=0.3, description='Long. eslabón inferior')
w=interactive(interact,Sh=0.1,Sb=0.1,Ls=0.2,Li=0.3,theta1=theta1_slider,theta2=theta2_slider,theta3=theta3_slider)
display(w)

Produciendo una salida como la que se muestra en la siguiente captura:

 

Captura de pantalla de 2016-02-05 12:33:50

 

 

 

Licencia Creative Commons
CINEMÁTICA DIRECTA Y CINEMÁTICA INVERSA DE UN ROBOT DELTA ANGULAR Y DE UN ROBOT DELTA LINEAL por Eduardo Vieira se distribuye bajo una Licencia Creative Commons Atribución 4.0 Internacional.
Basada en la obra “The Delta Parallel Robot: Kinematics Solutions” de R.L. Williams II en http://www.ohio.edu/people/williar4/html/pdf/DeltaKin.pdf.

Advertisements