🗞️ 🏙️ 👩🏿‍🚒 Cinemática de un robot Delta ✊🏼 🏔️ ☁️

En 2009, me entusiasmó construir mi propio robot industrial que pudiera hacer algo útil (es decir, clasificar piezas pequeñas en una cinta transportadora). Debo decir de inmediato que construí un robot (puede ver el resultado en la foto del título), y al mismo tiempo, como subproducto, escribí un breve artículo sobre la cinemática de los robots delta en el foro TrossenRobotics, un vendedor estadounidense de kits de piezas de robots. Acaban de celebrar un concurso para autores en ese momento. Por supuesto, no gané la competencia, pero el artículo en inglés permaneció. Varias veces intenté traducirlo a mi idioma nativo, pero logré completar lo que comencé solo ahora.

Si desea construir su modelo de un robot delta, o simplemente descubrir cómo puede derivar fórmulas cinemáticas para este tipo de robot (sin ir más allá del plan de estudios de la escuela en álgebra y geometría), bienvenido a cat. Para aquellos a quienes no les gusta la teoría, al final del artículo hay ejemplos de código prefabricado en C.

¿Qué es un robot delta?

El científico suizo Raymond Clavel inventó el robot delta ( wiki ) a principios de la década de 1980, la siguiente es una ilustración de la patente original US4976582 para "Un dispositivo para mover y colocar un elemento en el espacio":

El robot consta de dos plataformas: una base superior fija (1) y una pequeña plataforma móvil (8) conectada por tres palancas. Cada palanca consta de dos partes: el hombro superior (4) está rígidamente conectado al motor (3) ubicado en la base superior, y el inferior es un paralelogramo (5), en cuyas esquinas se denomina juntas universales (6, 7) ( wiki) que permiten que los ángulos cambien. Cada paralelogramo está conectado a la parte superior del brazo por una bisagra (16) de modo que su lado superior siempre permanece perpendicular a su brazo y paralelo al plano de la base superior. Gracias a esto, la plataforma móvil del robot unida a los lados inferiores de los paralelogramos también será siempre paralela a la base superior. Podemos controlar la posición de la plataforma cambiando el ángulo de rotación de las palancas superiores con respecto a la base del robot utilizando motores.

En el centro de la plataforma inferior (8), la llamada el cuerpo de trabajo (en inglés se usa el término efector final) del robot (9). Esto puede ser un manipulador, un dispositivo de agarre o, por ejemplo, una extrusora en el caso de una impresora 3D. Además, se puede usar otro motor (11), que proporciona rotación del cuerpo de trabajo a través de la varilla (14).

La principal ventaja de los robots delta es la velocidad: los motores pesados se colocan sobre una base fija, solo se mueven las palancas y la plataforma inferior, que intentan hacer con materiales compuestos ligeros, lo que reduce su inercia. Aquí, por ejemplo, un artículo sobre Geektimes con un par de videos muy efectivos.

Declaración de tarea

