🥜 👩‍❤️‍💋‍👨 🤺 Кинематика дельта-робота 🍚 ⌨️ 🤓

2009 , - ( — ). , ( ), , , - TrossenRobotics — . - . , , , . , .

-, , ( ) — . , , C.

-

- (wiki) 1980- , US4976582 « »:

: (1) (8), . : (4) (3), , (5), .. (6, 7) (wiki), . (16) , . , . , .

(8) .. ( end effector) (9). , , , 3D . (11), (14).

- : , , , . , , Geektimes .

-, . , (, , (x, y, z). , , , . ( «») .
, ( , , ), (, ). — .

. , : . ( — ) Ѳ₁, Ѳ₂ Ѳ₃, ₀, — (x₀, y₀, z₀).

, :

f_inverse(x₀, y₀, z₀) → (Ѳ₁, Ѳ₂, Ѳ₃)
f_forward(Ѳ₁, Ѳ₂, Ѳ₃) → (x₀, y₀, z₀) .

, :

f, e, r_f ( ) r_e. , . Z , , z- .

, F₁J₁ (. ) YZ, r_f F₁ ( ). F₁, J₁ E₁ , E₁J₁ E₁, r_e E₁.

YZ E’₁ E’₁J₁, E’₁ E₁ YZ. J₁ E’₁ F₁, . : , — y, , «» . J₁, Ѳ₁.

YZ:

, E₀(x₀, y₀, z₀). ,

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

E₁ E’₁ YZ:

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

E₁E’₁=x₀, , ,

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

, F₁

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

J₁, , :

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

, , :

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

, z- J₁ y-, , , y, ( ). J₁,

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

: F₁J₁ YZ, x. Ѳ₂ Ѳ₃, -. 120° XY Z:

X’Y’Z’, Ѳ₂. , E₀ . ( ), . Ѳ₃ , . : Ѳ YZ, .

: Ѳ₁, Ѳ₂ Ѳ₃, (x₀, y₀, z₀) E₀, . , J₁, J₂ J₃ (. ). J₁E₁, J₂E₂ J₃E₃ J₁, J₂ J₃ , r_e.

: J₁, J₂, J₃ XY Z, E₁E₀, E₂E₀ E₃E₀ ( ). , E₀, :

, (x₀, y₀, z₀) E₀ , . , , :

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

(x_i, y_i, z_i) — J’₁, J’₂ J’₃, :

J’₁, J’₂ J’₃ (x₁, y₁, z₁), (x₂, y₂, z₂) (x₃, y₃, z₃) . , x₁=0 ( J’₁ YZ). :

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

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

, , , :

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

(, , y) ( x), x y 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$

, x y, z, ( J’₁), :

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

( , ), z ( , z!), x y.

- . , , theta1, theta2 theta3 .

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

- . Paul Zsombor-Murray «Descriptive Geometric Kinematic Analysis of Clavel’s «Delta» Robot». , , , .

, . , - -.

Кинематика дельта-робота

-

More articles: