DE102015217449B3

DE102015217449B3 - Sensorvereinigungsverfahren zum Bestimmen der Orientierung eines Objektes

Info

Publication number: DE102015217449B3
Application number: DE102015217449.2A
Authority: DE
Inventors: Wessel Harm Lubberhuizen; Rob Macaulay
Original assignee: Dialog Semiconductor BV
Current assignee: Dialog Semiconductor BV
Priority date: 2015-09-11
Filing date: 2015-09-11
Publication date: 2016-12-29
Anticipated expiration: 2035-09-12
Also published as: US20170074689A1

Abstract

Ein Sensorvereinigungsverfahren kombiniert Messwerte von verschiedenen Typen von Orientierungssensoren, um die Orientierung eines Objekts abzuschätzen. Eine analytische Lösung wird geschaffen, die recheneffizient ist und in einer Fest- oder Gleitkommaarchitektur implementiert werden kann.

Description

Die vorliegende Offenbarung bezieht sich auf ein Sensorvereinigungsverfahren zum Bestimmen der Orientierung eines Objekts zusammen mit einer entsprechenden Vorrichtung.
Um die Orientierung eines Objekts abzuschätzen, ist es bekannt, Daten von mehreren Sensoren zu kombinieren, die dem Objekt zugeordnet sind, einschließlich Orientierungssensoren wie z. B. Beschleunigungsmessern, Magnetometern und Gyroskopen. Diese können einem Objekt zugeordnet sein, so dass sie sich zusammen mit dem Körper des Objekts bewegen und drehen.
Im Folgenden nehmen wir an, dass ein rechtsgängiges Referenzkoordinatensystem angenommen wird, in dem x in die Nordrichtung zeigt, y nach Osten zeigt und z nach unten zeigt.
Ein dreiachsiger Beschleunigungsmesser liefert Beschleunigungsmessungen in m/s² entlang jeder der x-, y- und z-Achsen. Da die Schwerkraft als konstante Beschleunigung wirkt, kann ein Beschleunigungsmesser verwendet werden, um die Orientierung in der Aufwärts-Abwärts-Ebene zu messen.
Ein dreiachsiges Magnetometer misst das Magnetfeld (in Mikrotesla) in den x-, y- und z-Achsen. Es kann eine absolute Orientierung in der x-y-Ebene vorsehen.
Ein dreiachsiges Gyroskop misst Änderungen der Orientierung, wobei es Winkelgeschwindigkeiten in rad/s entlang jeder der x-, y- und z-Achsen liefert.
Die Orientierung der Vorrichtung kann aus einem, zwei oder mehr dieser Typen von Orientierungssensoren und möglicherweise ebenso mit zusätzlichen Typen von Orientierungssensoren bestimmt werden. Der Betrieb von Beschleunigungsmesser-, Magnetometer- und Gyroskopvorrichtungen ist bekannt und viele verschiedene Typen jeder Vorrichtung sind verfügbar, einschließlich Vorrichtungen auf der Basis von Komponenten von mikroelektromechanischen Systemen (MEMS). Wenn diese Vorrichtungen zum Messen der Orientierung eines Objekts verwendet werden, ist es außerdem bekannt, mehrere von einem oder mehreren Beschleunigungsmessern, Magnetometern und Gyroskopen bereitzustellen, um eine bessere Leistung zu ermöglichen.
Orientierungssensoren werden für die Orientierungsbestimmung in einer breiten Vielfalt von Zusammenhängen, einschließlich eines Kraftfahrzeugs und anderer Fahrzeuge, und für Verbraucherelektronik wie z. B. Smartphones, Tabletcomputer und die tragbare Technologie, verwendet.
Sobald mehrere Sensoren vorgesehen sind, müssen ihre Ausgaben kombiniert werden, um eine Messung der Orientierung des Objekts zu erhalten.
Ein gut bekanntes Verfahren zum Kombinieren von mehreren Sensoreingaben ist das Kalman-Filter, das eine Reihe von Messungen verwendet, die über die Zeit beobachtet werden, und Abschätzungen von unbekannten Variablen erzeugt, die gewöhnlich genauer sind als jene auf der Basis einer einzelnen Messung allein. Es verarbeitet rekursiv Ströme von rauschbehafteten Eingangsdaten, um eine statistisch optimale Abschätzung des zugrundeliegenden Systemzustandes zu erzeugen.
Das Problem der Abschätzung der Orientierung eines Objekts ist jedoch nicht linear und das Standard-Kalman-Filter ist linear. Daher muss ein erweitertes Kalman-Filter angewendet werden. Ein Beispiel dessen ist von Sabatini ”Quaternion-Based Extended Kalman Filter For Determining Orientation By Inertial And Magnetic Sensing”, IEEE Transactions On Biomedical Engineering, Band 53, Nr. 7, Juli 2006, beschrieben. Dies ist ein ressourcenintensiver und komplexer Algorithmus, der eine Matrixinversions-Gleitkomma-Arithmetik erfordert. Daher erfordert er, dass große Verarbeitungsressourcen angewendet werden, die insbesondere in der mobilen Umgebung eine Herausforderung sein können.
Andere Methoden wurden vorgeschlagen, die geringere Verarbeitungsressourcen erfordern, unter Verwendung von iterativen Verfahren auf der Basis von Fehlerrückkopplung oder steilstem Gefälle. Beispiele dieser verbesserten Techniken sind zu sehen in:

