🍭 📗 🖖 Kinematik eines Delta-Roboters 🌑 🌴 🛂

Im Jahr 2009 war ich begeistert davon, meinen eigenen Industrieroboter zu bauen, der etwas Nützliches bewirken kann (nämlich kleine Teile auf einem Förderband sortieren). Ich muss sofort sagen, dass ich einen Roboter gebaut habe (das Ergebnis sehen Sie auf dem Titelfoto) und gleichzeitig als Nebenprodukt einen kurzen Artikel über die Kinematik von Delta-Robotern im TrossenRobotics-Forum geschrieben habe - einem amerikanischen Verkäufer von Roboterteilen. Zu dieser Zeit veranstalteten sie gerade einen Wettbewerb für Autoren. Natürlich habe ich den Wettbewerb nicht gewonnen, aber der Artikel in englischer Sprache blieb. Ich habe mehrmals versucht, es in meine Muttersprache zu übersetzen, aber ich habe es geschafft, das zu vervollständigen, was ich erst jetzt begonnen habe.

Wenn Sie Ihr Modell eines Delta-Roboters erstellen oder einfach nur herausfinden möchten, wie Sie kinematische Formeln für diesen Robotertyp ableiten können (ohne über den Lehrplan in Algebra und Geometrie hinauszugehen), sind Sie bei cat willkommen. Für diejenigen, die Theorie nicht wirklich mögen, finden Sie am Ende des Artikels Beispiele für vorgefertigten Code in C.

Was ist ein Delta-Roboter?

Der Schweizer Wissenschaftler Raymond Clavel hat den Delta-Roboter ( Wiki ) in den frühen 1980er Jahren erfunden. Das Folgende ist eine Illustration aus dem ursprünglichen US4976582-Patent für „Ein Gerät zum Bewegen und Positionieren eines Elements im Raum“:

Der Roboter besteht aus zwei Plattformen: einer festen oberen Basis (1) und einer kleinen beweglichen Plattform (8), die durch drei Hebel verbunden sind. Jeder Hebel besteht aus zwei Teilen: Die obere Schulter (4) ist fest mit dem Motor (3) verbunden, der sich an der oberen Basis befindet, und der untere ist ein Parallelogramm (5), in dessen Ecken das sogenannte Universalgelenke (6, 7) ( Wiki), mit denen sich Winkel ändern können. Jedes Parallelogramm ist durch ein Scharnier (16) mit dem Oberarm verbunden, so dass seine Oberseite immer senkrecht zu seinem Arm und parallel zur Ebene der oberen Basis bleibt. Dank dessen ist die bewegliche Plattform des Roboters, die an den unteren Seiten der Parallelogramme angebracht ist, auch immer parallel zur oberen Basis. Wir können die Position der Plattform steuern, indem wir den Drehwinkel der oberen Hebel relativ zur Basis des Roboters mithilfe von Motoren ändern.

In der Mitte der unteren Plattform (8) befindet sich die sogenannte der Arbeitskörper (auf Englisch wird der Begriff Endeffektor verwendet) des Roboters (9). Dies kann ein Manipulator, eine Greifvorrichtung oder beispielsweise ein Extruder im Fall eines 3D-Druckers sein. Zusätzlich kann ein anderer Motor (11) verwendet werden, der eine Drehung des Arbeitskörpers durch die Stange (14) bereitstellt.

Der Hauptvorteil von Delta-Robotern ist die Geschwindigkeit: Schwere Motoren stehen auf einer festen Basis, nur die Hebel und die untere Plattform bewegen sich, was sie versuchen, aus leichten Verbundwerkstoffen herzustellen, wodurch ihre Trägheit verringert wird. Hier zum Beispiel ein Artikel über Geektimes mit ein paar sehr effektiven Videos.

Aufgabenstellung

Um Ihren eigenen Delta-Roboter zu bauen, müssen Sie lernen, wie Sie zwei Probleme lösen. In der ersten Situation kennen wir die Position, in die wir den Roboterarm unseres Roboters bewegen möchten (zum Beispiel möchten wir einen Cookie auf dem Förderband an einem Punkt mit Koordinaten (x, y, z) greifen. Dazu müssen wir die Winkel bestimmen, in denen wir uns befinden Sie müssen die mit den Hebeln des Roboters verbundenen Motoren drehen, um ihn in die richtige Position zum Greifen zu bringen. Das Verfahren zum Bestimmen dieser Winkel wird als inverses kinematisches Problem bezeichnet (in einigen russischsprachigen Quellen wird das Wort „invers“ verwendet).
In der zweiten Situation kennen wir die Winkel, in denen die Steuermotoren des Roboters eingeschaltet sind (wenn wir Servomotoren verwenden, können wir die Ecken leicht finden, indem wir die Messwerte von den Winkelsensoren ablesen), und wir möchten die Position der Roboterplattform im Raum ermitteln (zum Beispiel, um ihre Position anzupassen). . Dies ist eine direkte kinematische Aufgabe.

Wir formalisieren beide Aufgaben. Sowohl die feste Basis des Roboters als auch seine bewegliche Plattform können als gleichseitige Dreiecke dargestellt werden: In der folgenden Abbildung sind sie grün bzw. pink lackiert. Die Drehwinkel der Roboterarme relativ zur Basisebene (sie sind auch die Drehwinkel der Motoren) werden als Ѳ ₁ , Ѳ ₂ und Ѳ _{3 bezeichnet} , und die Koordinaten des Punktes sind E ₀befindet sich in der Mitte der beweglichen Plattform und in der der Roboterarm unseres Roboters im realen Leben fixiert wird - wie (x ₀ , y ₀ , z ₀ ) .

Es stellt sich heraus, dass wir uns zwei Funktionen einfallen lassen müssen:

f _invers (x ₀ , y ₀ , z ₀ ) → (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) , um das inverse kinematische Problem zu lösen und
f _vorwärts (Ѳ ₁ , Ѳ ₂ , Ѳ ₃ ) → (x ₀ , y ₀ , z ₀ ) , um das direkte kinematische Problem zu lösen.

Inverse Kinematik

Lassen Sie uns einige Schlüsselparameter einstellen, die durch die geometrischen Abmessungen unseres Roboters bestimmt werden:

Bezeichne obere Basisseitenlänge f , die untere Seite der Plattform e , obere Hebelarmlänge R _f und die unteren Armlänge (die lange Seite des Parallelogramms) r _e . Für Berechnungen wählen wir ein Koordinatensystem mit einem Referenzpunkt, der mit dem geometrischen Zentrum des oberen Dreiecks übereinstimmt. Die Z- Achse wird nach oben gerichtet, sodass die Z-Koordinate der sich bewegenden Plattform immer negativ ist.

Die Konstruktion des Roboters impliziert, dass sich der Hebel F ₁ J ₁ (siehe Abbildung unten) nur in der YZ- Ebene drehen kann , während ein Kreis mit dem Radius r _{f beschrieben wird} , der an einem Punkt zentriert istF ₁ (an dieser Stelle ist es am Motor befestigt). Im Gegensatz zu F _{1 verwenden} die Knoten J ₁ und E ₁ Universalgelenke, wodurch sich die Schulter E ₁ J ₁ relativ zu E ₁ frei drehen kann , was eine Kugel mit dem Radius r _{e beschreibt,} die am Punkt E ₁ zentriert ist .

Der Schnittpunkt dieser Kugel und der YZ- Ebene ist ein Kreis, der am Punkt E ' _{1 des} Radius E' ₁ J ₁ zentriert ist , wobei sich der Punkt E ' ₁ als Projektion des Punktes E ₁ auf die YZ- Ebene befindet . Dann liegt der Punkt J ₁ am Schnittpunkt zweier Kreise mit Mittelpunkten an den Punkten E ' ₁ und F ₁ , und wir können die Radien dieser Kreise bestimmen. Es gibt eine leichte Subtilität: Die Kreise schneiden sich an zwei Punkten, aber wir interessieren uns nur für einen von ihnen - mit einem kleineren Wert der y- Koordinate, weil wir wollen, dass die Arme des Roboters immer „Ellbogen“ herausragen. Nachdem wir so die Koordinaten des Punktes J _{1 bestimmt haben} , können wir leicht den Winkel _{1 finden, der} für uns _von Interesse ist .

Zur Erleichterung der Wahrnehmung ist die Projektion unseres dreidimensionalen Bildes auf die YZ- Ebene unten dargestellt :

Die untere Plattform ist ein gleichseitiges Dreieck, dessen Mittelpunkt der Punkt E ₀ (x ₀ , y ₀ , z ₀ ) ist . Mittlere Entfernung

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

Dies gibt uns die folgenden Koordinaten des Punktes E ₁ und seiner Projektion E ' ₁ auf die Ebene YZ :

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

Der Abstand E ₁ E ' ₁ = x ₀ , dann nach dem Satz von Pythagoras,

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

Da die obere Plattform auch ein gleichseitiges Dreieck ist, werden die Koordinaten an dem Punkt F ₁ wird

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

Um die Koordinaten des Punktes J _{1 zu finden} , der der Schnittpunkt zweier Kreise ist, muss das Gleichungssystem gelöst werden:

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

Wir kennen die Koordinaten der Kreismittelpunkte. Wenn wir sie ersetzen, erhalten wir den folgenden Ausdruck:

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

Wenn wir die Klammern öffnen und eine Gleichung von der anderen subtrahieren, können wir die z-Koordinate des Punktes J ₁ linear als y-Koordinate ausdrücken. Danach erhalten wir durch Einsetzen in die zweite Gleichung die übliche quadratische Gleichung für y, aus der zwei Lösungen die kleinste auswählen (mehr dazu wir haben oben gesprochen). Wenn wir also die Koordinaten des Punktes J _{1 erhalten} , finden wir den Winkel

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

Alle Ausdrücke erwiesen sich aufgrund der erfolgreichen Wahl des Koordinatensystems als recht einfach: Der Hebelarm F ₁ J ₁ bewegt sich immer in der YZ- Ebene , sodass wir die x- Koordinate einfach ignorieren können . Um diesen Vorteil beim Finden der beiden verbleibenden Winkel Ѳ ₂ und Ѳ _{3 zu erhalten} , verwenden wir die Symmetrie der Konstruktion des Delta-Roboters. Drehen Sie zuerst unser Koordinatensystem in der XY- Ebene um 120 ° gegen den Uhrzeigersinn um die Z- Achse :

Wir haben ein neues Koordinatensystem X'Y'Z ' und in diesem neuen System können wir unsere vorgefertigten Formeln verwenden, um den Winkel Ѳ ₂ zu finden . Die einzige Subtilität besteht darin, dass wir zuerst die Koordinaten des Punktes E ₀ im neuen Referenzrahmen neu berechnen müssen . Dies geschieht einfach mit der bekannten Formel (Transformationen, wenn sich das System um den Ursprung dreht), die in der obigen Abbildung dargestellt ist. Um den Winkel Ѳ ₃Es ist auch erforderlich, den ursprünglichen Referenzrahmen zu drehen, jetzt jedoch im Uhrzeigersinn. Diese Technik wird sehr bequem in Form eines Programms implementiert: Es reicht aus, eine Funktion zur Berechnung des Winkels Ѳ in der YZ-Ebene zu schreiben und sie dann für jedes der Winkel- und Referenzsysteme dreimal aufzurufen.

Direkte Kinematik

Versuchen wir, das inverse Problem zu lösen: Jetzt kennen wir die Winkel Ѳ ₁ , Ѳ ₂ und Ѳ ₃ und wollen die Koordinaten (x ₀ , y ₀ , z ₀ ) des Punktes E _{0 finden, der} sich in der Mitte der beweglichen Plattform unseres Roboters befindet. Wenn wir die Winkel kennen, können wir leicht die Koordinaten der Punkte J ₁ , J ₂ und J _{3 finden} (siehe die folgende Abbildung). Hebelschultern J ₁ E ₁ , J ₂ E ₂ und J ₃ E.₃ kann sich frei drehen können über die Punkte J ₁ , J ₂ und J ₃ jeweils einen Raumin den drei Kugeln mit Radien bildet , r _e .

Wir verwenden eine knifflige Technik: Wir verschieben die Zentren jeder dieser Kugeln von den Punkten J ₁ , J ₂ , J ₃ in der XY- Ebene in Richtung der Z- Achse unter Verwendung der Verschiebungsvektoren E ₁ E ₀ , E ₂ E ₀ bzw. E ₃ E ₀ (in der Abbildung sind sie) als rote Pfeile dargestellt). Nach dieser Transformation stellt sich heraus, dass sich alle drei Kugeln am Punkt E ₀ schneiden , wie in der folgenden Abbildung gezeigt:

Es stellt sich heraus, dass wir zur Bestimmung der Koordinaten (x ₀ , y ₀ , z ₀ ) des Punktes E ₀ den Schnittpunkt von drei Kugeln finden müssen, deren Radien und Koordinaten der Zentren uns bekannt sind. Mit anderen Worten, wir müssen ein System von drei Gleichungen lösen, die dreidimensionale Kugeln beschreiben:

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

wobei (x _i , y _i , z _i ) die Koordinaten der Zentren der Kugeln J ' ₁ , J' ₂ und J ' _{3 sind} , die wie folgt gefunden werden können:

Um die Notation zu verkürzen, verwende ich unten die Notation (x ₁ , y ₁ , z ₁ ) , (x ₂ , y ₂ , z ₂ ) und (x ₃ , y als Koordinaten der Punkte J ' ₁ , J' ₂ und J ' ₃₃ , z ₃ ) . Ich möchte auch beachten, dass x ₁ = 0 ist (da der Punkt J ' ₁ in der YZ- Ebene liegt ). Wir erhalten das folgende Gleichungssystem:

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

Wir führen die Notation ein

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

und wenn wir die zweite und dritte von der oberen Gleichung sowie die dritte von der zweiten subtrahieren, erhalten wir:

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

Subtrahiert man die zweite von der ersten Gleichung (wodurch y reduziert wird ) und die dritte von der zweiten Gleichung ( reduziert x ), so kann man x und y in Form von z ausdrücken :

