Das Projekt, das zu dieser Anmeldung geführt hat, wurde durch das Forschungsund Innovationsprogramm Horizont 2020 der Europäischen Union unter der Fördervereinbarung Nr. 101004205 gefördert.

Die Erfindung betrifft ein Verfahren zum Bestimmen der Lage eines Objektes durch Ermittlung der Drehraten des Objektes, wobei Drehwinkelinkremente A0 ^k für aufeinanderfolgende Zeitpunkte t _meas>k gemessen und zu einem kumulierten Drehwinkel AO^ut für einen die Zeitpunkte t _meas>k umfassenden Kumulations- Zeitbereich I zusammengefasst werden. Hierzu wird auf die Figur 1 verwiesen.

Die Erfindung betrifft weiterhin eine Messeinrichtung zum Bestimmen der Lage eines Objektes durch Ermittlung der Drehraten des Objektes, wobei die Messeinrichtung eine Sensoreinheit zum Messen von Drehwinkelinkrementen A0 ^k für aufeinanderfolgende Zeitpunkte t _meas>k hat und eine Datenverarbeitungseinheit, die eingerichtet ist, die an aufeinanderfolgenden Zeitpunkten t _meas>k gemessenen Drehwinkelinkremente A0 ^k zu einem kumulierten Drehwinkel A0' _ou t für einen die Zeitpunkte t-meas.fc mit den ganzen Zahlen k = 1 , ... , n umfassenden Zeitbereich I des Intervalls + £ _k=1 At _k ] zusammenzufassen, wobei At _k die Länge jeweils eines

Zeitintervalls t _mpa . _k — t _{mp:K k} i ist.

Zum Verständnis der Notation wird darauf hingewiesen, dass der Zeitbereich I (das Zeitintervall durch die Folge von n Messzeitpunkten t _meas>k , k = wobei t _n = t, ist, in n Zeitintervalle [t _m eas,fc-i>t _m eas,fc] aufgeteilt wird, wobei die individuellen Längen At _k = t _meas>k — t _mea s,fc-i unterschiedlich sein können. Der /-te Zeitbereich kann sich einen vorausgehenden, (/-1 )-ten Zeitbereich anschließen.

Gyroskope dienen der Messung der Drehrate (Winkelgeschwindigkeit) um eine vordefinierte Achse. Bei Verwendung mindestens dreier Gyroskope, die so orientiert i sind, dass die Richtungsvektoren entlang der Achsen ihrer Messung linear unabhängig sind, lässt sich der Winkelgeschwindigkeitsvektor w einer Drehung um eine beliebig im Raum orientierte Drehachse bestimmen.

Wenn die Gyroskope fest mit einem Objekt, wie insb. einem Fahrzeug (z. B. einer Rakete) verbunden sind, lässt sich aus der Drehrate durch Integration des Signals die (relative) Lageänderung des Fahrzeugs (Orientierung im Raum) berechnen. Bei bekannter Anfangsorientierung ist dann die absolute Orientierung des Fahrzeugs im Raum bekannt. Die Kenntnis der Orientierung ist die Eingangsgröße für die Lageregelung und für die Bestimmung der Position, wenn orientierungsabhängige Kräfte auf das Fahrzeug wirken, z. B. verursacht durch fahrzeugfeste Antriebs- oder Bremssystemen.

Üblicherweise werden die Gyroskope mit Beschleunigungssensoren zur Messung der Linearbeschleunigung in einem Sensorpaket (einer Inertialmesseinheit bzw. englisch: Inertial Measurement Unit (IMU)) gebündelt, welches als Modul verfügbar ist und zentraler Baustein für die Bestimmung der Orientierung und Position des Fahrzeugs ist. Die IMU enthält neben den Sensoren auch noch Stufen der Signalverarbeitung, mit dem Ziel an der Benutzerschnittstelle ein im Boardcomputer weiterzuverwendendes Signal zu erzeugen. Nach der Digitalisierung und evtl. Filterung der Signale erfolgt eine Reduktion der Frequenz, so dass an der Schnittstelle ein die Spezifikationen des Schnittstellenprotokolls erfüllendes hinreichend niederfrequentes Signal zur Weiterverarbeitung zur Verfügung steht.

Im Fall der Gyroskope werden oft nicht direkt die Drehraten w gemessen, sondern ihre Integrale, d. h. die über einen Abtastzeitschritt integrierte (d. h. kumulierte) Drehrate, die z. B. im Zeitschrift von durch gegeben ist. Im Fall einer zeitlich konstanten Drehachse entspricht dieses Integral dem (relativen) Drehwinkel (Winkelinkrement) A0 ^k , um den sich die IMU während dieses Abtastzeitschrittes gedreht hat. Soll nun an der Benutzerschnittstelle ein kumulierter Drehwinkel ausgegeben werden, der sich auf einen längeren Zeitbereich (also eine niedrigere Frequenz /out < f _n ) bezieht, so sind die n individuellen Drehwinkel L6 ⁱ ~' ^{[ +k} , k = \ n, die innerhalb des Zeitintervalls t/ ] gemessen werden, aufzuaddieren.

Beträgt beispielsweise die Abtastfrequenz der Gyroskope f = 2 kHz, so dass die gemessenen Winkelinkremente auf eine konstante Abtastrate von s bezogen sind und soll die Frequenz an der Benutzerschnittstelle /out = 100 Hz -4 betragen, so sind jeweils n der mit Zeitabständen Af = 5 * 10 s gemessenen Drehwinkelinkremente summieren, um das Winkelinkrement bezogen auf einen Zeitschrift s am Ausgang zu erhalten, also den relativen Drehwinkel, um den sich die IMU während eines Zeitschritts Af _ou t gedreht hat.

Diese Überlegungen sind allerdings nur korrekt, wenn die Rotation um eine zeitlich konstante Drehachse erfolgt. Diese Annahme ist jedoch in der Regel nicht erfüllt. Rotiert beispielsweise ein starrer Körper ohne Einwirkung äußerer Drehmomente um eine Achse, die keine der drei Hauptträgheitsachsen ist, so führt der Winkelgeschwindigkeitsvektor im körperfesten Bezugssystem eine Präzessionsbewegung aus und seine Richtung ist nicht zeitlich konstant. Im Kontext der Navigation wird dieses Phänomen als „Coning“ bezeichnet. Im Fall einer zeitlich veränderlichen Drehachse ergeben sich nun folgende Änderungen:

1 . Das von den Sensoren gemessene Integral Lcp ^k der Drehrate w ergibt nicht direkt das Winkelinkrement A0 ^k , das direkt Drehachse und den Drehwinkel der im Abtastintervall erfolgten Drehung angibt. Stattdessen ist A0 ^k aus den Integralen der aktuellen und vergangenen Drehraten zu berechnen.

2. Die direkte Summation zeitlich aufeinanderfolgender mit den Zeitintervallen [C-2+k, ti-i+k], [ti-i+k, ti+k] assoziierter Winkelinkremente A0 ^k , A0 ^k+1 ergibt nicht das zur Drehung im Zeitintervall [f/-2+kJ/+k] gehörende Winkelinkrement. Anstelle der einfachen Summation der Winkelinkremente ist eine kompliziertere mathematische Operation auszuführen.

John E. Bortz: „A New Mathematical Formulation for Strapdown Inertial Navigation“, in: IEEE Transactions on Aerospace and Electronic Systems AES-7.1 (1971 ), pp. 61-66. DOI: 10.1109/TAES.1971 .310252, schlägt eine (nichtlineare) Differential- gleichung für den Drehwinkel 9(f) als Funktion der Zeit vor, die von der Drehrate w(f) abhängt und wie folgt lautet: mit0 - 0 = 0 - 0) = 00, 0 = ||0|| ₂ . (2.1 )