– Mahoney u. a., ”Complementary Filter Design On The Special Orthogonal Group SO(3)”, Proceedings Of The 44^th IEEE Conference On Decision And Control, And European Control Conference Of 2005, Sevilla, Spanien, 12.–15. Dezember 2005.
– Madgwick, An Efficient Orientation Filter For Inertial And Inertial/Magnetic Sensor Arrays, 30. April 2010.
– Cavallo, Experimental Comparison Of Sensor Fusion Algorithms For Attitude Estimation, Preprints Of The 19^th World Congress Of The International Federation Of Automatic Control South Africa, 24.–29. August 2014.
– DE 103 12 154 A1 zeigt ein Verfahren zum Berechnen einer Orientierung eines Objekts. Das Verfahren umfasst in einem ersten Schritt das Empfangen einer Startorientierung und danach ein Empfangen von Messwerten von zwei Orientierungssensoren unterschiedlichen Typs. In DE 103 12 154 A1 erfolgt die Berechnung der Rotation aus drei Drehratensensorsignalen und wird auf die Startorientierung angewendet. Die Orientierung wird durch Vergleich mit den Orientierungen, die durch die Magnetfeldsensoren und Beschleunigungssensoren ermittelt wurden, gewonnen.
– DE 10 2004 057 933 A1 und DE 10 2004 057 959 A1 bilden das in DE 103 12 154 A1 . entwickelte Verfahren weiter.
– DE 10 2011 081 049 A1 zeigt ein Verfahren, bei dem Zwischenwerte von Orientierungen ermittelt und ausgewertet werden. Diese werden jedoch nicht auf Referenzwerte bezogen.
– In DE 10 2006 032 127 A1 wird ein Verfahren zur Bestimmung der Orientierung eines chirurgischen Instruments gezeigt, bei dem Sensoren unterschiedlichen Typs verwendet werden und deren Messergebnisse auf ein Referenzsystem bezogen werden.
– DE 198 30 359 A1 zeigt verschiedene Verfahren zur Bestimmung von Orientierungs- und Bewegungsparametern mittels verschiedener Sensorsysteme. Ausgehend von Startpositionen werden Drehmatrizen bestimmt und damit die Lage eines Körpers im Raum berechnet.
– US 2004/0089474 A1 , US 5 645 077 A und US 6 421 622 B1 zeigen jeweils Verfahren zur Bestimmung von Orientierungen, wobei Kalman-Filter und Quarternionen-Auswertungen eingesetzt werden.

