DE60120363T2

DE60120363T2 - Orientationsbestimmung in einem neigbaren körper mittels einer modifizierten quaternionsdatendarstellung

Info

Publication number: DE60120363T2
Application number: DE60120363T
Authority: DE
Inventors: D. Jeffrey Huntsville ROWE
Original assignee: Independence Technology LLC
Current assignee: Independence Technology LLC
Priority date: 2000-02-03
Filing date: 2001-02-01
Publication date: 2007-05-10
Anticipated expiration: 2021-02-02
Also published as: EP1257784B1; CN1270162C; BR0108066A; CA2398704C; WO2001057474A1; DK1257784T3; CA2398704A1; EP1257784A1; DE60120363D1; AU3472701A; IL151006A0; ATE329228T1; NO20023641L; JP4808354B2; CN1422380A; IL151006A; NO335201B1; ES2266158T3; MXPA02007560A; NO20023641D0

Description

Die Erfindung betrifft das Verfolgen und Steuern neigbarer Körper.
Es ist bekannt, Quaternions zu benutzen, um die räumliche Orientierung eines Objekts wiederzugeben. Die Quaternion-Schreibweise ist im Allgemeinen rechnerisch effizienter nutzbar als die breitere Anwendung findende Euler-Datenwiedergabe. Außerdem ist die Quaternion-Schreibweise keinen Singularitäten ausgesetzt, die bei der Euler-Schreibweise auftreten können. Die folgenden US-Patentschriften offenbaren die Benutzung von Quaternions, um die räumliche Orientierung eines Objekts zu steuern, zu bestimmen und/oder darzustellen: Nr. 5,875,993; 5,212,480; 4,797,836; 4,742,356; und 4,737,794.
Allgemeine Erläuterungen zu Quaternions
Ein Quaternion ist eine hyperkomplexe Zahl mit vier Elementen, die zuerst von Sir William Rowan Hamilton im Jahr 1843 entwickelt wurde. Ein Quaternion besteht aus einem skalaren Teil und einem komplexen Vektorteil. Der Vektorteil besteht aus einem geordneten Tripel (Vektor) mit drei realen Komponenten, denen durch drei orthogonale komplexe Einheitsvektoren: i, j, k eine Richtung zugewiesen wird. Ein Beispiel eines allgemeinen Quaternions Q ist unten dargestellt: Q = q0 + iq1 + jq2 + kq3 (Gl. 1)
Eine Addition von Quaternions wird durchgeführt, indem Komponenten in gleichen Richtungen addiert werden. Eine Multiplikation wird durchgeführt, indem die folgenden Produkte der Einheitsbasisvektoren notiert werden: i2 = j2 = k2 = ijk = –1 (Gl. 2) ij = –ji = k (Gl. 3) jk = –kj = i (Gl. 4) ki = –ik = j (Gl. 5)
Da das Quaternion hyperkomplex ist, weist es auch eine komplex Konjugierte auf, in der die Richtung des Vektorteils umgekehrt ist. Ein Beispiel ist unten dargestellt: Q* = q0 – iq1 – jq2 – kq3 (Gl. 6)
Die Quadratgröße des Quaternions kann berechnet werden, indem das Produkt des Quaternions mit seinem komplex Konjugierten wie folgt berechnet wird: Q2 = QQ* = q0 2 + q1 2 + q2 2 + q3 2 (Gl. 7)
Ein Quaternion mit einer Einheitsgröße (Q² = 1) weist eine spezielle Bedeutung auf. Genau ausgedrückt, dient es als ein zweiseitiger Drehungsoperator. Es sollte erwähnt werden, daß Hamilton Quaternions im Zuge seiner Bemühungen entdeckte, eine dreidimensionale Erweiterung des Rotationseffekts zu entwickeln, der auf der komplexen Ebene produziert wird, wenn eine komplexe Zahl mit einer komplexen Einheitszahl der Form exp (iθ) multipliziert wird. Der Rotationseffekt von exp (iθ) ergibt sich, da die Multiplikation komplexer Zahlen die Multiplikation ihrer jeweiligen Größen und die Addition ihrer jeweiligen Phasen erforderlich macht. Da exp (iθ) eine Einheitsgröße aufweist, kann es nur die Phase des Produkts beeinflussen. Auf der komplexen Ebene zeigt sich dies als eine Drehung um den Ursprung um einen Winkel θ. Beim Versuch, diesen Effekt auf Vektordrehungen zu verallgemeinern, unternahm Hamilton ursprünglich einen Versuch mit hyperkomplexen Zahlen mit drei Elementen. Erst als er feststellte, daß vier Elemente nötig waren, um den „Phasen"-Änderungen im dreidimensionalen Raum Rechnung zu tragen, konnte er erfolgreich das gewünschte Ergebnis erzielen.
Üblicherweise wird eine Vektordrehung mit Hilfe eines einseitigen Rotationsoperators R erzielt, der im dreidimensionalen Raum als eine reale orthgonale 3 × 3 Matrix dargestellt werden kann. Diese Transformationsmatrix dreht einen Vektor x zu einem Vektor x' durch linksseitige Multiplikation, wie dargestellt: x' = Rx wobei x ∊ R3x1 und R ∊ R3x3 (Gl. 8)
Ein zweiseitiger Operator muß unter Benutzung sowohl linksseitiger als auch rechtsseitiger Multiplikation angewandt werden. Im Fall des Quaternion-Operators wird die Drehung erreicht, wenn ein jeweiliges Quaternion (X) mit einem Skalarteil von Null (d.h. ein Vektor) mit dem Einheitsquaternion und seiner Konjugierten wie folgt vor- und nachmultipliziert wird: X' = QXQ* (Gl. 9)
Der resultierende Vektor X' wird um eine allgemeine Achse um einem bestimmten Winkel gedreht, welche beide durch das Einheitsquaternion Q bestimmt werden. Wenn die Rotationsachse durch einen Einheitsvektor n dargestellt ist und der Rotationswinkel durch einen Winkel θ dargestellt ist, können die Einheitsquaternion-Komponenten wie folgt wiedergegeben werden: q0 = cos(θ/2) und q = nsin(θ/2) wobei q = (q1, q2, q3) (Gl. 10)
Diese Komponenten erfüllen die Normierungsbedingung: 1 = q0 2 + q1 2 + q2 2 + q3 2 (Gl. 11)
Die so definierten Quaternion-Komponenten werden auch als Euler-Parameter bezeichnet. Diese Parameter enthalten alle notwendigen Informationen, um die Achse und den Winkel der Drehung abzuleiten. Die Rotationsachse, die durch den Einheitsvektor n definiert ist, wird auch als Eigenachse bezeichnet, da sie der Eigenvektor der einseitigen Rotationsmatrix R ist, entsprechend dem Eigenwert λ = +1. Dies geschieht, weil die Rotationsachse sowohl dem ursprünglichen als auch dem gedrehten Bezugssystem gemein sein muß und deshalb nicht durch den Rotationsoperator verändert werden darf. Es ist zu beachten, daß die so genannte Eigenachsenrotation eine einzelne Rotation um eine allgemeine Achse ist, im Gegensatz zur Euler-Achsenrotation, welche dieselbe Transformation durch das Durchführen von drei separaten Rotationen erreicht: Yaw, Pitch und Roll, jeweils um die z-, y- bzw. x-Achse.
Die komplexen Quaternion-Einheitsvektoren (i, j, k) sind wie folgt mit den Pauli-Spin-Matrizes verwandt: i = –iσ1 (Gl. 12) j = –iσ2 (Gl. 13) k = –iσ3 (Gl. 13A)wobei i = √–1 (Gl. 14)
(Siehe The Theory of Spinors von E. Cartan.)
Unter Benutzung der Definitionen der Pauli-Spin-Matrizes und der Einheitsquaternion-Koeffizienten kann das Einheitsquaternion wiedergegeben werden als: Q = σ0cos(θ/2) – i(n·σ)sin(θ/2) (Gl. 19)wobei n·σ = n1σ1 + n2σ2 + n3σ3 (Gl. 20)
Dies kann als äquivalent zu dem folgenden Matrix-Exponential gezeigt werden: Q = e–σ·θ/2 wobei θ = nθ (Gl. 21)
Zu beachten ist die Ähnlichkeit dieser Form des Quaternions und der exponentiellen Form einer komplexen Einheitszahl, die zuvor erläutert wurde. Diese Form suggeriert die dreidimensionale Änderung der „Phase", die ursprünglich Hamiltons Ziel war. Dem Erscheinen des halben Winkels wird dadurch Rechnung getragen, daß Q eine zweiseitige Transformation ist. So tragen der linke und der rechte Faktor Q und Q* jeweils die Hälfte der erwünschten räumlichen Phasenverschiebung bei.
Es ist oft praktischer, anstelle der traditionellen Hamiltonschen Form des Quaternions die Pauli-Spin-Matrix-Form zu benutzen. Beispielsweise kann ein Vektor als eine Matrix wiedergegeben werden, indem das innere Produkt wie folgt gebildet wird: X = x·σ = x1σ1 + x2σ2 + x3σ3 (Gl. 22)was ergibt:
Diese Matrixform weist viele nützliche Eigenschaften auf. Beispielsweise kann gezeigt werden, daß die Spiegelung des Vektors x durch eine Ebene, die durch die Einheitsnormale a definiert ist, leicht wie folgt mit Hilfe der Matrixform der Vektoren erzeugt werden kann: X' = –AXA wobei A = a·σ (Gl. 24)
Es kann auch gezeigt werden, daß durch zwei Spiegelungen jede Rotation erzeugt werden kann. Wenn die Spiegelungsebenen sich bei einem Winkel von θ/2 schneiden und die Schnittlinie durch den Einheitsvektor n definiert ist, dreht die resultierende Transformation jeden Vektor x um die Eigenachse n um einen Winkel θ. Dies ist im Folgenden dargestellt, wobei die Einheitsnormalen zu den Ebenen Vektoren a und b sind. X' = BAXAB (Gl. 25)
Dieser zweiseitige Operator, der eine Drehung durchführt, weist große Ähnlichkeit zu der Quaternion-Drehung auf, die zuvor beschrieben wurde. In der Tat kann gezeigt werden, daß Q = BA. Die folgende multiplikative Identität ergibt sich aus den Eigenschaften der Matrixform eines Vektors: AB = σ0a·b + i(axb)·σ (Gl. 26)
Im Fall einer Drehung um Winkel θ müssen sich die Einheitsnormalenvektoren a und b beim Winkel θ/2 schneiden. Deshalb ergeben ihr Skalarprodukt und ihr Kreuzprodukt a·b = cos(θ/2) und axb = nsin(θ/2), wobei n zur Schnittlinie parallel ist. Eine Ersetzung dieser Werte in Gl. 26 ergibt das gewünschte Resultat: Q = BA = σ0cos(θ/2) – i(n·σ)sin(θ/2) = e–σ·θ/2 (Gl. 27)
Ein interessantes Merkmal dieser Entwicklung ist, daß die Koeffizienten des Quaternions mit Hilfe einfacher Skalar- und Kreuzprodukte der geeigneten Einheitsvektoren ermittelt werden können. Insbesondere müssen die zwei Vektoren a und b zu der Rotationsachse senkrecht und durch einen Winkel von θ/2 separiert sein. Es kann auch gezeigt werden, daß bei Kenntnis von zwei beliebigen Vektoren α und β in einem Bezugssystem und Kenntnis ihrer gedrehten Gegenstücke α' und β' im gedrehten Bezugssystem das Quaternion und die zugehörige Rotationsmatrix, die für die Transformation verantwortlich ist, eindeutig bestimmt werden können. So kann anhand von zwei Vektoren, die in dem Basissystem zwischen dem Ursprung und zwei externen Referenzpunkten definiert sind, die Lage des Systems eindeutig bestimmt werden, indem zusätzliche „Sichtungen" derselben zwei externen Referenzpunkte von dem gedrehten System genommen werden und die Koordinaten der Referenzvektoren, die in dem letzten System gemessen werden, mit denen des Ausgangssystems verglichen werden.
Es kann auch vorgesehen sein, Quaternion-Komponenten durch direkte Integration einer Quaternion-Geschwindigkeit zu berechnen. Dies macht einen Ausdruck für die Ableitung des Quaternions als Funktion der Winkelgeschwindigkeit erforderlich. Dies läßt sich durch Differenzierung von Gl. 21 ableiten. Die resultierenden Ableitungen der Quaternion-Koeffizienten sind jeweils eine lineare Kombination der Winkelgeschwindigkeitskomponenten, die mit den vorliegenden Quaternion-Komponenten gewichtet wurden. Wenn das Quaternion und die Winkelgeschwindigkeiten in Vektorform gebracht werden, ergibt sich die folgende Matrixgleichung:
wobei die punktierten q_i die Komponenten der Quaternion-Geschwindigkeit sind, und die ω_i die Komponenten der Winkelgeschwindigkeit ω(= θ) sind.
Die Orientierung und/oder Drehung eines Körpers kann in mehr als einem Bezugssystem dargestellt werden. Beispielsweise ist es möglich, ein Bezugssystem im Verhältnis zu dem fraglichen Körper selbst zu definieren, oder ein Bezugssystem im Verhältnis zu einem externen fest angeordneten Objekt zu definieren. Für einige Anwendungen kann die Erde als ein fest angeordnetes Objekt zum Definieren eines Bezugssystems benutzt werden. In Gl. 28 bezieht sich die Winkelgeschwindigkeit auf das Körpersystem. Eine ähnliche Matrixgleichung, wobei die Winkelgeschwindigkeit sich auf das Erde-System bezieht, ist unten als Gl. 29 gezeigt. Es ist zu beachten, daß das Verschieben des Bezugssystems zu mehreren Vorzeichenänderungen in der Quaternion-Matrix führt.
Gl. 29 kann mit Hilfe von Matrixvariablen in kompakterer Weise wie folgt geschrieben werden: dq/dt = QωE/2 (Gl. 30)
Gl. 30 kann ebenfalls für ω_E gelöst werden, wie im Folgenden gezeigt, indem geschrieben wird, daß Q^TQ = I: ωE = 2QT(dq/dt) (Gl. 31)
Wenn die Gleichung der Quaternion-Geschwindigkeit mit Hilfe der momentanen Winkelgeschwindigkeit des fraglichen Körpers integriert wird, enthält das resultierende Quaternion die notwendige Information zum Transformieren von Koordinaten von ihrem Ausgangsbezugssystem zu dem gedrehten System. So können die Quaternion-Komponenten benutzt werden, um die Rotationsmatrix R zu bilden, die zuvor beschrieben wurde (Gl. 8). Die Rotationsmatrix, die die Körperkoordinaten in Erdkoordinaten transformiert, ist im Folgenden hinsichtlich der Quaternion-Komponenten gezeigt:
Da die Transformationsmatrix R_EB orthogonal ist, ist ihre Transponierte die inverse Transformation R_BE. Aus diesem Grund können Quaternions benutzt werden, um die Lage eines Körpers durch jede Winkelbewegung zu verfolgen.
Aus Vergleichszwecken sind im Folgenden die Gleichungen zum Berechnen der Euler-Winkelgeschwindigkeiten gezeigt. Die zugehörige Rotationsmatrix R ist ebenfalls als eine Funktion der Euler-Winkel gezeigt. dφ/dt = P + (dψ/dt)sinθ dθ/dt = Qcosφ – Rsinφ dψ/dt = (Qsinφ + Rcosφ)secθwobei

