Hexapode - Inverse Kinematik PDF Drucken E-Mail
Geschrieben von: Malte   
Mittwoch, den 17. März 2010 um 16:58 Uhr

Gelenkstellungen | Das Problem der inversen Kinematik stellt sich in unserem Zusammenhang folgendermaßen dar: Wir möchten erreichen, dass der Fuß eines Beines eine bestimmte Position im Raum einnimmt. Diese Fußposition ist dabei Resultat der Stellung der drei Gelenke ist, die uns pro Bein zur Verfügung stehen. Welche Gelenkstellungen führen nun zu welchen Fußpositionen? Auf den ersten Blick mag dieses Problem etwas kompliziert erscheinen. Aber auch wenn hier tatsächlich das ein oder andere Teufelchen im Detail steckt, ist die Mathematik, derer es zur Lösung bedarf, nicht übermäßig anspruchsvoll. Wenn man ein wenig mit trigonometrischen Funktionen umgehen kann (bzw. bereit ist, sie sich zu diesem Anlass anzueignen) kann man die hier relevante inverse Kinematik gut im Alleingang bewältigen.
Betrachten wir das Problem konkreter anhand der folgenden Skizze. In Anlehnung an die Beinanatomie von Arthropoden können wir an unserem künstlichen Bein b als Tibia ("Unterschenkel"), a als Femur ("Oberschnenkel") und BN als Coxa ("Hüfte") bezeichnen. Mithilfe des ersten Hüftservos kann das gesamte Bein in x-z- Ebene um N rotiert werden. Der entsprechende Winkel sei hier mit ρ (roh) bezeichnet. Konstruktionsbeding gibt es in der Hüfte eine Verschiebung zwischen der ersten und zweiten Drehschse, letztere liegt in B. Hier setzt der Oberschenkel a an, er kann dort vom zweiten Hüftservo in einer Ebene gedreht werden, die orthogonal zu der des ersten Hüftservos orientiert ist. Der Drehwinkel in B, gennant ε (epsilon), ist in Beziehung zu einer Senkrechten (Parallelen zur y-Achse) durch B definiert. Ingesamt hat die Hüfte so wie sie hier konstruiert ist also zwei Freiheitsgrade - die Natur hat hier typischerweise einen mehr im Angebot. In Punkt C ist der Unterschenkel b drehbar mit dem Oberschenkel verbunden, er rotiert in der gleichen Ebene wie der Oberschenkel. Der von den beiden Schenkeln eingeschlossene Winkel heißt γ (gamma). Punkt A ist der Fuß des Beines, aus dessen Koordinaten Ax, Ay und Az müssen letztlich die Stellwinkel ρ, ε und γ für die Servos errechnet werden.
Beginnen wir mit der Berechnung von ρ. Dieser Winkel ist natürlich unabhängig von der Höhe, die der Fuß einnehmen soll, dementsprechend fließt nur Ax und Az in die Berechnung ein. Wie man sich an obiger Skizze klar machen kann, entspricht der Tangens von ρ dem Verhältnis von Az zu Ax. Den Winkel ρ erhält man also mittels des Arkustangens von Az / Ax:

ρ = atan(Az / Ax)
(1)

Hier lauert nun aber schon die erste kleine Falle, denn wie man sieht, hat der Quotient, der hier das Argument bildet, das gleiche Vorzeichen für - Az und Ax wie für Az und - Ax. Diese beiden geometrisch klar unterschiedenen Situationen würden zu einem gleichen Wert für ρ führen - durchaus misslich! Wir können dem nun in zweierlei Weise begegnen. Zum einen könnte man den Arkustangens in einer Vierquadrantenversion verwenden, dieser würde nicht nur Rückgabewerte zwischen -½π (-90°) und ½π (90°), sondern zwischen -π (-180°) und π (180°) liefern und damit unterschiedliche Winkel für positive und negative z- und x-Werte ergeben. Viele Programmiersprachen stellen die entsprechende Funktion fertig zur Verfügung, in Matlab z. B. heisst sie atan2, in Bascom atn2. Die andere Variante ist das Einführen einer Randbedingung. Wenn man schlicht und einfach verbietet, dass einer der Werte Ax oder Az negativ werden darf, bleibt die Sache auch eindeutig. Ich habe mich hier für diese letztere Variante entschieden, Ax darf also nicht kleiner als 0 sein.

Ax < 0 → fehler = 1, sonst fehler = 0
(2)

Warum diese schlichte Lösung? Erstens deshalb, weil konstruktionsbedingt ohnehin nur wenige Positionen mit negativen Ax-Werten einnehmbar sind, und zweitens, weil diese Ax-Werte für die Laufkoordination völlig verzichtbar sind. Somit können wir an der Berechnung von ρ gemäß (1) festhalten, sollten aber im Hinterkopf behalten, dass wir es dabei nicht mit einer allgemeinen Lösung zu tun haben. (1) erfüllt seinen Zweck, solange wir vorraussetzen, dass der Fuß nur in einem Bereich von 180° um die x-Achse drehbar ist.
Mit der Berechnung von ρ sind wir unserem Ziel schon ein gutes Stück näher gekommen: wir können das Bein damit bereits in eine Ebene drehen, in der der gesuchte Punkt A liegt. Um den Fuß genau auf diesen Punkt zu setzen, müssen wir nun die Servowinkel ε und γ berechnen.
Betrachten wir wieder obige Skizze. Wenn wir den Fuß, also Punkt A, in Gedanken mit dem Drehpunkt des zweiten Hüftgelenks, also Punkt B, verbinden, erhalten wir ein ordinäres Dreieck mit den Seiten a, b und c. Die Längen von a und b sind konstruktionsgemäß bekannt, denn es handelt sich dabei ja um Ober- und Unterschenkel des Beines. Wenn wir nun noch die Länge der Seite c kennen würden, ließe sich daraus ein Dreieck eindeutig konstruieren (wenn es unter den gegebenen Umständen denn überhaupt konstruierbar ist). Und wenn es eindeutig konstruierbar ist, muss es möglich sein, aus den bekannten Seitenlängen die resultierenden Winkel des Dreieicks zu bestimmen. Zunächst müssen wir also die Länge der Seite c berechnen. Man darf sich dabei nicht von den zweidimensionalen Darstellungen in die Irre führen lassen, c liegt im Raum, d.h. wir müssen bei der Rechnung alle drei Raumdimensionen berücksichtigen. Seite c ist die Hypotenuse eines gedachten rechtwinkligen Dreiecks, wie man in der kleinen Abbildung oben rechts sieht. Eine der Seiten dieses Dreiecks hat die Länge Ay, die andere Seite liegt in der x-z-Ebene, doch wie lang ist sie? Sie ist nun eben nicht Ax - BN lang, wie man aufgrund der 2D-Darstellung vermuten könnte. Unter Zurhilfenahme der kleinen Abbildung oben links kann man sich klar machen, dass wir eigentlich die Länge der Projektion von c (AB) - worauf Punkt C liegt - in die x-z-Ebene suchen. Klar wird dabei auch, dass diese Berechnung im Grunde einfach ist. Die linke Abbildung zeigt ja genau die gesuchte Projektion. Wir müssen also nur per Pythagoras aus Az und Ax die Länge von AN bestimmen und davon noch BN abziehen, damit hätten wir dann die Länge der x-z-Projektion von AB bestimmt. Zusammen mit Ay können wir dann damit abermals über Pythagoras die Länge von c bestimmen. Überführen wir das gesagte in eine kompakte mathematische Formulierung kommen wir zu:

c = (((Az2 + Ax2)½ - BN)2 + Ay2)½
(3)

Damit haben wir schon einen wesentlichen Schritt zur Berechnug der anderen beiden Servowinkel, beides Winkel des Dreiecks ABC, getan. Wir lassen nun ersteinmal die Fälle außer Acht, in denen dieses Dreieck garnicht konstruierbar ist, weil die gewünschte Fußkoordinate außerhalb des Aktionsbereiches unseres Beins liegt. Berechnen wir zunächst Winkel γ, er ist ja der Stellwinkel für das Gelenk zwischen Ober- und Unterschenkel. Da wir die drei Seiten des fraglichen Dreiecks mittlerweile kennen, können wir diesen Winkel einfach über den Consinussatz bestimmen. Dieser lautet, passend für unsere Zwecke umgestellt:

γ = acos((a2 + b2 - c2) / (2ab))
(4)

Somit kennen wir nun auch γ, haben also schon ein gutes Stück des Weges hinter uns gebracht. Zu berechnen wäre nun noch der Winkel ε.
Dieses muss etwas indirekter geschehen. Wir benötigen zunächsteinmal den Winkel β, ein weiterer Innenwinkel des Dreiecks ABC und somit wiederrum ein Fall für den Cosinussatz:

β = acos((c2 + a2 - b2) / (2ca))
(5)

Wie die obige Skizze zeigt, ist der uns interessierende Winkel ε die Differenz zwischen einem als δ bezeichneten Winkel und dem grade berechneten Winkel β:

ε = δ - β
(6)

Der fragliche Winkel δ muss nun also bestimmt werden. Wie wir sehen, handelt es sich dabei um den Winkel, der zwischen c und der y-Achse (bzw. einer Parallelen zur y-Achse durch Punkt B) besteht. δ ist gegeben durch:

δ = acos(y/c)
(7)

Dass y und c Berechnungsgrundlage sind, dürfte klar sein, dass der Arkuscosinus zur Anwendung kommen muss, hingegen möglicherweise nicht sofort. Schließlich ist δ in der oben dargestellten Konfiguration nicht Innenwinkel des von y und c und der Verbindung deren beider Endpunkte gebildeten Dreiecks. Klarer wird die Sache, wenn man sich einen Fall vor Augen hält, bei dem Punkt A (als Endpunkt von c) eine positive y-Koordinate hat. Dann lässt sich ein rechtwinkeliges Dreieck konstruieren, dessen von y und c eingeschlossener Winkel gemäß (6) zu bestimmen ist. Die negative y-Koordinate in oben dargestelltem Falle führt zu Winkeln, die in einem rechtwinkligen Dreieck keine Innenwinkel sein können, eben weil der der Arkuscosinus für Werte <0 Winkel >90° ergibt. In diesem Falle ist das Ergebnis als Winkel aufzufassen, der den spitzen Winkel zwischen y und c zu 180° ergänzt, eben so wie es hier gewünscht ist. Nun ist uns auch δ bekannt, zusammen mit β, berechnet nach (4), können wir mittels (5) den Winkel ε bestimmen. Damit sind wir - eigentlich - am Ziel. Wir kennen die Servostellwinkel, die einer gewünschten Fußkoordinate korrespondieren. Allerdings müssen wir uns auch hier noch über einige Sezialfälle Gedanken machen, die unseren Ansatz austricksen würden. Stellen wir uns vor, wir gehen von dem in obiger Skizze dargestellen Fall aus, und nähern der Fuß immer weiter einer Senkrechten durch B. Die x-z-Projektion von c wird dadurch immer kürzer bis sie irgendwann 0 ist. Dann nämlich, wenn A auf der besagten Senkrechten durch B liegt. Was passiert, wenn wir diesen Weg weiter fortsetzen? Natürlich, die Länge der Projektion von c wird wieder größer und damit auch die nach (2) berechnete Länge von c. Somit ensteht hier aber offensichtlich eine Mehrdeutigkeit, denn wiederrum führen geometrisch verschiedene Situationen zu gleichen Ergebnissen in unserer Berechnung. Das wäre nur dann ein Mangel unseres Ansatzes, wenn wir dem nicht adäquat begegnen würden. Wir wählen wieder den restriktivsten, aber dafür auch einfachsten Weg, nämlich ein weiteres Verbot:

(Az2 + Ax2)½ - BN < 0 → fehler = 1, sonst fehler = 0
(8)

Und "Fehler" heisst hier wiederum, dass die Fußposition, die zur Erfüllung dieser Bedingung führt, als Ganze schlicht und einfach als unmöglich gilt.

 

