👩🏾‍🤝‍👩🏻 🤹🏻 🧗 Cinemática de um robô Delta 🧑🏽 🐕 🛄

Em 2009, fiquei empolgado com a construção do meu próprio robô industrial que poderia fazer algo útil (a saber, classificar pequenas peças em uma correia transportadora). Devo dizer imediatamente que construí um robô (você vê o resultado na foto do título) e, ao mesmo tempo, como subproduto, escrevi um pequeno artigo sobre a cinemática dos robôs delta no fórum TrossenRobotics - um vendedor americano de kits de peças de robôs. Eles apenas realizaram um concurso para autores na época. Obviamente, não ganhei a competição, mas o artigo em inglês permaneceu. Várias vezes tentei traduzi-lo para o meu idioma nativo, mas consegui concluir o que comecei apenas agora.

Se você deseja construir seu modelo de robô delta, ou apenas descobrir como você pode derivar fórmulas cinemáticas para esse tipo de robô (sem ir além do currículo escolar em álgebra e geometria) - bem-vindo ao gato. Para quem realmente não gosta de teoria, no final do artigo são exemplos de códigos prontos em C.

O que é um robô delta

O cientista suíço Raymond Clavel inventou o robô delta ( wiki ) no início dos anos 80, a seguir é uma ilustração da patente original US4976582 para “Um dispositivo para mover e posicionar um elemento no espaço”:

O robô consiste em duas plataformas: uma base superior fixa (1) e uma pequena plataforma móvel (8) conectada por três alavancas. Cada alavanca consiste em duas partes: o ombro superior (4) é rigidamente conectado ao motor (3) localizado na base superior e o inferior é um paralelogramo (5), nos cantos dos quais o chamado juntas universais (6, 7) ( wiki) que permitem que os ângulos mudem. Cada paralelogramo é conectado ao braço por uma dobradiça (16), para que o lado superior permaneça sempre perpendicular ao braço e paralelo ao plano da base superior. Graças a isso, a plataforma móvel do robô acoplada aos lados inferiores dos paralelogramos também será sempre paralela à base superior. Podemos controlar a posição da plataforma alterando o ângulo de rotação das alavancas superiores em relação à base do robô usando motores.

No centro da plataforma inferior (8), o chamado o corpo de trabalho (em inglês é utilizado o termo efetor final) do robô (9). Pode ser um manipulador, um dispositivo de preensão ou, por exemplo, uma extrusora no caso de uma impressora 3D. Além disso, outro motor (11) pode ser usado, o que fornece rotação do corpo de trabalho através da haste (14).

A principal vantagem dos robôs delta é a velocidade: os motores pesados são colocados em uma base fixa, apenas as alavancas e a plataforma inferior se movem, que eles tentam fazer com materiais compostos leves, reduzindo sua inércia. Aqui, por exemplo, um artigo sobre o Geektimes com alguns vídeos muito eficazes.

Declaração de tarefa