φ: = Roll
θ: = Pitch
ψ: = Yaw
ω: = Winkelgeschwindigkeitsvektor = [P Q R]^T

Wie bekannt, ist das Euler-Verfahren stark von trigonometrischen Funktionen abhängig und ist relativ rechenintensiv. Zusätzlich sind die Euler-Ratengleichungen von der Sekante des Pitch-Winkels ab, was zu einer Singularität führt, wenn der Pitch ±90 Grad erreicht. Im Gegensatz dazu enthält das Quaternion-Verfahren keine trigonometrischen Funktionen, und verläßt sich nur auf Multiplikation und Addition. Aus diesem Grund ist das Quaternion-Verfahren wesentlich recheneffizienter. Wie oben erwähnt, kann die Lage eines Körpers eindeutig in Quaternion-Form wiedergegeben werden, wenn zwei Vektoren vorliegen, die in dem Körpersystem definiert sind, und zwei externe Bezugspunkte. Allerdings kann es schwierig und/oder teuer sein, zwei externe Bezugspunkte aufzustellen, insbesondere im Fall von terrestrischen Objekten, sei es, daß sie an Land, auf oder unter der Fläche eines Wasservolumens angeordnet sind.
Der Erfinder der vorliegenden Erfindung glaubt, daß er als erster erkannt hat, wie die Quaternion-Schreibweise in nützlicher Weise derart modifiziert werden kann, daß nur ein externer Bezugspunkt benötigt wird, um die modifizierte Quaternion-Form zu erzeugen, und daß eine solche modifizierte Quaternion-Form nutzbringend auf Körper angewandt werden kann, die frei neigbar sind, für die jedoch der Kurs (Yaw) beschränkt ist oder nicht von dem System gesteuert werden muß, das die modifizierten Quaternions benutzt.
AUFGABEN UND KURZDARSTELLUNG DER ERFINDUNG
Es ist eine Aufgabe der Erfindung, eine verbesserte Verfolgung und/oder Steuerung neigbarer Objekte bereitzustellen.
Es ist außerdem Aufgabe der Erfindung, ein Verfolgungs- und Steuerungssystem bereitzustellen, das weniger Rechenaufwand benötigt als Systeme des Stands der Technik.
Es ist des Weiteren eine Aufgabe der Erfindung, Quaternion-Verarbeitung für ein System bereitzustellen, wobei nur eine externe Referenz verfügbar ist.
Es ist eine weitere Aufgabe der Erfindung, eine zuverlässige Verfolgung bei hoher Bandbreite der Orientierung eines Objekts bereitzustellen.
Ein Aspekt der Erfindung stellt ein Verfahren zum Schätzen der Lage eines neigbaren Körpers bereit, wobei der Körper Neigungsabtastmittel wie z.B. fluidische Neigungssensoren und Winkelgeschwindigkeitsmessvorrichtungen wie z.B. Gyroskope umfaßt. Das Verfahren umfaßt die folgenden Schritte: Ausgeben einer Winkelgeschwindigkeitsinformation von den Winkelgeschwindigkeitsabtastvorrichtungen, Transformieren und Integrieren der ausgegebenen Winkelgeschwindigkeitsinformation zur Erzeugung einer ersten Quaternion-Positionsinformation in der Weise, daß die erste Quaternion-Positionsinformation darauf beschränkt ist, eine Drehung um eine horizontale Achse in einem Erde-Bezugssystem wiederzugeben, Ausgeben von Neigungsinformation von den Neigungsabtastvorrichtungen, Verarbeiten der ausgegebenen Neigungsinformation zur Erzeugung einer zweiten Quaternion-Positionsinformation in der Weise, daß die zweite Quaternion-Positionsinformation darauf beschränkt ist, eine Drehung um eine horizontale Achse in dem Erde-Bezugssystem wiederzugeben, Vergleichen der ersten Quaternion-Positionsinformation mit der zweiten Quaternion-Positionsinformation zur Erzeugung einer Fehlerinformation, und Verwenden der Fehlerinformation zur Kompensation einer Drift in den Winkelgeschwindigkeitsabtastvorrichtungen.
Ein weiterer Aspekt der Erfindung stellt ein Verfahren zum Schätzen der Lage eines neigbaren Körpers bereit, wobei der Körper Neigungsabtastvorrichtungen, wie z.B. fluidische Neigungssensoren und Winkelgeschwindigkeitsabtastvorrichtungen, wie z.B. Gyroskope umfaßt. Das Verfahren umfaßt folgende Schritte: Ausgeben einer Winkelgeschwindigkeitsinformation von den Winkelgeschwindigkeitsabtastvorrichtungen, Erzeugen einer modifizierten Quaternion-Geschwindigkeitsinformation aus der ausgegebenen Winkelgeschwindigkeitsinformation, wobei die modifizierte Quaternion-Geschwindigkeitsinformation in der Form einer skalaren Größe und eines zweikomponentigen Vektors vorliegt, Integrieren der modifizierten Quaternion-Geschwindigkeitsinformation zur Erzeugung einer ersten modifizierten Quaternion-Lageinformation, wobei der Schritt des Erzeugens der modifizierten Quaternion-Geschwindigkeitsinformation darauf beschränkt ist, daß die erste modifizierte Quaternion-Lageinformation die Form e = e₀ + e₁i + e₂j aufweist, wobei i, j imaginäre Einheitsvektoren sind, welche miteinander und mit einem dritten Einheitsvektor k die folgenden Gleichungen erfüllen: i2 = j2 = k2 = ijk = –1, ij = –ji = k, jk = –kj = i, ki = –ik = j,
Ausgeben von Neigungsinformation aus den Neigungsabtastvorrichtungen, Erzeugen einer zweiten Quaternion-Positionsinformation aus der ausgegebenen Neigungsinformation, wobei die zweite Quaternion-Positionsinformation die Form e = e₀ + e₁i + e₂j aufweist, Vergleichen der ersten modifizierten Quaternion-Positionsinformation mit der zweiten modifizierten Quaternion-Positionsinformation zur Erzeugung einer modifizierten Quaternion-Positionsfehlerinformation, Transformieren der modifizierten Quaternion-Positionsfehlerinformation in ein Winkelgeschwindigkeitsfehlersignal, und Verwenden des Winkelgeschwindigkeitsfehlersignals zum Kompensieren eines Fehlers in der ausgegebenen Winkelgeschwindigkeitsinformation.
Bei dem erfindungsgemäß bereitgestellten Verfahren wird Neigungsinformation, die von einem fluidischen Sensor oder Ähnlichem ausgegeben wird, benutzt, um Drift oder andere Verschiebungen in einem Winkelgeschwindigkeitssensor mit hoher Bandbreite, wie z.B. einer Gruppe von Gyroskopen, zu kompensieren. Die Fehlerkompensation, die durch die Neigungsreferenzdaten bereitgestellt wird, ermöglicht es, daß korrigierte Winkelgeschwindigkeitsdaten mit hoher Bandbreite integriert werden, um eine Echtzeit-Lageverfolgungsschätzung bereitzustellen, die keine Verschiebungsfehler akkumuliert. Kurs-Winkelgeschwindigkeitsinformation (Yaw-Geschwindigkeitsinformation) wird verworfen, um eine modifizierte (3-Komponenten) Quaternion-Darstellung zu erzeugen, die mit einer ähnlich modifizierten Quaternion-Geschwindigkeitsinformation verglichen wird, die von Neigungsinformation abgeleitet wird und auf einem einzigen Schwerkraft-Referenzpunkt basiert. Fehlerkompensation und Filtern werden mit der Quaternion-Schreibweise durchgeführt, um Recheneffizienz bereitzustellen.
Die oben genannten sowie weitere Aspekte, Merkmale und Vorteile der Erfindung werden in der folgenden Beschreibung und den Figuren erläutert und/oder gehen aus diesen hervor.
KURZE BESCHREIBUNG DER FIGUREN
1 ist eine schematische und verallgemeinerte Seitenansicht eines neigbaren Körpers, wobei die vorliegende Erfindung unter Bezugnahme auf diesen angewandt ist.
2 ist eine Draufsicht von oben auf den neigbaren Körper aus 1.
3 ist eine Blockdiagrammdarstellung eines Steuersystems für den neigbaren Körper aus 1.
4 ist eine Blockdiagrammdarstellung eines Lage- und Rotationsschätzungsabschnitts des Steuersystems aus 3, der gemäß der Erfindung bereitgestellt ist.
GENAUE BESCHREIBUNG BEVORZUGTER AUSFÜHRUNGSFORMEN
1 zeigt eine schematische Seitenansicht eines verallgemeinerten neigbaren Körpers 10, auf den die vorliegende Erfindung anwendbar ist.
2 ist eine Draufsicht von oben auf denselben Körper 10. Der Körper 10 kann jedes Objekt sein, für das die Neigung von einem horizontalen Zustand aus verfolgt, dargestellt und/oder gesteuert werden soll. Beispielsweise kann der Körper 10 ein Roboter, ein neigbarer motorgesteuerter Rollstuhl, eine Bohrplattform, ein schwimmendes Schiff oder Boot oder ein Unterwasserfahrzeug, ein neigbares Schienenzugfahrzeug oder ein neigbarer Schienenwagen, ein Raum oder Fahrzeug zum Befördern von Personen bei einer Vergnügungsfahrt, ein Flugsimulator, oder ein Behälter sein, der benutzt wird, um ein Material aufzunehmen und wahlweise abzuschütten, das in einer chemischen Reaktion oder einem anderen Herstellungsprozeß benutzt wird. Der Körper 10 kann auch ein Automobil sein. Dem Körper 10 ist ein Steuerungssystem 12 zugeordnet, das von einer oder mehreren Neigungsabtastvorrichtungen 14 und von einer oder mehreren Winkelgeschwindigkeitsabtastvorrichtungen 16 Eingaben empfängt. Die Neigungsabtastvorrichtungen 14 können übliche fluidische Neigungssensoren und/oder Beschleunigungsmesser sein. Es ist vorgesehen, daß einer oder mehrere Neigungssensoren angeordnet sind. Der Winkelgeschwindigkeitssensor 16 kann durch übliche Gyroskope oder andere bekannte Winkelgeschwindigkeitsabtastvorrichtungen aufgebaut sein. Es ist vorgesehen, daß einer oder mehrere Winkelgeschwindigkeitssensoren angeordnet sind.
Ein Koordinatensystem für die folgenden Erläuterungen kann wie in 1 und 2 dargestellt aufgestellt sein. Die horizontale Vorwärtsrichtung wird als die positive x-Achse betrachtet, wie angezeigt durch Pfeil 20. Die positive y-Achse soll in horizontaler Richtung nach rechts weisen und senkrecht zu der x-Achse sein, wie durch Pfeil 22 angezeigt (2). Die positive z-Achse soll vertikal gerade nach unten weisen, wie durch Pfeil 24 angezeigt (1). Die Neigung ist als eine Winkelabweichung von der Vertikalen definiert, mit einer Spanne begrenzt auf plus oder minus 180°. Die Neigungsrichtung ist in Bezug auf die x- und y-Achsen definiert. Die Drehung um die x-Achse soll als Roll betrachtet werden, wobei ein positiver Roll eine Neigung nach rechts ist. Der Pitch ist als Drehung um die y-Achse definiert, wobei ein positiver Pitch eine Neigung nach hinten ist. Yaw ist als Drehung um die z-(vertikale)Achse definiert, wobei ein positiver Yaw als Drehung nach rechts definiert ist. Es ist zu beachten, daß diese Definitionen eine Rechte-Hand-Regel befolgen. Die genannten Definitionen von Pitch und Roll sind ähnlich wie diejenigen für den Euler-Pitch und -Roll, nur daß Euler-Winkel in einer spezifischen Reihenfolge angewandt werden und intermediäre Bezugssysteme erzeugen. In dem hier erlüterten neigungsbasierten System wird angenommen, daß Drehungen um die vertikale Achse nicht vorkommen oder ignoriert werden können. Beispielsweise kann der Kurs des Körpers 10 fest sein, oder für die Neigungsverfolgung und -steuerung irrelevant sein, oder durch einen menschlichen Bediener gesteuert sein, oder von einem System gesteuert sein, das Neigungsinformation nicht beachtet oder beachten muß.
3 zeigt in Blockdiagrammform Komponenten des Steuerungssystems 12, das in 1 gezeigt ist. Das Steuerungssystem 12 weist eine Schnittstelle 40 auf, durch die das Steuerungssystem Signale empfängt, die von den Neigungssensoren 14 und Gyroskopen 16 ausgegeben werden. Diese Signale werden erfindungsgemäß durch einen Lage/Rotationsschätzungsblock 42 verarbeitet. Auf der Basis der Signale, die von den Gyroskopen und den Sensoren ausgegeben werden, stellt der Block 42 eine Schätzung der Lage und/oder Winkeldrehgeschwindigkeiten des Körpers 10 bereit, in einer Weise, die im Folgenden beschrieben werden soll. Auf der Basis der Lage- und/oder Rotationsinformation, die von Block 42 bereitgestellt wird, erzeugt ein Block 44 Steuersignale zum Steuern von Stellmitteln 46 zum Steuern der Lage des Körpers 10. Die Stellmittel können Motoren, Solenoide, flutbare Kammern, oder andere übliche mechanische, elektromechanische, hydraulische oder pneumatische Vorrichtungen sein, die benutzt werden, um die Lage der oben genannten Körpertypen zu steuern. Elektronische Hardware zum Durchführen von wenigstens Teilen der Blöcke 42 und 44 kann durch einen oder mehrere übliche MikroProzeßoren gebildet sein. Zusätzlich weist der Steuerungssignal-Erzeugungsblock 44 auch vorzugsweise eine Antriebschaltung auf, um geeignete Antriebsignale für die Stellmittel 46 bereitzustellen.
Obwohl das Steuerungssystem 12 in 1 als von dem Körper 10 getragen gezeigt ist, versteht es sich, daß wenigstens einige Abschnitte des Steuerungssystems 12 physisch von dem Körper 10 getrennt sein können. Beispielsweise können Verarbeitungsschaltungen zum Ausführen einiger oder -aller Blöcke 42 und 44 entfernt von dem Körper 10 angeordnet sein, und können Ausgaben von den Neigungssensoren 14 und Gyroskopen 16 über Telemetrie empfangen, und können auch über geeignete Funkkommunikationskanäle Steuersignale zurück an den Körper 10 übertragen.
Schätzung von Lage und Drehung
Im Folgenden soll der Betrieb des Lage/Rotationsschätzungsblocks 42 beschrieben werden.
Neigungssensoren, wie z.B. übliche fluidische Sensoren, können verhältnismäßig frei von Drift sein und deshalb eine zuverlässige Anzeige eines Schwerkraftvektors bereitstellen. So kann die Lage eines statischen Systems im Verhältnis zur Schwerkraft mit Hilfe solcher Sensoren bestimmt werden. Allerdings sind in einem dynamischen System Sensoren dieser Art empfindlich gegenüber Winkel- und Vibrationsbeschleunigungen, und müssen deshalb tiefpassgefiltert werden, um die Auswirkungen anderer Beschleunigungen als der Schwerkraft abzuschwächen. Die resultierenden Daten stellen die mittlere Richtung der Schwerkraft über eine endliche Zeitperiode hinweg bereit, die durch die Bandbreite des Filters bestimmt wird. Bei Systemen, die eine schnelle Antwortzeit benötigen, sind Schwerkraftsensoren allein nicht ausreichend.
Andererseits können Gyroskope oder andere Winkelgeschwindigkeitsmesser eine hohe Bandbreite und schnelle Reaktionen bereitstellen, ohne daß sie von Beschleunigung störend beeinflusst werden.
Obwohl die Winkelgeschwindigkeit, die von solchen Sensoren erfasst wird, nicht direkt die Lage des Systems anzeigt, können die Geschwindigkeitsdaten integriert werden, um Positionsinformation zu erzeugen. Allerdings sind Geschwindigkeitssensoren Drift ausgesetzt, was bei der Integration zu signifikanten Fehlern führen kann. Der Schätzungsprozeß, der von Block 42 bereitgestellt wird, kombiniert die Information hoher und niedriger Bandbreite, die jeweils von den Gyroskopen 16 und den Neigungssensoren 14 bereitgestellt wird, um genaue Positions- und Drehungsdaten bereitzustellen, mit Hilfe einer modellbasierten Schätzertopologie.
Bei einem modellbasierten Schätzer wird eine Schätzung der tatsächlichen Position durch Vergleichen von Sensordaten mit einem internen Modell der Systemdynamik erzeugt. Der Fehler zwischen den gemessenen Daten und den vorhergesagten Daten wird benutzt, um die Schätzung ständig zu verfeinern. Das Ausmaß, in dem dieser Fehler die Schätzung beeinflusst, wird durch die Gewichtungsmatrix H bestimmt, die die geeigneten Zustandsfehler zurück an den Schätzer führt. Die Zustandsvariablen-Gleichungen für einen solchen Schätzer sind im Folgenden dargestellt: dxe/dt = Axe + Bu + H(y – ye) ye = Cxe + Du dx/dt = Ax + Bu y = Cx + Duwobei

