Der
vorliegenden Erfindung liegt aber nicht nur die Aufgabe zugrunde,
eine Vorrichtung zu schaffen, die für den vorstehend genannten
Zweck verwendbar ist und sicherstellt, dass stets korrekte Informationen
vermittelt werden, sondern es soll eine Vorrichtung geschaffen werden,
die dem Benutzer als Montageanleitung, als Bedienungsanleitung oder
als Handbuch für
die Durchführung
von Wartungsmaßnahmen,
vorzugsweise bei komplexen technischen Geräten und Anlagen, dienen kann.
Es stellt sich häufig
beim Studium komplexer technischer Anleitungen das Problem, dass
nicht erkennbar ist, auf welchen Anlagenteil oder auf welchen technischen
Gegenstand sich eine Anweisung bezieht. Gegenwärtig werden zeichnerische Darstellungen
mit Bezugszeichen beigefügt
oder auf solche Zeichnungen Bezug genommen, die aber oftmals nicht
gleichzeitig einsehbar sind oder sich auf mehreren Seiten befinden.
Gerade bei größeren Anlagen,
bei denen das Bedienungspersonal sich vor Ort bewegt, ist es oftmals
schwer möglich,
technische Zeichnungen zur Hand zu nehmen oder einsehbar auszubreiten.
Es ist eine Aufgabe der vorliegenden Erfindung, hier Abhilfe zu
schaffen.
Die
vorstehende Aufgabe wird erfindungsgemäß gelöst durch eine Vorrichtung zur
Vermittlung von akustischen und/oder visuell wahrnehmbaren Informationen
an einen Benutzer, wobei die Vorrichtung beim bestimmungsgemäßen Gebrauch
durch den Benutzer mitgeführt
wird, ohne dass sein Gesichtsfeld hierdurch eingeschränkt wird,
wobei die an den Benutzer zu vermittelnden Informationen vom Ort
und von der Orientierung der durch ihn mitgeführten Vorrichtung relativ zu
einem referenzierenden Gegenstand oder Referenzpunkt abhängig sind,
mit einem Speichermittel, auf dem die Informationen mit ihrer Ortsabhängigkeit
und Orientierungsabhängigkeit
gespeichert sind, und mit einem eine dreidimensionale Inertialsensorik
aufweisenden Sensormittel, welches Beschleunigungssensoren und Drehratensensoren
umfasst, und mit Rechenmitteln zur Umwandlung der Beschleunigungsmesswerte
und Messwerte der Drehratensensoren in Positionsdaten und Orientierungsdaten,
mit Mitteln zur positionsabhängigen
und orientierungsabhängigen
Auswahl der gespeicherten Informationen, mit dem Ziel, diese zu
vermitteln und hierfür
auf einem Display anzuzeigen oder akustisch zu vermitteln.
Die
Vorrichtung ist vorteilhafterweise als portabler Kleincomputer ausgebildet
oder in einen portablen Kleincomputer, wie einen Palm, Laptop, Notebook,
Handy integriert. Hierfür
weist die erfindungsgemäße Vorrichtung vorteilhafterweise
Tragemittel auf, die es gestatten, die Vorrichtung bequem am Körper des
Benutzers zu tragen. Im einfachsten Fall kann es sich hierbei um
einen Gurt, der um die Hüfte,
um die Schulter oder um den Hals des Benutzers getragen wird, handeln,
der mit der Vorrichtung insbesondere lösbar verbunden ist. Es könnten aber
auch taschenartige Aufnahmemittel für die Vorrichtung in vorteilhafter
Weise in Frage kommen, die ihrerseits an einem Gürtel oder einem Band im weitesten
Sinn am Körper
des Benutzers gehalten sein können.
Die
an den Benutzer zu vermittelnden Informationen können beispielsweise eine komplette
Montageanleitung oder ein Handbuch zur Durchführung von Wartungsmaßnahmen
umfassen oder einfach eine Bedienungsanleitung für ein technisches Gerät, beispielsweise
eine komplexe Industrieanlage oder einen Videorecorder. Erfindungsgemäß sind die
an den Benutzer zu vermittelnden Informationen mit einer Ortsabhängigkeit
und vorzugsweise auch mit einer Orientierungsabhängigkeit in Bezug auf einen
referenzierenden Gegenstand oder einen Referenzpunkt gespeichert.
Bei diesem referenzierenden Gegenstand handelt es sich beispielsweise
um denjenigen Gegenstand, auf den sich die Informationen, insbesondere
die Montageinformationen, die Bedieninformationen oder die Wartungsinformationen
beziehen. Ausgehend von einer Startorientierung, wenn der Benutzer
mit der mitgeführten
Vorrichtung auf einer bestimmten Seite des referenzierenden Gegenstands,
vorzugsweise mit einer bestimmten Orientierung auf den referenzierenden
Gegenstand steht, werden Bewegungen, also Lageveränderungen
des Benutzers und der durch ihn mitgeführten erfindungsgemäßen Vorrichtung
relativ zu dem referenzierenden Gegenstand durch Erfassung und Auswertung
der Messwerte der Beschleunigungssensoren und Drehratensensoren
erfasst. Hieraus werden Positionsdaten und vorzugsweise auch Orientierungsdaten
ermittelt. In Abhängigkeit
dieser Daten werden dann automatisch diejenigen Informationen aus
dem Speichermittel ausgelesen und visuell oder akustisch vermittelt,
welche der Position und vorzugsweise auch der Orientierung des Benutzers
und der erfindungsgemäßen Vorrichtung
in Bezug auf den referenzierenden Gegenstand entsprechen.
Beispielsweise
kann ausgehend von einer referenzierenden Startorientierung dem
Benutzer die Information vermittelt werden, dass er sich auf die
ihm abgewandte Seite des Geräts
begeben möge.
Folgt der Benutzer dieser Aufforderung, so erhält er bei Erreichen der betreffenden
Position automatisch eine weitere Information, die sich auf einen
ihm nunmehr zugänglichen
Teil der Anlage oder des technischen Gegenstands beziehen mag.
In
Weiterbildung der Erfindung kann es sich als vorteilhaft erweisen,
wenn die Vorrichtung sowohl Mittel für die akustische Vermittlung
der ortsabhängigen
Informationen als auch ein Display zur visuellen Vermittlung von
Informationen aufweist. Beispielsweise kann auf dem Display ein
Bild desjenigen Anlagenteils, auf das sich die Information bezieht, oder
eine Detaildarstellung angezeigt werden, oder es werden die akustische Vermittlung
ergänzende
oder erläuternde
visuelle Informationen angezeigt.
Die
Sensormittel weist bevorzugtermaßen drei Beschleunigungssensoren
auf, deren Signale zur Berechnung einer translatorischen Bewegung
verwendbar sind, und zusätzlich
drei Drehratensensoren, deren Messwerte zum Bestimmen der Orientierung
im Raum verwendbar sind.
Zusätzlich zu
den Drehratensensoren können
in vorteilhafter Weise Magnetfeldsensoren zur Ermittlung der Orientierung
des Gegenstands im Raum vorgesehen sein. Die Magnetfeldsensoren
erfassen dabei Komponenten des Erdmagnetfelds und vermögen ihrerseits
Informationen über
die Orientierung der Inertialsensorik im Raum zu geben.
Solchenfalls
erweist es sich als vorteilhaft, wenn Mittel zum Vergleichen der
aus Messwerten der Magnetfeldsensoren ermittelten Orientierung im
Raum mit der aus Messwerten der Drehratensensoren ermittelten Orientierung
im Raum vorgesehen sind. Zusätzlich
kann ein Beschleunigungssensor für
die Messung der Erdbeschleunigung vorgesehen sein, welcher ebenfalls
zur Ermittlung der Orientierung der betrachteten Sensoren im Raum
verwendet werden kann.
Es
erweist sich des Weiteren als besonders vorteilhaft, wenn die Rechenmittel
der erfindungsgemäßen Vorrichtung
Mittel zur Ausführung
eines an sich aus
DE
103 12 154 A1 bekannten Quaternionen-Algorithmus aufweisen.
Des
Weiteren erweist es sich als vorteilhaft, wenn die Rechenmittel
Mittel zur Anwendung einer vor Beginn der Positionierung ermittelten
und gespeicherten Ausgleichsmatrix aufweisen, welche Ausgleichsmatrix
einer Abweichung der Orientierung der Achsen der drei Drehratensensoren
von einer angenommenen Orientierung der Achsen zueinander Rechnung
trägt und
durch ihre Anwendung einen bei der Berechnung der Drehwinkel resultierenden
Fehler kompensiert.
Ferner
erweist es sich als vorteilhaft, wenn die Rechenmittel Mittel zur
Ausführung
eines Kalman-Filter-Algorithmus aufweisen.
Da
Inertialsensoren auf Beschleunigungsvorgänge zurückgehende Messwerte liefern,
werden diese Messwerte zur Bestimmung von Positionsdaten im Falle
von Beschleunigungssensoren zweifach integriert und im Falle von
Drehratensensoren einfach integriert. Im Zuge dieser Integrationsvorgänge werden
Fehler nach und nach durch Integration aufaddiert. Insofern erweist
es sich als vorteilhaft, dass das Verfahren und die hierfür verwandte
Vorrichtung so ausgebildet ist, dass vor Beginn einer Datennahme
und dann immer wieder, sobald ein Ruhen der Vorrichtung für eine bestimmte
Zeitdauer festgestellt wird, ein Offset-Wert des Ausgangssignals
der Drehratensensoren ermittelt wird und nachfolgend bis zur nächstfolgenden Ermittlung
dieser Offset-Wert für
die jeweiligen Drehratensensoren in Abzug gebracht wird, so dass
er nicht in die Integration eingeht. Auf diese Weise wird sichergestellt,
dass immer wieder ein neuer aktueller Offset-Wert ermittelt wird, so
dass größtmögliche Genauigkeit
erreicht wird.
Es
wurde auch festgestellt, dass die konstruktionsbedingt nicht zu
vermeidende Abweichung der Orientierung der Achsen der drei Drehratensensoren
von einer angenommenen Orientierung sehr rasch bei der Berechnung
der Drehwinkel zu Ungenauigkeiten führt. Dadurch, dass dieser Fehler
durch Anwendung der für die
betreffende Sensoreinrichtung zu bestimmenden Ausgleichsmatrix kompensiert
wird, lässt
sich eine den Anforderungen genügende
Steigerung der Genauigkeit erreichen. Zur Ermittlung der Ausgleichsmatrix
wird vor Beginn der Objektverfolgung eine hierfür verwandte Sensoreinrichtung
in eine rotierende Bewegung um eine jeweilige Achse gebracht, und
zwar unter Stillsetzung der beiden anderen Achsen. Anhand der hierbei
erhaltenen Drehratensensorsignale wird die Ausgleichsmatrix errechnet
und in einem Speicher der beanspruchten Vorrichtung hinterlegt.
Für den
rotatorischen Antrieb der Sensorvorrichtung kann ein Industrieroboter
verwendet werden. Auf diese Weise kann die 3 × 3-Nichtorthogonalitätsmatrix durch Drehung um die
einzelnen Raumachsen nacheinander aufgenommen werden.
Die
Nebendiagonalelemente der Nichtorthogonalitätsmatrix in Gleichung wären bei
einem idealen System 0. Die Ungenauigkeiten beruhen auf Fertigungsungenauigkeiten,
die dazu führen,
dass die Achsen der Drehratensensoren weder in einer vorbestimmten
Orientierung zu einem Gehäuse
der Sensoreinrichtung noch exakt orthogonal zueinander angeordnet
sind.
Wenn
vorausgehend davon die Rede war, dass ein Offset-Wert bestimmt wird,
wenn ein Ruhen der Vorrichtung für
eine bestimmte Zeitdauer festgestellt wird, so wird hierunter die
Bestimmung eines Driftvektors verstanden, und zwar für drei vorzugsweise
orthogonal zueinander orientierte Drehratensensoren und für drei vorzugsweise
orthogonal zueinander orientierte Beschleunigungssensoren. Auf diese
Weise kann eine Matrix D ermittelt werden, deren Zeilen die Offsets
der einzelnen Sensoren sind. Sie werden vorzugsweise zu Beginn einer
Objektverfolgung und dann immer wieder bei einer detektierten Ruhelage
neu ermittelt und für
die weitere Datenverarbeitung zugrunde gelegt.
Die
Genauigkeit der Bestimmung der Orientierung wird nach einer Ausführungsform
der Erfindung auch dadurch erhöht,
dass bei jeder Messdatennahme und Ermittlung der drei Drehwinkel
ein Quaternionen-Algorithmus der nachfolgend definierten Art auf
die drei Drehwinkel angewandt wird, um hieraus die aktuelle Orientierung
des Objekts im Raum zu errechnen. Die hierdurch erzielte weitere
Verbesserung beruht auf folgendem Umstand: Wenn man die durch einfache
Integration bei jedem infinitesimalen Schritt der Abtastung, d.
h. der Messdatennahme erhaltbaren infinitesimalen Drehwinkel um
eine jeweilige Achse zur Bestimmung der Orientierungsänderung
des Objekts so heranziehen würde,
dass die Drehungen nacheinander um die jeweilige Achse durchgeführt würden, so
würde hieraus
ein Fehler resultieren. Dieser Fehler liegt darin begründet, dass
die Messdaten der drei Sensoren zum gleichen Zeitpunkt genommen
werden, weil eine Drehung in der Regel gleichzeitig um drei Achsen
stattfindet und ermittelt wird. Wenn dann zur Ermittlung der Positionsveränderung
die drei bestimmten Drehwinkel aber nacheinander als Drehungen um
die jeweiligen Achsen berücksichtigt
würden,
so würde
bei der Drehung um die zweite und die dritte Achse ein Fehler resultieren,
da diese Achsen bereits im Zuge der ersten Drehung fehlerbehaftet
in eine andere Orientierung gebracht wurden. Dem wird durch Anwendung
des Quaternionen-Algorithmus auf die drei Drehwinkel begegnet. Auf
diese Weise werden die drei Drehungen durch eine einzige Transformation
ersetzt. Der Quaternionen-Algorithmus ist folgendermaßen definiert:
Die
Quaternion ist in Gl.1. definiert: q = q0 + q1·i + q2·j
+ q3·k (3)mit den Bedingungen
Gl.4. bis Gl.8. q0..3 ∊ R (4) i2 =
j2 = k2 = –1 (5) i·j = –j·i = k (6) j·k = –k·j = i (7) k·i = –i·k = j (8)
Durch
Zusammenfassen der komplexen Anteile zu einem Vektor v und mit q0 = w gelangt man zu der Schreibweise in
Gl.9. q ∊ (w·v)T (9) mit
den Bedingungen Gl.10. und Gl.11. w ∊ R (10) v ∊ R3 (11)
Für die Benutzung
der Quaternionen gelten die Definitionen Gl. 12 bis Gl.17.
Besondere
Bedeutung für
die inertiale Objektverfolgung erhält die Multiplikation. Diese
stellt eine Rotation einer Quaternion dar. Dafür wird eine Rotationsquaternion
Gl.18. eingeführt.
Der
Vektor ϕ besteht aus den einzelnen Drehungen um die Koordinatenachsen.
Eine
Drehung eines Punkts oder Vektors kann nun wie folgt errechnet werden.
Zunächst
müssen
die Koordinaten des Punkts oder der Vektor mittels Gleichung Gl.16
in eine Quaternion umgewandelt und anschließend die Multiplikation mit
der Rotationsquaternion durchgeführt
werden (Gl.18). Die Ergebnisquaternion enthält den rotierten Vektor in
der gleichen Schreibweise. Unter der Voraussetzung, dass der Betrag
einer Quaternion gleich eins ist, kann die invertierte Quaternion
durch die konjugierte ersetzt werden (Gl.19). q v' = q rot·q v·(q rot)–1 = q rot·q v·(q rot)–k (19)wegen Gl.20. |q rot| = 1 (20)
Wie
kann man diese Operation verdeutlichen? Der Vektor ϕ ist
der Normalenvektor zu der Ebene, in der eine Drehung um den Winkel ½ϕ ausgeführt wird.
Der Winkel entspricht dem Betrag des Vektors ϕ. Siehe 1.
Aus
1 ist
ersichtlich, dass eine Rotation in jeder beliebigen Ebene und unter
Angabe nur eines Winkels durchgeführt werden kann. Darin zeigen
sich auch die besonderen Vorteile für dieses Verfahren. Weitere
Vorteile sind die geringe Anzahl an notwendigen Parametern und trigonometrischer
Funktionen, welche durch Näherungen
für kleine
Winkel völlig ersetzt
werden könnten.
Für die
Rotation mit dem Vektor ω gilt
die Differenzialgleichung 21:
Die
konkrete Umsetzung des Quaternionen Algorithmus ist in 2 dargestellt
und erfolgt folgendermaßen:
Insgesamt erfolgt die gesamte Berechnung mit Hilfe von Einheitsvektoren.
Ausgehend von der Startorientierung werden die Starteinheitsvektoren
Ex, Ey und Ez ermittelt.
Aus
den Einheitsvektoren wird mit Hilfe von Gleichung Gl. 22 die Rotationsmatrix
berechnet.
Ausgehend
von einer Startorientierung des mit dem Objekt verbundenen Koordinatensystems,
insbesondere ausgehend von sogenannten Einheitsstartvektoren, wird
gemäß Gleichung
22 die Rotationsmatrix R, bei der es sich um eine 3 × 3-Matrix
handelt, errechnet. Aus einer Umkehrung dieser Gleichung 22 lässt sich ein
Rotationsquaternion qrot(k) erhalten. Mit
Hilfe des Nullquaternions, welches sich aus den Nulleinheitsvektoren
ergibt, wird durch eine Multiplikation mit dem Rotationsquaternion
das Startquarternion berechnet. Bei der nächstfolgenden Abtastung, also
bei der nächsten
Messdatennahme und Integration zu aktuellen infinitesimalen Drehwinkeln
A, B, C errechnet sich dann ein für diesen Schritt zu benutzendes
Rotationsquaternion qrot(k). Das im vorherigen
Schritt gebildete Quaternion qakt(k – 1) wird
dann mit diesem Rotationsquaternion qrot(k)
gemäß Gleichung
13 multipliziert, um das aktuelle Quaternion des vorliegenden k-ten
Schritts, nämlich qakt(k) zu erhalten. Aus diesem aktuellen
Quaternion kann dann für
den gerade durchgeführten
Abtastschritt die aktuelle Orientierung des Objekts in beliebiger
Darstellung angegeben werden.
Es
wurde vorausgehend schon darauf hingewiesen, dass ein Kalman-Filteralgorithmus
angewandt werden kann, um die Genauigkeit bei der Ermittlung oder
Errechnung von Positionsdaten zu erhöhen. Das Konzept einer Kalman-Filterung,
insbesondere einer indirekten Kalman-Filterung, geht von der Existenz
einer Stützinformation
aus. Die Differenz von Informationen, die aus Messwerten von Sensoren
gewonnen werden, und dieser Stützinformation
dient dann als Eingangssignal des Kalman-Filters. Da das erfindungsgemäße Verfahren
und die Vorrichtung aber keine kontinuierliche Information von einem
Referenzsystem erhalten, steht eine Stützinformation für die Positionsbestimmung
jedenfalls grundsätzlich
nicht zur Verfügung.
Um dennoch die Anwendung indirekter Kalman-Filterung zu ermöglichen,
wird vorgeschlagen, einen zweiten, parallel angeordneten Beschleunigungssensor
zu verwenden. Die Differenz der Sensorsignale der parallelen Beschleunigungssensoren
dient dann als Eingangssignal für
das Kalman-Filter. 3, 4 und 5 zeigen
schematisch das erfindungsgemäße Konzept
eines redundanten Parallelsystems für die Kalman-Filterung, wobei
zwei Sensoren so angeordnet sind, dass sich ihre sensitiven Sensorachsen
parallel zueinander erstrecken (4).
In
vorteilhafter Weise werden beide Integrationsschritte in die Modellierung
aufgenommen. Man erhält also
einen Schätzfehler
für den
durch zweifache Integration zwangsläufig sich ergebenden Positionsfehler. 5 verdeutlicht
dies schematisch in einer Feed-Forward-Konfiguration als konkrete
Realisierung eines allgemeinen indirekten Kalman-Filters.
In
diesem Konzept erzeugt ein Gauss-Markov-Prozess 1. Ordnung den Beschleunigungsfehler
mit Hilfe von weißem
Rauschen. Das Modell beruht darauf, dass aus dem Beschleunigungsfehler
durch zweifache Integration der Positionsfehler ermittelt wird.
Es ergeben sich die Gleichungen 23 und 25. ės(t) = ev(t) (23) ėv(t)
= ea(t) (24) ėa(t)
= –β·ea(t)·wa(t) (25)
In
Anlehnung an die allgemeine stochastische Zustandsraumbeschreibung
eines zeitkontinuierlichen Systemmodells mit Zustandsvektor
x(t), Zustandsübergangsmatrix
Φ(T), stochastischer Streumatrix
G und Messrauschen
w(t) ergibt sich die Systemgleichung 26
bzw. 27.
ẋ(t) = ϕ(T)·x(t)
+ G·w(t) (26) Für das Messrauschen w(t) gelten die Gleichungen
28 und 29. E{w(t)} = 0 (28) E{w(t)·w(t)T}
= Q d·δ(t – tT) (29)
Die
allgemeine stochastische Zustandsraumbeschreibung für das äquivalente
zeitdiskrete Systemmodell ergibt sich nach Gleichung 30 bzw. 31.
x(k
+ 1|k) = ϕ(T)·x(k|k) + w d(k|k) (30) Für die erforderliche
zeitdiskrete Messgleichung gelten die Gleichungen 32 und 33.
y(k) = C·x(k) + v(k) (32) In
Gleichung 33 ist
v(k) ein vektorieller
weißer
Rauschprozess. Als Eingang für
das Kalman-Filter gilt die Differenz der beiden Sensorsignale. Damit
ergeben sich für
die Messgleichung die Gleichungen 34 bis 36.
y(k) = Δea(k)
= [a2(k) + ea2(k)] – [a1(k) + ea1(k)] (34) y(k) = ea2(k) – ea1(k) (35) In
diesem Modell sollte sowohl e a1(k) als auch e a2(k) als Gauss-Markov-Prozess erster Ordnung modelliert
werden. Dazu dient der Ansatz nach Gleichung 37. ėa2(t)
= –β·ea2(t)·wa2(t) (37)
Der äquivalente
zeitdiskrete Ansatz dazu folgt nach Gleichung 38. ea2(k
+ 1|k) = e–βT·ea2(k|k)·wa2(k|k) (38)
Betrachtet
man e
a2(k) als weiteren Zustand ergibt sich
das erweiterte Systemmodell nach Gleichung 39 und 40.
x e(k + 1|k) = ϕ e(T)·x e(k|k)
+ w de(k|k) (39) Dabei
gelten die Gleichungen 41 bis 43.
Für das erweiterte
Messmodell gelten die Gleichungen 44 bis 47.
y(k) = [a2(k)
+ ea2(k)] – [a1(k)
+ ea1(k)] (44) y(k) = ea2(k) – ea1(k) (45) y(k) = C·x(k) + v(k) (46) Bei
dieser Messgleichung handelt es sich um eine perfekte und damit
fehlerfreie Messung, d.h. es tritt kein Messrauschen v(k) auf. Damit
bedarf es einer Modellierung nach Gleichung 48. R(k) = r(k) = 0 (48)
Die
Kovarianzmatrix R des Messrauschens ist damit singulär, d.h.
R–1 existiert
nicht. Die Existenz von R–1 ist hinreichende aber
nicht notwendige Bedingung für
die Stabilität
bzw. für
die stochastische Beobachtbarkeit des Kalman-Filters. Es gibt nun
zwei Möglichkeiten
auf den Umstand der Singularität
zu reagieren:
- 1. Verwendung von R = 0. Das
Filter kann stabil sein. Da hier ohnehin nur der Bedarf für kurzzeitige
Stabilität
besteht, kann auf Langzeitstabilität verzichtet werden.
- 2. Verwendung eines reduzierten Beobachters.
In
diesem Konzept wird die Varianz R = 0 verwendet. Damit arbeiten
die verwendeten Filter hinreichend stabil.
Die
Kalman-Filter-Gleichungen für
das eindimensionale System in diskreter Form ergeben sich zu Gleichungen
49 bis 50.
Bestimmung
der Kalmanverstärkung
nach Gleichung 49. K(k + 1|) = P(k + 1|k)·C T(k)[C(k)·P(k + 1|k)·C T(k) + R(k)]–1 (49)
Aktualisierung
der Zustandsvorhersage nach Gleichung 50.
Wobei
Gleichung 51 gilt.
Aktualisierung
der Kovarianzmatrix des Schätzfehlers
nach Gleichung 52 bzw. 53. P(k + 1|k
+ 1) = (I – K(k
+ 1)·C T(k))·P(k + 1|k)·(I – K(k + 1)·C T(k))T + K(k + 1)·R(k)·K T(k + 1) (52) P(k
+ 1|k + 1) = (I – K(k + 1)·C T(k))·P(k + 1|k) (53)
Ermittlung
des Prädiktionswertes
des Systemzustandes nach Gleichung 54.
Ermittlung
des Prädiktionswertes
der Kovarianzmatrix des Schätzfehlers
nach Gleichung 55. P(k + 2|k + 1) = ϕ(T)·P(k
+ 1|k + 1)·ϕ T(T)
+ Q(k) (55)
Damit
ist der Filterzyklus komplett durchlaufen und beginnt für den nächsten Messwert
erneut. Das Filter arbeitet rekursiv, wobei die Schritte der Vorhersage
und der Korrektur bei jeder Messung durchlaufen werden.
Das
verwendete System beschreibt eine dreidimensionale Translation in
drei orthogonalen Raumachsen. Diese Translationen werden durch den
Weg s, die Geschwindigkeit v und die Beschleunigung a beschrieben.
Für ein
indirektes Kalman-Filter liefert jeweils ein zusätzlicher Beschleunigungssensor
für jede
Raumrichtung ebenfalls eine Beschleunigungsinformation.
Der
grundsätzliche
Algorithmus des Aufbaus wird in Bild 5 dargestellt. Das eigentliche
Messsignal kommt für
jede Raumachse von einem Beschleunigungssensor in Form der Be schleunigung
a. Mit Hilfe der Zusatzinformation in Form des Sensorsignals des
zweiten Beschleunigungssensors für
jede Raumachse liefert der Kalman-Filter-Algorithmus einen Schätzwert für die Abweichung
des Beschleunigungssignals ea für
die drei Raumrichtungen x, y und z.
Die
erfindungsgemäße Vorrichtung
ist in vorteilhafter Weise so ausgebildet, dass mit einer Abtastrate von
10 bis 50 Hz, insbesondere von 10 bis 40 Hz, insbesondere von 10
bis 30 Hz und weiter insbesondere von 15 bis 25 Hz, vorzugsweise
von ca. 20 Hz die Messwerte der Beschleunigungssensoren und Drehratensensoren
erfassbar und weiterverarbeitbar sind.
Es
erweist sich als zweckmäßig, wenn
das Sensormittel drei Beschleunigungssensoren, deren Signale zur
Berechnung einer translatorischen Bewegung verwendbar sind, und
zusätzlich
drei Drehratensensoren, deren Messwerte zur Bestimmung der Orientierung
im Raum verwendbar sind, umfasst.
Wie
vorausgehend bereits erläutert,
umfasst die Vorrichtung vorteilhafterweise Rechenmittel, die Mittel zur
Ausführung
eines Quaternionen-Algorithmus aufweisen.
Des
Weiteren erweist es sich als vorteilhaft, dass die Rechenmittel
Mittel zur Anwendung einer vor Beginn der Positionierung ermittelten
und gespeicherten Ausgleichsmatrix aufweisen, welche Ausgleichsmatrix einer
Abweichung der Orientierung der Achsen der drei Drehratensensoren
von einer angenommenen Orientierung der Achsen zueinander Rechnung
trägt und
durch ihre Anwendung einen bei der Berechnung der Drehwinkel resultierenden
Fehler kompensiert.
Nach
einer weiteren bevorzugten Ausführungsform
weisen die Rechenmittel Mittel zur Ausführung eines Kalman-Filter-Algorithmus auf.
Insbesondere
hierfür
erweist es sich als vorteilhaft, dass zusätzlich zu den Drehratensensoren
Magnetfeldsensoren zur Ermittlung der Orientierung des Gegenstands
im Raum vorgesehen sind.
Weiter
erweist es sich als vorteilhaft, dass Mittel zum Vergleichen der
aus Messwerten der Magnetfeldsensoren ermittelten Orientierung um
Raum mit der aus Messwerten der Drehratensensoren ermittelten Orientierung
im Raum vorgesehen sind.
Nach
einer weiteren Ausführungsform
sind Mittel zum Vergleichen der aus Messwerten der Magnetfeldsensoren
ermittelten Orientierung im Raum mit der aus Messwerten eines Beschleunigungssensors
für die Messung
der Erdbeschleunigung ermittelten Orientierung im Raum vorgesehen.
Nach
einer ganz besonders bevorzugten Ausführungsform der erfindungsgemäßen Vorrichtung
ist zur Durchführung
einer Kalman-Filterung für
jeden Beschleunigungssensor ein parallel hierzu angeordneter redundanter
Beschleunigungssensor vorgesehen.
Es
wird mit der vorliegenden Anmeldung auch Schutz beansprucht für ein Verfahren
mit den Merkmalen des Anspruchs 13 und insbesondere mit Verfahrensmerkmalen,
die sich aus der vorausgegangenen Beschreibung der Vorrichtung und
der sich darauf beziehenden Ansprüche ergeben.
Außerdem wird
Schutz beansprucht für
eine Verwendung nach den Ansprüchen
14 bis 18.