Para criar seu próprio robô delta, você precisa aprender como resolver dois problemas. Na primeira situação, sabemos a posição para a qual queremos mover o braço do robô (por exemplo, queremos pegar um cookie que está no transportador em um ponto com coordenadas (x, y, z) . Para fazer isso, precisamos determinar os ângulos em que eles devem girar os motores conectados às alavancas do robô para ajustá-lo na posição correta de preensão. O procedimento para determinar esses ângulos é chamado de problema cinemático inverso (em algumas fontes em língua russa, a palavra “inverso” é usada).
Na segunda situação, sabemos os ângulos em que os motores de controle do robô estão ligados (se usarmos servomotores, podemos encontrar facilmente os cantos lendo as leituras dos sensores de ângulo) e queremos encontrar a posição da plataforma do robô no espaço (por exemplo, para ajustar sua posição) . Esta é uma tarefa cinemática direta.

Formalizamos as duas tarefas. Tanto a base fixa do robô quanto sua plataforma móvel podem ser representadas como triângulos equilaterais: no diagrama abaixo, eles são pintados em verde e rosa, respectivamente. Os ângulos de rotação dos braços do robô em relação ao plano base (também são os ângulos de rotação dos motores) são designados como Ѳ ₁ , Ѳ ₂ e Ѳ ₃ , e as coordenadas do ponto são E ₀localizado no centro da plataforma móvel e no qual o braço do robô será fixado na vida real - como (x ₀ , y ₀ , z ₀ ) .

Acontece que precisamos criar duas funções:

f _inversa (x ₀ , y ₀ , z ₀ ) → (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) para resolver o problema cinemático inverso e
f _{para a frente} (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) → (x ₀ , y ₀ , z ₀ ) para resolver o problema cinemático direto.

Cinemática inversa

Vamos definir alguns parâmetros importantes que são determinados pelas dimensões geométricas do nosso robô:

Designações de dimensão para o Delta Robot

Vamos denotar o comprimento do lado da base superior f , o lado da plataforma inferior e , o comprimento do braço da alavanca r _f e o comprimento do braço (lado longo do paralelogramo) r _e . Para os cálculos, escolhemos um sistema de coordenadas com um ponto de referência que coincide com o centro geométrico do triângulo superior. O eixo Z é direcionado para cima, portanto a coordenada z da plataforma móvel sempre será negativa.

O design do robô implica que a alavanca F ₁ J ₁ (veja a figura abaixo) possa girar apenas no plano YZ , enquanto descreve um círculo de raio r _f centralizado em um pontoF ₁ (neste local, é anexado ao motor). Ao contrário da M ₁ , os nós J ₁ e E ₁ utilizações as juntas de articulação, por meio da qual o ombro E _1, J ₁ pode rodar livremente em relação a E ₁ , descrevendo uma esfera de raio r _e centrada no ponto E ₁ .

A intersecção da esfera e o plano YZ é um círculo com o centro no ponto E ' _um raio E' ₁ J ₁ , em que o ponto E ' ₁ é uma projecção do ponto E ₁ no plano YZ . Em seguida, o ponto J ₁ vai ser localizado na intersecção de dois círculos com centros nos pontos de E ' ₁ e F ₁ , com os raios dos círculos, podemos determinar. Há uma leve sutileza: os círculos se cruzam em dois pontos, mas estamos interessados apenas em um deles - com um valor menor da coordenada y, porque queremos que os braços do robô sempre fiquem "cotovelos". Tendo assim determinado as coordenadas do ponto J ₁ , podemos encontrar facilmente o ângulo _{1 de seu} interesse .

Para facilitar a percepção, a projeção de nossa imagem tridimensional no plano YZ é mostrada abaixo :

A plataforma inferior é um triângulo equilátero, cujo centro é o ponto E ₀ (x ₀ , y ₀ , z ₀ ) . Distância média

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

que nos fornece as seguintes coordenadas do ponto E ₁ e sua projeção E ' ₁ no plano YZ :

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

A distância E ₁ E ' ₁ = x ₀ , então, de acordo com o teorema de Pitágoras,

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

Como a plataforma superior também é um triângulo equilátero, as coordenadas do ponto F ₁ serão

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

Para encontrar as coordenadas do ponto J ₁ , que é a interseção de dois círculos, você precisa resolver o sistema de equações:

$\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*}$

Conhecemos as coordenadas dos centros dos círculos; se as substituirmos, obtemos a seguinte expressão:

$\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*}$

Se abrirmos os colchetes e subtrairmos uma equação da outra, podemos expressar linearmente a coordenada z do ponto J _{1 em} termos da coordenada y, após o que, substituindo-a na segunda equação, obtemos a equação quadrática usual para y, da qual duas soluções escolhemos a menor (mais sobre isso nós conversamos acima). E assim, obtendo as coordenadas do ponto J ₁ , encontramos o ângulo

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

Todas as expressões foram bastante simples devido à escolha bem-sucedida do sistema de coordenadas: o braço da alavanca F ₁ J ₁ sempre se move no plano YZ , para que possamos simplesmente ignorar a coordenada x . Para preservar essa vantagem ao encontrar os dois ângulos restantes Ѳ ₂ e Ѳ ₃ , usamos a simetria da construção do robô delta. Primeiro, gire nosso sistema de coordenadas 120 ° no sentido anti-horário no plano XY em torno do eixo Z :