Verbotene Gelenkstellungen | Im vorigen Abschnitt haben wir eine Möglichkeit dargestellt, aus einer angestrebten Fußposition diejenigen Servostellwinkel zu berechnen, die zu dieser Position führen. Grundsätzlich besteht aber das Problem, dass es bestimmte Kombinationen von Gelenkstellungen in Hüfte und Knie gibt, die aufgrund der Konstruktion tatsächlich nicht einnehmbar sind weil bestimmte Elemente des Beines sich in diesen Fällen im Wege stehen würden. Wüsste das System um diese Tatsache nicht, könnte der Versuch, eine solche Konstellation herzustellen, zu mechanischen Schäden führen, allemal würde dadurch ein Bewegungsablauf schwerwiegend gestört. Anhand der folgenden Abbildung möchte ich die Problematik erläutern und die von mir gewählte Lösung darstellen. Das schwarze Rechteck stellt die Hüftservoeinheit dar, von der der Oberschenkel ausgeht. Um den Endpunkt des Oberschenkels rotiert wiederrum der Unterschenkel. Beschreibender Text für Surfer ohne Bilder Aufgrund der Konstruktion des Beines liegt nun die Hüftservoeinheit in der Drehebene des Unterschenkels. Und genau daraus resultiert das oben angesprochene Problem: es gibt Gelenkstellungen, die die Konsequenz hätten, dass der Unterschenkel an die Servoeinheit gedrückt würde. Wie man sieht, ist dieses Problem abhängig von den Gelenkstellungen γ und ε, nicht aber von ρ, dieser Winkel dreht ja die ganze dargestellte Einheit. Da γ und ε in einer Ebene liegen, lässt sich dieses Problem auch in der Ebene beschreiben - und lösen. Um die räumlichen Verhältnisse einfach beschreiben zu können, denken wir uns den Drehpunkt des Unterschenkels, Punkt B, als Ursprung eines zweidimensionalen Koordniatensystems. Wohlbemerkt, dies ist nicht der Ursprung des Koordinatensystems, das wir für die inverse Kinematik definiert haben, der Ursprung ist um BN verschoben. Die Rotation des Oberschenkels bildet mit der y-Achse den uns ja bereits geläufigen Winkel ε. Um den anderen Endpunkt des Oberschenkels, Punkt C, rotiert der Unterschenkel, Winkel γ bildend, so wie wir es im vorherigen Abschnitt defniniert haben. Das genannte Problem tritt offensichtlich auf, wenn die Winkel γ und ε klein werden. Doch welche Kombinationen von γ und ε genau sind möglich und welche nicht? Wir betrachten der Einfachheit halber zunächst einmal die Elemente des Beines als nur zweidimensional ausgedehnt, sie haben also ein Länge, aber keinen Druchmesser. Nun bestimmen wir in dem eben beschriebenen Koordinatensystem (Ursprung: Punkt B) die Geradengleichung für diejenige Gerade, auf der b, der Unterschenkel, liegt. Allgemein formuliert heißt diese natürlich:

by = mb sx + ob
(9)

Hier sind erstmal noch einige Unbekannte im Spiel. Wir müssen die Steigung mb und den y-Achsenabschnitt ob berechnen. Die Steigung von b ergibt sich aus dem Kotangens des Winkels zwischen b und der y-Achse, dieser Winkel ist γ - ε. Also:

mb = cot(γ - ε)
(10)

Um den y-Achsenabschnit bestimmen zu können, benötigen wir zusätzlich einen Punkt auf b, wir wählen C:

ob' = Cx mb - Cy
(11)