Ein Algorithmus des steilsten Gefälles startet von einem Punkt im Lösungsraum und findet ein lokales Minimum (oder Maximum) durch Bewegen zur nächsten Lösung, die den steilsten Gradienten darstellt. Er weist Schritte mit fester Größe auf, was zu Problemen führt, wenn die Schrittgröße entweder zu klein oder zu groß ist. In der Praxis kann dies zu sehr kleinen Verbesserungsschritten führen, die viele Iterationen und/oder eine sehr lange Zeit erfordern, bevor die optimale Lösung gefunden wird.
Diese Methoden weisen daher Probleme beim Konvergieren auf die optimale Lösung in Fällen auf, in denen der Abstand zwischen der aktuellen Abschätzung und der optimalen Lösung groß ist. Während sie relativ recheneffizient sind, können sie daher mit bestimmten Szenarios der realen Welt kämpfen, beispielsweise wenn die Optimierungsfunktion nicht konvex ist oder mehrere lokale Minima aufweist.
Es besteht ein Bedarf an einer Art und Weise, die Ausgaben von verschiedenen Sensoren zu kombinieren, die recheneffizient und dennoch robust ist, um mit Situationen der realen Welt zurechtzukommen.
Gemäß einem ersten Aspekt der Offenbarung wird ein Verfahren zum Berechnen einer Orientierung eines Objekts geschaffen, das umfasst: ein Empfangen einer Eingangsorientierung; Empfangen eines Messwerts von einem ersten Orientierungssensor; ein Empfangen eines Messwerts von einem zweiten Orientierungssensor; wobei der erste und der zweite Orientierungssensor von verschiedenen Typen sind; und ein Bestimmen einer aktualisierten Orientierung durch Berechnen einer Drehung auf der Basis der Orientierungssensormesswerte und ein Anwenden der berechneten Drehung auf die Eingangsorientierung; wobei das Berechnen einer Drehung umfasst: ein Berechnen einer ersten Drehung, die den Messwert von einem der Orientierungssensoren so dreht, dass er auf eine erste Referenzrichtung ausgerichtet ist; ein Anwenden der ersten Drehung auf den Messwert von dem anderen der Orientierungssensoren, um eine Zwischenorientierung zu erhalten; ein Berechnen einer zweiten Drehung, die die Zwischenorientierung so dreht, dass sie auf eine Referenzebene ausgerichtet ist, die durch Achsen, einschließlich einer Achse, die auf die erste Referenzrichtung ausgerichtet ist, aufgespannt wird; und ein Kombinieren der ersten und der zweiten Drehung, um die berechnete Drehung zu erhalten.
Die Messwerte vom ersten und vom zweiten Typ von Orientierungssensoren können in irgendeiner Reihenfolge empfangen werden.
Ein Orientierungssensor ist irgendein Sensor, der Messwerte erzeugt, aus denen eine dreiachsige Darstellung der Orientierung eines Objekts im Raum abgeleitet werden kann. Dies kann direkt, durch die Verwendung von dreiachsigen Orientierungssensoren oder indirekt durch die Verwendung von anderen Messwerten, von denen dreiachsige Darstellungen berechnet oder abgeleitet werden können, sein. Der ”Typ” eines Orientierungssensors kann durch die Art der Daten, die er erfasst, kategorisiert werden. Beispiele von verschiedenen Orientierungssensortypen umfassen Beschleunigungsmesser, Magnetometer und Gyroskope.
Wahlweise umfasst das Berechnen einer zweiten Drehung das Berechnen einer zweiten Drehung, die die Zwischenorientierung so dreht, dass sie auf eine zweite Referenzrichtung ausgerichtet ist, die zur ersten Referenzrichtung orthogonal ist.
Wahlweise umfasst der erste Sensor einen Beschleunigungsmesser und der zweite Sensor umfasst ein Magnetometer; und wobei die Berechnung einer ersten Drehung das Drehen des Messwerts vom Beschleunigungsmesser in eine Beschleunigungsmesser-Referenzachse und das Drehen des Messwerts vom Magnetometer in eine Magnetometer-Referenzebene umfasst.
Wahlweise umfasst die Beschleunigungsmesser-Referenzachse eine Gravitationsachse und die Magnetometer-Referenzebene umfasst eine Nord-Abwärts-Ebene.
Wahlweise umfasst das Verfahren ferner das Empfangen eines Messwerts von einem dritten Orientierungssensor, der von einem anderen Typ als der erste und der zweite Orientierungssensor ist, und wobei das Berechnen einer Drehung das Kombinieren einer dritten Drehung, die vom dritten Orientierungssensor abgeleitet ist, zusammen mit der ersten und der zweiten Drehung umfasst.
Wahlweise umfasst der dritte Sensor ein Gyroskop.
Wahlweise umfasst das Berechnen einer Drehung das Anwenden einer Drehung auf die Eingangsorientierung auf der Basis der Messwerte vom Gyroskop, um eine vorläufige Orientierung zu erhalten; und dann das Anwenden der ersten und der zweiten Drehung auf die vorläufige Orientierung.
Wahlweise werden der erste und der zweite Orientierungssensormesswert in eine Quaternionform umgesetzt und die berechneten Drehungen umfassen Einheitsquaternionen.
Wahlweise wird der dritte Orientierungssensormesswert in Quaternionform umgesetzt und die berechneten Drehungen umfassen Einheitsquaternionen.
Wahlweise umfasst die Kombination von aufeinander folgenden Drehungen die Bewegung entlang der Oberfläche einer Einheitsquaternion-Hypersphäre.
Wahlweise weisen die Sensoren verschiedene Abtastraten auf; und wobei das Verfahren wiederholt wird und von irgendwelchen zur Verfügung stehenden Messungen Gebrauch macht, die bei oder zwischen aufeinander folgenden Iterationen des Verfahrens durchgeführt wurden.
Wahlweise wird die für die Messwerte jedes Sensors angewendete Drehung gemäß einem Gewichtungsfaktor modifiziert und die aktualisierte Objektorientierung hängt von den gewichteten Beiträgen ab.
Wahlweise werden die Gewichtungsfaktoren für jede Drehung von den relativen Rauschpegeln abhängig abgestimmt, die jedem Sensor zugeordnet sind.
Wahlweise wird die Drehung für jeden Sensor modifiziert, bevor Daten vorn nächsten Sensor verarbeitet werden.
Wahlweise werden die Drehungen für jeden Sensor modifiziert, nachdem Daten von allen Sensoren verarbeitet wurden.
Wahlweise werden beim Schritt des Berechnens einer Drehung auf der Basis der Orientierungssensormesswerte Multiplikationen, die bekannte Nullen beinhalten, weggelassen.
Wahlweise wird das Verfahren in einer Gleitkommaarchitektur implementiert.
Wahlweise wird das Verfahren in einer Festkommaarchitektur implementiert.
Gemäß einem zweiten Aspekt der Offenbarung wird eine Vorrichtung zum Bestimmen der Orientierung eines Objekts mit einem oder mehreren Sensoren, die dem Objekt zugeordnet sind, und einem Prozessor geschaffen, der ausgebildet ist, um: eine Eingangsorientierung zu empfangen; einen Messwert von einem ersten Orientierungssensor zu empfangen; einen Messwert von einem zweiten Orientierungssensor zu empfangen, wobei der erste und der zweite Orientierungssensor von verschiedenen Typen sind; und eine aktualisierte Orientierung durch Berechnen einer Drehung auf der Basis der Orientierungssensormesswerte zu bestimmen und die berechnete Drehung auf die Eingangsorientierung anzuwenden; wobei das Berechnen einer Drehung ein Berechnen einer ersten Drehung, die den Messwert von einem der Orientierungssensoren so dreht, dass er auf eine erste Referenzrichtung ausgerichtet ist; ein Anwenden der ersten Drehung auf den Messwert von dem anderen der Orientierungssensoren, um eine Zwischenorientierung zu erhalten; ein Berechnen einer zweiten Drehung, die die Zwischenorientierung so dreht, dass sie auf eine Referenzebene ausgerichtet ist, die durch Achsen, einschließlich einer Achse, die die auf die erste Referenzrichtung ausgerichtet ist, aufgespannt ist; und ein Kombinieren der ersten und der zweiten Drehung umfasst, um die berechnete Drehung zu erhalten.
Die vorliegende Offenbarung wird nun lediglich beispielhaft mit Bezug auf die begleitenden Zeichnungen beschrieben, in denen:
1 eine Ausführungsform eines Sensorvereinigungsverfahrens zum Bestimmen der Orientierung eines Objekts gemäß einem Beispiel der Offenbarung zeigt;
2 und 3 Aspekte einer Einheitsquaternion-Darstellung von Drehungen darstellen;
4 und 5 Aspekte eines Verfahrens zum Bestimmen der Orientierung eines Objekts gemäß einer Ausführungsform der Offenbarung darstellen; und
6 die Leistung eines Sensorvereinigungsverfahrens gemäß der Offenbarung im Vergleich zu anderen Techniken darstellt.
Gemäß der vorliegenden Offenbarung kann die Orientierung eines Objekts durch Bestimmen einer Drehung berechnet werden, die aus einer Sequenz von Teildrehungen besteht, die durch mehrere Orientierungssensoren beigesteuert werden. Die Drehbeiträge von den mehreren Orientierungssensoren umfassen eine Sequenz von aufeinander folgenden orthogonalen Drehungen. Eine erste Teildrehung bewegt einen Messwert von einem ersten Orientierungssensor in eine erste Referenzrichtung und ihr folgt eine zweite Teildrehung, die einen Messwert von einem zweiten Orientierungssensor in eine Referenzebene oder eine zweite Referenzrichtung bewegt. Wenn er zu einer Referenzebene bewegt wird, ist eine der Aufspannachsen der Ebene durch die Richtung der ersten Referenzachse definiert. Wenn er zu einer zweiten Referenzachse bewegt wird, ist die zweite Referenzachse zur ersten Referenzachse orthogonal. Die Orthogonalität der ersten Referenzrichtung zur zweiten Referenzebene oder Referenzrichtung bedeutet, dass die zweite angewendete Teildrehung den gedrehten ersten Messwert nicht ändert, so dass eine analytische Lösung bereitgestellt werden kann.
Wie in 1 gezeigt, wird in Schritt 100 eine anfängliche Orientierung empfangen. Diese kann eine vorherige Orientierung sein, oder wenn das System startet, kann sie eine Referenzorientierung sein, die als Ausgangspunkt für die Messungen verwendet wird. In Schritt 102 werden Sensormesswerte empfangen. Die vorliegende Offenbarung bezieht sich auf Systeme, in denen zwei oder mehr Typen von Sensoren vorhanden sind. In Schritt 102 ist es jedoch möglich, dass Messwerte nur von einem einzelnen Typ von Sensor empfangen werden, da verschiedene Sensoren mit verschiedenen Raten abgetastet werden können. Im Allgemeinen können zeitlich korrelierte Messwerte von einem, zwei oder mehr Typen von Sensor in Schritt 102 empfangen werden.
Eine Drehung wird in Schritt 104 berechnet, die einen ersten Messwert von einem ersten Sensor zu einem definierten Referenzrahmen oder einer definierten Referenzachse bewegt. Dann wird in Schritt 106 die durch die transformierte erste Sensormessung dargestellte Drehung auf die vorher in Schritt 100 empfangene anfängliche Orientierung angewendet. Das Ergebnis dessen ist eine neue Zwischenorientierung, die den Effekt des ersten Sensors auf die empfangene anfängliche Orientierung darstellt. Schritt 108 prüft, ob andere Sensormesswerte verfügbar sind. Wenn zeitlich korrelierte Daten von anderen Sensoren zur Verfügung stehen, dann werden die Schritte 104 und 106 wiederholt. Eine zweite nachfolgende Drehung wird bestimmt, die den Messwert vom zweiten Sensor zu einem definierten Referenzrahmen oder einer definierten Referenzachse bewegt. Diese Drehung wird dann auf die Zwischenorientierung angewendet, die vom ersten Sensormesswert abgeleitet wurde. Der Prozess wird für beliebige dritte und anschließende Sensormesswerte wiederholt, bis alle Sensormesswerte verarbeitet wurden. Nach dieser Zeit wird das Endergebnis als endgültige Orientierung in Schritt 110 ausgegeben. Diese endgültige Orientierung wirkt dann als in Schritt 100 empfangene anfängliche Orientierung für die nächste Iteration des Prozesses.
Jede Berechnung ist eine analytische Lösung, die in einem Raum durchgeführt wird, der eine Annahme über die Achse der Empfindlichkeit trifft. Sie nutzt die grundlegende Art des speziellen Typs von Daten, die durch jeden Sensor erfasst werden, um Daten in einer Drehrichtung zu beschneiden, für die jeder spezielle Sensor unempfindlich ist. Die Richtungen, die ignoriert werden, sind zwischen verschiedenen Sensortypen orthogonal.
In einer Ausführungsform werden die Drehungen durch Einheitsquaternionen dargestellt. Die Verwendung von Quaternionen ist rechnerisch einfacher im Vergleich zu Euler-Winkeln oder Drehmatrizen und vermeidet Singularitäten (kardanische Blockade).
Eine Quaternion ist eine komplexe Zahl der Form w + xi + yj + zk, wobei w, x, y, z reale Zahlen sind und i, j, k imaginäre Einheiten sind, wobei i² = j² = k² = ijk = –1. Eine Quaternion stellt einen Punkt im vierdimensionalen Raum dar. Das Einschränken einer Quaternion, so dass sie einen Einheitsbetrag aufweist (wobei w² + x² + y² + z² = 1), ergibt einen dreidimensionalen Raum, der zur Oberfläche einer Hypersphäre äquivalent ist, so dass die Einheitsquaternion eine effiziente Weise zum Darstellen von Euklidischen Drehungen in drei Dimensionen ist. Der Vektorteil der Einheitsquaternion (x, y, z-Komponenten) stellt einen Radius einer 2-Sphäre dar, die einer Drehachse entspricht, und ihr Betrag gibt den Drehwinkel an.
2 und 3 stellen Quaternion-Hypersphären 200, 300 dar. Eine Einheitsquaternionsphäre weist vier Dimensionen und einen Durchmesser von Eins auf. Jede Quaternion ist ein Punkt auf der Sphäre.
2 stellt Techniken des Standes der Technik dar, die, wie vorstehend erörtert, eine Normierung zwischen jedem Gradientenschritt erfordern, wenn von einer aktuellen Abschätzung zu einer anderen Abschätzung bewegt wird, und vielmehr eine Bewegung durch das Innere der Sphäre als entlang ihrer Oberfläche beinhalten. Um die optimale Orientierung zu erhalten, sind viele aufeinander folgende Iterationen erforderlich, wobei eine Normierung nach jedem Iterationsschritt erforderlich ist, um zur Oberfläche zurückzukehren, was eine Rechenbelastung ist, die die vorliegende Offenbarung vermeidet.
Gemäß der Offenbarung wird eine kombinierte Drehung als Kombination einer ersten Drehung und einer zweiten Drehung abgeleitet, die vorzugsweise zueinander orthogonal sind. Jede der ersten und der zweiten Drehung wird analytisch im Referenzrahmen berechnet. Intuitiv kann dies durch Erkennen verstanden werden, dass die Quaternion, die eine Orientierung darstellt, ein Punkt auf einer Hypersphäre ist. Ein Beispiel dessen ist in 3 dargestellt, wobei ein Übergang von einem Punkt 302 zu einem anderen 304 auf der Sphäre 300 durch eine sphärische lineare Interpolation bereitgestellt wird. Diese analytische Lösung konvergiert direkt zur optimalen Lösung. Es ist zu beachten, dass zwei Quaternionen vorhanden sind, die jede Drehung darstellen, so dass der kürzeste Weg gewählt wird.
Wir stellen nun eine Beispielausführungsform der Offenbarung dar, in der der Drehreferenzrahmen aus Einheitsquaternionen gebildet ist und die Sensoren, deren Daten kombiniert werden, ein Gyroskop, einen Beschleunigungsmesser und ein Magnetometer umfassen.
Ein rechtsgängiges Referenzkoordinatensystem wird angenommen, in dem x in die Nordrichtung zeigt, y nach Osten zeigt und z nach unten zeigt. Die x-Achse ist eine Gravitationsachse. Wenn die Sensoren nicht bereits in dieser Weise orientiert sind, kann eine geeignete Transformationsmatrix angewendet werden.
Eingaben für den Prozess sind:

gs: Vektor von Gyroskop-Abtastwerten [gs.x, gs.y, gs.z]
as: normierter Vektor von Beschleunigungsabtastwerten [as.x, as.y, as.z]
ms: normierter Vektor von Magnetometer-Abtastwerten [ms.x, ms.y, ms.z]
qp: vorherige Orientierung in Einheitsquaternionform, qp = [qp.w, qp.x, qp.y, qp.z]

Dies nimmt an, dass die x-, y- und z-Rohdaten, die durch den Beschleunigungsmesser und das Magnetometer ausgegeben werden, normiert sind, so dass (x² + y² + z² = 1). Es ist möglich, dass die Sensoren direkt normierte Ausgaben ausgeben, wenn jedoch die Daten noch nicht normiert sind, kann ein Normierungsschritt, bevor mit dem Prozess fortgefahren wird, oder als eine Vorstufe in jedem Schritt ausgeführt werden, wenn die Daten von jedem Sensor erstmalig verarbeitet werden.
Das Ergebnis des Prozesses ist:

qn: Nächste Orientierung in Einheitsquaternionform qn = [qn.w, qn.x, qn.y, qn.z]

In einem Beispiel werden die folgenden Schritte durchgeführt, um qn auf der Basis von gs, as, ms und qp zu finden.
Schritt 1
Berechnen eines skalierten Gyroskopvektors Ω:

Ω = [gs.x, gs.y, gs.z] × gyro_sensitivity/gyro_sample_rate
wobei der Faktor gyro_sensitivity die Gyromessungen in rad/s umsetzt und die gyro_sample_rate die Abtastrate in Hz ist. Die Gyrosensordaten können vorher hochpassgefiltert worden sein, um irgendeinen Versatz und/oder Niederfrequenzrauschen zu entfernen. Die Gyrosensordaten können dann vorher tiefpassgefiltert worden sein, um Hochfrequenzrauschen zu verringern.

Schritt 2
Um den skalierten Sensormesswert Ω in eine Drehdarstellung zu transformieren, wird eine Einheitsquaternion qg gebildet unter Verwendung von: qg = [1 – sqrt(Ω.x² + Ω.y² + Ω.z²), Ω.x, Ω.y, Ω.z] wenn |Ω| < 1 qg = [1, 0, 0, 0] ansonsten
Fälle, in denen |Ω| < 1, stellen gültige Ausgaben des Gyro dar. In anderen Fällen wird eine Nullquaternion gebildet, da hier die Messwerte angeben, dass die Ausgabe ignoriert werden sollte.
Schritt 3
Der Effekt des Gyroskops wird auf die vorherige Orientierung angewendet unter Verwendung von: qi = qp ⊗ qg
Wobei ⊗ das Quaternion-Produkt angibt. Die Quaternion qi bezeichnet eine erste Zwischenorientierung, die durch Drehen der vorherigen Orientierung gemäß den Gyroskopmesswerten gebildet wird.
Schritt 4
Aus den Beschleunigungsmessermesswerten (as) berechnen wir den Erdrahmen-Beschleunigungsmesservektor ae durch Drehen der gemessenen Beschleunigungsmesserwerte mit einer Drehung, die durch die erste Zwischenorientierung qi dargestellt wird: ae = qi ⊗ as ⊗ Konjugat (qi)
Vor dem Ausführen dieser Berechnung wird der Beschleunigungsmessermesswert (as) auf eine Quaternion mit der Komponente as.w = 0 erweitert. Eine alternative Weise zum Berechnen desselben Ergebnisses besteht darin, die Quaternion qi in eine Drehmatrix zu transformieren und die Matrix auf as anzuwenden.
Schritt 5
Eine Drehung wird berechnet, die den Vektor ae in eine Beschleunigungsmesser-Referenzrichtung dreht. Die Drehachse wird so gewählt, dass sie zur Beschleunigungsmesser-Referenzrichtung senkrecht ist. Eine solche Drehung kann durch eine Quaternion qa' dargestellt werden, die berechnet werden kann als: qa' =[Sa(1 + ae.z), ae.y, –ae.x, 0] falls ae.z ≠ –1 qa' =[0, 1, 0, 0] ansonsten
Der Skalierungsfaktor Sa, (0 < Sa ≤ 1) kann verwendet werden, um den Effekt des Messrauschens zu verringern. Ein niedrigerer Wert für Sa verringert den Effekt des Rauschens.
Anschließend kann eine Einheitsquaternion qa durch Dividieren der Qaternion qa' durch ihre Länge gebildet werden: qa = qa'/|qa'|
Gemäß einer bevorzugten Ausführungsform kann die Beschleunigungsmesser-Referenzrichtung die z-Achse (auch als Gravitationsachse oder Abwärtsrichtung bezeichnet) sein, obwohl zu erkennen ist, dass andere Referenzen gewählt werden können.
Schritt 6
Die in Schritt 5 gefundene Drehung qa wird auf die erste Zwischenorientierungsabschätzung (qi) angewendet, um eine zweite Zwischenorientierungsabschätzung qr zu finden.
Wenn qa eine Einheitsquaternion ist, kann dies berechnet werden als: qr = qa ⊗ qi
Wobei ⊗ das Quaternion-Produkt angibt.
Schritt 7
Die zweite Zwischenorientierungsabschätzung (qr) wird verwendet, um den Magnetvektor ms in den abgeschätzten Erdrahmen-Magnetvektor mr zu transformieren. Dies kann berechnet werden als: mr = qr ⊗ ms ⊗ Konjugat(qr)
Wobei ⊗ das Quaternion-Produkt ist und ms auf eine Quaternion mit der Komponente ms.w = 0 erweitert wird.
Als Alternative kann eine Drehmatrix aus qr gebildet werden und auf ms angewendet werden. Dies kann zu einer niedrigeren Anzahl von Berechnungen im Vergleich zum vorstehend erwähnten Quaternion-Produktverfahren führen.
Schritt 8
Anschließend wird ein neigungskompensierter Magnetvektor ma gebildet, der die z-Komponente des abgeschätzten Erdrahmen-Magnetvektors verwirft. Dies kann berechnet werden als:

ma = [mr.x, mr.y, 0]/sqrt(mr.x² + mr.y²) falls mr.x ≠ 0 oder

mr.y ≠ 0,

ma = [1, 0, 0] ansonsten
Falls mr.x und mr.y beide null sind, ist dies eine spezielle Situation und ma wird auf die magnetische Nordrichtung gesetzt.
Schritt 9
Dann wird eine Drehung um die z-Achse berechnet, die den neigungskompensierten Magnetvektor ma in die magnetische Referenzrichtung (1, 0, 0) dreht. Eine Quaternion qm', die diese Drehung darstellt, kann berechnet werden als: qm' = [Sm(1 + ma.x), 0, 0, –ma.y] wenn ma.x ≥ ma_xmin qm' = [Sm(ma.y), 0, 0, ma.x –1] wenn –1 < ma.x < ma_xmin qm' = [0, 0, 0, 1] ansonsten
Wobei ma_xmin ein Schwellenwert (–1 < ma_xmin < 1) ist, der verwendet wird, um die Berechnung für qm auszuwählen, die zum geringsten numerischen Fehler führt. Ein möglicher Wert für ma xmin ist null.
Sm ist ein Skalierungsfaktor (0 < Sm ≤ 1), der verwendet werden kann, um die Effekte von Messrauschen in den Magnetsensordaten zu verringern.
Anschließend wird eine Einheitsquaternion qm durch Dividieren von qm durch ihre Länge berechnet: qm = qm'/|qm'|
Schritt 10
Die Drehung qm wird dann auf die Zwischenorientierungsabschätzung qr angewendet, um eine verbesserte Orientierungsabschätzung qs zu bilden. In Einheitsquaternionform kann dies berechnet werden als: qs = qm ⊗ qr
Schritt 11
Eine kombinierte Orientierungsabschätzung wird unter Verwendung einer gewichteten Summe der Zwischenorientierungsabschätzungen berechnet: qt = α × qi + β × qr + γ × qs wobei alpha, beta und gamma Gewichtungsfaktoren mit einem Bereich von 0–1 sind, die der Bedingung unterliegen: α + β + γ = 1
α, β, γ können für optimale Leistung abgestimmt werden. Die optimalen Werte hängen von relativen Rauschpegeln der Gyroskopdaten, der Beschleunigungsmesserdaten und der Magnetometerdaten ab.
Schritt 12
Die Quaternion qn wird durch Normieren der Quaternion qt berechnet. Wenn die Länge von qt null ist, dann wird qn auf qp gesetzt. qn = qt/|qt| falls |qt| ≠ 0 qn = qp ansonsten
Es ist zu erkennen, dass die obige Ausführungsform (Schritte 1–12) nur ein Beispiel dessen ist, wie ein Verfahren gemäß der Offenbarung arbeiten kann. Die Offenbarung erfordert nicht, dass alle Schritte 1–12 vorhanden sind, und zieht auch Variationen dessen in Erwägung, wie jeder der Schritte 1–12 ausgeführt werden kann.
Insbesondere ist es nicht erforderlich, dass alle drei der Sensortypen vorhanden sind. Die Offenbarung kann immer noch funktionieren, wenn einer oder mehrere dieser Sensortypen fehlen, oder kann tatsächlich auch mit anderen Typen von Sensor funktionieren, die nicht auf Beschleunigungsmesser, Magnetometer und Gyroskope begrenzt sind.
Wenn beispielsweise Daten von einem Gyroskop nicht zur Verfügung stehen, können die Schritte 1 bis 3 weggelassen werden. In diesem Fall kann in Schritt 4 der Erdrahmen-Beschleunigungsvektor ae auf der Basis der Interpolation des Beschleunigungsmessermesswerts (as) mit qp direkt berechnet werden. Der Rest der Schritte fährt dann fort wie vorher mit qi = qp in der gewichteten Summe von Schritt 11.
Wenn Daten von einem Beschleunigungsmesser nicht zur Verfügung stehen, können auch die Schritts 4 bis 6 weggelassen werden. In diesem Fall wird die erste Zwischenquaternion qi (vom Effekt des Gyroskops abgeleitet) auf den Magnetvektor ms in Schritt 7 angewendet. Der Rest der Schritte fährt dann wie vorher fort mit qr = qi in der gewichteten Summe von Schritt 11.
Wenn Daten vom Magnetometer nicht zur Verfügung stehen, können ferner die Schritte 7–10 weggelassen werden. Der Rest der Schritte fährt dann fort wie vorher mit qs = qr in der gewichteten Summe von Schritt 11.
Daten von einem der Sensoren können nicht zur Verfügung stehen, wenn die Vorrichtung, deren Orientierung erfasst wird, nicht mit dem vollständigen Satz von Sensoren ausgestattet ist. Die Sensoren können auch verschiedene Abtastraten aufweisen, so dass zu irgendeinem gegebenen Zeitpunkt Messwerte von einem langsamer abgetasteten Sensor fehlen können. In diesem Fall kann dennoch eine neue Orientierung auf der Basis der Messwerte, die vorhanden sind, berechnet werden. Das Verfahren funktioniert auch, wenn nur ein Sensormesswert vorhanden ist.
Die Verarbeitungsschritte können in Gleitkomma- oder Festkommapräzision ausgeführt werden und als zweckgebundene Hardwareblöcke (mit optionaler gemeinsamer Ressourcennutzung) oder an einer Zentraleinheit implementiert werden, wobei die vorgeschriebenen Schritte als Software/Firmware-Programm ausgeführt werden.
In der Implementierung können Multiplikationen mit bekannten Nullen weggelassen werden, um die Verarbeitungslast zu verringern.
Wahlweise können die Quadrat- und reziproken Quadratwurzelfunktionen durch Polynome genähert werden, um die Verarbeitungslast zu verringern.
Die Eingangsdaten gs, as, ms können gefiltert oder kalibriert werden, wobei Versätze, Verstärkungsdifferenzen und Nebensprechen kompensiert werden.
4 und 5 stellen die Anwendung von aufeinander folgenden orthogonalen Drehungen auf Beschleunigungsmesser- und Magnetometermesswerte dar. Eine Drehung q wird gefunden, die einen Beschleunigungsmessermesswert a zu einer Referenz aref dreht und auch einen Magnetometermesswert m zu einer Referenz mref dreht. Die Beschleunigungsmesserreferenz aref ist die Gravitationsachse (Abwärtsrichtung) und die Magnetometerreferenz mref ist eine Richtung in der Ebene y = 0 (Nord-Abwärts). 4 zeigt die Anwendung einer ersten Teildrehung qa auf den Beschleunigungsmessermesswert, die den Beschleunigungsmesservektor a in die Erd-z-Richtung dreht. Diese Drehung qa weist eine Drehachse in z = 0 auf und wird auch auf den Magnetometermesswert angewendet, um einen gedrehten Magnetometermesswert ma zu erhalten.
Wie in 5 gezeigt, wird dann eine zweite Teildrehung qm auf den gedrehten Magnetometermesswert ma angewendet. Diese Drehung qm weist eine Drehachse entlang z auf und dreht den gedrehten Magnetometermesswert ma in die Nord-Abwärts-Ebene (wobei y = 0). Es ist zu beachten, dass qm aref nicht dreht, da es um die Achse z = 0 ist. Die Drehung q besteht dann aus der Drehung qa, gefolgt von qm; a = qm.qa.
Die vorliegende Offenbarung verbessert die Reaktionszeit und die Genauigkeit der Orientierungssensorvereinigung mit der geringstmöglichen Rechenkomplexität.
Sie schafft ein genaues momentanes Ergebnis nach dem Einschalten, Aufwecken oder einer plötzlichen Änderung mit viel geringerer Rechenkomplexität als das erweiterte Kalman-Filter und arbeitet selbst dann, wenn Sensordaten nur teilweise zur Verfügung stehen.
Der Graph von 6 stellt die Vorteile von dem neuen Algorithmus (”intelligent” genannt) für simulierte rauschbehaftete Sensordaten dar. Der absolute Fehler zwischen der wahren Orientierung und der abgeschätzten Orientierung ist als Funktion der Zeit gezeigt. Hier ist die Abtastrate 128 Hz und der anfängliche Fehler in der Orientierung ist 10 Grad.
Der intelligente Systemalgorithmus erlangt eine sehr gute Abschätzung der Orientierung mit einem Fehler von weniger als 0,2 Grad innerhalb 1 Abtastwerts. Im Gegensatz dazu erfordern die iterativen Mahony- und Madgwick-Algorithmen 10–20 Sekunden zum Konvergieren und sind nur bis zu 0,5–1,0 Grad genau. Die Festkomma- und Gleitkommaversionen des intelligenten Algorithmus arbeiten gleichermaßen gut.
Verschiedene Verbesserungen und Modifikationen können am Obigen durchgeführt werden, ohne vom Schutzbereich der vorliegenden Offenbarung abzuweichen.