x: = Systemzustandsvektor
x_e: = Schätzungszustandsvektor
y: = Systemausgangsvektor
y_e: = Schätzungsausgangsvektor
u: = System/Schätzer-Eingangsvektor
A: = Systemmatrix
B: = Eingangsmatrix
C: = Ausgangsmatrix
D: = Eingangs/Ausgangsmatrix
H: = Rückkopplungsmatrix

Es ist zu beachten, daß der tief gestellte Index „e" geschätzte Parameter bezeichnet. Das interne Systemmodell wird durch die Matrizen (A, B, C, D) definiert. Die Rückkopplungsmatrix H bestimmt die Stabilität des Schätzers. Ein stabiler Schätzer erreicht im Laufe der Zeit ein Equilibrium, wobei die tatsächlichen oder geschätzten Zustände gleich sind (x_e = x). Das interne Modell basiert oft auf einfachen Integratoren. Die Effektivität eines solchen einfachen Modells hängt mit der Tatsache zusammen, daß Integration und Differenzierung für physikalische Phänomene fundamental sind. Beispielsweise ist die Geschwindigkeit eine Ableitung der Position: v = dx/dt. Deshalb ist es anhand eines einfachen Integrators erster Ordnung mit den Parametern A = 0, B = 1, C = 1, D = 0, und einem Rückkopplungszuwachs H = k₁ möglich, einen Schätzer zu erzeugen, der die gemessene statische Position des Systems θ₀ und die gemessene dynamische Position θ kombiniert, so daß die Schätzung θ_e über den gesamten Frequenzbereich hinweg genau ist. Im Effekt ist die Schätzung eine frequenzabhängige gewichtete Summe der zwei Messungen. Die Laplace-Transformation dieses Ergebnisses ist im Folgenden gezeigt: θe(s) = θ0(s)[k1/(s + k1)] + θ(s)[s/(s + k1)] (Gl. 33)
Es ist zu beachten, daß bei einer niedrigen Frequenz (s = 0) die geschätzte Position sich der Positionsmessung bei niedriger Bandbreite θ₀, und bei einer hohen Frequenz der Positionsmessung bei hoher Bandbreite θ annähert. So werden die Daten für die hohe und die niedrige Bandbreite kombiniert.
Eine nützlichere Schätzung kann erzielt werden, wenn der Schätzer auf einem einfachen Integrator zweiter Ordnung basiert. In diesem Fall kann eine Rückkopplungsmatrix der Form H = [k₁ k₂]^T benutzt werden, um die Messung mit niedriger Bandbreite θ₀ über die Ausgangsmatrix C = [1 0] rückzukoppeln. Die Laplace-Transformation der resultierenden Schätzungen zu Position und Geschwindigkeit ist im Folgenden gezeigt: θe(s) = ω(s)[s/(s2 + k1s + k2)] + θ0(s)[(k1s + k2)/(s2 + k1s + k2)] (Gl. 34) ωe(s) = ω(s)[(s2 + k1s)/(s2 + k1s + k2)] + θ0(s)[(k2s/(s2 + k1s + k2)/(s2 + k1s + k2)] (Gl. 35)wobei ω(s) der gemessene Wert von dθ/dt in der s-Domäne ist. Also kombiniert diese Schätzertopologie die Position mit niedriger Bandbreite θ₀ mit der Geschwindigkeit hoher Bandbreite ω. Bei einer niedrigen Frequenz nähert sich die geschätzte Position der Messung mit niedriger Bandbreite θ₀ an, wie zuvor. Bei einer hohen Frequenz nähert sich die geschätzte Position dem Integral der Messung der Geschwindigkeit bei hoher Bandbreite ω an, was äquivalent ist zu der Position bei hoher Bandbreite θ. Ein ähnliches Ergebnis wird für die geschätzte Geschwindigkeit erzielt. So ist diese An von Schätzer gut zum Erzeugen einer Schätzung der Winkelposition und Winkelgeschwindigkeit von Neigungsdaten einer niedrigen Bandbreite und Winkelgeschwindigkeitsdaten einer hohen Bandbreite geeignet. Die Zuwächse k₁ und k₂ sind so ausgewählt, daß die geeignete Filterbandbreite und Stabilität erreicht werden. Es ist zu beachten, daß, indem k₂ = 0 und ω(s) = sθ(s) in Gl. 34, dieselbe Positionsschätzung erster Ordnung erzeugt wird, wie sie in Gl. 33 gezeigt ist.
Es ist bekannt, daß Quaternions benutzt werden können, um die Lage eines Systems über jede Winkelbewegung zu verfolgen. Darüber hinaus sind Quaternions eine rechnerisch effiziente Schreibweise, und sind außerdem immun gegenüber Singularitäten, die bei Euler-Winkeln auftreten. Es ist auch bekannt, daß Daten von Sensoren mit hoher und niedriger Bandbreite mit Hilfe eines Schätzers kombiniert werden können, um für die gesamte Bandbreite genaue Daten bereitzustellen. Allerdings eignen sich Quaternions nicht für direkte physikalische Messungen, und dies macht die Ableitung einer absoluten Quaternion-Referenz von Neigungssensordaten kompliziert. Ohne eine solche Referenz ist es für den Schätzer unmöglich, zu verhindern, daß Gyroskopdrift das integrierte Quaternion stört, was möglicherweise dazu führt, daß das System seine Winkelposition aus den Augen verliert.
Aus den vorangegangenen Erläuterungen wird man sich erinnern, daß übliche Quaternions zwei gedrehte Vektoren und zwei externe Referenzen benötigen, um eine Drehung eindeutig wiederzugeben. Allerdings ist allein mit Neigungssensoren nur eine einzige Referenz zum Erdmittelpunkt verfügbar. Die vorliegende Erfindung löst dieses Problem für neigungsbasierte Systeme, indem es eine modifizierte Quaternion-Schreibweise bereitstellt, wobei nur drei Elemente der vier üblichen Elemente Nicht-Null sind, und nur eine einzige Referenz benötigt wird, um Lage (Neigung) in Quaternion-Form eindeutig wiederzugeben. Diese Lösung geht davon aus, daß das System keine Kenntnis seines Kurses benötigt und deshalb keine Veränderungen im Yaw (Drehung um die vertikale Achse) benötigt.
Basierend auf der vorangegangenen Entwicklung erfordert das Berechnen der Quaternion-Koeffizienten von einem einzigen Bezugspunkt, daß der Referenzvektor in jedem System senkrecht zu der Drehachse ist. Für ein neigungsbasiertes System mit Schwerkraft als Referenz entspricht dies der Anforderung, daß die Drehachse stets horizontal bleibt. Wenn die Rotationsachse n stets horizontal ist, muß die z-Komponente der Achse identisch zu Null sein. Bezüglich der Definition der Einheits-Quaternion-Komponenten impliziert dies, daß die Komponente q₃ des Quaternions ebenfalls identisch zu Null sein muß. Also wird die Bedingung q₃ = 0 eine Beschränkung. Das resultierende Quaternion wurde auf diese Weise modifiziert und enthält nur drei signifikante Komponenten.
Die Beschränkung q₃ = 0 muß auch erfüllt werden, während die Quaternion-Geschwindigkeitsgleichung (Gl. 28 oder 29) integriert wird. Diese Bedingung kann während der Integration eingehalten werden, solange die Bedingung dq₃/dt = 0 ebenfalls eingehalten wird. Von Gl. 28 und 29 impliziert dies, daß eine der folgenden Bedingungen erfüllt sein muß: 0 = dq3/dt = –q2ω1 + q1ω2 + q0ω3 (Zur Integration im Körpersystem) (Gl. 36) 0 = dq3/dt = –q2ωE1 – q1ωE2 + q0ωE3 (Zur Integration im Erde-System) (Gl. 37)
Um eine dieser Beschränkungen zu erfüllen, muß es wenigstens einem Parameter erlaubt sein, nach Bedarf beliebig zu variieren. Im Körpersystem enthalten die drei Winkelgeschwindigkeitskomponenten und die drei modifizierten Quaternion-Komponenten allesamt notwendige Information. Allerdings wurde bei dem Erde-System angenommen, daß die Yaw-Komponente der Winkelgeschwindigkeit unwichtig ist, da das System neigungsbasiert ist. Also kann die wahre Yaw-Komponente durch eine virtuelle Yaw-Komponente ersetzt werden, die die genannte Beschränkung erfüllt. In der Praxis kann dies geschehen, indem die gemessene Winkelgeschwindigkeit von dem Körpersystem in das Erde-System transformiert wird. Der resultierende Erde-Yaw kann dann durch einen virtuellen Erde-Yaw ersetzt werden, der die folgende Beschränkungsgleichung erfüllt: ω'E3 = (q1ωE2 – q2ωE1)/q0 (Virtueller Erde-Yaw, der dq3/dt = 0 erfüllt) (Gl. 38)
Diese Erde-Yaw-Komponente kann zurück gegen die ursprüngliche Quaternion-Geschwindigkeitsgleichung (Gl. 29) ausgetauscht werden, um ihre explizite Abhängigkeit von Yaw zu eliminieren. Um das modifizierte Quaternion mit drei Elementen von anderen Quaternions zu unterscheiden, wird das modifizierte Quaternion im Folgenden mit „e" anstelle von „q" bezeichnet. Die Komponenten können dann wie folgt geschrieben werden: e0 = cos(θ/2) und e = [e1 e2]T = nsin(θ/2) (Gl. 39)wobei n = [e₁ e₂]^T und e₃ = n₃ = 0
Nach dem Eliminieren von ω_E3 und e₃ wird die modifizierte Quaternion-Geschwindigkeitsgleichung zu:
Die Rotationsmatrix R_EB (Gl. 32) kann ebenfalls vereinfacht werden, indem ω_E3 und e₃ wie folgt eliminiert werden: REB = [I – 2ffT 2fe0] wobei f = [e2 – e1]T (Gl. 41)
Durch Kombinieren dieser Ergebnisse unter Beachtung von ω_E = R_EBω kann die modifizierte, auf das Erde-System bezogene Quaternion-Geschwindigkeit wie folgt als eine Funktion der Winkelgeschwindigkeit in dem Körpersystem notiert werden.
Dies kann kürzer wie folgt in Matrixform ausgedrückt werden: de/dt = Eω/2 (Gl. 43)
Unter Benutzung dieses Ergebnisses können die modifizierten Quaternion-Geschwindigkeiten integriert werden, um das modifizierte Quaternion zu erzeugen. Allerdings nimmt diese Gleichung einen idealen Winkelgeschwindigkeitsvektor ω an, und stellt keine Kompensation für die Drift in den tatsächlichen Daten zur Winkelgeschwindigkeit von den Gyroskopen bereit. Die korrigierte Winkelgeschwindigkeit der Form ω' = ω – ω_drift wird dann anstelle von ω in der oben aufgeführten Gleichung benutzt. Die Situation wird außerdem dadurch verkompliziert, daß die Gyroskopdrift eine Funktion der Zeit ist. Aus diesem Grund muß der Driftkorrekturausdruck stets aktualisiert werden. Dies macht die Benutzung der zuvor beschriebenen Schätzer-Topologie zweiter Ordnung erforderlich.
Der Zweck des Schätzers ist es, das Quaternion, das durch Integration der Quaternion-Geschwindigkeit ermittelt wurde, mit dem Quaternion zu vergleichen, das von den Neigungsdaten ermittelt wurde. Ausgehend von der Annahme, daß jeder Dauerzustandsfehler ausschließlich auf Gyroskopdrift zurückgeht, kann die Größe dieses Fehlers benutzt werden, um den Gyroskopdrift-Korrekturausdruck einzurichten. Die Schätzerzuwächse werden so ausgewählt, daß der Dauerzustandsfehler im Laufe der Zeit auf Null geführt wird. Die Auswahl der Schätzerzuwachswerte liegt im Bereich der Fähigkeiten von Fachleuten und muß deshalb nicht weiter erläutert werden.
Um den Korrekturausdruck richtig einzurichten, muß der Quaternion-Fehler zurück in einen Winkelgeschwindigkeitsfehler umgewandelt werden. Dies wird unter Benutzung der ursprünglichen Quaternion-Geschwindigkeitsgleichung erreicht, die für die Winkelgeschwindigkeit in dem Erde-System wie folgt gelöst wurde: ωE = 2QT(dq/dt) (Gl. 31, wiederholt)
Die Integration dieses Ausdrucks hinsichtlich der Zeit ergibt folgendes Resultat: ΔθE = 2QT(Δq) = 2QT(q' – q) (Gl. 44)
Also kann die oben genannte Gleichung benutzt werden, um den Fehler zwischen dem geschätzten Quaternion und dem Neigungsquaternion (δq = q' – q) in einen Neigungswinkelfehler Δθ_E umzuwandeln. Man wird sich erinnern, daß in diesem Fall das modifizierte Quaternion benutzt wird, wobei q₃ = 0 sowohl für q als auch für Q gilt. Allerdings ist die auf diese Weise ermittelte Erde-Yaw-Komponente nicht gültig, da das Quaternion mit Hilfe des virtuellen Erde-Yaw erzeugt wurde. Um einen gültigen Winkelfehler zu erzeugen, muß der ursprüngliche Erde-Yaw anstelle der Fehlerkomponente Δθ_E3 eingesetzt werden. Der Ausdruck für den Erde-Yaw kann abgeleitet werden von ω_E = R_EBω mit q₃ = 0 und ist im Folgenden angegeben: ωE3 = (–2q2q0)ω1 + (2q1Q0)ω2 + (q0 2 – q1 2 –q2 2)ω3 (Gl. 45)
Nach der Ersetzung ergibt sich der resultierende Winkelfehlervektor im Erde-System durch Δθ_E' = [Δθ_E1 Δθ_E2 ω_E3]^T. Da die Gyroskope sich in dem Körpersystem befinden, muß der Winkelfehler mit Hilfe von Δθ = R_BEΔθ_E' in den Körperfehler transformiert werden. Der resultierende Winkelfehler kann dann von dem Schätzer benutzt werden, um den Gyroskopdrift-Korrekturausdruck zu erzeugen.
Das Verfahren, mit dem die Neigungsdaten in ein modifiziertes Quaternion umgewandelt werden, soll im Folgenden erläutert werden. Es wurde gezeigt, daß die Quaternion-Komponenten erzeugt werden können, indem die Skalar- und Kreuzprodukte von zwei Einheitsvektoren erzeugt werden, die senkrecht zu der Rotationsachse sind und durch einen Winkel von θ/2 separiert sind. Zwei solche Vektoren können unter Benutzung des externen Bezugspunktes am Erdmittelpunkt (d.h. der Richtung der Schwerkraft, wie sie von dem Neigungssensor gemessen wird) gebildet werden. Der Anfangsschwerkraftvektor soll dabei als G_i bezeichnet werden, und der finale Schwerkraftvektor als G_f. (Die Daten, die dem Anfangsschwerkraftvektor G_i entsprechen, können während eines Startvorgangs ermittelt werden und zur weiteren Benutzung gespeichert werden. Der „finale" Schwerkraftvektor G_f wird aktualisiert, während der Schätzer arbeitet.) Diese zwei Vektoren sollen wie folgt normiert werden: gi = Gi/|Gi| und gf = Gf/|Gf| (Gl. 46)
Unter Benutzung dieser normierten Schwerkraftvektoren können die gewünschten Einheitsvektoren wie folgt gebildet werden: a = (gi + gf)/|gi + gf| und b = gi (Gl. 47)
Schließlich können die Quaternion-Komponenten durch Berechnen der Skalar- und Kreuzprodukte wie folgt berechnet werden: e0 = a·b und e = a × b (Gl. 48)
Durch diese Konstruktion erzeugt das Kreuzprodukt von a und b einen Vektor, der senkrecht zu dem Schwerkraftvektor in jedem System ist. Dieser Vektor muß also horizontal sein. Dies impliziert, daß die z-Komponente des Quaternions wie gewünscht identisch zu Null ist. Also sind nur die ersten drei Elemente des Quaternions nicht-trivial und erfüllen die Form des modifizierten Quaternions.
Das oben beschriebene Schätzungsverfahren insgesamt ist in dem Blockdiagramm von 4 dargestellt.
Die Schätzertopologie ist in zwei Schleifen unterteilt: die k₁-Schleife und die k₂-Schleife. Die k₁-Schleife stellt die Filterung der Daten von dem Neigungssensor bei niedriger Bandbreite bereit, und bestimmt die Fehlergröße, die benutzt wird, um die geschätzte Quaternion-Geschwindigkeit zu schätzen. Die k₂-Schleife stellt die Driftkorrektur der Gyroskopdaten bereit. Da die Gyroskope im Körpersystem angeordnet sind, muß die k₂-Schleife auf der Körperseite der Koordinatentransformation liegen. Der k₂-Zuwachs bestimmt die Fehlergröße, die benutzt wird, um die Drift der Gyroskopdaten zu korrigieren. Der gewichtete Fehler wird dann integriert, so daß die Schleife im Laufe der Zeit einen Dauerzustandfehler von Null erreicht. Mit anderen Worten, wenn der Quaternion-Fehler Null erreicht, hört der Ausgang des k₂-Integrator auf, sich zu verändern, und der Driftkorrekturausdruck bleibt konstant.
In 4 wendet ein Summierungsblock 50 in der k₂-Schleife ein gewichtetes Fehlersignal (das auch als ein Driftkorrektursignal betrachtet werden kann) auf die Winkelgeschwindigkeitsinformation an, die von den Gyroskopen 16 ausgegeben wird, um eine korrigierte Winkelgeschwindigkeitsinformation zu erzeugen. Die korrigierte Geschwindigkeitsinformation wird in das Erde-Bezugssystem umgewandelt und an Block 52 in ein modifiziertes Quaternion transformiert. Wie durch die Markierung E/2 in Block 52 angezeigt, entsprechen die in diesem Block angewandten Berechnungen den Gleichungen 43 und 42, die oben erläutert wurden. Die resultierende geschätzte Geschwindigkeitsinformation, die in der oben beschriebenen modifizierten Quaternion-Form vorliegt, wird als eine Eingabe an einen Summierungsblock 54 bereitgestellt. An Block 54 wird eine Korrektur, die von der k₁-Schleife bereitgestellt wird, auf die geschätzte Quaternion-Geschwindigkeitsinformation angewandt, um Daten zu erzeugen, die die Veränderung der geschätzten Position anzeigen. Diese Daten (die als ein geschätztes Positionsdifferenzsignal bezeichnet werden können), werden wiederum an Block 56 integriert, um die geschätzten Positionsdaten des modifizierten Quaternions bereitzustellen. An Block 58 werden die geschätzten Positionsdaten des modifizierten Quaternions, die von Block 56 ausgegeben wurden, und auch die geschätzte Geschwindigkeitsinformation des modifizierten Quaternions, die von Block 52 ausgegeben wurde, in Euler-Winkel umgewandelt, die an den Steuersignalerzeugungsblock 44 (3) ausgegeben werden. Diese Umwandlung kann mit Hilfe der folgenden Gleichungen leicht erreicht werden. (Obwohl diese Gleichungen trigonometrische Funktionen benutzen, ist die rechnerische Komplexität minimal und kann mit Hilfe von Suchtabellen leicht durchgeführt werden.) θ = asin(2e2e0) und φ = asin(2e1e0/cosθ) (Gl. 49)
Diese Gleichungen können auch mit Hilfe von kleinwinkligen Annäherungen vereinfacht werden, für den Fall, daß die Bewegungsspanne des neigungsbasierten Systems begrenzt ist.
Wenn der Steuersignalerzeuger von dem Typ ist, der Quaternion-Signale anstelle von Euler-Signalen ausgibt, kann Block 58 ausgelassen werden, und die geschätzte Quaternion-Position und -Geschwindigkeitsinformation (in der modifizierten Form, die gemäß der Erfindung bereitgestellt wird) kann direkt an den Steuersignalgenerator bereitgestellt werden.
Auf jeden Fall wird die geschätzte Positionsinformation in der Form eines modifizierten Quaternions als eine Eingabe an einen Summierungsblock 60 bereitgestellt, wo sie mit der gegenwärtigen Neigungsinformation verglichen wird, die von den Neigungssensoren 14 ausgegeben wird, wie sie in Block 62 in die modifizierte Quaternion-Form umgewandelt wurde (Das am Block 62 erzeugte Signal kann als ein modifiziertes Quaternion-Referenz-Positionssignal bezeichnet werden). Die an Block 62 durchgeführte Umwandlung entspricht dem Prozeß, wie er oben im Zusammenhang mit Gl. 46 bis 48 beschrieben wurde. Das resultierende Fehlersignal, das von dem Summierungsblock 60 ausgegeben wird, wird an Block 64 mit dem Zuwachsfaktor k₁ gewichtet, und wird dann als das oben genannte Korrektursignal an Block 54 angewandt. Der Ausgang von Block 60 (der als ein Fehlerpositionssignal betrachtet werden kann) wird ebenfalls als ein Eingang an Block 66 bereitgestellt. Gemäß Gl. 44, wandelt Block 66 den Fehler zwischen dem geschätzten Quaternion und dem Neigungsquaternion in einen Neigungswinkelfehler um. Die ungültige Yaw-Komponente des resultierenden Signalausgangs von Block 66 wird durch die Erde-System-Yaw-Komponente ersetzt, die durch die Verarbeitung an Block 52 verfügbar ist, und der resultierende Winkelfehlervektor wird an Block 68 in das Körperbezugssystem umgewandelt. Der Ausgang von Block 68 wird dann durch den k₂-Zuwachs an Block 70 integriert und gewichtet, und das resultierende Signal wird an Block 50 als das oben erwähnte Driftkorrektursignal angewandt.
Es ist zu beachten, daß die Koeffizienten, die für die Prozeße in Block 52, 66 und 68 benötigt werden, aus der Positionsinformation des modifizierten Quaternions stammen, die von dem Integrationsblock 56 ausgegeben wird, wie unter 72 in 4 gezeigt.
Wie Fachleute verstehen werden, können die Prozeße, die in Blockdiagrammform in 4 dargestellt sind, vorteilhaft von einer oder mehreren in geeigneter Weise programmierten Rechnervorrichtungen (z.B. einem oder mehreren MikroProzeßoren) durchgeführt werden, die einen Teil des Steuerungssystems 12 bilden.
Bei dem Lage- und Drehungsschätzer, der gemäß der Erfindung bereitgestellt wird, wird hohe Bandbreiteninformation, die von den Rotationssensoren bereitgestellt wird, und niedrige Bandbreiteninformation, die von den Neigungssensoren bereitgestellt wird, in einer neuartigen modifizierten Quaternion-Schreibweise kombiniert, die drei Elemente anstelle der üblichen vier Elemente aufweist. Das resultierende geschätzte Quaternion ist für die ausgelegte Bandbreite gültig und driftet nicht im Laufe der Zeit. Die durchzuführenden Berechnungen können effizient behandelt werden, indem einfache Arithmetik wie Multiplikation, Addition und Wurzelbildung benutzt wird. Der modifizierte Quaternion-Schätzer ist auf eine breite Spanne von Systemen anwendbar, bei denen Neigung verfolgt und/oder gesteuert werden soll.
Die obige Beschreibung der Erfindung dient erläuternden Zwecken und ist nicht beschränkend. Fachleute können zu verschiedenen Änderungen und Modifikationen der beschriebenen Ausführungsformen gelangen, und diese können ohne Abweichung von dem Umfang der Erfindung erfolgen, wie er in den Ansprüchen definiert ist.

Claims

Verfahren zur Abschätzung der Lage eines neigbaren Körpers (10), wobei der Körper ein Neigungsabtastmittel (14) und ein Winkelgeschwindigkeitsabtastmittel (16) umfaßt, wobei das Verfahren die Schritte aufweist: Ausgeben einer Winkelgeschwindigkeitsinformation vom Winkelgeschwindigkeitsabtastmittel (16) und Ausgeben einer Neigungsinformation vom Neigungsabtastmittel (14), gekennzeichnet durch Transformieren (52) und Integrieren (56) der ausgegebenen Winkelgeschwindigkeitsinformation zur Erzeugung einer ersten Quaternion-Positionsinformation in der Weise, daß die erste Quaternion-Positionsinformation darauf beschränkt ist, eine Drehung um eine horizontale Achse in einem Erde-Bezugssystem wiederzugeben; Verarbeiten (62) der ausgegebenen Neigungsinformation zur Erzeugung einer zweiten Quaternion-Positionsinformation in der Weise, daß die zweite Quaternion-Positionsinformation darauf beschränkt ist, eine Drehung um eine horizontale Achse im Erde-Bezugssystem wiederzugeben; Vergleichen (66) der ersten Quaternion-Positionsinformation mit der zweiten Quaternion-Positionsinformation zur Erzeugung (68) einer Fehlerinformation; und Verwenden (50) der Fehlerinformation zur Kompensation einer Drift im Winkelgeschwindigkeitsabtastmittel (16).
Verfahren zur Abschätzung der Lage eines neigbaren Körpers (10), wobei der Körper ein Neigungsabtastmittel (14) und ein Winkelgeschwindigkeitsabtastmittel (16) umfaßt, wobei das Verfahren die Schritte aufweist: Ausgeben einer Winkelgeschwindigkeitsinformation vom Winkelgeschwindigkeitsabtastmittel (16) und Ausgeben einer Neigungsinformation vom Neigungsabtastmittel (14), gekennzeichnet durch Erzeugen (52) einer modifizierten Quaternion-Geschwindigkeitsinformation aus der ausgegebenen Winkelgeschwindigkeitsinformation, wobei die modifizierte Quaternion-Geschwindigkeitsinformation in der Form einer skalaren Größe und eines zweikomponentigen Vektors vorliegt; Integrieren (56) der modifizierten Quaternion-Geschwindigkeitsinformation zur Erzeugung einer ersten modifizierten Quaternion-Lageinformation, wobei der Schritt des Erzeugens (52) der modifizierten Quaternion-Geschwindigkeitsinformation darauf beschränkt ist, daß die erste modifizierte Quaternion-Lageinformation die Form e = e₀ + e₁i + e₂j aufweist, wobei i, j imaginäre Einheitsvektoren sind, welche miteinander und mit einem dritten imaginären Einheitsvektor k die folgenden Gleichungen erfüllen: i2 = j2 = k2 = ijk = –1, ij = –ji = k, jk = –kj = i, ki = –ik = j;Erzeugen (62) einer zweiten modifizierten Quaternion-Positionsinformation aus der ausgegebenen Neigungsinformation, wobei die zweite modifizierte Quaternion-Positionsinformation die Form e = e₀ + e₁i + e₂j aufweist; Vergleichen (66) der ersten modifizierten Quaternion-Positionsinformation mit der zweiten modifizierten Quaternion-Positionsinformation zur Erzeugung einer modifizierten Quaternion-Positionsfehlerinformation; Transformieren (70) der modifizierten Quaternion-Positionsfehlerinformation in ein Winkelgeschwindigkeitsfehlersignal; und Verwenden (50) des Winkelgeschwindigkeitsfehlersignals zum Kompensieren eines Fehlers in der ausgegebenen Winkelgeschwindigkeitsinformation.
Verfahren nach Anspruch 2, das des weiteren den Schritt eines Tiefpaßfilterns der zweiten modifizierten Quaternion-Positionsinformation umfaßt.
Verfahren nach Anspruch 2, wobei: die erste Winkelgeschwindigkeitsinformation in einem ersten Bezugssystem bereitgestellt wird und die Neigungsinformation in einen zweiten Bezugssystem erzeugt wird, das sich vom ersten Bezugssystem unterscheidet; der Schritt des Erzeugens (52) der modifizierten Quaternion-Geschwindigkeitsinformation ein Umwandeln (68) der ausgegebenen Winkelgeschwindigkeitsinformation vom ersten Bezugssystem in das zweite Bezugssystem umfaßt; und das Verfahren des weiteren den Schritt eines Umwandelns (70) des Winkelgeschwindigkeitsfehlersignals vom zweiten Bezugssystem in das erste Bezugssystem vor dem Schritt des Verwendens (50) umfaßt.
Verfahren nach Anspruch 2, wobei die zweite modifizierte Quaternion-Positionsinformation auf der Basis einer ersten Neigungsinformation, die vom Neigungsabtastmittel (14) bei einem ersten Ereignis ausgegeben und gespeichert wird, und einer zweiten Neigungsinformation erzeugt wird, die vom Neigungsabtastmittel (14) bei einem zweiten Ereignis ausgegeben wird, das sich vom ersten Ereignis unterscheidet.
Verfahren zur Abschätzung der Lage eines neigbaren Körpers (10), umfassend die Schritte: Erzeugen einer Winkelgeschwindigkeitsinformation in einem Körpersystem; Anwenden (56) eines Driftkorrektursignals auf die Winkelgeschwindigkeitsinformation zur Erzeugung einer korrigierten Winkelgeschwindigkeitsinformation; gekennzeichnet durch Verarbeiten (52) der korrigierten Winkelgeschwindigkeitsinformation unter Verwendung einer modifizierten abgeschätzten Quaternion-Positionsinformation zur Erzeugung einer modifizierten abgeschätzten Quaternion-Geschwindigkeitsinformation mit Bezugnahme auf ein Erdsystem, wobei die modifizierte abgeschätzte Quaternion-Geschwindigkeitsinformation aus einer skalaren Komponente und zwei Vektorkomponenten besteht; Einrichten (54) der modifizierten abgeschätzten Quaternion-Geschwindigkeitsinformation auf der Basis eines gewichteten Positionsfehlersignals zur Erzeugung eines abgeschätzten Positionsdifferenzsignals; Integrieren (56) des abgeschätzten Positionsdifferenzsignals zur Erzeugung der modifizierten abgeschätzten Quaternion-Positionsinformation, wobei die modifizierte abgeschätzte Quaternion-Positionsinformation aus einer skalaren Komponente und zwei Vektorkomponenten besteht; Erzeugung einer Neigungsinformation im Erdsystem; Verwenden der Neigungsinformation zur Erzeugung (62) einer modifizierten Quaternion-Referenzpositionsinformation, wobei die modifizierte Quaternion- Referenzpositionsinformation aus einer skalaren Komponente und zwei Vektorkomponenten besteht; Subtrahieren der modifizierten Quaternion-Referenzpositionsinformation von der modifizierten abgeschätzten Quaternion-Positionsinformation zur Erzeugung eines Fehlerpositionssignals; Gewichten (64) des Fehlerpositionssignals zur Erzeugung des gewichteten Positionsfehlersignals; Umwandeln (66) des Fehlerpositionssignals in ein Neigungswinkelfehlersignal unter Verwendung der modifizierten abgeschätzten Quaternion-Positionsinformation; Bilden eines Winkelfehlervektors aus Pitch- und Roll-Komponenten des Neigungswinkelsignals und aus einer Yaw-Komponente eines Winkelgeschwindigkeitssignals durch Umwandeln der korrigierten Winkelgeschwindigkeitsinformation in das Erdsystem unter Verwendung der modifizierten abgeschätzten Quaternion-Positionsinformation; Umwandeln (68) des Winkelfehlervektors aus dem Erdsystem in das Körpersystem unter Verwendung der modifizierten abgeschätzten Quaternion-Positionsinformation; und Anwenden (50) einer gewichteten Integration auf den umgewandelten Winkelfehlervektor zur Erzeugung des Driftkorrektursignals.
Verfahren nach Anspruch 6, das des weiteren die Schritte umfaßt: Umwandeln (58) der modifizierten abgeschätzten Quaternion-Geschwindigkeitsinformation und der modifizierten abgeschätzten Quaternion-Positionsinformation zum Erzeugen einer Euler-Winkelausgangsinformation; und Ausgeben der Euler-Winkelausgangsinformation an ein Steuerungssignal-Erzeugungsmittel (44).
Vorrichtung zur Abschätzung der Lage eines neigbaren Körpers (10), umfassend: ein Neigungsabtastmittel (14) zur Ausgabe einer Neigungsinformation, das am Körper montiert ist; und ein Winkelgeschwindigkeitsabtastmittel (16) zur Ausgabe einer Winkelgeschwindigkeitsinformation, das am Körper montiert ist; gekennzeichnet durch ein Mittel zur Erzeugung einer modifizierten Quaternion-Geschwindigkeitsinformation aus der Winkelgeschwindigkeitsinformation, die vom Winkelgeschwindigkeitsabtastmittel (16) ausgegeben wird, wobei die modifizierte Quaternion-Geschwindigkeitsinformation in der Form einer skalaren Größe und eines zweikomponentigen Vektors besteht; Mittel zur Integration der modifizierten Quaternion-Geschwindigkeitsinformation zur Erzeugung der ersten modifizierten Quaternion-Lageinformation, wobei die erste modifizierte Quaternion-Lageinformation die Form e = e₀ + e₁i + e₂j aufweist, wobei i, j imaginäre Einheitsvektoren sind, die miteinander und mit einem dritten imaginären Einheitsvektor k die folgenden Gleichungen erfüllen: i2 = j2 = k2 = ijk = –1, ij = –ji = k, jk = –kj = i, ki = –ik = j;ein Mittel zur Erzeugung einer zweiten modifizierten Quaternion-Positionsinformation aus der vom Neigungsabtastmittel (14) ausgegebenen Neigungsinformation, wobei die zweite modifizierte Quaternion-Positionsinformation die Form e = e₀ + e₁i + e₂j aufweist; ein Mittel zum Vergleichen der ersten modifizierten Quaternion-Positionsinformation mit der zweiten modifizierten Quaternion-Positionsinformation zur Erzeugung einer modifizierten Quaternion-Positionsfehlerinformation; ein Mittel zum Umwandeln der modifizierten Quaternion-Positionsfehlerinformation in ein Winkelgeschwindigkeitsfehlersignal; und ein Mittel zur Kompensation eines Fehlers in der vom Winkelgeschwindigkeitsabtastmittel ausgegebenen Winkelgeschwindigkeitsinformation auf der Basis des Winkelgeschwindigkeitsfehlersignals.
Vorrichtung nach Anspruch 8, wobei das Neigungsabtastmittel (14) zumindest einen fluidischen Neigungssensor umfaßt.
Vorrichtung nach Anspruch 9, wobei das Winkelgeschwindigkeitsabtastmittel (16) zumindest ein Gyroskop umfaßt.
Vorrichtung nach Anspruch 8, die des weiteren ein Mittel zur Umwandlung der modifizierten Quaternion-Geschwindigkeitsinformation in eine Euler-Winkelgeschwindigkeitsinformation und zur Umwandlung der ersten modifizierten Quaternion-Lageinformation in eine Euler-Winkelpositionsinformation umfaßt.