In der obigen Gleichung ist (einmalig zur Klarstellung) der mehrkomponentige Drehwinkel 9(f) in Fettdruck dargestellt, um ihn von seiner Norm ||0|| ₂ zu unterscheiden.

Diese Differentialgleichung kann für kleine Drehwinkel 9 vereinfacht werden, indem sie um Drehwinkel 9 = 0 in eine Taylorreihe entwickelt wird, wobei nur die führende(n) Ordnung(en) beibehalten werden. Sie lässt sich dann beispielsweise in einem Zeitintervall [f _M , t/], also für das Winkelinkrement A0p _Mt lösen. Hierzu wird w(f) in diesem Intervall durch ein Polynom (n - 1 )-ten Grades approximiert durch

Dessen n Koeffizienten können aus n gemessenen integrierten Drehraten Lcp ^k , k = 1, . . . , n bestimmt werden. Hierzu müssen die Koeffizienten in die Polynomapproximation eingesetzt, das Integral berechnet und dann die k = 1,..., n Gleichungen nach den Koeffizienten aufgelöst werden. Dieses Verfahren benötigt bereits n gemessene Winkelinkremente Lcp ^k von Zeitintervallen, die zusammen den Zeitbereich des kumulierten Drehwinkels A9 ^k bilden, und liefert den kumulierten Drehwinkel A0p _Mt des Intervalls fj.

M. B. Ignagni: „Efficient class of optimized coning compensation algorithms“, in: Journal of Guidance, Control, and Dynamics 19.2 (1996), pp. 424-429. DOI: 10.2514/3.21635 (https://doi.Org/10.2514/3.21635) beschreibt einen Algorithmus höherer Ordnung. Allerdings steigt für höhere Ordnungen n der Rechenaufwand stark an und die Approximation der Messdaten durch Polynome hohen Grades n — 1 führt zu starken Oszillationen an den Rändern des Interpolationsintervalls, die als Runges Phänomen bekannt sind. R.A. McKern: „A study of transformation algorithms for use in a digital computer“, MA thesis, Massachusetts Institute of Technology, Dept, of Aeronautics and Astronautics, 1968 (http://hdl.handle.net/1721.1Z14164) offenbart einen Algorithmus zur Kombination der relativen Rotationen aus aufeinanderfolgenden Zeitintervallen. Dieser Algorithmus berechnet über den Umweg der Einheitsquaternionen aus den Winkelinkrementen zweier zeitlich aufeinanderfolgender Zeitintervalle das Einheitsquaternion der relativen Lageänderung während des gesamten Zeitintervall. Dabei wird der zuvor beschriebene Algorithmus zur Berechnung des kumulierten Drehwinkels A0p _Mt für das Intervall t/] genutzt, allerdings nur für zwei Ordnungen n = 2. Dann wird mit dem erhaltenen Drehwinkel A0p _Mt das Einheitsquaternion der relativen Lageänderung in dem Zeitintervall [/M , t/] berechnet zu:

Der Drehwinkel-Vektor im Argument der Exponentialfunktion und auf der rechten Seite der Gleichung ist als sogenanntes reines Quaternion zu interpretieren, das nur einen mit den Vektorkomponenten des Winkelinkrements A0p _Mt gebildeten Imaginärteil besitzt. Darüber hinaus werden die trigonometrischen Funktionen durch ihre Taylorpolynome bis zur 3. Ordnung approximiert.

Das Ergebnis wird dann verwendet, um ausgehend von einem Startzeitpunkt t ₀ und dem Quaternion = 1 das Quaternion der relativen Lageänderung rekursiv zu berechnen gemäß:

Hierbei sind die Quaternionen mittels des Quaternionproduktes zu multiplizieren.

In Analogie zu der Formel (2.3) werden die zu den Winkelinkrementen A0 ^k-1 , A0 ^k gehörenden Quaternionen berechnet und dann in Analogie zur Formel (2.4) multipliziert, um das Quaternion zu erhalten. Anschließend ist dann das Winkelinkrement der kombinierten

Drehungen durch Inversion der Funktion (2.3) zurückzugewinnen. Die entsprechende Relation lautet für das aus den Vektorkomponenten des Winkelinkrements A0(„ _zt gebildete Quaternion wie folgt:

Diese Lösung für den Spezialfall von n=2 zeitlich aufeinanderfolgenden Winkelinkrementen A0 ^k-1 , A0 ^k kann auf beliebige Anzahl n von Winkelinkrementen für einen Zeitbereich und einen zugehörigen Drehwinkel A0p _Mt verallgemeinert werden.

Die iterative Berechnung des Drehwinkels kann dann durch die Prozedur beschrieben werden:

Prozedur

Setze oQ = 1

Für k = l,...,n

Berechne aus A0 ^k mit der Gleichung (2.3)

Berechne aus mit der Gleichung (2.5)

Berechne ^ mit der Gleichung (2.6)

Die Berechnung des Drehwinkelinkrementes A0p” _t erfolgt indirekt über Quaternionen und beruht daher auf den drei Gleichungen (2.3), (2.4) und (2.5). Dies erfordert die Berechnung verschiedener transzendenter Funktionen (cos, sin, arcsin) z. B. nach Approximation durch ihre Taylorpolynome, wodurch der Rechenaufwand erhöht wird. Nach jeder n-ten rekursiven Aktualisierung des Quaternions wird die Gleichung (2.6) ausgewertet, um aus dem Quaternion das iterative Drehwinkelinkrement A0^ _t zurückzugewinnen. Auch dies ist mit erhöhtem Rechenaufwand verbunden, der jeden n-ten Rekursionsschritt anfällt. Bei Approximation der trigonometrischen Funktionen in der Gleichung (2.3) ist das Quaternion kein Einheitsquaternion mehr. Die Relation der Gleichung (2.6) gilt jedoch nur für Einheitsquaternionen. Ihre Anwendung auf nicht normierte Quaternionen führt zu einem zusätzlichen (numerischen) Fehler.

Die Berechnung der Gleichung (2.6) bei hinreichend großen Winkelinkrementen A0o _Mt , wie sie z. B. entstehen, wenn eine große Anzahl n von Inkrementen A0 ^k summiert wird, erfordert bei Approximation durch ihr Taylorpolynom eine Berücksichtigung recht hoher Ordnungen, um die gewünschte numerische Genauigkeit zu erreichen. Dieses erhöht den Rechenaufwand.

Die bekannten Verfahren zur Bestimmung der Lage aus einem Drehwinkel A0p _Mt eines Zeitbereichs, bspw. des Intervalls [f/_ _1; tj\ durch Zusammenfassung einer Anzahl von n Winkelinkrementen Acp ^k an Zeitpunkten t _meas>k , bspw. die zusammen den Zeitbereich bilden, sind sehr rechenaufwändig und benötigen daher relativ viel Rechenzeit, Rechenkapazität und erfordern eine relativ leistungsfähige und damit aufwändige Elektronik mit Hardware-Rechenleistung.

Ausgehend hiervon ist es Aufgabe der vorliegenden Erfindung, ein verbessertes Verfahren zu schaffen.

Die Aufgabe wird mit dem Verfahren mit den Merkmalen des Anspruchs 1 und die Messeinrichtung mit den Merkmalen des Anspruchs 7 gelöst. Vorteilhafte Ausführungsformen sind in den Unteransprüchen beschrieben.

Es wird vorgeschlagen, dass zum Bestimmen der Lage eines Objektes Drehwinkelinkremente A9 ^k an aufeinanderfolgenden Messzeitpunkten t _meas>k mit k = 1 , ... , n gemessen (bzw. aus gemessenen 4cp ^k berechnet) und zu einem kumulierten Drehwinkel A9 ^I _0U t für einen die Messzeitpunkte t _{meas k} umfassenden Kumulations- Zeitbereich I zusammengefasst werden, indem jeweils ein kumulierter Drehwinkel A9 ^I _0U t für einen Kumulations-Zeitbereich I als Funktion von Exponenten einer Approximation mindestens zweiter Ordnung der Baker-Campbell-Hausdorff-Formel für die Lie-Rotationsgruppe SO(3) berechnet wird. Dabei werden iterativ kumulierte Drehwinkelinkremente A9 ^{I k} _0U t für die Zeitpunkte t _meas>k mit k = 1 ,... , n jeweils aus der Funktion der Exponenten mit jeweils einem Drehwinkelinkrement A9 ^k des Messzeitpunktes t _k und dem für das vorhergehende Zeitintervall bestimmten kumulierten Drehwinkelinkrement A9 ^{I k} _0U t oder einem vorgegebenen Ausgangs- Drehwinkelinkrement A9 ^I ’ ⁰ _out als Eingangsgrößen bestimmt. Der kumulierte Drehwinkel A9 ^I _0U t für den Zeitbereich I ergibt sich als Funktion des letzten kumulierten Drehwinkelinkrements A9 ^{I n} _0U t, das für das letzte Zeitintervall t _n des Intervalls des Zeitbereichs I bestimmt wurde. Es kann diesem kumulierten Drehwinkelinkrement A9 ^{I n} _0Ut entsprechen.

Die Drehwinkelinkremente und Drehwinkel enthalten Winkelinformationen für die erforderlichen Raumrichtungen und können Vektoren mit den Vektorkomponenten der drei Einheiten i, j, k des oben beschriebenen Einheitsquaternions bzw. der drei Raumrichtungen x, y, z haben. Auch wenn die Drehwinkelinkremente und Drehwinkel in der vorliegenden Anmeldung und den Ansprüchen zur Vereinfachung nicht explizit als Vektoren bezeichnet sind, ist die Ausführungsform als Vektor oder Darstellungen durch schiefsymmetrische Matrizen davon umfasst.

Das Verfahren hat den Vorteil, dass die Berechnung der iterativen Drehwinkelinkremente direkt ohne Umwege über Quaternionen erfolgt. In jedem Iterationsschritt ist die Auswertung nur einer Gleichung, die später als Gleichung (3.5) näher erläutert wird, erforderlich. Dies hat die folgenden Konsequenzen:

- Es ist nur eine einzige transzendente Funktion, ggf. approximiert durch ihr Taylorpolynom, zu berechnen.

- Die Iteration ist vereinfacht, da (bis auf die in jedem Fall notwendige Initialisierung) keine Ausführung eines zusätzlichen Rechenschrittes bei jedem n-ten Zeitschrift erforderlich ist. Dies ist insbesondere für die Implementierung auf stark limitierter Hardware (wie beispielsweise einem Field Programmable Gate Array (FPGA)) ein wichtiger Vorteil.

- Die Analyse der Fortpflanzung von Messunsicherheiten und Messrauschen wird erleichtert.

- Eine Polynomapproximation der einzigen transzendente Funktion, bspw. durch ihr Taylorpolynom sollte genauer sein als die Approximation der Gleichung (2.6) durch ihr Taylorpolynom gleicher Ordnung. Für große kumulierte Drehwinkel Ad _o ' _ut ^ur| d damit insbesondere für eine größere Anzahl n wird eine größere Genauigkeit erreicht. - Durch eine getrennte Wahl der beiden Ordnungen für die Approximation in einer Taylorreihe in zwei Variablen lässt sich das Verfahren recht einfach an die gegebenen Anforderungen (bspw. die maximale Drehrate, Sensorfrequenz, Teiler zur Frequenz, Anzahl der zu summierenden Winkelinkremente) für den Benutzer anpassen.

Im Fall einer zeitlich veränderlichen Drehachse ergeben sich nun folgende Änderungen:

1 . Das von den Sensoren gemessene Integral Lcp ^k der Drehrate w ergibt nicht direkt das Winkelinkrement A0 ^k , das direkt Drehachse und den Drehwinkel der im Abtastintervall erfolgten Drehung angibt. Stattdessen ist l\Q ^k aus den Integralen der aktuellen und vergangenen Drehraten zu berechnen.

2. Die direkte Summation zeitlich aufeinanderfolgender mit den Zeitintervallen rt _niPa s fr-2Tn _1Pa s fr-il ftmeas fr-i Trn _Pa s frl assoziierter Winkelinkremente A0 ^k ’ ⁷ , A0 ^k ergibt nicht das zur Drehung im Zeitintervall [t _me as,fc-2Tmeas,fc] gehörende Winkelinkrement. Anstelle der einfachen Summation der Winkelinkremente ist eine kompliziertere mathematische Operation auszuführen.

Das Verfahren beruht auf den bei zeitlich veränderlichen Drehachsen erforderlichen komplexen mathematischen Operationen anstelle einer einfachen Summation der Winkelinkremente. Die Ursache hierfür ist, dass Drehungen um nichtparallele Drehachsen miteinander nicht vertauschen. Vielmehr bilden die Drehungen im (dreidimensionalen) Raum eine Gruppe, die Drehgruppe SO(3), welche eine sogenannte Lie-Gruppe ist. Das mathematische Gebiet der Lie-Gruppen bildet den theoretischen Rahmen, aus welchem sich die Darstellungen der Gruppenelemente in Form von Rotationsmatrizen oder in Form der Einheitsquaternionen ergeben.

Die Rotationsmatrizen sind spezielle (ihre Determinante ist 1 ) orthogonale (ihr Transponiertes ist ihr eigenes Inverses) Matrizen in drei Dimensionen. Der Name der Gruppe, SO(3), leitet sich aus den Begriffen Spezielle-Orthogonale Rotationsmatrizen in 3 Dimensionen ab.

Einheitsquaternionen sind die Erweiterung komplexer Zahlen auf drei unabhängige imaginäre Einheiten, deren aus Real- und Imaginärteil berechnete Norm 1 ist und mittels eines nichtkommutativen, normerhaltenden Produktes multipliziert werden.

Jeweils zwei Einheitsquaternionen repräsentieren eine Rotation.

In Abhängigkeit der gewählten Darstellung folgen dann auch die oben beschriebenen Operationen für die Berechnung der Winkelinkremente und deren Summation.

Der kumulierte Drehwinkel A0^ _ut für einen Zeitbereich I, der eine Anzahl von n aufeinanderfolgenden Zeitpunkten t _meaSik mit k = 1 , ... , n beinhaltet, kann hierzu mit der Funktion des Exponenten y^ > ^OMt , y-J rekursiv (d. h. iterativ) aus einer Anzahl n von Drehwinkelinkrementen A9 ^k , die an aufeinanderfolgenden Messzeitpunkten tmeas.k mit k = 1 , ... , n gemessen wurden, und dem jeweils für das vorhergehende

Zeitintervall bestimmten kumulierten Drehwinkelinkrement berechnet werden.

Der Exponent nullten Ordnung kann dabei sein. Der

Exponent erster Ordnung kann dabei Y ^(i:) ( > ^Ae °y \ + > ^Ae 'p ¹ x y- + B die transzendente Funktion B(Q +

7 > i QÖ? r'io

+ d? + ife + + 63SS1287S +°« ) ^{mit dem} Bernoulli-Zahlen ß _2k und dem

Operator 0(^ ¹² ) berücksichtigt.

Es wird für den Messzeitpunkt t _{meas k} ein kumuliertes Drehwinkelinkrement jeweils aus der Summe der Exponenten nullten, ersten und mindestens zweiten Ordnung m _a = 0, 1 und 2 bestimmt.

Die Berechnung beruht auf nur einer einzigen transzendenten Funktion, was den Zeit- und Rechenaufwand reduziert. Die Funktion des Exponenten kann die Summe der Argumente der

Approximationen in der nullten, ersten, zweiten und dritten Ordnung m _a = 0, 1 , 2 und 3 umfassen. Durch die Berücksichtigung auch eines Exponenten der höheren dritten Ordnung kann die Genauigkeit weiter gesteigert werden. Auch hierbei ist nur die eine einzige transzendente Funktion enthalten.

Es kann eine Approximation der transzendenten Funktion für den l-ten Zeitbereich erfolgen. Dies beruht auf den Bernoulli-Zahlen B ₂ k als Koeffizienten und dem zur vereinfachten Darstellung eingeführten Argument 0 _X , das als Platzhalter für den vorhergehenden kumulierten Drehwinkel Aö^r ¹ steht. Die Approximation kann von k gleich 1 bis zu einer als Funktion der Genauigkeit gewählten maximalen Iterationsstufe k = k _max durchgeführt werden (hier ist k der Term k-ter Ordnung in der Taylorentwicklung, nicht zu verwechseln mit dem k-ten Zeitschrift).

Das Ausgangs-Drehwinkelinkrement A9 ^{I 0} _O ut für k = 0 kann bei einer Initialisierung mit dem Wert Null vorgegeben werden. Zur Bestimmung des kumulierten Drehwinkels Aö^ut für den Zeitbereich [tpiTi] mit den gemessenen Drehwinkelinkrementen A0 ¹ bis A0 ⁿ für die jeweiligen Zeitpunkte t _meas>k mit k = 1 , ... , n als Eingangsgrößen wird vor dem ersten Iterationsschritt die Initialisierung durchgeführt. Anschließend werden die kumulierten Drehwinkelinkremente A0 ^{I k} _ou t als Zwischenergebnisse iterativ mit k = 1 bis n aus dem jeweils für den vorhergehenden Iterationsschritt k-1 bestimmten oder vorgegebenen kumulierten Drehwinkelinkrement AO^-^ut und dem für den Zeitpunkt t _{meas fc} , der zum jeweiligen Iterationsschritt k gehörig ist, gemessenen Drehwinkelinkrement A0 ^{I k} als Funktion von Exponenten der Approximation mindestens zweiter Ordnung der Baker-Campbell-Hausdorff-Formel für die Lie-Rotationsgruppe SO(3) berechnet.

Die Ermittlung der Drehrate des Objektes kann als Funktion des für den Zeitbereich [ti-iTi] bestimmten kumulierten Drehwinkels AO^ut und der Zeit At _/;OUt = Sfc=i At _fc erfolgen. Die Drehrate w kann hierzu aus dem Quotient des kumulierten Drehwinkels Aö^ut und der Zeit At _{/ out} berechnet werden.

Die Erfindung wird nachfolgend anhand der beigefügten Zeichnung mit einem Ausführungsbeispiel näher erläutert. Es zeigt:

Figur 1 - Zeitverlauf der fc = gemessenen bzw. des aus den Messungen zu den Messzeitpunkten t _{meas k} ermittelten Inputs Aß ^k , dem k-ten Iterationsschritt zur Bestimmung des k-ten kumulierten Winkelinkrements Output 40^ zum Zeitpunkt t/

Figur 2 - Skizze einer Messeinrichtung zur Bestimmung der Lage eines Objektes;

Figur 3 - Flussdiagramm des Verfahrens zur Bestimmung eines kumulierten Drehwinkelinkrementes aus gemessenen Drehwinkelinkrementen;

Figur 4 - Weiteres Flussdiagram des Verfahrens.

Figur 1 zeigt einen Zeitverlauf der k = gemessenen bzw. des aus den Messungen zu den Messzeitpunkten t _meas>k ermittelten Messwerte für die Drehwinkelinkremente, d. h. den Inputs Aß ^k , dem k-ten Iterationsschritt zur Bestimmung des k-ten kumulierten Winkelinkrements Aß _a ^ _t und dem kumulierten Drehwinkel, d. h. dem Messergebnis als Output A6 ^I ^ _Jt zum Zeitpunkt t _; . Damit wird die Zeitabfolge mit den Messzeitpunkten t _{meas k} , der Aufsummierung für unterschiedliche Zeitintervalle k = 1 , ...., n und der Zusammenfassung zum niederfrequenten (Low Frequency) Messergebnis deutlich.

Figur 2 zeigt eine Skizze einer Messeinrichtung 1 zur Bestimmung der Lage eines Objektes 2. Die Messeinrichtung hat mindestens ein Inertiale Messeinheit IMU, die zur Messung der Drehwinkelinkremente A0 ^k aufeinanderfolgender Zeitpunkte t _meas>k für die Erfassung rotierender (kreisender) Bewegungen der Inertialen Messeinheit IMU und des damit verbundenen Objektes 2 in den drei zueinander orthogonal stehenden Raumachsen (X- bzw. Y- bzw. Z-Achse) des kartesischen Koordinatensystems eingerichtet ist. Diese gemessenen Drehwinkelinkremente A0 ^k werden als Eingangswerte einer Datenverarbeitungseinheit zugeführt, die zur Berechnung eines kumulierten Drehwinkels Aö^ut für einen Kumulations-Zeitbereich I, der aufeinanderfolgende Zeitpunkte t _{meas k} mit i = 1 , ... , n umfasst, eingerichtet.

Die Datenverarbeitungseinheit 3 kann beispielsweise ein Mikroprozessor oder

Mikrocontroller sein, der ein Computerprogramm mit Programmbefehlen ausführt, die den Mikroprozessor oder Mikrocontroller veranlassen, die Verfahrensschritte des anspruchsgemäßen Verfahrens auszuführen. Die Datenverarbeitungseinheit 3 kann aber auch als festverdrahtete Hardware-Logik, wie beispielsweise ein Field- Programmable-Gate-Array o.ä. ausgeführt sein.

Mit dem Verfahren wird eine direkte Berechnung des kumulierten Drehwinkels A9' _ou t iterativ aus jeweils dem im letzten Schritt gespeicherten und dem aktuell gemessenen Drehwinkelinkrement A0 ^k durchgeführt. Grundlage bildet die aus der Theorie der Lie-Gruppen stammende Baker-Campbell-Hausdorff (BCH) Formel, die ein Vertauschungsgesetz für bestimmte lineare Operatoren angibt. Sie liefert für die Multiplikation zweier durch die Exponentialabbildung erhaltenen Elemente e ^a , eß einer Lie-Gruppe den Exponenten y(a, ß) des resultierenden Elementes e^“- ß), welches also die Relation e/(a. ß) = e ^a eß (3.1 ) erfüllt. Das Ergebnis y(a, ß) wird durch die Baker-Campbell-Hausdorff Formel als formale Reihenentwicklung in der Gesamtordnung m von f und g angegeben und hat daher schematisch folgende Form: mit dem Koeffizienten c(m _ai mp), wobei die tatsächliche Form wegen der allgemeinen Nichtvertauschbarkeit der Gruppenelemente a ß ß a wesentlich komplizierter ist.

Die allgemeine Baker-Campbell-Hausdorff (BCH) Formel ist als formale Reihe noch nicht in einer Form, die für die Summation zeitlich aufeinanderfolgender mit den Zeitintervallen [ti- _3+k , tj. ₂ +k], [ti-2+k, tj.i _+k ] assoziierter Winkelinkremente A9 ^k ~ ¹ , L6 ^k geeignet ist, um das tatsächliche zur Drehung im Zeitintervall [f/-3+k, ti-i+k] gehörende Drehwinkelinkrement, d. h. den Drehwinkel A9' _ou t für den Zeitbereich I (in diesem Fall bestehend aus zwei Unterintervallen), zu erhalten. Die BCH-Formel muss noch auf den Spezialfall der Lie-Gruppe SO(3) spezialisiert werden. Zudem muss eine geeignete Auswertung / Berechnung der Reihenentwicklung erfolgen. Nach der Spezialisierung auf SO(3) wird eine geeignete Auswertung des dann immer noch als Reihe vorliegenden Ausdrucks durchgeführt. Die Spezialisierung auf die Lie-Gruppe S0(3) liefert zunächst nur die benötigte Relation für die Summation zweier Winkelelemente, für den zuvor beschriebenen Fall wobei y nach wie vor als Reihenentwicklung in der Gesamtordnung m wie in Gleichung (3.2) gegeben ist und A0o _Mt hier das auf das aus den beiden Einzelintervallen [f/- ₃ +k, ti-2+k], [ti-2+k, ti-i+k] zusammengesetzte Intervall [f _z-3+k , f/. _7+k ] bezogen ist.

Die Auswertung der Reihenentwicklung gemäß Gleichung (3.2) der Funktion y in der Gleichung (3.3) erfolgt unter der Annahme, dass nur ihr zweites Argument klein ist und daher nur in diesem Argument eine Approximation durch das Taylorpolynom vorgenommen werden kann. Dieses ist erforderlich, denn bei iterativer Anwendung der Relation (3.3) auf insgesamt n Elemente gemäß der Vorschrift: beinhaltet das erste Argument von y das (iterative) kumulative Drehwinkelinkrement den vorangegangenen k-1 Schritten. Dieses ist also für große k < n nicht notweniger Weise klein. Daher kann eine Approximation der Taylorreihe im ersten Argument durch ihr Taylorpolynom zu einem inakzeptabel großen numerischen Fehler führen.

Die BCH-Formel lässt sich nun für die Lie-Gruppe SO(3) wie folgt approximieren, um die Exponenten der Ordnungen m _ß = 0, 1 , 2 zu erhalten:

Die einzige, in dem Gleichungssystem enthaltene und durch Approximation zu berechnende Funktion ist die transzendenten Funktion B(Q. Diese beinhaltet die Bernoulli-Zahlen ß _2fc und den Operator O(^ ¹² ).

Die Herleitung der Approximation mindestens zweiter Ordnung m _ß der Baker- Campbell-Hausdorff-Formel für die Lie-Rotationsgruppe SO(3) wird später noch im Detail beschrieben.

Es reicht aus, wenn lediglich im zweiten Argument 0 ₂ = ~ ^e i ^ne Approximation durch das Taylporpolynom mindestens in den Ordnungen m _ß = 0, 1 , 2 vorgenommen wird.

Die Ausdrücke sind hingegen im ersten Argument exakt.

01 A0 ^/,fc — 1

Es tritt nur eine einzige transzendente Funktion B(y) = B( — ^-) auf, die je nach Anwendungsfall durch ihr Taylorpolynom approximiert werden kann. Ein Vergleich der die Bernoulli-Zahlen B ₂ k beinhaltenden Koeffizienten des Taylorpolynoms mit denen des Taylorpolynoms in der Gleichung (2.6) zeigt, dass erstere bei Erhöhung der Ordnung jenseits der 3. Ordnung um ungefähr einen Faktor 1/10 kleiner werden, während letztere nur um etwa einen Faktor zwischen 5/10 und 1 kleiner werden.

Daher ist es für numerische Berechnungen, bei denen die transzendente Funktion B( und die Gleichung (2.6) durch ihre Taylorpolynome approximiert werden müssen, von Vorteil, den Ausdruck gemäß Gleichung (3.5) zu berechnen, anstatt den herkömmlichen Algorithmus zu verwenden, der sich auf die Gleichung (2.6) stützt. AClI’k — 1

Die maximal erforderlichen Ordnungen der Taylorpolynome für B( ° ₂ ^ut ) und den Ausdruck (2.6) hängen dabei von der gewünschten numerischen Genauigkeit bei gegebenen maximal zu erwartenden Werten für die Winkelinkremente und deren Anzahl n ab, die in einem einzigen Winkelinkrement A0o _Mt zusammengefasst werden sollen.

Bei Verwendung der Gleichungen (3.5) kann die erforderliche Ordnung bei gleicher numerischer Genauigkeit deutlich niedriger gewählt werden als bei Verwendung der Gleichungen (2.3), (2.5) und (2.6). Damit kann die Komplexität reduziert und der Rechenaufwand, d. h. Berechnungszeit und Hardwareaufwand, reduziert werden.

Die iterative Berechnung des kumulierten Drehwinkels A0p _Mt des Intervalls [tj.^ tj] mit Hilfe der Approximation mindestens zweiter Ordnung der Baker-Campbell- Hausdorff-Formel für die Lie-Rotationsgruppe SO(3) kann auf Grundlage der Gleichungen (3.4) und (3.5) durch die Prozedur wie folgt durchgeführt werden:

Prozedur

Setze

Für iterativ:

Berechne A0{'^ aus dem (iterativen) kumulierten Drehwinkelinkrement A0^7 ¹ und dem gemessenen Drehwinkelinkrement A0 ^k gemäß Gleichung (3.4) mit der expliziten Form (3.5)

Setze den resultierenden kumulierten Drehwinkel A0o _Mt gleich dem letzten (iterativen) kumulierten Drehwinkelinkrement

Figur 2 zeigt ein zugehöriges Flussdiagramm dieses Verfahrens bzw. der obigen Prozedur zur Bestimmung eines kumulierten Drehwinkelinkrementes aus mit einem Drehwinkelsensor für aufeinanderfolgende Zeitintervalle gemessenen Drehwinkelinkrementen.

Berechnung des expliziten Ausdrucks für die BCH Formel für die Lie-Gruppe SO(3) Im folgenden wird ein Ausdruck für die BCH Formel für die Lie-Gruppe SO(3) hergeleitet, die Rotationen im dreidimensionalen Euklidischen Raum beschreibt.

Strategie ist hierbei, die Entwicklung der Formel in Ordnungen nur ihres zweiten Argumentes 0 ₂ = ^— durchzuführen und die Abhängigkeit vom ersten Argument

= — y— in allen Ordnungen zu resummieren, so dass die Formel bezüglich ihres ersten Argumentes exakt ist. Das Ergebnis stellt also eine Entwicklung der Formel in Ordnungen ihres zweiten Argumentes 0 ₂ bei gleichzeitiger Resummation aller Abhängigkeiten von ihrem ersten Argument dar.

M. Müger: „Note on the theorem of Baker-Campbell-Hausdorff-Dynkin“ (https://www.math.ru.nl/~mueger/PDF/BCHD.pdf) gibt einen Überblick über einige Grundlagen, Fakten und Beweise zur BCH-Formel. Dort wird im Abschnitt 8 und insbesondere in Bemerkung 8.2 auf Seite 24 darauf hingewiesen, dass auf Grundlage einer speziellen Stufung der Algebra, basierend auf der Wortlänge im zweiten Argument anstelle der Gesamtwortlänge in beiden Argumenten, eine Entwicklung der BCH Formel im zweiten Argument erfolgen kann. Der entsprechende Ausdruck ist bis zu ersten Ordnung im zweiten Argument angegeben, ohne allerdings eine Spezialisierung auf die Lie-Gruppe SO(3) vorzunehmen und die die Bernoulli-Zahlen beinhaltende Serie explizit zu resummieren.

1 . Berechnung der nullten und ersten Ordnung, = 0,1

Wird die in erster Ordnung einfachen Schritte der Spezialisierung und der Resummation unter Verwendung der Erzeugendenfunktion der Bernoullizahlen aus, welche durch

_ - _ y°° ^B n „n e ^z — 1 ^n=0 _n[ ^z (A.1 ) gegeben ist, ergibt sich für den in M. Müger: „Note on the theorem of Baker- Campbell-Hausdorff-Dynkin“ als Reihe gegebenen Ausdruck der BCH-Formel die folgende explizite Darstellung in resummierter Form: Die Taylorreihe der Funktion (2fc) enthält nur gerade Ordnungen von $ ² , so dass die Berechnung der Gleichung (A.2) keine Berechnung der in der Norm des Vektors enthaltenen Quadratwurzel erfordert. Dieses ist bei numerischen Berechnungen ein Vorteil.

2. Grundlagen zum Formalismus und Zusammenhang zwischen der Differentialgleichung der Winkelinkremente und der BCH Formel

Die zur Herleitung der zweiten Ordnung erforderlichen Grundlagen und Schritte werden im Folgenden dargestellt.

Ausgangspunkt bildet die Exponentialabbildung, die auch für Argumente a definiert ist, die Elemente einer Lie-Algebra, also des Tangentialraums einer Lie-Gruppe am Element der Identität sind, und die durch Matrizen dargestellt werden können. Die Exponentialabbildung ist über ihre Taylorreihe definiert wobei a ein Element einer Darstellung einer Lie-Algebra ist. Dieses Element a kann z.B durch die reinen Quaternionen im Fall der Rotationen gegeben sein. Potenzen in der obigen Gleichung sind so zu verstehen, dass sie mit dem entsprechenden Produkt durchzuführen sind, was für die Darstellung definiert ist, also z. B. dem Matrixprodukt im Fall einer Matrixdarstellung oder dem Quaternionenprodukt im Fall einer Darstellung durch Quaternionen.

Da verschiedene Elemente a, ß einer Lie-Algebra in der Regel nicht miteinander vertauschen, also aß ß a gilt, ist die Ableitung der Exponentialabbildung nicht durch den bekannten Ausdruck gegeben. Vielmehr liefert die Anwendung der Ableitung auf die Reihendarstellung gemäß Gleichung (A.4) den folgenden Ausdruck: wobei der n-fache Kommutator folgendermaßen definiert ist: und die adjungierten Wirkung des Elements a auf ein anderes Element ß repräsentiert.

Aus der Ableitung gemäß Gleichung (A.5) folgt nun die Relation

Diese lässt sich invertieren. Dazu wird die Erzeugendenfunktion der Bernoulli-Zahlen verwendet, welche in Gleichung (A.1 ) gegeben ist. Das Inverse von der Relation (A.7) ist dann gegeben durch

Es wird nun eine für numerische Berechnungen in der Praxis bedeutsame Eigenschaft erkennbar, nämlich, dass die Reihe in der Inverse gemäß (A.8) nur gerade Potenzen von a enthält. Deshalb hängt der dann auf die Lie-Gruppe SO(3) spezialisierte Ausdruck nur von geraden Potenzen der Norm von a ab und beinhaltet somit keine Quadratwurzeln. Zunächst wird die Relation für die Erzeugendenfunktion auf ihre geraden und ungeraden Potenzen projiziert, so dass die Gleichungen erhalten werden:

1

Daraus folgt unmittelbar, dass B ₁ = — ~ B = 0 für n > 1 ist Es gilt also:

Dieses Ergebnis wird verwendet, um die Inverse gemäß Gleichung (A.8) in folgende Form zu bringen:

(A.11 )

Dieser Ausdruck (A.11 ) wird nun spezialisiert auf die Lie-Gruppe SO(3), bzw ihrer doppelten Überlagerung, die Lie-Gruppe SU(2), dargestellt durch die Einheitsquaternionen. Für zwei Elemente 6, w aus ihrer Lie-Algebra su(2) reduzieren sich die multiplen Kommutatoren wie folgt:

Durch wiederholte Anwendung dieser Relation ergibt sich für die Gleichung (A.11 ) dann der Ausdruck Nach Identifikation des einfachen Kommutators mit dem Kreuzprodukt und Reduktion des doppelten Kommutators / Kreuzproduktes ist das Ergebnis (A.13) in der Tat die Differentialgleichung für das Winkelinkrement 0 in Abhängigkeit von der Winkelgeschwindigkeit w, die in der Gleichung (2.1 ) gegeben ist.

Das Ergebnis der Gleichung (A.8) und im Spezialfall der Lie-Gruppe SO(3) dessen expliziter Ausdruck durch (A.13) sind eng verknüpft mit der Baker-Campbell- Hausdorff Formel, wie im Folgenden gezeigt wird. Die Baker-Campbell-Hausdorff Formel liefert das Lie-algebra-wertige Element / als Funktion zweier Lie-algebra- wertiger Elemente a, ß, als Lösung folgender Relation _eY (a,ß) _{= e} a _e ß (A.15)

Um diesen Zusammenhang zu zeigen, wird der Exponent y in Ordnungen von ß entwickelt, wie dieses z. B. in M. Müger: „Note on the theorem of Baker-Campbell- Hausdorff-Dynkin“ dargestellt ist. Hierzu wird einen Parameter t ER eingeführt und die Entwicklung geschrieben als wobei klarer Weise y(°) = y(cr, 0) = a ist.

Das Ergebnis für yW in der Entwicklung der Gleichung (A.16) kann direkt aus der Gleichung (A.11 ) erhalten werden, wenn a durch y(a, t ß) ersetzt wird. Dieses führt dann auf die folgende Differentialgleichung für y. (A.17)

Durch Einsetzen von t = 0 in die obige Gleichung wird wegen der Gleichung (A.16) ein Ergebnis für yCD erhalten, das wie folgt lautet:

Ein Vergleich dieses Ergebnisses mit der Gleichung (A.11 ) zeigt, dass die Differentialgleichung für das Lie-algebra-Element a bei gegebenem Lie-algebra- Element ß = e~ ^a d _t e ^a identisch mit der Gleichung für den in ß linearen Beitrag in der BCH Reihe gemäß Gleichung (A.16) ist. Hinzu kommt, dass durch Einsetzen von t = At die Approximation von y durch y = a + zu einer Lösung der Differentialgleichung (A.11 ) zur linearen Ordnung in Af ß wird.

3 Berechnung der zweiten Ordnung, m _p = 2

Im Folgenden wird der Term der BCH Reihe (A.16) hergeleitet. Dieser beinhaltet die Terme 2. Ordnung in ß.

Zunächst müssen hierfür die Ableitung der Relation (A.11 ) berechnet werden. Dies führt zu:

Eine Gleichung für ist analog zum Vorgehen im Fall für zu erhalten. Hierzu kann a durch y(a, t ß) ersetzt und außerdem ß = 0,t = 0 eingesetzt werden. Damit ergibt sich

Um ein explizites Ergebnis für den Spezialfall der Lie-Gruppe S0(3) zu erhalten, wird die Reihe in (A.20) zu einem geschlossenen Ausdruck summiert. Es kann analog wie zur Herleitung von (A.13) vorgegangen werden, wobei die Gleichung (A.12) verwendet wird, um die multiplen Kommutatoren zu reduzieren. Außerdem wird direkt 0 0 a = y,ß = y gesetzt, um das Ergebnis in den im Haupttext verwendeten Variablen zu erhalten. Um die Reduktionsformel (A.12) nutzen zu können, kann zunächst festgestellt werden, dass sich eine allgemeine Folge mit Argument f _k wie folgt in Folgen der nur geraden und nur ungeraden Anteile aufteilen lässt:

(A.21 )

Mit dieser Identität lässt sich die innere Summe in (A.20) wie folgt aufteilen: wobei die Heavyside Stufenfunktion ist. Einsetzen dieses Ergebnisses in die Reihe in (A.20) liefert die Taylorreihe:

Ein geschlossener Ausdruck für die erste der beiden Reihen ist bereits aus (A.9) bekannt. Ein geschlossener Ausdruck für die zweite Reihe kann durch Anwenden eines geeigneten Ableitungsoperators auf das bekannte Ergebnis erhalten werden:

Analytische Fortsetzung der Ausdrücke durch Identifikation z = i 0 _X führt dann für den Term der BCH Reihe, der quadratisch in 0 ₂ ist, auf das Ergebnis das nur von einer einzigen transzendenten Funktion, B(£), abhängt, die bereits im Ausdruck für erscheint und in (A.3) gegeben ist. Dieser Umstand ist für numerische Berechnungen von Vorteil, da nur eine einzige transzendente Funktion approximiert und berechnet werden muss.

4. Expliziter Ausdruck für die BCH Formel bis zur zweiten Ordnung, = 0, 1, 2 Das explizite Ergebnis der BCH Formel für SO(3) in der Entwicklung (A.16), d. h. bis einschließlich der Terme quadratischer Ordnung im zweiten Argument -^- wird im Folgenden zusammengefasst und vereinfacht. Der Term y<°) ist einfach durch das 0 erste Argument y der Funktion gegeben. Der Term ist durch (A.13) gegeben, wenn darin 0 0 _X und co 0 ₂ ersetzt oder wahlweise direkt der Ausdruck (A.2) verwendet wird. Der Term ist in (A.25) dargestellt. Damit ergibt sich für die BCH Formel

Nun werden die folgenden Identitäten verwendet: um zunächst die in der Gleichung (A.26) auftretenden multiplen Kommutatoren zu berechnen und zu vereinfachen

Damit kann die Formel (A.26) schrittweise auf folgende Form gebracht werden:

5. Berechnung höherer Ordnungen, > 3 Das beschriebene Verfahren lässt sich direkt auch für die Berechnung höherer Ordnungen anwenden. Insbesondere die Ordnung m _ß = 3 in 0 ₂ , also der Term in der BCH Reihe (A.16) kann genutzt werden, um eine noch größere Genauigkeit zu erhalten. Figur 4 zeigt ein Flussdiagramm des Verfahrens, bei dem mindestens ein Gyroskop in x-Richtung, in y-Richtung und in z-Richtung eine Messung der Winkelinkremente A <Pi,2,3 vornimmt. Diese können aus dem Integral der Winkelgeschwindigkeit Wi, 2.3(f) über eine Zeit von einem vorhergehenden Messzeitpunkt t _meas ,k-i bis zu einem aktuellen Messzeitpunkt t _meas ,k gebildet werden. Die drei Messwerte für die Winkelinkremente A<p ^k ₂ , ₃ können vektorisiert werden, um einen Drehwinkelinkrement-Vektor Acp ^k zu erhalten.

Damit können dann die einzelnen Drehwinkelinkremente A0 ^k rekursiv aus dem Exponenten A0 ^k (A<p ^k-1 ,A<p ^k ) mit der Verzögerung (Delay) z’ ¹ zur Einbeziehung des vorhergehenden Drehwinkelinkrement-Vektors A<p ^k-1 berechnet werden.

Die Berechnung der kumulierten Drehwinkelinkremente erfolgt mit dem Exponenten rekursiv mit einer Verzögerung (Delay) z’ ¹ zur

Einbeziehung des vorher berechneten kumulierten Drehwinkelinkrementes Aö^r ¹ aus den voher bestimmten einzelnen Drehwinkelinkrementen A0 ^k .

Das letzte so berechnete kumulierte Drehwinkelinkrement bildet dann den kumulierten Drehwinkel A0o _Mt als Messergebnis.