Claims

Verfahren zum Berechnen einer Orientierung eines Objekts, das umfasst: Empfangen einer Eingangsorientierung; Empfangen eines Messwerts von einem ersten Orientierungssensor; Empfangen eines Messwerts von einem zweiten Orientierungssensor; wobei der erste und der zweite Orientierungssensor von verschiedenen Typen sind; und Bestimmen einer aktualisierten Orientierung durch Berechnen einer Drehung auf der Basis der Orientierungssensormesswerte und Anwenden der berechneten Drehung auf die Eingangsorientierung; wobei das Berechnen einer Drehung umfasst: Berechnen einer ersten Drehung, die den Messwert von einem der Orientierungssensoren so dreht, dass er auf eine erste Referenzrichtung ausgerichtet ist; Anwenden der ersten Drehung auf den Messwert von dem anderen der Orientierungssensoren, um eine Zwischenorientierung zu erhalten; Berechnen einer zweiten Drehung, die die Zwischenorientierung so dreht, dass sie auf eine Referenzebene ausgerichtet ist, die von Achsen, einschließlich einer Achse, die auf die erste Referenzrichtung ausgerichtet ist, aufgespannt wird; und Kombinieren der ersten und der zweiten Drehung, um die berechnete Drehung zu erhalten.
Verfahren nach Anspruch 1, wobei das Berechnen einer zweiten Drehung ein Berechnen einer zweiten Drehung umfasst, die die Zwischenorientierung so dreht, dass sie auf eine zweite Referenzrichtung ausgerichtet ist, die zur ersten Referenzrichtung orthogonal ist.
Verfahren nach Anspruch 1, wobei der erste Sensor einen Beschleunigungsmesser umfasst und der zweite Sensor ein Magnetometer umfasst; und wobei das Berechnen einer ersten Drehung ein Drehen des Messwerts vom Beschleunigungsmesser in eine Beschleunigungsmesser-Referenzachse und das Drehen des Messwerts vom Magnetometer in eine Magnetometer-Referenzebene umfasst.
Verfahren nach Anspruch 3, wobei die Beschleunigungsmesser-Referenzachse eine Gravitationsachse umfasst und die Magnetometer-Referenzebene eine Nord-Abwärts-Ebene umfasst.
Verfahren nach einem vorangehenden Anspruch, das ferner ein Empfangen eines Messwerts von einem dritten Orientierungssensor umfasst, der von einem anderen Typ als der erste und zweite Orientierungssensor ist, und wobei das Berechnen einer Drehung ein Kombinieren einer dritten Drehung, die vom dritten Orientierungssensor abgeleitet ist, zusammen mit der ersten und der zweiten Drehung umfasst.
Verfahren nach Anspruch 5, wobei der dritte Sensor ein Gyroskop umfasst.
Verfahren nach Anspruch 6, wobei das Berechnen einer Drehung ein Anwenden einer Drehung auf die Eingangsorientierung auf der Basis der Messwerte vom Gyroskop, um eine vorläufige Orientierung zu erhalten; und darin ein Anwenden der ersten und der zweiten Drehung auf die vorläufige Orientierung umfasst.
Verfahren nach einem vorangehenden Anspruch, wobei der erste und der zweite Orientierungssensormesswert in Quaternionform umgesetzt werden und die berechneten Drehungen Einheitsquaternionen umfassen.
Verfahren nach einem der Ansprüche 5 bis 8, wobei der dritte Orientierungssensormesswert in Quaternionform umgesetzt wird und die berechneten Drehungen Einheitsquaternionen umfassen.
Verfahren nach Anspruch 9, wobei die Kombination von aufeinander folgenden Drehungen das Bewegen entlang der Oberfläche einer Einheitsquaternion-Hypersphäre umfasst.
Verfahren nach einem vorangehenden Anspruch, wobei die Sensoren verschiedene Abtastraten aufweisen; und wobei das Verfahren wiederholt wird und von irgendwelchen verfügbaren Messungen Gebrauch macht, die bei oder Zwischen aufeinander folgenden Iterationen des Verfahrens durchgeführt wurden.
Verfahren nach einem vorangehenden Anspruch, wobei die auf die Messwerte jedes Sensors angewendete Drehung gemäß einem Gewichtungsfaktor modifiziert wird und die aktualisierte Objektorientierung von den gewichteten Beiträgen abhängt.
Verfahren nach Anspruch 12, wobei die Gewichtungsfaktoren für jede Drehung von den relativen Rauschpegeln abhängig abgestimmt Werden, die jedem Sensor zugeordnet sind.
Verfahren nach Anspruch 12 oder Anspruch 13, wobei die Drehung für jeden Sensor modifiziert wird, bevor Daten vom nächsten Sensor verarbeitet werden.
Verfahren nach Anspruch 12 oder Anspruch 13, wobei die Drehungen für jeden Sensor modifiziert werden, nachdem Daten von allen Sensoren verarbeitet wurden.
Verfahren nach einem vorangehenden Anspruch, wobei der Schritt des Berechnens einer Drehung auf der Basis der Orientierungssensormesswerte ein Weglassen von Multiplikationen aufweist, die bekannte Nullen beinhalten.
Verfahren nach einem vorangehenden Anspruch, das in einer Gleitkommaarchitektur implementiert wird.
Verfahren nach einem der Ansprüche 1 bis 16, das in einer Festkommaarchitektur implementiert wird.
Vorrichtung zum Bestimmen der Orientierung eines Objekts mit einem oder mehreren Sensoren, die dem Objekt zugeordnet sind, und einem Prozessor, der angeordnet ist, um eine Eingangsorientierung zu empfangen; einen Messwert von einem ersten Orientierungssensor zu empfangen; einen Messwert von einem zweiten Orientierungssensor zu empfangen, wobei der erste und der zweite Orientierungssensor von verschiedenen Typen sind; und um eine aktualisierte Orientierung durch Berechnen einer Drehung auf der Basis der Orientierungssensormesswerte zu bestimmen, und die berechnete Drehung auf die Eingangsorientierung anzuwenden; wobei das Berechnen einer Drehung ein Berechnen einer ersten Drehung, die den Messwert von einem der Orientierungssensoren so dreht, dass er auf eine erste Referenzrichtung ausgerichtet ist; ein Anwenden der ersten Drehung auf den Messwert von dem anderen der Orientierungssensoren, um eine Zwischenorientierung zu erhalten; ein Berechnen einer zweiten Drehung, die die Zwischenorientierung so dreht, dass sie auf eine Referenzebene ausgerichtet ist, die von Achsen, einschließlich einer Achse, die auf die erste Referenzrichtung ausgerichtet ist, aufgespannt wird; und ein Kombinieren der ersten und der zweiten Drehung umfasst, um die berechnete Drehung zu erhalten.