Para construir su propio robot delta, debe aprender a resolver dos problemas. En la primera situación, sabemos la posición en la que queremos mover el brazo del robot de nuestro robot (por ejemplo, queremos tomar una cookie que está en el transportador en un punto con coordenadas (x, y, z) . Para hacer esto, necesitamos determinar los ángulos en los que deben girar los motores conectados con las palancas del robot para colocarlo en la posición correcta para el agarre. El procedimiento para determinar estos ángulos se llama problema cinemático inverso (en algunas fuentes en ruso se usa la palabra "inverso").
En la segunda situación, sabemos los ángulos en los que se activan los motores de control del robot (si usamos servomotores, podemos encontrar fácilmente las esquinas leyendo las lecturas de los sensores de ángulo), y queremos encontrar la posición de la plataforma del robot en el espacio (por ejemplo, para ajustar su posición) . Esta es una tarea cinemática directa.

Formalizamos ambas tareas. Tanto la base fija del robot como su plataforma móvil se pueden representar como triángulos equiláteros: en el siguiente diagrama, están pintados en verde y rosa, respectivamente. Los ángulos de rotación de los brazos del robot con relación al plano base (también son los ángulos de rotación de los motores) se designan como Ѳ ₁ , Ѳ ₂ y Ѳ ₃ , y las coordenadas del punto son E ₀ubicado en el centro de la plataforma móvil y en el que el brazo del robot de nuestro robot se fijará en la vida real (x ₀ , y ₀ , z ₀ ) .

Resulta que debemos proponer dos funciones:

f _inversa (x ₀ , y ₀ , z ₀ ) → (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) para resolver el problema cinemático inverso y
f _adelante (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) → (x ₀ , y ₀ , z ₀ ) para resolver el problema cinemático directo.

Cinemática inversa

Establezcamos algunos parámetros clave que están determinados por las dimensiones geométricas de nuestro robot:

Designaciones de dimensiones para Delta Robot

Denotar superior lado de la base longitud f , el lado inferior de la plataforma e , superior palanca de la longitud del brazo r _f y la longitud del brazo inferior (el lado largo del paralelogramo) r _e . Para los cálculos, elegimos un sistema de coordenadas con un punto de referencia que coincida con el centro geométrico del triángulo superior. El eje Z se dirigirá hacia arriba, por lo que la coordenada z de la plataforma móvil siempre será negativa.

El diseño del robot implica que la palanca F ₁ J ₁ (ver la figura a continuación) solo puede girar en el plano YZ , mientras describe un círculo de radio r _f centrado en un puntoF ₁ (en este lugar está conectado al motor). A diferencia de F ₁ , los nodos J ₁ y E ₁ utilizan juntas universales, gracias a las cuales el hombro E ₁ J ₁ puede girar libremente en relación con E ₁ , describiendo una esfera de radio r _e centrada en el punto E ₁ .

La intersección de esta esfera y el plano YZ es un círculo centrado en el punto E ' _{1 de} radio E' ₁ J ₁ , donde el punto E ' ₁ se encuentra como la proyección del punto E ₁ en el plano YZ . Entonces el punto J ₁ estará en la intersección de dos círculos con centros en los puntos E ' ₁ y F ₁ , y podemos determinar los radios de estos círculos. Hay una ligera sutileza: los círculos se cruzan en dos puntos, pero solo nos interesa uno de ellos, con un valor menor de la coordenada y, porque queremos que los brazos del robot sobresalgan siempre "codos". Habiendo así determinado las coordenadas del punto J ₁ , podemos encontrar fácilmente el ángulo ₁ que nos interesa .

Para facilitar la percepción, a continuación se muestra la proyección de nuestra imagen tridimensional en el plano YZ :

La plataforma inferior es un triángulo equilátero, cuyo centro es el punto E ₀ (x ₀ , y ₀ , z ₀ ) . Distancia media

$E_0E_1 = {e \ over2} \ tan (30 ^ o) = {e \ over2 \ sqrt3},$

que nos da las siguientes coordenadas del punto E ₁ y su proyección E ' ₁ en el plano YZ :

$E_1(x_0, y_0-{e\over2\sqrt3}, z_0) \Rightarrow E'_1(0, y_0-{e\over2\sqrt3}, z_0)$

La distancia E ₁ E ' ₁ = x ₀ , entonces, de acuerdo con el teorema de Pitágoras,

$E'_1J_1=\sqrt{E_1J_1^2-E_1E'_1^2}=\sqrt{r_e^2-x_0^2}$

Como la plataforma superior también es un triángulo equilátero, las coordenadas del punto F ₁ serán

$F_1(0, {-f\over{2\sqrt3}}, 0)$

Para encontrar las coordenadas del punto J ₁ , que es la intersección de dos círculos, es necesario resolver el sistema de ecuaciones:

$\begin{equation*} \begin{cases} (y_{J_1}-y_{F_1})^2+(z_{J_1}-z_{F_1})^2 = r_f^2 \\ (y_{J_1}-y_{E'_1})^2+(z_{J_1}-z_{E'_1})^2 = r_e^2-x_0 \end{cases} \end{equation*}$

Conocemos las coordenadas de los centros de los círculos, si los sustituimos, obtenemos la siguiente expresión:

$\begin{equation*} \begin{cases} (y_{J_1}+{f\over{2\sqrt3}})^2+z_{J_1}^2 = r_f^2 \\ (y_{J_1}-y_0+{e\over{2\sqrt3}})^2+(z_{J_1}-z_0)^2 = r_e^2-x_0 \end{cases} \end{equation*}$

Si abrimos los corchetes y restamos una ecuación de la otra, podemos expresar linealmente la coordenada z del punto J _{1 en} términos de la coordenada y, después de lo cual, sustituyéndola en la segunda ecuación, obtenemos la ecuación cuadrática habitual para y, de la cual dos soluciones elegimos la más pequeña (más sobre esto hablamos arriba). Y así obteniendo las coordenadas del punto J ₁ , encontramos el ángulo

$\theta_1=\arctan\left({z_{J_1}\over{y_{F_1}-y_{J_1}}}\right)$

Todas las expresiones resultaron ser bastante simples debido a la elección exitosa del sistema de coordenadas: el brazo de palanca F ₁ J ₁ siempre se mueve en el plano YZ , por lo que simplemente podemos ignorar la coordenada x . Para preservar esta ventaja al encontrar los dos ángulos restantes Ѳ ₂ y Ѳ ₃ , utilizamos la simetría de la construcción del robot delta. Primero, gire nuestro sistema de coordenadas 120 ° en sentido antihorario en el plano XY alrededor del eje Z :

