Heute war es also endlich so weit. Nach unzähligen Problemen konnte ich endlich die ersten koordinierten Bewegungen bewundern. Die Inverse Kinematik ermöglicht es, die Position des Bots alleine durch einen Punkt im Raum, sowie einem Drehwinkel pro Raumachse zu bestimmen.
Die größte Schwierigkeit hierbei ist es, die Winkel der 18 Gelenke zu jeder Zeit so zu bestimmen, dass der Punkt, an dem ein Bein den Boden berührt - zumindest so lange nicht anders von der Software vorgegeben - stets der selbe bleibt.
Nun aber zu allererst mal ein kurzes Video welches diesen Erfolg auch belegt. Die theoretische Abhandlung folgt dann weiter unten.
Nun also mehr dazu, wie es zu diesen eleganten Bewegungen kommt. Grundlage der Berechnungen ist die genaue Kenntnis über die mechanischen Abmessungen des Körpers, und auch die Längen aller Extremitäten. Als Referenz der Körperabmessungen habe ich einen Mittelpunkt definiert, dieser liegt auf halber Länge der Strecke zwischen den 2 mittleren Hüftservos und hat am Anfang der Berechnungen die Koordinaten (0/0/0) [X/Y/Z].
Ausgehen von diesem Punkt ist es nun nötig, die Koordinaten aller anderen Hüftservos zu bestimmen, diese ergeben sich allein druch die Abmessungen und müssen daher nicht zur Laufzeit berechnet werden. Da ich mir als Datentyp durchgehend der Luxus von double geleistet habe, kann ich ohne große Umrechnungen alle Koordinaten direkt als Millimeter interpretieren. eine verschiebung um 2cm in X Richtung würde demnach den Koordinaten (20/0/0) entsprechen. Diesen Datensatz bestehend aus den 6 Koordinaten der Hüftservos nenne ich im Folgenden "Normalebene", weil sie eben im Raum liegt.
Sind die Koordinaten der Hüftservos bekannt kommt die erste größere Berechnung. Sämtliche Punkte müssen um den Mittelpunkt gedreht werden, und das nacheinander um die Y, X und letzlich noch um die Z Achse (Achtung, eine andere Reihenfolge führt zu völlig anderen Ergebnisen!). Dazu ist jeder Punkt mit folgender Matrix zu multiplizieren:
Alpha entspricht dem Winkel um die X- Beta dem Winkel um die Y- und Gamma dem Winkel um die Z-Achse. Als nächstes werden diese gedrehten Punkte an die gewünschte Stelle im Raum transformiert. Dazu muss einfach zu jedem Punkt der gewünschte Vektor addiert werden.
Nun haben wir also bereits die Ausgangslage für alle weiteren Berechnungen. Als nächstes gilt es den Winkel des Hüftservos zu bestimmen. Wir gehen davon aus, das der Punkt, an dem das Bein den Boden berühren soll bekannt ist.
Diese Punkt hat mir lange Kopfzerbrechen bereitet, auf den ersten Blick sieht die Berechnung simpel aus - ist sie aber leider nicht, den dieser Winkel ändert sich praktisch in Abhängigkeit aller anderen Parameter. Um dem beizukommen berechne ich den Winkel zwischen 2 Ebenen, welche sich durch jeweils 3 Punkte definieren.
Dazu ist er zuerst nötig, eine neue Normalebene anzulegen und wie gehabt um den Mittelpunkt zu rotieren. Noch vor der Rotation wird jedoch jeder Punkt um einen beliebigen Betrag "Delata" in Z richtung verschoben. Dadurch ergibt sich quasi eine 2., virtuelle Ebene, die parallel zur eigentlichen Bot Ebeneverläuft, jedoch senkrecht zum Bot einen Abstand von "Delta" hat.
Nun können wir also die beiden Ebenen definieren, die nötig sind um den Hüftservowinkel eines Beins zu berechnen. Die Ebenen sind durch folgende Vektoren definiert:
Ebene 1 (Für jedes Bein neu zu bestimmen)
- Vektor vom Hüftservopunkt der Bot Ebene zum selben Punkt in der virtuellen Ebene
- Vektor vom Hüftservopunkt der Bot Ebene zum angepeilten Bodenpunkt des jeweiligen Beines
Ebene 2 (Dient für alle Beine als Referenz)
- Vektor zwischen 2 Hüftservopunkten, die in Längsrichtung des Bots direkt gegenüber liegen
- Vektor zwischen einem der eben verwendeten Hüftservopunkten und dem gleichen in der virtuellen Ebene.
Klingt kompliziert, ist es aber eigentlich nicht. Wenn jemand eine gute Software kennen sollte, mit der man solche Sachverhalte anschaulich darstellen kann bin ich gerne bereit einige Grafiken zu erstellen, um das ganze verständlicher aufzubereiten. Bis dahin empfehle ich jedem Bleistift und Geodreieck in die Hand zu nehmen und zu Zeichnen - so ist im übrigen der gesamte Algorithmus hier entstanden :)
Da nun die Ebenen durch jeweils 2 Vektoren bestimmt sind, brauchen wir zu jeder dieser Ebenen eine Normale. Eine Normale steht auf jedem der zwei Vektoren senkrecht. Eine elegante Methode diese zu berechnen bietet das so genannte Kreuzprodukt, es liefert ohne Umwege einen solchen Vektor.
Nachdem wir also die Normalvektoren der Ebene 1 und 2 gefunden haben, müssen wir nun nur noch den Schnittwinkel dieser 2 Vektoren bestimmen.
Alpha sei der gesuchte Winkel, a und b die beiden Vektoren. Die Multiplikation im Zähler ist ein Skalarprodukt.
Und schon haben wir mit einen der schwersten Teile geschafft, schlimmer wird es jetzt jedenfalls nicht mehr! Allen die mit Begriffen wie Vektor oder Skalarprodukt nichts anfanngen können möchte ich nahe legen sich die Grundlagen der Vektorgeometrie einzuverleiben. Mit ein wenig räumlichem Vorstellungsvermögen ist das keine Kunst.
Für alle weiteren Berechnungen brauchen wir einen neuen Ausgangspunkt. Nämlich die Koordinaten des Oberschenkelservos. Diese ergeben sich aus dem gefundenen Winkel und der Länge der Hüfte. Um diese Berechnung im schon gedrehten Körper vorzunehmen fehlen mir die Mathematischen Fähigkeiten. Daher gehe ich an dieser Stelle zurück in die Normalebene und berechne mir hier in der "Draufsicht" den entstehenden Versatz in X und Y Richtung. Dieser Versatz addiert zu den schon bekannten Koordinaten des Hüftservos ergeben die Position des Oberschenkelservos. Anschließend werden die Oberschenkelpunkte wie gehabt um den Mittelpunkt gedreht, und anschließend in den Raum transformiert. Somit sind auch diese Koordinaten bekannt.
Ab jetzt sind wir in der Glcülichen Lage, uns für alle weiteren Winkelberechnungen in die "Ebene 1" von weiter oben verziehen zu können, ganz egal wie schräg diese auch im raum liegen mag, die dortigen Winkel sind ab jetzt davon (fast) vollkommen unabhängig.
Blick auf diese Ebene:
Unter verwendung des Cosinussatzes kommt man zu folgenden Formeln:
Das wars, somit ist jeder Winkel bekannt. Es gibt jedoch noch einige Details die es zu beachten gilt, ohne Garantie auf Vollständigkeit...
- Abhängig von der Einbaurichtung der Servos, rechte/linke Seite des Bots etc, gewünschte Nulllage etc. kann, bzw. wird es nötig sein, den Wertebereich der Winkel anzupassen, also z.b. 90° addieren, oder mit -1 multiplizieren. Dies hängt jedoch von vielen Faktoren ab, und kann im Zweifel auch empirisch ermittelt werden.
- Die Bot Ebene liegt in Z Richtung auf Höhe des Oberschenkelgelenks und somit nicht zwangsweise auf gleicher Höhe mit der Unterkante des Körpers. Wer mit der Z Koordinate direkt die Höhe über Grund angeben will, muss hier noch nach bessern.
Wie immer... bei Fragen stehe ich gerne zu Verfügung und werde bei Bedarf auch Teile der Umsetzung in C herausgeben :)