Nun hatten wir die Situation zunächst dadurch vereinfacht, dass wir davon ausgingen, dass der Unterschenkel keinen Durchmesser hat. Das wäre schon sehr dünn. Wir müssen nun also noch die gewonnene Geradengleichung so in y-Richtung verschieben, dass wir tatsächlich die äußeren Grenzen dieses Elementes erhalten. Diese Verschiebung erreichen wir natürlich durch entsprechende Korrektur von ob. Der Betrag dieser Verschiebung hängt von der Stellung des Unterschenkels zur y-Achse ab. Wenn er 90° dazu ausgerichtet wäre, bräuchten wir nur um den Radius des Beines zu korrigieren, je steiler er zur y-Achse steht, je größer wird offensichtlich die erforderliche Verschiebung. Genaugenommen berechnet sie sich folgendermaßen:

v = r/sin(γ - ε)

ob = ob' - v

(12)
(13)

Wir müssen nun x- und y-Koordinate von C berechnen. Die Lage von C hängt natürlich von ε und der Länge von a ab, nämlich folgermaßen:

Cx = sin(-ε) a
Cy = cos(-ε) a

(14)
(15)

Jetzt können wir (9) bis (15) kombinieren und erhalten den etwas unübersichtlichen Ausdruck:

by = cot(γ - ε) sx + sin(-ε) a cot(γ - ε) - cos(-ε) a - r/sin(γ - ε)

(16)

Dieser gibt uns also ein gesuchtes by für ein beliebiges sx. Dies lässt die Lösung für unser Problem erkennen: eine verbotene Gelenkstellung liegt  augenscheinlich vor, wenn die y-Koordinate by kleiner als symax und gleichzeitig größer als symin wird. In diesen Fällen würde der Unterschenkel versuchen, in den vom zweiten Hüftservo belegten Raum einzudringen - woraus langfristig das Gehäuse des zweiten Hüftservos als Sieger über das Getriebe des Knieservos hervorgehen würde. Wir berechnen also für den maximal und minimal zulässigen sx-Wert die korrespondierenden by-Werte und überprüfen, ob diese im verbotenen Intervall zwischen symax und symin liegen:

(symax>by>symin) → fehler = 1, sonst fehler = 0
(17)

Die analoge Überprüfung führen wir nun auch noch für die x-Kordinate bx durch. Wir kommen zu dem Ausdruck:

bx = (sy - sin(-ε) a cot(γ - ε) - cos(-ε) a - r/sin(γ - ε)) / cot(γ - ε)

(18)

Und ob wir mit bx im problematischen Bereich zwischen sxmax und sxmin liegen definiert wiederum, ob die entsprechende Gelenkstellung möglich ist:

(sxmax>bx>sxmin) → fehler = 1, sonst fehler = 0
(19)

Damit wäre unsere der inversen Kinematik nachgeschalteten Fehleranalyse komplett.

Elektronik
Laufkoordination
Zuletzt aktualisiert am Freitag, den 02. September 2011 um 23:08 Uhr
 

Kommentare  

 
#1 Manuel Rodriguez 2016-12-01 13:35
Ich suche schon seit ungefähr einer halben Stunde über Google nach Weblogs auf Deutsch die sich mit Robotik beschäftigen. Dieses Blog hier und der obige Beitrag mit der inversen Kinematik ist der erste richtige Treffer den ich gefunden haben. Und wie ich sehe gibt es auch noch eine Blogroll wo weitere Links gesetzt sind.

Es ist jedenfalls schön zu wissen, dass es in der deutschen Blogosphäre überhaupt jemanden gibt der sich mit dem Problem der inversen Kinematik schonmal auseinandergese tzt hat. Und wie ich sehe wurde die Aufgabe mit einer geometrischen Methode (cosinus, sinus) gelöst, was schonmal ein guter Einstieg ist. Da ich natürlich diesen Kommentar ausschließlich in der Absicht der Verlinkung gesetzt habe, hier die obligatorische URL: https://trollheaven.wordpress.com/ Und ja, es handelt sich um Spam, und nein ich bin kein Bot.

P.S. der Captcha ist extrem schwer zu erkennen. Ist das womöglich ein versteckter IQ-Test?
 
 

Suche

Benutzer

Wir haben 32 Gäste online

unterstützt von

batronix logo

spacer

rotacaster logo

spacer

spacer

spacer

Meine Tweets

Extern

youtube logo twitter logo email logo

spacer

spacer

spacer

Reklame