Tenemos un nuevo sistema de coordenadas X'Y'Z ' , y en este nuevo sistema podemos usar nuestras fórmulas listas para encontrar el ángulo Ѳ ₂ . La única sutileza es que primero debemos recalcular las coordenadas del punto E ₀ en el nuevo marco de referencia. Esto se hace fácilmente usando la fórmula bien conocida (transformaciones cuando el sistema gira alrededor del origen), que se muestra en la figura anterior. Para encontrar el ángulo Ѳ ₃También será necesario rotar el marco de referencia original, pero ahora en sentido horario. Esta técnica se implementa muy convenientemente en forma de programa: es suficiente escribir una función para calcular el ángulo Ѳ en el plano YZ, y luego llamarlo tres veces para cada uno de los ángulos y sistemas de referencia.

Cinemática directa

Intentemos resolver el problema inverso: ahora conocemos los ángulos Ѳ ₁ , Ѳ ₂ y Ѳ ₃ , y queremos encontrar las coordenadas (x ₀ , y ₀ , z ₀ ) del punto E ₀ ubicado en el centro de la plataforma móvil de nuestro robot. Conociendo los ángulos, podemos encontrar fácilmente las coordenadas de los puntos J ₁ , J ₂ y J ₃ (ver la figura a continuación). Palancas J ₁ E ₁ , J ₂ E ₂ y J ₃ E₃ puede girar libremente alrededor de los puntos J ₁ , J ₂ y J _3, respectivamente, formando en el espacio tres esferas con radios r _e .

Utilizamos una técnica complicada: desplazamos los centros de cada una de estas esferas desde los puntos J ₁ , J ₂ , J ₃ en el plano XY en la dirección del eje Z , utilizando los vectores de desplazamiento E ₁ E ₀ , E ₂ E ₀ y E ₃ E _0, respectivamente (en la figura se muestra como flechas rojas). Después de esta transformación, resulta que las tres esferas se cruzan en el punto E ₀ , como se muestra en la siguiente figura:

Resulta que para determinar las coordenadas (x ₀ , y ₀ , z ₀ ) del punto E _0, debemos encontrar el punto de intersección de tres esferas, los radios y coordenadas de los centros que conocemos. En otras palabras, necesitamos resolver un sistema de tres ecuaciones que describan esferas tridimensionales:

$(x-x_i)^2 + (y-y_i)^2 + (z-z_i)^2 = r_e^2,$

donde (x _i , y _i , z _i ) son las coordenadas de los centros de las esferas J ' ₁ , J' ₂ y J ' ₃ , que se pueden encontrar de la siguiente manera:

A continuación, para acortar la notación, usaré la notación (x ₁ , y ₁ , z ₁ ) , (x ₂ , y ₂ , z ₂ ) y (x ₃ , y como las coordenadas de los puntos J ' ₁ , J' ₂ y J ' ₃₃ , z ₃ ), respectivamente. También quiero señalar que x ₁ = 0 (ya que el punto J ' ₁ está en el plano YZ ). Obtenemos el siguiente sistema de ecuaciones:

$\begin{equation*} \begin{cases} x^2+(y-y_1)^2+(z-z_1)^2 = r_e^2 \\ (x-x_2)^2+(y-y_2)^2+(z-z_2)^2 = r_e^2 \\ (x-x_3)^2+(y-y_3)^2+(z-z_3)^2 = r_e^2 \end{cases} \end{equation*} \Rightarrow \begin{equation*} \begin{cases} x^2+y^2+z^2-2y_1y-2z_1z=r_e^2-y_1^2-z_1^2 \\ x^2+y^2+z^2-2x_2x-2y_2y-2z_2z=r_e^2-x_2^2-y_2^2-z_2^2 \\ x^2+y^2+z^2-2x_3x-2y_3y-2z_3z=r_e^2-x_3^2-y_3^2-z_3^2 \end{cases} \end{equation*}$

Introducimos la notación

$w_i=x_i^2+y_i^2+z_i^2$

y, restando el segundo y el tercero de la ecuación superior, así como el tercero de la segunda, obtenemos:

$\begin{equation*} \begin{cases} x_2x+(y_1-y_2)y+(z_1-z_2)z=(w_1-w_2)/2 \\ x_3x+(y_1-y_3)y+(z_1-z_3)z=(w_1-w_3)/2 \\ (x_2-x_3)x+(y_2-y_3)y+(z_2-z_3)z=(w_2-w_3)/2 \end{cases} \end{equation*}$