Temos um novo sistema de coordenadas X'Y'Z ' e, neste novo sistema, podemos usar nossas fórmulas prontas para encontrar o ângulo Ѳ ₂ . A única sutileza é que devemos primeiro recalcular as coordenadas do ponto E ₀ no novo quadro de referência. Isso é feito facilmente usando a fórmula conhecida (transformações quando o sistema gira em torno da origem), mostrada na figura acima. Para encontrar o ângulo Ѳ ₃também será necessário girar o quadro de referência original, mas agora no sentido horário. Essa técnica é muito convenientemente implementada na forma de um programa: basta escrever uma função para calcular o ângulo Ѳ no plano YZ e chamá-la três vezes para cada um dos ângulos e sistemas de referência.

Cinemática direta

Vamos tentar resolver o problema inverso: agora conhecemos os ângulos Ѳ ₁ , Ѳ ₂ e Ѳ ₃ e queremos encontrar as coordenadas (x ₀ , y ₀ , z ₀ ) do ponto E ₀ localizado no centro da plataforma móvel do nosso robô. Conhecendo os ângulos, podemos encontrar facilmente as coordenadas dos pontos J ₁ , J ₂ e J ₃ (veja a figura abaixo). Alavancas J ₁ E ₁ , J ₂ E ₂ e J ₃ E₃ pode ser rodado livremente em torno dos pontos J ₁ , J ₂ e J ₃ , respectivamente, formando um espaço de três esferas com raios r _e .

Utilizamos uma técnica complicada: deslocamos os centros de cada uma dessas esferas dos pontos J ₁ , J ₂ , J ₃ no plano XY na direção do eixo Z , usando os vetores de deslocamento E ₁ E ₀ , E ₂ E ₀ e E ₃ E _0, respectivamente (na figura eles mostrado como setas vermelhas). Após essa transformação, as três esferas se cruzam no ponto E ₀ , como mostra a figura abaixo:

Acontece que, para determinar as coordenadas (x ₀ , y ₀ , z ₀ ) do ponto E _0, devemos encontrar o ponto de interseção de três esferas, cujos raios e coordenadas são conhecidos por nós. Em outras palavras, precisamos resolver um sistema de três equações descrevendo esferas tridimensionais:

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

onde (x _i , y _i , z _i ) são as coordenadas dos centros das esferas J ' ₁ , J' ₂ e J ' ₃ , que podem ser encontradas da seguinte forma:

Abaixo, para encurtar a notação, usarei a notação (x ₁ , y ₁ , z ₁ ) , (x ₂ , y ₂ , z ₂ ) e (x ₃ , y como coordenadas dos pontos J ' ₁ , J' ₂ e J ' ₃₃ , z ₃ ), respectivamente. Também quero observar que x ₁ = 0 (já que o ponto J ' ₁ está no plano YZ ). Temos o seguinte sistema de equações:

$\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*}$

Introduzimos a notação

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

e, subtraindo o segundo e o terceiro da equação superior, bem como o terceiro da segunda, obtemos:

$\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*}$

Subtraindo a segunda da primeira equação (reduzindo assim y ) e a terceira da segunda (reduzindo x ), podemos expressar x e y em termos 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 \ direita]$

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

Agora, substituindo x e y , expresso por z , na equação do primeiro círculo (centrado no ponto J ' ₁ ), obtemos:

$(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$

Resta resolver esta equação quadrática (da maneira padrão, através do discriminante) para encontrar z (lembramos que devemos escolher o menor dos dois z !), E através dela x e y .

Exemplos de código-fonte

O que se segue são exemplos de funções para cálculo da cinemática do robô delta em C. Os nomes das variáveis correspondem às utilizadas no artigo, os ângulos theta1, theta2e theta3são especificadas em graus.

//  
// ( .  )
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

Peguei todas as idéias-chave sobre a cinemática do robô delta do trabalho do prof. Paul Zsombor-Murray " Análise Cinemática Geométrica Descritiva do Robô" Delta "de Clavel . Sinceramente, admito que meu treinamento matemático não foi suficiente para compreendê-lo até o fim, por isso muito foi deduzido por mim.

Conclusão

Obrigado a todos que leram este artigo até o fim. Espero que alguém seja útil e inspirador para criar suas próprias versões do robô delta.

Cinemática de um robô Delta