$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 \ über {2d}} \ links [(w_2-w_1) (y_3-y_1) - (w_3-w_1) (y_2-y_1) \ rechts] \ qquad b_2 = {1 \ über {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$

Wenn wir nun x und y , ausgedrückt durch z , in der Gleichung für den ersten Kreis (zentriert am Punkt J ' ₁ ) einsetzen, erhalten wir:

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

Es bleibt, diese quadratische Gleichung (auf die übliche Weise durch die Diskriminante) zu lösen, um z zu finden (wir erinnern uns, dass wir das kleinste der beiden z wählen müssen !) Und durch sie x und y .

Beispiele für Quellcode

Die folgende sind Beispiele von Funktionen zur Berechnung der Kinematik des Delta - Roboters in C. Die Variablennamen entsprechen die in dem Artikel verwendet wird , der Winkel theta1, theta2und theta3sind in Grad angegeben.

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

Gebrauchte Literatur

Ich habe alle wichtigen Ideen zur Kinematik des Delta-Roboters aus der Arbeit von prof. Paul Zsombor-Murray " Beschreibende geometrische kinematische Analyse von Clavels" Delta "-Roboter ." Ich gebe ehrlich zu, dass meine mathematische Ausbildung nicht ausreichte, um sie bis zum Ende zu verstehen, daher musste viel von mir selbst abgeleitet werden.

Fazit

Vielen Dank an alle, die diesen Artikel bis zum Ende gelesen haben. Ich hoffe, dass es für jemanden nützlich und inspirierend sein wird, eigene Versionen des Delta-Roboters zu erstellen.

Kinematik eines Delta-Roboters