Restando el segundo de la primera ecuación (reduciendo así y ) y el tercero del segundo (reduciendo x ), podemos expresar x e y en términos de z :

$x=a_1z+b_1\qquad y=a_2z+b_2$

$a_1={1\over{d}}\left[(z_2-z_1)(y_3-y_1)-(z_3-z_1)(y_2-y_1)\right] \qquad a_2=-{1\over{d}}\left[(z_2-z_1)x_3-(z_3-z_1)x_2\right]$

$b_1 = - {1 \ over {2d}} \ left [(w_2-w_1) (y_3-y_1) - (w_3-w_1) (y_2-y_1) \ right] \ qquad b_2 = {1 \ over {2d}} \ left [(w_2-w_1) x_3- (w_3-w_1) x_2 \ right]$

$d = (y_2-y_1) x_3- (y_3-y_1) x_2$

Ahora, sustituyendo x e y , expresado a través de z , en la ecuación para el primer círculo (centrado en el punto J ' ₁ ), obtenemos:

$(a_1 ^ 2 + a_2 ^ 2 + 1) z ^ 2 + 2 (a_1 + a_2 (b_2-y_1) -z_1) z + (b_1 ^ 2 + (b_2-y_1) ^ 2 + z_1 ^ 2-r_e ^ 2) = 0$

Queda por resolver esta ecuación cuadrática (de la manera estándar, a través del discriminante) para encontrar z (¡recordamos que debemos elegir la más pequeña de las dos z !), Y a través de ella x e y .

Ejemplos de código fuente

Los siguientes son ejemplos de funciones para el cálculo de la cinemática del robot delta en C. Los nombres de las variables corresponden a los utilizados en el artículo, los ángulos theta1, theta2y theta3se especifican en grados.

//  
// ( .  )
const float e = 115.0;     //   
const float f = 457.3;     //   
const float re = 232.0;
const float rf = 112.0;

//  
const float sqrt3 = sqrt(3.0);
const float pi = 3.141592653;    // PI
const float sin120 = sqrt3/2.0;
const float cos120 = -0.5;
const float tan60 = sqrt3;
const float sin30 = 0.5;
const float tan30 = 1/sqrt3;

//  : (theta1, theta2, theta3) -> (x0, y0, z0)
//  : 0=OK, -1= 
int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
    float t = (f-e)*tan30/2;
    float dtr = pi/(float)180.0;

    theta1 *= dtr;
    theta2 *= dtr;
    theta3 *= dtr;

    float y1 = -(t + rf*cos(theta1));
    float z1 = -rf*sin(theta1);

    float y2 = (t + rf*cos(theta2))*sin30;
    float x2 = y2*tan60;
    float z2 = -rf*sin(theta2);

    float y3 = (t + rf*cos(theta3))*sin30;
    float x3 = -y3*tan60;
    float z3 = -rf*sin(theta3);

    float dnm = (y2-y1)*x3-(y3-y1)*x2;

    float w1 = y1*y1 + z1*z1;
    float w2 = x2*x2 + y2*y2 + z2*z2;
    float w3 = x3*x3 + y3*y3 + z3*z3;

    // x = (a1*z + b1)/dnm
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

    // y = (a2*z + b2)/dnm;
    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

    // a*z^2 + b*z + c = 0
    float a = a1*a1 + a2*a2 + dnm*dnm;
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

    // 
    float d = b*b - (float)4.0*a*c;
    if (d < 0) return -1; //  

    z0 = -(float)0.5*(b+sqrt(d))/a;
    x0 = (a1*z0 + b1)/dnm;
    y0 = (a2*z0 + b2)/dnm;
    return 0;
}

//  
//  ,   theta1 (  YZ)
int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
    y0 -= 0.5 * 0.57735 * e;       //    
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // 
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
    if (d < 0) return -1; //  
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); //   
    float zj = a + b*yj;
    theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
    return 0;
}

//  : (x0, y0, z0) -> (theta1, theta2, theta3)
//  : 0=OK, -1= 
int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
    theta1 = theta2 = theta3 = 0;
    int status = delta_calcAngleYZ(x0, y0, z0, theta1);
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2);  // rotate coords to +120 deg
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
    return status;
}

Literatura usada

Tomé todas las ideas clave sobre la cinemática del robot delta del trabajo del profesor. Paul Zsombor-Murray " Análisis cinemático geométrico descriptivo del robot" Delta "de Clavel" . Honestamente admito que mi entrenamiento matemático no fue suficiente para entenderlo hasta el final, así que tuve que deducir mucho.

Conclusión

Gracias a todos los que leyeron este artículo hasta el final. Espero que para alguien sea útil e inspirador para crear sus propias versiones del robot delta.

Cinemática de un robot Delta