DE102021129225A1

DE102021129225A1 - Verfahren und System zur Schätzung von Zustandsgrößen eines beweglichen Objekts mit modularer Sensorfusion

Info

Publication number: DE102021129225A1
Application number: DE102021129225.5A
Authority: DE
Inventors: Christian Brommer; Stephan Michael Weiss
Original assignee: Alpen Adria Univ Klagenfurt; Alpen-Adria-Universitat Klagenfurt
Current assignee: Alpen Adria Univ Klagenfurt; Alpen-Adria-Universitat Klagenfurt
Priority date: 2020-11-10
Filing date: 2021-11-10
Publication date: 2022-05-12
Also published as: US20220146264A1; AT523734B1; AT523734A4

Abstract

Die vorliegende Erfindung betrifft ein computer-implementiertes Verfahren zur Schätzung von Zustandsgrößen eines beweglichen Objekts, bei welchem Kernzustandsgrößen des beweglichen Objekts mittels eines rekursiven Bayesschen Filters unter Verwendung von Beobachtungswerten von Sensoren, die seit Aufstarten des beweglichen Objekts eingesetzt werden, propagiert und im Fall von Beobachtungswerten von während der Laufzeit hinzugefügten zusätzlichen Sensoren eine Kovarianzmatrix des rekursiven Bayesschen Filters aus einer auf einen einen Zeitschritt vor dem zweiten Zeitpunkt liegenden Zeitpunkt propagierten Kovarianz der Kernzustandsgrößen des beweglichen Objekts, der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors geformt wird, die Kovarianzmatrix mit Hilfe der zu dem zweiten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte aktualisiert wird und nach Ermitteln der Kernzustandsgrößen die Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und die Kalibrierungszustandsgrößen des zusätzlichen Sensors wieder von der Kovarianz der Kernzustandsgrößen des zusätzlichen Sensors getrennt werden.

Description

GEBIET DER TECHNIK
Die vorliegende Erfindung betrifft ein computer-implementiertes Verfahren, ein System und ein computerlesbares Speichermedium zur Schätzung von Zustandsgrößen eines beweglichen Objekts, bei welchen ein modularer Sensorfusionansatz verfolgt wird. Sensorfusion wird auch als Sensordatenfusion bzw. als Multi-Sensor-Datenfusion bezeichnet.
STAND DER TECHNIK
Das Schätzen von Zustandsgrößen bildet einen wesentlichen Teil sowohl der Robotik als auch von anderen ingenieurwissenschaftlichen Disziplinen wie beispielsweise der Regelungstechnik. So ist der genaue Standort einer Roboterplattform beispielsweise wesentlich für deren Regelung und Navigation.
Bei den meisten Zustandsschätzern handelt es sich um dedizierte Zustandsschätzer, die für eine bestimmte Aufgabe an einer bestimmten Roboterplattform unter bestimmten Bedingungen entwickelt worden sind und somit eine geringe Wiederverwendbarkeit für den Fall haben, dass sich das Szenario, insbesondere die eingesetzten Sensoren oder die eingesetzte Roboterplattform ändern.
Zwar wird diese Problematik von bekannten sogenannten Extended-Kalman-FilterSystemen adressiert. Bei Extended-Kalman-Filtern handelt es sich um rekursive Filter. Allerdings gehen auch die bekannten Extended-Kalman-Filter-Systeme von einer Sensorkonfiguration aus, die während der Kompilierungsphase bzw. Aufstartphase des Extended-Kalman-Filter-Systems definiert wird. Die Referenzrahmen zusätzlicher Sensoren sind in der Regel vordefiniert und werden nicht dynamisch an ein sich änderndes Szenario bzw. eine sich ändernde Situation angepasst. Die derart vordefinierten Referenzrahmen erlauben üblicherweise keine Sensorinitialisierung während der Laufzeit, insbesondere dann nicht, wenn die Sensordefinition dem System nicht von vornherein bekannt ist.
Dies begrenzt die Anwendbarkeit derartiger Extended-Kalman-Filter-Systeme auf statische Hardwarekonfigurationen, während eine Eignung für modulare Hardwareplattformen, welche während der Laufzeit durch weitere, vorab nicht bekannte Module erweitert werden, eher gering ist. Untereinander verbindbare Schlangenroboter (snake robots) und humanoide Roboter mit austauschbaren Greiforganen stellen Beispiele derartiger modularer Hardwareplattformen dar, deren Konfiguration/Aufbau sich nach deren Aufstarten ändern kann.
Für die Integration zusätzlicher, während der Laufzeit hinzugefügter Sensoren müssen zusätzlich Kalibrierungszustandsgrößen berücksichtigt werden, da die zusätzlichen Sensoren in der Regel weder auf den Körperrahmen (body frame), d.h. das objektbezogene Koordinatensystem, der Hardware- bzw. Roboterplattform ausgerichtet sind, noch intrinsisch kalibriert sind. Die Anzahl der Kalibrierungszustandsgrößen nimmt mit der Anzahl der zusätzlichen Sensoren zu. Alternativ müssen Annahmen bezüglich der Unveränderbarkeit der anfänglichen Kalibrierung der zusätzlichen Sensoren gemacht werden. Beides macht den regulären, nicht-modularen Extended-Kalman-Filter-Ansatz komplex und unflexibel. Eine zunehmende Anzahl an Zustandsgrößen führt dazu, dass mehr Operationen bzw. Berechnungsschritte für die Zustandsschätzung durchgeführt werden müssen, z.B. bei der Propagierung und Korrektur/Aktualisierung der Systemzustandsgrößen. Bei sogenannten naiven Zustandsschätzern, d.h. Zustandsschätzern, welche weder die Modularität der Sensoren noch vereinfachende Annahmen in Betracht ziehen; nimmt die Bearbeitungszeit aufgrund der erforderlichen Matrizenmultiplikationen kubisch, also mit der Ordnung O(n³), mit der Anzahl der Sensoren n zu. Für verzögerte oder außerhalb der Reihe liegende Messsignale erhöht sich dieser Effekt in einem Mehrsensorensystem noch weiter, da verzögerte Messsignale zahlreiche Neuberechnungsschritte auslösen. Hardwaresynchronisation kann diese Problematik abschwächen, allerdings ist eine solche nicht in jedem Fall durchführbar, beispielsweise nicht bei dynamischen Sensormessraten bei Sensoren, welche dies vom Hersteller grundsätzlich nicht vorsehen. Außerdem ist mit einer Hardwaresynchronisation in der Regel ein nicht unerheblicher technischer Aufwand verbunden und das grundsätzlich verzögerte, nun zwar mit richtigem Zeitstempel versehene, Signal kann immer noch zahlreiche Neuberechnungsschritte auslösen.
Nicht-rekursive Filtersysteme, die beispielsweise auf Graphenoptimierung (graph optimization) basieren, können zwar vorab unbekannte Sensoren während der Laufzeit des Systems initialisieren. Allerdings ist ihr Bedarf an Rechenleistung in der Regel derart hoch, dass sie sich nicht für den Einsatz auf ressourcenbegrenzten Plattformen wie beispielsweise Drohnen oder generell kleine, leichte Roboter eignen.
Die Zustandsschätzung mit vordefinierter Konfiguration mehrerer Sensoren und Berücksichtigung ergänzender Kalibrierungszustandsgrößen inklusive deren Selbstkalibrierung und Verzögerungskompensation ist in der einschlägigen Fachliteratur behandelt worden. So behandelt das Single Sensor Fusion Framework (SSF), welches in „Real-time metric state estimation for modular vision-inertial systems,“ von S. Weiss und R. Y. Siegwart, Proceedings - IEEE International Conference on Robotics and Automation, Vol. 231855, S. 4531-4537, 2011, vorgeschlagen wird, sowohl Online-Selbstkalibrierung als auch die genaue Handhabung von Sensorverzögerungen. Eine erweiterte Fassung von SSF wurde in „Long-duration autonomy for small rotorcraft UAS including recharging,“ von C. Brommer, D. Malyuta, D. Hentzen, und R. Brockers in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Okt. 2018, S. 7252-7258, in einer Mehrsensorenkonfiguration für lange währende Autonomie eingesetzt. S. Lynen, M. W. Achtelik, S. Weiss, M. Chli, und R. Siegwart, „A robust and modular multi-sensor fusion approach applied to MAV navigation,“ in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Nov. 2013, stellt ein Multi-Sensor Fusion Framework (MSF) vor. In S. Shen, Y. Mulgaonkar, N. Michael, und V. Kumar, „Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV,“ Proceedings - IEEE International Conference on Robotics and Automation, S. 4974-4981, 2014, wird ein ähnlicher Ansatz verfolgt, welcher relative und absolute Sensoraktualisierungen am Beispiel von lokalen Sichtaktualisierungen und globalen Positionsinformationen darstellt. Während sowohl SSF als auch MSF Sensorausfällen Rechnung tragen können, erweitert „Selfcalibrating multi-sensor fusion with probabilistic measurement validation for seamless sensor switching on a UAV,“ von K. Hausman, S. Weiss, R. Brockers, L. Matthies, und G. S. Sukhatme, Proceedings - IEEE International Conference on Robotics and Automation, Vol. 2016-June, S. 4289-4296, den MSF-Ansatz und beschreibt Online-Sensorinitialisierungen und -Sensorumschaltungen basierend auf Sensorverfügbarkeit und Gesundheitsmetriken (health metrics).
C. Tessier, C. Cariou, C. Debain, F. Chausse, R. Chapuis, und C. Rousset, „A real-time, multi-sensor architecture for fusion of delayed observations: application to vehicle localization,“ in 2006 IEEE Intelligent Transportation Systems Conference, https://doi.org/10.1109/itsc.2006.1707405 schlagen ein Verfahren zur Handhabung von verzögerten Messsignalen vor, welches für rechenleistungsmäßig beschränkte eingebettete Systeme entwickelt worden ist. T. Moore und D. Stouch, „A generalized extended kalman filter implementation for the robot operating system,“ in Proceedings of the 13th International Conference on Intelligent Autonomous Systems (IAS-13), Springer, Jul. 2014, beschreiben eine generalisierte Extended-Kalman-Filter-Implementierung, welche auf dem sogenannten Robot Operating System (ROS) basiert. Die Sensorstruktur wird beim Aufstarten definiert. Modifikationen sind während der Laufzeit nicht möglich. Es wird angenommen, dass sich Sensormesswerte auf den Ursprung des Roboterkoordinatensystems beziehen. Weder Sensorkalibrierung noch Online-Selbstkalibrierung sind beschrieben. Bei der inertialen Messeinheit (Inertial Measurement Unit - IMU) erfolgt keine Schätzung von gyroskopischem Bias. Prozessrauschen wird von Hand kompensiert.
M. Darms und H. Winner, „A modular system architecture for sensor data processing of ADAS applications,“ IEEE Intelligent Vehicles Symposium, Proceedings, Vol. 2005, S. 729-734, 2005, beschreibt den aktuellen Stand von Wissenschaft und Technik in Bezug auf zentralisierte und dezentralisierte Systeme, welche Sensorfusion zur Fahrunterstützung bei in Fahrzeugen eingesetzten Fahrerassistenzsystemen (advanced driver-assistance systems (ADAS) einsetzen. Zentralisierte Ansätze ermöglichen eine eng gekoppelte Schätzung, aber benötigen eine hohe Kommunikationsbandbreite und sind schwer auf andere Anwendungen auszudehnen. Die Arbeitslast bei der Einbindung neuer Sensoren ist hoch. Dezentralisierte Systeme verwenden lose gekoppelte Sensoren, was aufgrund von unzulänglicher Handhabung der Kreuzkovarianzen von Sensor- und Kernzustandsgrößen den Nachteil von Inkonsistenzen in sich birgt.
D. A. Cucci and M. Matteucci, „On the Development of a Generic Multi-Sensor Fusion Framework for Robust Odometry Estimation,“ Journal of Software Engineering for Robotics, Vol. 5, Maiausgabe, S. 48-62, 2014, und H.-P. Chiu, X. S. Zhou, L. Carlone, F. Dellaert, S. Samarasekera, und R. Kumar, „Constrained optimal selection for multi-sensor robot navigation using plug-and-play factor graphs,“ in 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, Mai 2014, beschäftigen sich mit der Modularisierung von Mehrfachsensorfusion und schlagen ein auf Vektorgraphen basiertes Verfahren vor, welches einen Echtzeit-Batch-Optimierungsprozess einsetzt. Der Fokus liegt dabei auf der optimalen Auswahl einer minimalen Untermenge aus der gegebenen Sensorkonfiguration und der Beobachtbarkeit für die Sensorauswahl. Der Einsatz von auf Vektorgraphen basierten Verfahren ist jedoch in Hinblick auf die Skalierbarkeit, insbesondere in Kombination mit rechenleistungsmäßig beschränkten Ressourcen. von Nachteil
Ferner beschreiben die WO 2015/105597 A1 , die US 9,031,782 B1 , die US 7,181,323 B1 , die US 10,274,318 B1 bekannte Verfahren zur Sensordatenfusion und Positionsschätzung, welche einen Extended-Kalman-Filter (EKF) einsetzen. Zur Sensorfusion wird ferner auf die folgenden Dokumente verwiesen: Emter, T. et al.: „Stochastic Cloning for Robust Fusion of Multiple Relative and Absolute Measurements“, 2019 IEEE Intelligent Vehicles Symposium (IV), 09. Juni 2016 (09.06.2019), Seiten 1782-1788, XP033606100; Asadi, E. et al.: „Delayed Fusion of Relative State Measurements by Extending Stochastic Cloning via Direct Kalman Filtering“, ISIF 16th International Conference on Information Fusion, 09. Juli 2013 (09.07.2013), Seiten 2049-2056, XP032512439; Allak, E. et al.: „Covariance Pre-Integration for Delayed Measurements in Multi-Sensor Fusion“, 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 03. November 2019 (03.11.2019), Seiten 6642-6649, XP033695454. Zusätzlich wird auf die folgenden Dokumente verwiesen: X. R. Li, Z. Zhao, and V. P. Jilkov, „Practical Measures and Test for Credibility of an Estimator,“ Proc. Workshop on Estimation, Tracking, and Fusion, S. 481--495, 2001; W. J. Hughes, „Global positioning system (gps) standard positioning service (sps) performance analysis report,“ Tech. Cntr. NSTB/WAAS T and E Team, Nr. 87, 2014; L. Serrano, D. Kim, R. B. Langley, K. Itani, and M. Ueno, „A gps velocity sensor: how accurate can it be?-a first look,“ in ION NTM, Vol. 2004, 2004, S. 875-885.
ZUSAMMENFASSUNG DER ERFINDUNG
Ausgehend vom oben genannten Stand der Technik stellt sich die vorliegende Erfindung die Aufgabe, ein Verfahren, ein System und ein computerlesbares Speichermedium zur Schätzung von Zustandsgrößen eines beweglichen Objekts bereitzustellen, welche eine dynamische und effiziente Einbindung während der Laufzeit des beweglichen Objekts hinzugefügter zusätzlicher Sensoren ermöglichen. Es ist ferner Aufgabe der vorliegenden Erfindung, ein Verfahren, ein System und ein computerlesbares Speichermedium zur Schätzung von Zustandsgrößen eines beweglichen Objekts bereitzustellen, welche eine dynamische und effiziente Entfernung von Sensoren während der Laufzeit des beweglichen Objekts zu erlauben.
Die oben genannten Aufgaben werden durch ein computer-implementiertes Verfahren, ein computerlesbares Speichermedium und ein Datenverarbeitungssystem mit den Merkmalen der unabhängigen Ansprüche gelöst.
Unter der Laufzeit wird die Zeit nach dem Aufstarten des beweglichen Objekts verstanden, wobei beispielsweise ein Anhalten (z.B. zum Auftanken bzw. Aufladen eines als Fahrzeug ausgestalteten beweglichen Objekts) als während der Laufzeit stattfindend anzusehen ist.
Das Hinzufügen eines zusätzlichen Sensors umfasst neben einem nach dem Aufstarten des beweglichen Objekts erfolgenden physischen Hinzufügens (z.B. ein Montieren) unter anderem auch, dass der zusätzliche Sensor bereits beim Aufstarten des beweglichen Objekts vorhanden ist, aber erst nach dessen Aufstarten zugeschaltet bzw. eingeschaltet wird und/oder erst nach dem Aufstarten Beobachtungswerte liefert. Entsprechend umfasst ein Entfernen eines Sensors ein Ausschalten und/oder ein Nicht-mehr-Liefern von Beobachtungswerten neben einem physischen Entfernen (beispielsweise einem Abmontieren).
Das erfindungsgemäße computer-implementierte Verfahren zur Schätzung von Zustandsgrößen eines beweglichen Objekts weist die folgenden Schritte auf: In einem ersten Schritt a) erfolgt eine Initialisierung eines zum Schätzen von vorab definierten Kernzustandsgrößen eines beweglichen Objekts eingesetzten rekursiven Bayesschen Filters. Bei dem rekursiven Bayesschen Filter handelt es sich vorzugsweise um einen rekursiven Kalman-Filter, insbesondere um einen sogenannten Extended-Kalman-Filter (EKF; auch erweiterter Kalman-Filter genannt). Bei den Kernzustandsgrößen des beweglichen Objekts handelt es sich bevorzugt um Navigationszustandsgrößen, d.h. für die Navigation relevante Zustandsgrößen wie beispielsweise Position, Geschwindigkeit und Orientierung.
In einem darauffolgenden Schritt b) werden technische Eigenschaften des beweglichen Objekts mit Hilfe von einem oder mehreren Sensoren unter Bildung von Beobachtungswerten (auch Messwerte genannt) beobachtet. Unter technischen Eigenschaften eines beweglichen Objekts werden insbesondere physikalische Eigenschaften (wie beispielsweise Position und Geschwindigkeit), biologische Eigenschaften und/oder chemische Eigenschaften verstanden.
In Schritt c) werden die Kernzustandsgrößen des beweglichen Objekts und eine Kovarianz der Kernzustandsgrößen mittels eines Zustandsgrößenmodells des rekursiven Bayesschen Filters unter Verwendung derjenigen Beobachtungswerte zeitlich propagiert, welche mit Hilfe von einem oder mehreren Sensoren gebildet worden sind, die seit Aufstarten des beweglichen Objekts eingesetzt werden. Dabei kann es sich beispielweise um einen oder mehrere Propagierungssensoren handeln, d.h. um Sensoren, die für als Navigationszustandsgrößen ausgebildete Kernzustandsgrößen relevante Beobachtungen liefern. Insbesondere kann es sich um eine inertiale Messeinheit (IMU) handeln.
Wird in einem Schritt d) festgestellt, dass Beobachtungswerte mit Hilfe eines nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors gebildet werden, so erfolgt in einem Schritt e1) ein Initialisieren einer Kovarianz von Kalibrierungszustandsgrößen des zusätzlichen Sensors und von Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe der zu einem ersten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte. Bei den zu dem ersten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerten handelt es sich um die ersten Beobachtungswerte bzw. die ersten Messwerte, welche der zusätzliche Sensor ausgibt.
Werden von dem zusätzlichen Sensor zu einem zweiten Zeitpunkt, welcher nach dem ersten Zeitpunkt liegt, weitere Beobachtungswerte gebildet bzw. Messwerte ausgegeben, so wird in einem darauffolgenden Schritt e2) eine Kovarianzmatrix des rekursiven Bayesschen Filters aus (i) der Kovarianz der Kernzustandsgrößen des beweglichen Objekts, ii) der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und iii) den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors gebildet. Bei der Kovarianz der Kernzustandsgrößen des beweglichen Objekts handelt es sich dabei um die auf einen einen Zeitschritt vor dem zweiten Zeitpunkt (dem zweiten Beobachtungszeitpunkt) liegenden Zeitpunkt propagierte Kovarianz der Kernzustandsgrößen.
Bei der ersten Durchführung von Schritt e2) handelt es sich bei der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors um die im Sensorinitialisierungsschritt e1) gebildete Kovarianz bzw. gebildeten Kreuzkovarianzen.
Nach dem Formen/Bilden der Kovarianzmatrix des rekursiven Bayesschen Filters in Schritt e2) wird die Kovarianzmatrix in Schritt e3) mit Hilfe der zu dem zweiten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte aktualisiert bzw. korrigiert. Im anschließenden Schritt e4) werden dann die Kernzustandsgrößen des beweglichen Objekts mit Hilfe der aktualisierten Kovarianzmatrix durch den rekursiven Bayesschen Filter berechnet bzw. geschätzt.
Danach werden in Schritt e5) die Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors von der Kovarianz der Kernzustandsgrößen des zusätzlichen Sensors separiert, sodass eine getrennte Verarbeitung/ein getrenntes Propagieren erfolgen kann.
Werden nach dem zweiten Zeitpunkt von dem zusätzlichen Sensor bei späteren Beobachtungen weitere Beobachtungswerte gebildet, so werden die obigen Schritte e2) bis e5) für diese Beobachtungswerte an ihren jeweiligen späteren Beobachtungszeitpunkten wiederholt. Dabei wird in Schritt e2), in welchem die Kovarianzmatrix gebildet wird, die auf einen einen Zeitschritt vor dem jeweiligen späteren Zeitpunkt liegenden Zeitpunkt propagierte Kovarianz der Kernzustandsgrößen des beweglichen Objekts verwendet. Bei der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors handelt es sich dann um die bei der letzten Aktualisierung der Kovarianzmatrix im vorhergehenden Schritt e3) ebenfalls aktualisierte Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die aktualisierten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors.
Gemäß bevorzugter Ausgestaltung werden für den Fall, dass nach dem Aufstarten des beweglichen Objekts mehrere zusätzlichen Sensoren hinzugefügt werden, die Schritte e1) bis e6) des erfindungsgemäßen Verfahrens für jeden einzelnen der hinzugefügten Sensoren durchgeführt. Dabei werden die mit den jeweiligen zusätzlichen Sensoren assoziierten Kovarianzen und Kreuzkovarianzen vorzugsweise nicht zum gleichen Zeitpunkt aktualisiert. D.h. es werden keine Kreuzkovarianzen zwischen unterschiedlichen, nach dem Aufstarten hinzugefügten zusätzlichen Sensoren gebildet.
Werden bei der in Schritt e2) des erfindungsgemäßen Verfahrens gebildeten Kovarianzmatrix die Kovarianz der Kernzustandsgrößen des beweglichen Objekts und die mit den Kalibrierungszuständen des zusätzlichen Sensors assoziierte Kovarianz und assoziierten Kreuzkovarianzen zu unterschiedlichen Zeitpunkten gebildet, so beinhalten die Kovarianz der Kernzustandsgrößen und die mit dem zusätzlichen Sensor assoziierte Kovarianz und assoziierten Kreuzkovarianzen nicht die gleiche Menge an Daten, was zu einer nicht-positiven semidefiniten und somit ungültigen Kovarianzmatrix führen kann. Dies ist beispielsweise dann der Fall, wenn sich die mit den Kalibrierungszuständen des zusätzlichen Sensors assoziierte Kovarianz und assoziierten Kreuzkovarianzen auf den ersten (Beobachtungs-)Zeitpunkt beziehen, während sich die Kovarianz der Kernzustandsgrößen des beweglichen Objekts auf einen Zeitpunkt bezieht, der einen Zeitschritt vor dem zweiten (Beobachtungs-)Zeitpunkt des zusätzlichen Sensors liegt. Eine nicht-positiv semidefinite und somit ungültige Kovarianzmatrix würde jedoch längerfristig zur Divergenz und zu Fehlverhalten des rekursiven Bayesschen Filters führen, so dass bevorzugterweise wie folgt vorgegangen wird, um eine mit einer nicht-positiv semidefiniten Kovarianzmatrix einhergehende Inkonsistenz und eine sich aus einer solchen Inkonsistenz ergebende Diverganz des rekursiven Bayesschen Filters zu vermeiden.
Um die im vorhergehenden Absatz genannte Problematik zu adressieren und zu lösen, werden gemäß weiterer bevorzugter Ausgestaltung in Schritt e2) des erfindungsgemäßen Verfahrens die neueste Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe einer Reihe von einer oder mehreren Zustandsübergangsmatrizen auf denselben Zeitpunkt wie die Kovarianz der Kernzustandsgrößen des beweglichen Objekts propagiert. Auf diese Weise beziehen sich sämtliche der Kovarianzen und Kreuzkovarianzen der Kovarianzmatrix auf denselben Zeitpunkt.
Dabei ist die Reihe der Zustandsübergangsmatrizen Φ(m,n) zwischen zwei Zeitpunkten t(m) und t(n) wie folgt definiert: $Φ (m, n) = Φ_{n} Φ_{n - 1} \dots Φ_{m}$
mit t(m) < t(n) und Φ_k als diskreter Zustandsübergangsmatrix (state-transition matrix) (D_k|k-1, welche die Zustandsdynamik abbildet und basierend auf Eingangswerten des beweglichen Objekts evaluiert und für den Propagierschritt δt = t(k) - t(k-1) integriert wird (vergleiche S. I. Roumeliotis und G. A. Bekey, „Distributed multirobot localization,“ IEEE Transactions on Robotics and Automation, Vol. 18, Nr. 5, S. 781-795, 2002).
Entsprechend kann die Kreuzkovarianz Pcs zwischen den Kernzustandsgrößen X_C des beweglichen Objekts und den Kalibrierungszustandsgrößen X_S des zusätzlichen Sensors wie folgt von einem Zeitpunkt t(m) auf einen Zeitpunkt t(n) propagiert werden: $P_{CS n (-)} = Φ (m, n) P_{CS m} Φ {(m, n)}^{T} .$
Entsprechendes gilt für die Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors. Die derart berechnete und beispielsweise in einem Zwischenspeicher hinterlegte Reihe von (sich zeitlich entwickelnden) Zustandsübergangsmatrizen kann vorteilhafterweise verwendet werden, um die mit dem zusätzlichen Sensor assoziierte Kovarianz und assoziierten Kreuzkovarianzen auf denselben Zeitpunkt zu propagieren wie die propagierte Kovarianz der Kernzustandsgrößen des beweglichen Objekts. Die mit dem zusätzlichen Sensor assoziierte Kovarianzen und assoziierten Kreuzkovarianzen „erben“ während des Propagierens sozusagen diejenigen Informationen, die ihnen für den Zeitraum fehlten, während dem allein die Kovarianz der Kernzustandsgrößen des beweglichen Objekts propagiert worden war. Durch das vorerst alleinige Propagieren der Kovarianz der Kernzustandsgrößen wird deren Komplexität vorteilhafterweise unabhängig von der Anzahl zusätzlicher, nach dem Aufstarten hinzugefügter Sensoren und somit konstant gehalten. Die mit den zusätzlichen Sensoren assoziierten Kovarianzen und Kreuzkovarianzen werden vorzugsweise erst beim Bilden der Kovarianzmatrix propagiert.
Werden mehrere zusätzliche Sensoren nach dem Aufstarten des beweglichen Objekts hinzugefügt, so kann es dazu kommen, dass Beobachtungen eines ersten zusätzlichen Sensors die Kernzustandsgrößen des beweglichen Objekts und die Kalibrierungszustandsgrößen eines zweiten zusätzlichen Sensors indirekt aufgrund von Kreuzkorrelationen beeinflussen, obgleich der erste zusätzliche Sensor vor dem Hinzufügen des zweiten zusätzlichen Sensors bereits entfernt und erst nach Entfernen des zweiten zusätzlichen Sensors wieder hinzugefügt worden ist. Diese unerwünschte Beeinflussung kann zu einer nicht-positiv semidefiniten und damit ungültigen Kovarianzmatrix (einer sogenannten Pseudokovarianzmatrix) führen. Kovarianzmatrizen sollen definitionsgemäß symmetrisch und positiv semidefinit sein. Dies garantiert, dass ihre Korrelationen bzw. Kovarianzen kohärent sind.
Um sicherzustellen, dass die bei dem erfindungsgemäßen Verfahren gebildete Kovarianzmatrix positiv semidefinit ist, wird gemäß weiterer bevorzugter Ausgestaltung die Kovarianzmatrix vor der Schätzung der Kernzustandsgrößen des beweglichen Objekts in Schritt e3) des erfindungsgemäßen Verfahrens in eine positiv semidefinite Kovarianzmatrix korrigiert.
P. J. Rousseeuw und G. Molenberghs, „Transformation of non positive semidefinite correlation matrices,“ Communications in Statistics - Theory and Methods, Vol. 22, Nr. 4, S. 965-984, Jan. 1993, verfügbar unter: https://doi.org/10.1080/03610928308831068, und R. Rebonato und P. Jaeckel, „The most general methodology to create a valid correlation matrix for risk management and option pricing purposes,“ SSRN Electronic Journal, 2011, verfügbar unter: https://doi.org/10.2139/ssrn.1969689. beschreiben verschiedene Verfahren, um die naheste positiv semidefinite Kovarianzmatrix einer gegebenen Pseudokovarianzmatrix zu schätzen. Beschrieben sind unter anderem das Eigenwert-Verfahren (Eigenvalue method) und das Skalier-/Hypersphäre-Dekompositionsverfahren (scaling/hypersphere decomposition) mit Winkelparametrisierung (angular parametrization). Das Skalier-/Hypersphäre-Dekompositionsverfahren verwendet ein Optimierungsverfahren zur Minimierung der sogenannten Frobeniusnorm A: $A= \sum_{i}^{n} \sum_{j}^{n} {(p_{i, j} - {\tilde{p}}_{i, j})}^{2}$
in Bezug auf eine gegebene Matrix, wobei p Elemente der tatsächlichen, gegebenen Matrix und P̃ Elemente ihrer nahesten Approximation sind.
Das Eigenwert-Verfahren approximiert eine positiv semidefinite Matrix mittels Korrektur der Eigenwerte einer gegebenen Matrix. Gemäß N. J. Higham, „Computing a nearest symmetric positive semidefinite matrix,“ Linear Algebra and its Applications, vol. 103, pp. 103-118, Mai 1988, verfügbar unter: https://doi.org/10.1016/0024-3795(88)90223-6. minimiert das Eigenwert-Verfahren ebenfalls die Frobeniusnorm.
Aufgrund seiner deterministischen Eigenschaften und geringeren Komplexität wird gemäß bevorzugter Ausgestaltung des erfindungsgemäßen Verfahrens die gebildete Kovarianzmatrix mit Hilfe eines Eigenwert-Verfahrens in eine positiv semidefinite Kovarianzmatrix korrigiert. Bei dem Eigenwert-Verfahren wird die Kovarianzmatrix in einem ersten Schritt in ihre Eigenwerte und Eigenvektoren zerlegt. In einem zweiten Schritt werden gegebenenfalls negative Eigenwerte korrigiert, und in einem dritten Schritt wird die Kovarianzmatrix mit den korrigierten Eigenwerten und den Eigenvektoren rekonstruiert. D.h. im ersten Schritt wird eine Kovarianzmatrix P wie folgt zerlegt: $P = D R D^{T},$
wobei R eine diagonale Matrix mit den Eigenwerten ist und D die Eigenvektoren sind. Wenn die Kovarianzmatrix nicht-positiv semidefinit ist, dann sind die Eigenwerte R(<0) negativ und werden bevorzugterweise korrigiert. Die negativen Eigenwerte können beispielsweise durch eines der folgenden Eigenwert-Verfahren korrigiert werden: das Absolute-Eigenwert-Korrekturverfahren, das Null-Eigenwert-Korrekturverfahren und das Delta-Eigenwert-Korrekturverfahren.
Bei dem Absolute-Eigenwert-Korrekturverfahren wird ein negativer Eigenwert durch seinen absoluten Wert ersetzt, sodass die durch die Eigenvektoren aufgespannten Dimensionen erhalten bleiben. Bei dem Null-Eigenwert-Korrekturverfahren wird ein negativer Eigenwert durch den Wert Null ersetzt, was die minimale Änderung darstellt, um eine positiv semidefinite Kovarianzmatrix zu erhalten. Bei dem Delta-Eigenwert-Korrekturverfahren werden negative Eigenwerte durch positive empirische Werte ersetzt. Aufgrund von Konsistenzvorteilen wird das Absolute-Eigenwert-Korrekturverfahren bevorzugt eingesetzt.
Nach Korrektur der Eigenwerte wird die Kovarianzmatrix basierend auf den korrigierten Eigenwerten und den Eigenvektoren rekonstruiert und im Rahmen des rekursiven Bayesschen Filters zur Schätzung der Kernzustandsgrößen des beweglichen Objekts eingesetzt. Eine nicht-positiv semidefinite und somit ungültige Kovarianzmatrix würde längerfristig zur Divergenz und Fehlverhalten des rekursiven Bayesschen Filters führen, was den hier beschriebene Korrekturschritt in diesem erfindungsgemäßen Verfahren unabdingbar macht, um die Fusion von mehreren, auch nach Aufstarten des beweglichen Objekts hinzugefügten Sensoren bzw. von deren Beobachtungswerten auf dynamische und effiziente Weise zu ermöglichen.
Bei dem erfindungsgemäßen Verfahren zur Schätzung von (Kern-)Zustandsgrößen eines beweglichen Objekts handelt es sich um ein rekursives Verfahren, welches die Fusion von mehreren, auch nach Aufstarten des beweglichen Objekts hinzugefügten Sensoren bzw. von deren Beobachtungswerten auf dynamische und effiziente Weise ermöglicht. Entsprechendes gilt für das Entfernen von Sensoren während der Laufzeit eines beweglichen Objekts. Die vorliegende Erfindung bietet einen robusten, modularen Ansatz für eine Multi-Sensor-Datenfusion (auch) für Sensoren, die weder dem zur Schätzung der Kernzustandsgrößen eingesetzten rekursiven Bayesschen Filters noch dem beweglichen Objekt a priori bekannt sind. Eine asynchrone und dynamische Verarbeitung der zu unterschiedlichen Zeitpunkten von den jeweiligen zusätzlichen Sensoren gemachten Beobachtungen ist ohne weiteres möglich. Dies ist insbesondere bei Langzeiteinsätzen von beweglichen Objekten, beispielsweise Roboterplattformen, von erheblichem Vorteil.
1 verdeutlicht den modularen Ansatz der vorliegenden Erfindung. 1a) zeigt einen Multi-Sensor-Datenfusionsansatz gemäß dem Stand der Technik. Die verwendete Kovarianzmatrix enthält zusätzlich zu der Kovarianz der Kernzustandsgrößen des beweglichen Objekts, die Kovarianzen der Kalibrierungszustände zweier zusätzlicher Sensoren A und B und die Kreuzkorrelationen bzw. Kreuzkovarianzen zwischen den Kernzustandsgrößen des beweglichen Objekts und den Kalibrierungszustandsgrößen der Sensoren A und B. Beim in 1a) gezeigten Stand der Technik nimmt also die Größe und Komplexität der Kovarianzmatrix mit jedem zusätzlichen Sensor zu, und es werden bei jedem Aktualisierungs-/Korrekturschritt die mit sämtlichen zusätzlichen Sensoren assoziierten Kovarianzen und Kreuzkovarianzen zusätzlich zu der Kovarianz der Kernzustandsgrößen des beweglichen Objekts aktualisiert bzw. korrigiert.
1b) veranschaulicht den modularen Sensorfusionansatz der vorliegenden Erfindung, bei welchem eine Segmentierung der Kovarianzmatrix für jeden Sensor erfolgt. Nur für denjenigen zusätzlichen Sensor, von welchem gerade Beobachtungswerte erhalten werden, wird die Kovarianzmatrix gebildet. Von daher werden im Aktualisierungsschritt nur die Kovarianz der Kernzustandsgrößen des beweglichen Objekts und die mit dem jeweils aktiven zusätzlichen Sensor assoziierte Kovarianz und assoziierten Kreuzkovarianzen bzw. Kreuzkorrelationen mit den neuen Beobachtungen dieses zusätzlichen Sensors aktualisiert/korrigiert. Entsprechendes gilt für den Propagierungsschritt. Beides, Propagieren und Aktualisieren/Korrigieren, wird modular, d.h. separat für jeden einzelnen zusätzlichen Sensor durchgeführt, wobei die Zeitdauer für die Ausführung sowohl des Propagierungsschrittes (Propagierungsphase) als auch des Aktualisierungsschrittes (Aktualisierungsphase bzw. Korrekturphase) konstant und unabhängig von der Anzahl der zusätzlichen Sensoren bleibt.
In 1b) werden - von links nach rechts - zuerst nur Beobachtungen von dem zusätzlichen Sensor A, dann nur von dem zusätzlichen Sensor B und schließlich wieder nur von dem Sensor A geliefert und für die Aktualisierung der Kovarianzmatrix verwendet. Komplexität und Größe der Kovarianzmatrix bleiben dabei vorteilhafterweise unabhängig von der Anzahl zusätzlicher Sensoren und konstant.
Bei dem erfindungsmäßen Verfahren wird das effiziente, nachträgliche Hinzufügen von zusätzlichen Sensoren durch ein Entkoppeln der Kernzustandsgrößen des beweglichen Objekts von den Kalibrierungszuständen der jeweiligen zusätzlichen Sensoren ermöglicht, wodurch die Kernzustandsgrößen des beweglichen Objekts und die Kalibrierungszustandsgrößen der jeweiligen zusätzlichen Sensoren jeweils unabhängig voneinander propagiert werden können. Bei dem erfindungsgemäßen Verfahren werden die nachträglich hinzugefügten, zusätzlichen Sensoren kontinuierlich selbst kalibriert. Dadurch kennzeichnet sich das erfindungsgemäße Verfahren durch eine hohe Flexibilität bei niedriger benötigter Rechenleistung aus.
Bei dem erfindungsgemäßen Verfahren ist somit die Komplexität insgesamt nur linear von der Anzahl der zusätzlichen Sensoren abhängig. Im Propagierungsschritt bleibt die Komplexität sogar im Wesentlichen konstant. Das erfindungsgemäße Verfahren benötigt also wesentlich weniger Rechenleistung als die aus dem Stand der Technik bekannten Verfahren und kann folglich Beobachtungen schneller verarbeiten als bekannte Verfahren, was wiederum eine genauere Navigation bzw. Regelung des beweglichen Objekts bei gleicher Rechenleistung ermöglicht. Alternativ oder zusätzlich kann die eingesparte Rechenleistung beispielsweise eingesetzt werden, die Reichweite des beweglichen Objekts zu erhöhen. Alternativ können leistungsschwächere und in der Regel kostengünstigere Prozessoren eingesetzt werden.
Gleichzeitig bleiben Konsistenz und Beobachtbarkeit des eingesetzten rekursiven Bayesschen Filters bei dem erfindungsgemäßen Verfahren erhalten, dies insbesondere dadurch, dass die Kovarianzmatritzen trotz modularer Behandlung konsistent und positiv semidefinit gehalten werden. Mit Hilfe der Erfindung können zusätzliche, nach dem Aufstarten hinzugefügte Sensoren effizient initialisiert und in Bezug auf das jeweilige bewegliche Objekt kalibriert werden. Insgesamt konnte bei dem erfindungsgemäßen Verfahren eine Effizienzsteigerung von 30% gegenüber bekannten Verfahren beobachtet werden. Eine dezentralisierte Implementierung des erfindungsgemäßen Verfahrens, die weniger Bandbreite als eine zentralisierte Implementierung benötigt, ist ohne weiteres möglich. Kalibrierungszustandsgrößen der zusätzlichen Sensoren können dabei lokal gespeichert und propagiert und aktualisiert werden.
Die vorliegende Erfindung kann beispielsweise auf den technischen Gebieten der Robotik, Automation und in der Automobil- und Fahrzeugindustrie eingesetzt werden, wobei der Begriff „Fahrzeug“ neben Automobilen auch Luft- und Wasserfahrzeuge umfasst. Wegen seiner vergleichsweise geringen Komplexität eignet sich das erfindungsgemäße Verfahren besonders für den Einsatz in autonomen Fahrzeugen mit begrenzten Dimensionen und begrenzter Rechenleistung wie beispielsweise Drohnen (unbemanntes Flugobjekt, UAV - unmanned aerial vehicles).
Aufgrund der oben beschriebenen Segmentierung der Kovarianzmatrix für jeden zusätzlich hinzugefügten Sensor und der damit einhergehenden Eignung für die Verarbeitung asynchroner Beobachtungen eignet sich die vorliegende Erfindung auch besonders für die Verarbeitung verzögerter Beobachtungen bzw. Messdaten, nicht ursprünglich geplante, „außerhalb der Reihe“ liegende Aktualisierungen/Korrekturen der Kovarianzmatrix und die Überwachung von Sensorgesundheit (sensor health).
Die vorliegende Erfindung betrifft weiter ein computerlesbares Speichermedium, welches Befehle umfasst, welche bei der Ausführung durch einen Computer diesen veranlassen, das erfindungsgemäße Verfahren auszuführen. Mit dem computerlesbaren Speichermedium werden die Vorteile des erfindungsgemäßen Verfahrens realisiert.
Ferner betrifft die vorliegende Erfindung ein System zur Datenverarbeitung, welches Mittel zur Ausführung des erfindungsgemäßen Verfahrens umfasst. Die Mittel sind ausgestaltet, eine oder mehrere Sensorkomponenten zu instantiieren, welche einen oder mehrere nach Aufstarten eines beweglichen Objekts hinzugefügte zusätzliche Sensoren repräsentieren, und ferner eine Filterkomponente zu instantiieren, welche einen rekursiven Bayesschen Filter repräsentiert, wobei die Filterkomponente abhängig von der Ausführung des rekursiven Bayesschen Filter ist.
Mit dem erfindungsgemäßen Datenverarbeitungssystem lässt sich eine modulare Implementierung des erfindungsgemäßen Verfahrens realisieren, die es auf einfache und effiziente Weise ermöglicht, zusätzliche Sensoren durch Instantiierung entsprechender Komponenten (auch Module, Instanzen genannt) zu berücksichtigen und wieder zu entfernen, wenn die jeweiligen Sensoren entfernt werden. Ferner ermöglicht das erfindungsgemäße Datenverarbeitungssystem einen schnellen und unkomplizierten Wechsel des verwendeten rekursiven Bayesschen Filter durch einfache Änderung bzw. Neuinstantiieren der ihn repräsentierenden Filterkomponente.
Figurenliste
Weitere vorteilhafte Ausgestaltungen der Erfindung ergeben sich aus den abhängigen Ansprüchen und den anhand der Zeichnungen nachfolgend dargestellten Ausführungsbeispielen. Es zeigen:

1 einen schematischen Vergleich der Kovarianzmatrix eines für die Schätzung von Kernzustandsgrößen eingesetzten als Kalman-Filter ausgeführten rekursiven Bayesschen Filters nach dem Stand der Technik (1a)) und nach dem erfindungsgemäßen Verfahren (1b)),
2 ein Ausführungsbeispiel des erfindungsgemäßen Verfahrens mit beispielhaft zwei zusätzlichen Sensoren,
3 eine beispielhafte Kalibrierung von Kalibrierungszustandsgrößen zusätzlicher Sensoren gemäß dem erfindungsgemäßen Verfahren in schematischer Darstellung,
4 eine beispielhaftes Flugprofil eines Anwendungsbeispiels des erfindungsgemäßen Verfahrens in Form einer Drohne,
5 eine beispielhafte Implementierung des erfindungsgemäßen Verfahrens und
6 einen beispielhaften Aufbau eines Zwischenspeichereintrags.

1 ist im Zusammenhang mit der Beschreibung der Vorteile der Erfindung in der obigen Beschreibungseinleitung beschrieben. Es wird auf die dortigen Textteile verwiesen.
BESCHREIBUNG DER AUSFÜHRUNGSBEISPIELE
Zur Steuerung und Regelung eines beweglichen Objekts werden typischerweise Kernzustandsgrößen des beweglichen Objekts vorab definiert, die die wesentlichen Variablen des beweglichen Objekts beschreiben. Für ein bewegliches Objekts, welches beispielsweise eine inertiale Messeinheit (IMU) umfasst bzw. an welchem eine IMU vorgesehen ist, sind die Kernzustandsgrößen insbesondere Navigationszustandsgrößen wie die Position des objektbezogenen Koordinatensystems (Körperrahmen, body frame) p_WI, welche in Bezug auf ein Referenzkoordinatensystem (world frame, Weltkoordinatensystem) definiert ist, die Geschwindigkeit v_WI der inertialen Messeinheit in Bezug auf das Referenzkoordinatensystem, die Orientierung der inertialen Messeinheit in Bezug auf das Referenzkoordinatensystem q_MI. das gyroskopische Bias b_ω und das Beschleunigungsbias b_a. Das objektbezogene Koordinatensystem und das Referenzkoordinatensystem sind gleichhändig, vorzugsweise rechtshändig definiert. Die Position und Orientierung des objektbezogenen Koordinatensystems entspricht bevorzugt der Position und Orientierung der inertialen Messeinheit in Bezug auf das Referenzkoordinatensystem. Es ergibt sich die folgende (Kern-)Zustandsgrößenmatrix X_C: $X_{C} = {[p_{W I}^{T}, v_{W I}^{T}, q_{W I}^{T}, b_{ω}^{T}, b_{α}^{T}]}^{T} .$
Bewegliche Objekte können grundsätzlich durch mathematische, zeitabhängige Modelle abgebildet werden, welche die Kernzustandsgrößen und deren Kovarianzen auf den nächsten Zeitpunkt propagieren und die auf den von der inertialen Messeinheit gemessenen Kernzustandsgrößen basieren. Die folgenden Differentialgleichungen beschreiben ein derartiges mathematisches Modell, auch Zustandsraummodell genannt, welches die Kernzustandsgrößen abbildet: ${\dot{p}}_{W I} = v_{W I}$
${\dot{v}}_{W I} = R_{(q_{W I})} (a_{m} - b_{α} - n_{α}) - g$
${\dot{q}}_{W I} = \frac{1}{2} Ω (ω_{m} - b_{w} - n_{w}) q_{W I}$
${\dot{b}}_{ω} = n_{b_{ω}}, {\dot{b}}_{a} = n_{b_{a}} .$
Dabei ist a_m ein Messwert der inertialen Messeinheit der linearen Beschleunigung des beweglichen Objekts, b_a das Bias der linearen Beschleunigung des beweglichen Objekts, n_a Messrauschen der linearen Beschleunigung, g die Gravitätskonstante, ω_m ein Messwert der inertialen Messeinheit der Winkelgeschwindigkeit des beweglichen Objekts, b_ω das Bias der Winkelgeschwindigkeit des beweglichen Objekts und n_ω das Messrauschen der Winkelgeschwindigkeit. Ferner sind R und Ω(ω) wie folgt definiert, wobei Ω(ω) die rechtsseitige Quaternionen-Multiplikations-Matrix ist: $R = [\begin{matrix} q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2} & 2 (q_{x} q_{y} + q_{w} q_{z}) & 2 (q_{x} q_{z} + q_{w} q_{y}) \\ 2 (q_{x} q_{y} + q_{w} q_{z}) & q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2} & 2 (q_{y} q_{z} + q_{w} q_{x}) \\ 2 (q_{x} q_{z} + q_{w} q_{y}) & 2 (q_{y} q_{z} + q_{w} q_{x}) & q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2} \end{matrix}]$
$Ω (ω) ≜ {[ω]}_{R} = [\begin{matrix} 0 & - ω^{T} \\ ω & {[ω]}_{x} \end{matrix}] = [\begin{matrix} 0 & - ω_{x} & - ω_{y} & - ω_{z} \\ ω_{x} & 0 & ω_{z} & - ω_{y} \\ ω_{y} & - ω_{z} & 0 & ω_{x} \\ ω_{z} & ω_{y} & - ω_{x} & 0 \end{matrix}]$
In Bezug auf R gilt: $_{c} p_{A B} = R {(_{q_{C A}})}_{A} p_{A B}$
$R_{(q_{C A})} \equiv R_{C A}$
Falls während der Laufzeit des beweglichen Objekts zusätzliche Sensor hinzugefügt bzw. berücksichtigt werden sollen, so sind diese in der Regel nicht auf das bewegliche Objekt ausgerichtet. Derartige zusätzliche Sensoren bzw. deren extrinsische Eigenschaften können als Kalibrierungszustandsgrößen in das obige Modell eingefügt werden, wobei die Kalibrierungszustandsgrößen geschätzt werden können. Werden zum Beispiel zwei zusätzliche Sensoren S₁ und S₂ während der Laufzeit des beweglichen Objekts, d.h. nach dessen Aufstarten, zu diesem hinzugefügt, können die Kernzustandsgrößen X_C wie folgt um die Kalibrierungszustandsgrößen X_S1 und X_S2 erweitert werden: $X = [X_{C}; X_{S_{1}}; X_{S_{2}}] .$
Die Beobachtung der zusätzlichen Sensoren bzw. von deren Kalibrierungszustandsgrößen, beispielsweise mittels eines als Extended-Kalman-Filter ausgeführten rekursiven Bayesschen Filters, führt Kreuzkorrelationen zwischen den Kernzustandsgrößen und den Kalibrierungszustandsgrößen der zusätzlichen Sensoren und somit zu Kreuzkovarianzen in der Kovarianzmatrix P des rekursiven Bayesschen Filters. Nach Beobachtungen durch die während der Laufzeit hinzugekommenen zusätzlichen Sensoren S₁ und S₂ ergibt sich die folgende Kovarianzmatrix P: $P = [\begin{matrix} P_{C} & P_{CS1} & P_{CS2} \\ P_{S1C} & P_{S1} & 0 \\ P_{S2C} & 0 & P_{S2} \end{matrix}],$
wobei P_CS2 = (P_S2C)^T ist (entsprechend für den zusätzlichen Sensor S₁) und die Gleichung (7) auf der Annahme basiert, dass die zusätzlichen Sensoren S₁ und S₂ unabhängig voneinander sind, d.h. sich nicht gegenseitig beeinflussen. Dies ist beispielsweise für einen GNSS-Sensor (GNSS - Globales Navigationssatellitensystem) und einen als Kamera ausgebildeten Sensor der Fall. Position und positionsmäßige Kalibrierung des GNSS-Sensors (oder eines GPS-Sensors) mit Bezug auf das bewegliche Objekt (z.B. einem Fahrzeug) sind unabhängig von der Position der Kamera an dem beweglichen Objekt. Entsprechendes gilt für ein Magnetometer mit drei Freiheitsgraden und einen GNSS-Sensor mit drei Freiheitsgraden. Die rotationsmäßige Kalibrierung des Magnetometers in Bezugs auf das objektbezogene Koordinatensystem, in welchem die Position der inertialen Messeinheit definiert ist, und die Translation des GNSS-Sensors in Bezug auf das objektbezogene Koordinatensystem bzw. die inertiale Messeinheit stehen in keiner physikalischen Beziehung zueinander. Selbst wenn aus analytischen Gesichtspunkten Kreuzkorrelationen bestehen sollten, so sind diese und die mit deren Berücksichtigung einhergehende Genauigkeitsabnahme im Vergleich zu der Zunahme der Rechengeschwindigkeit bei Weglassen dieser Kreuzkorrelationen vernachlässigbar.
2 zeigt ein Ausführungsbeispiel eines erfindungsgemäßes Verfahrens für den Fall von zwei zusätzlichen, nach dem Aufstarten hinzugefügten Sensoren 1 und 2. Bespielhaft ist der Zeitraum zwischen den Zeitpunkten t = 0 und t = 31 dargestellt. In 2 beziehen sich Felder mit einer Schraffur mit positiver Steigung auf den zusätzlichen Sensor 1, Felder mit einer Schraffur mit negativer Steigung auf den zusätzlicher Sensor 2 und Felder mit Rauten-Schraffur auf die Kernzustandsgrößen. Die Blöcke 3, 6 und 9 der 2 zeigen jeweils die Kovarianzmatrix zu unterschiedlichen Zeitpunkten unterteilt in ihre einzelnen (Kovarianz- und Kreuzkovarianz-)Segmente, die gemäß dem erfindungsgemäßen Verfahren separat verarbeitet werden (vgl. auch 1). Die in 2 beispielhaften Beobachtungszeitpunkte der Sensoren 1 und 2 sind von den Messdaten der eingesetzten zusätzlich Sensoren abhängig.
Zum Zeitpunkt t=0 wird das bewegliche Objekt aufgestartet, und der zum Schätzen der Zustandsgrößen eingesetzte rekursive Bayessche Filter und die vorab definierten Kernzustandsgrößen des beweglichen Objekts werden initialisiert (Block 1 in 2).
Nach dem Aufstarten werden während der Laufzeit des beweglichen Objekts zwei zusätzliche Sensoren hinzugefügt: Der Sensor 1 wird zum Zeitpunkt t = 5 hinzugefügt. Der Sensor 2 wird zum Zeitpunkt t = 9 hinzugefügt. Wird festgestellt, dass zusätzliche Sensoren hinzugefügt worden sind bzw. werden Beobachtungswerte von diesen zusätzlichen Sensoren gebildet, so werden die Kovarianz der Kalibrierungszustandsgrößen des j eweiligen zusätzlichen Sensors und die Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des jeweiligen zusätzlichen Sensors mit Hilfe der zum Zeitpunkt der Hinzufügung gebildeten Beobachtungswerte initialisiert bzw. kalibriert. D.h. im vorliegenden Bespiel werden die mit dem zusätzlichen Sensor 1 assoziierte Kovarianz und assoziierten Kreuzkovarianzen zum Zeitpunkt t = 5 initialisiert/kalibriert (ebenfalls Block 1 der 2), während die mit dem zusätzlichen Sensor 2 assoziierte Kovarianz und assoziierten Kreuzkovarianzen zum Zeitpunkt t = 9 initialisiert/kalibriert werden (ebenfalls Block S1 der 2). Handelt es sich bei dem zusätzlichen Sensor 1 beispielsweise um einen GPS-Sensor, so wird zum Zeitpunkt t = 5 die Position des GPS-Sensors im objektbezogenen Koordinatensystems des beweglichen Objekts kalibriert.
Unabhängig von der Initialisierung der zusätzlichen Sensoren 1 und 2 werden die Kernzustandsgrößen und deren Kovarianz bis einen Zeitschritt vor denjenigen nächsten Zeitpunkt propagiert, an welchem einer der zusätzlichen Sensoren 1 und 2 nächste Beobachtungswerte ausgibt/bildet. Im vorliegenden Beispiel ist dies der Zeitpunkt t = 20, denn zum Zeitpunkt t = 21 bildet der zusätzliche Sensor 1 erneut Beobachtungswerte (Block 2 in 2).
Zum Zeitpunkt t = 21 werden nun die mit dem zusätzlichen Sensor 1 assoziierte neueste Kovarianz P_S1 und die assoziierten neuesten Kreuzkovarianzen P_CS1 mit der neuesten Kovarianz P_C der Kernzustandsgrößen des beweglichen Objekts zur Bildung der Kovarianzmatrix P kombiniert. Dabei sind die mit dem zusätzlichen Sensor 1 assoziierte neueste Kovarianz P_S1 und die neuesten Kreuzkovarianzen P_CS1 die zum Zeitpunkt t = 5 initialisierte Kovarianz P_S1 und die zum Zeitpunkt t = 5 initialisierten Kreuzkovarianzen P_CS1. Die Kovarianz P_C der Kernzustandsgrößen entspricht der auf den Zeitpunkt t = 20 propagierten Kovarianz P_C (vgl. Block 3 in 2).
Vorzugsweise wird für die Bildung der Kovarianzmatrix P die mit dem zusätzlichen Sensor 1 assoziierte neueste Kovarianz P_S1 und die assoziierten neuesten Kreuzkovarianzen P_CS1 wie oben beschrieben mit Hilfe einer Reihe von Zustandsübergangsmatrizen auf den gleichen Zeitpunkt wie die Kovarianz der Kernzustandsgrößen, d.h. auf t = 20, propagiert. Ferner wird die Kovarianzmatrix P bevorzugterweise, insbesondere durch Einsatz eines Eigenwert-Verfahrens, in eine positiv semidefinite Kovarianzmatrix korrigiert, falls erforderlich, wie oben beschrieben. Schließlich wird die gebildete Kovarianzmatrix (bzw. deren Segmente) zwischengespeichert (ebenfalls Block 3 der 2).
Mit der in Block 3 gebildeten Kovarianzmatrix wird nun in Block 4 mit den zum Zeitpunkt t = 21 gebildeten Beobachtungen des zusätzlichen Sensors 1 eine Aktualisierung bzw. Korrektur der Kovarianzmatrix durchgeführt. Mit der aktualisierten Kovarianzmatrix werden dann Kernzustandsgrößen geschätzt. Danach werden die mit dem zusätzlichen Sensor 1 assoziierte Kovarianz und die assoziierten Kreuzkovarianzen von der Kovarianz der Kernzustandsgrößen getrennt (Block 4 in 2).
In Block 5 von 2 werden die Kernzustandsgrößen bzw. deren Kovarianz bis einen Zeitschritt vor Erhalt der nächsten Beobachtungswerte zum Zeitpunkt t = 26 propagiert. D.h. die Kovarianz der Kernzustandsgrößen wird vom Zeitpunkt 22 auf den Zeitpunkt t = 25 propagiert. Im vorliegenden Beispiel werden die nächsten Beobachtungswerte zum Zeitpunkt t = 26 von dem zusätzlichen Sensor 2 ausgegeben.
In Block 6 wird zum Aktualisieren der Kovarianzmatrix mittels der zum Zeitpunkt t = 26 erhaltenen Beobachtungswerte die Kovarianzmatrix erneut gebildet aus: der auf den Zeitpunkt t = 25 propagierten Kovarianz der Kernzustandsgrößen und den zum Zeitpunkt t = 9 initialisierten, mit dem zusätzlichen Sensor 2 assoziierten Kovarianz und assoziierten Kreuzkovarianzen (Block 6 von 2). Auch in Block 6 wird vorzugsweise für die Bildung der Kovarianzmatrix die mit dem zusätzlichen Sensor 2 assoziierte neueste Kovarianz und assoziierten neuesten Kreuzkovarianzen mit Hilfe einer Reihe von Zustandsübergangsmatrizen auf den gleichen Zeitpunkt wie die Kovarianz der Kernzustandsgrößen, d.h. auf t = 25, propagiert. Ferner wird die Kovarianzmatrix bevorzugterweise, insbesondere durch Einsatz des Eigenwert-Verfahrens, in eine positiv semidefinite Kovarianzmatrix korrigiert, falls erforderlich. Schließlich wird die gebildete Kovarianzmatrix (bzw. deren Segmente) zwischengespeichert (ebenfalls Block 6 der 2).
In Block 7 wird die in Block 6 gebildete Kovarianzmatrix dann mit Hilfe der zum Zeitpunkt t = 26 erhaltenen Beobachtungswerte des zusätzlichen Sensors 2 aktualisiert bzw. korrigiert, und die Kernzustandsgrößen werden mit der aktualisierten Kovarianzmatrix geschätzt. Danach werden die mit dem zusätzlichen Sensor 2 assoziierte Kovarianz und assoziierten Kreuzkovarianzen wieder von der Kovarianz der Kernzustandsgrößen getrennt (Block 7 in 2).
In den Blöcken 8 bis 10 von 2 setzt sich das erfindungsgemäße Verfahren wie oben beschrieben fort. Als nächstes werden zum Zeitpunkt t = 31 Beobachtungswerte vom zusätzlichen Sensor 1 erhalten. Entsprechend wird die Kovarianz der Kernzustandsgrößen vom Zeitpunkt t = 27 auf den Zeitpunkt t = 30 propagiert. Aus der derart propagierten Kovarianz der Kernzustandsgrößen und den neuesten mit dem Sensor 1 assoziierten Kovarianz und Kreuzkovarianzen (vom Zeitpunkt t = 21; siehe Beschreibung von Block 4) wird die Kovarianzmatrix gebildet, welche dann in Block 10 mit den Beobachtungswerten von zusätzlichen Sensor 1 zum Zeitpunkt t = 31 aktualisiert bzw. korrigiert und zur Schätzung der Kernzustandsgrößen herangezogen wird.
Auch in Block 9 wird vorzugsweise für die Bildung der Kovarianzmatrix die mit dem Sensor 1 assoziierte neueste Kovarianz und assoziierten neuesten Kreuzkovarianzen mit Hilfe der Reihe von Zustandsübergangsmatrizen auf den gleichen Zeitpunkt wie die Kovarianz der Kernzustandsgrößen, d.h. auf t = 30, propagiert. Ferner wird die Kovarianzmatrix bevorzugterweise, insbesondere durch Einsatz des Eigenwert-Verfahrens, in eine positiv semidefinite Kovarianzmatrix korrigiert, falls erforderlich. Schließlich wird die gebildete Kovarianzmatrix (bzw. deren Segmente) vor ihrer Aktualisierung in Block 10 zwischengespeichert (Block 9 der 2).
D.h. bei dem erfindungsgemäßen Verfahren werden vorteilhafterweise die Kernzustandsgrößen (bzw. deren Kovarianz) getrennt von den Kalibrierungszustandsgrößen der Sensoren (bzw. deren Kovarianz und Kreuzkovarianzen mit den Kernzustandsgrößen) propagiert. Die Kalibrierungszustandsgrößen der zusätzlichen Sensoren (bzw. deren Kovarianz und Kreuzkovarianzen mit den Kernzustandsgrößen) werden wiederum getrennt von den Kernzustandsgrößen initialisiert/kalibriert und getrennt voneinander aktualisiert. Mit der vorliegenden Erfindung können also nach dem Aufstarten hinzugefügte zusätzliche Sensoren berücksichtigt werden, ohne die Kovarianzen und mit den Kernzustandsgrößen assoziierten Kreuzkovarianzen anderer Sensoren zu beeinflussen.
3 zeigt schematisch eine Initialisierung/(Selbst-)Kalibrierung von beispielhaften Kalibrierungszustandsgrößen zusätzlicher Sensor, welche bei einem beweglichen Objekt 10, welches eine inertiale Messeinheit IMU umfasst, nachträglich hinzugefügt werden. Das bewegliche Objekt 10 und die inertiale Messeinheit IMU (inertial measurement unit) sind in einem Referenzkoordinatensystem „Nav World“ angeordnet. Nach dem Aufstarten des beweglichen Objekts 10 werden zusätzliche Sensoren in Form eines Sichtsensors („Vision“, beispielsweise eine Kamera), eines Drucksensors („Pressure“, beispielsweise ein Barometer) und eines GPS-Sensors hinzugefügt. Die Beobachtungen dieser Sensoren beziehen sich auf die jeweiligen Sensorkoordinatensysteme („Vision Ref.“, „Pressure Ref.“, „GPS Ref.“), wie in 3 durch durchgezogene Linien angegeben ist. Bei dem Koordinatensystem „GPS Ref.“ des GPS-Sensors handelt es sich um ein vorgegebenes globales Koordinatensystem, das in Bezug auf das Referenzkoordinatensystem „Nav World“ fixiert und in Bezug auf dieses definiert ist (strich-punktierte Linie in 3). Die gestrichelten Linien zwischen den Sensorkoordinatensystemen „Vision Ref.“ und „Pressure Ref“ geben deren Position und Orientierung in Bezug auf das Referenzkoordinatensystem „Nav World“ an. Die gestrichelte Linie zwischen der IMU und dem Referenzkoordinatensystem gibt die Position und Orientierung des objektbezogenen Koordinatensystems in Bezug auf das Referenzkoordinatensystem „Nav World“ an. Die gepunkteten Linien zwischen den Sicht-, Druck- und GPS-Sensoren und der das bewegliche Objekt 10 repräsentierenden IMU geben die im Rahmen des erfindungsgemäßen Verfahrens erfolgende, oben im Zusammenhang mit Block 1 der 2 beschriebene Initialisierung/Kalibrierung der Kalibrierungszustandsgrößen der zusätzlichen Sensoren in Bezug auf das objektbezogene Koordinatensystem des beweglichen Objekts 10 bzw. von dessen IMU an.
4 zeigt ein zu 3 gehörendes Anwendungsbeispiel, bei welchem das bewegliche Objekt als Drohne 10 ausgebildet ist. 4 zeigt ein beispielhaftes Flugprofil der Drohne 10. Das Flugprofil enthält verschiedene Phasen 1, 2, 3, 4, 5, in welchen unterschiedliche zusätzliche Sensoren (bzw. deren Messdaten/Beobachtungswerte) hinzugefügt oder entfernt werden. So wird in der Phase 1 (Start von der Landeplattform (landing pad)) und der Phase 2 (Geradeausflug) der Sichtsensor eingesetzt. In der Phase 2 kommen zusätzlich der Drucksensor (Barometer) und der GPS-Sensor hinzu, welche auch in der Phase 3 (Wenden) eingesetzt werden. In der Phase 3 wird der Sichtsensor dagegen nicht eingesetzt und ist ausgeschaltet. Er wird erst wieder bei der Rückkehr in die Phase 2 zugeschaltet und auf die jetzige Position der Drohne 10 kalibriert/initialisiert. Danach werden nach einer kurzen Überlappungsperiode der Drucksensor und der GPS-Sensor deaktiviert, sodass sie beim Eintritt in die finale Phase 5 nicht mehr berücksichtigt werden. In der Phase 5 erfolgt in diesem Beispiel also die Landung auf der Landeplattform allein mittels des Sichtsensor. Anstelle eines GPS-Sensors kann selbstverständlich auch ein anderer GNSS-Sensor eingesetzt werden.
Ein zusätzlicher Sensor kann auch als eine andere Drohne bzw. ein auf einer anderen Drohne vorgesehener oder mit einer solchen assoziierter Sensor ausgebildet sein, z.B. als eine an einer anderen Drohne installierte Kamera, welche diejenige Drohne, deren Kernzustandsgrößen geschätzt werden sollen, filmt.
5 zeigt eine beispielhafte modulare Implementierung des erfindungsgemäßen Verfahrens. Die Implementierung kann mittels Softwarekomponenten und/oder Hardwarekomponenten erfolgen. Jede der Komponenten (auch Modul, Instanz oder Einheit genannt) ist vorzugsweise eigenständig mit klar definierten Schnittstellen ausgeführt, um eine einfache und effiziente Austauschbarkeit der jeweiligen Komponente zu ermöglichen.
Bevorzugt ist eine Kernlogikkomponente 20 (auch Hauptlogikkomponente genannt) vorgesehen, welche bei der Ausführung des erfindungsgemäßen Verfahrens für den organisatorischen Teil verantwortlich ist und die Schnittstelle zwischen Zwischenspeicher 22 und den Sensorkomponenten 24, 26, 28 bildet. Die Sensorkomponenten 24 und 26 repräsentieren bzw. instantiieren zusätzliche, nach dem Aufstarten zugeschaltete Sensoren wie beispielsweise einem GPS-Sensor 24 und einem Sichtsensor 26 (z.B. eine Kamera). Die Sensorkomponente 28 repräsentiert vorzugsweise einen bereits beim Aufstarten zugeschalteten Sensor wie beispielsweise einen Propagierungssensor, d.h. einen Sensor, der für die Kernzustandsgrößen relevante Beobachtungen liefert. Handelt es sich bei den Kernzustandsgrößen um Navigationszustandsgrößen so kann der Propagierungssensor beispielsweise als IMU ausgebildet sein.
Die Kernlogikkomponente 20 ist insbesondere derart ausgestaltet, dass sie den Nutzen der von den Sensoren gebildeten Beobachtungswerte überprüft und diese gegebenenfalls nicht für eine Schätzung der Zustandsgrößen berücksichtigt, wenn kein Nutzen mehr gegeben ist, beispielsweise bei stark verzögerten Beobachtungen/Messwerten, deren Verzögerung eine vordefinierte Zeitdauer überschreitet oder deren Beobachtungszeitpunkt (Zeitstempel) älter ist als der letzte Zwischenspeichereintrag ist.
6 zeigt einen bespielhaften Eintrag im Zwischenspeicher (buffer) 22 für einen zusätzlichen Sensor. Neben einer Identifikation des jeweiligen zusätzlichen Sensors sind im Zwischenspeicher der Zeitpunkt der letzten Beobachtungen (Zeitstempel) und die folgenden an diesem Zeitstempel ermittelten Daten/Werte hinterlegt: die Kernzustandsgrößen, die Kovarianz der Kernzustandsgrößen, die Kalibrierungszustandsgrößen des j eweiligen zusätzlichen Sensors, die Kovarianz der Kalibrierungszustandsgrößen, die Kreuzkovarianzen zwischen den Kernzustandsgrößen und den Kalibrierungszustandsgrößen und die Zustandsübergangsmatrizen. Ferner sind Metadaten hinterlegt.
Wieder bezugnehmend auf 5 ist bevorzugt ein Sensormanager 30 vorgesehen, welcher bevorzugterweise über eine Liste sämtlicher Sensoren verfügt und auf einem höheren Level für die Verwaltung der Sensorkomponenten 24, 26, 28 zuständig ist.
Ferner ist vorzugsweise eine eigenständige Kernzustandsgrößenkomponente 32 vorgesehen, an welche die Kernlogikkomponente 20 Beobachtungen des bereits beim Aufstarten des beweglichen Objekts zugeschalteten Sensors 28 weiterleitet. Die Kernzustandsgrößenkomponente 32 ist ausgestaltet, basierend aus diesen Beobachtungen die Kernzustandsgrößen (Kemzustandsgrößenvektor) und deren Kovarianz zu propagieren, wie oben beschrieben ist. Die propagierten Kernzustandsgrößen und deren Kovarianzen werden in dem Zwischenspeicher 22 gespeichert.
Kommen aktuelle Beobachtungen von einem zusätzlichen, nachträglich hinzugefügten Sensor, so berechnet die Kernzustandsgrößenkomponente 32 außerdem individuelle Zustandsübergangsmatrizen für jeden einzelnen Propagationsschritt. Die individuellen Zustandsübergangsmatrizen werden dann ebenfalls in dem Zwischenspeicher 22 hinterlegt.
Bei Empfang von aktuellen Beobachtungswerten von einem zusätzlichen Sensor fordert die Kernlogikkomponente 20 den letzten bzw. neuesten Eintrag in dem Zwischenspeicher 22 an und berechnet die Reihe der Zustandsübergangsmatrizen beginnend vom Zeitstempel des abgerufenen Zwischenspeichereintrags bis einen Zeitschritt vor den Zeitpunkt der aktuellen Beobachtungen. Die dem jeweiligen zusätzlichen Sensor entsprechende Sensorkomponente 24, 26 propagiert die aus dem Zwischenspeicher 22 abgerufene Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die abgerufenen Kreuzkovarianzen zwischen Kalibrierungszustandsgrößen und Kernzustandsgrößen mit Hilfe der berechneten Reihe von Zustandsübergangsmatrizen auf einen Zeitschritt vor den Beobachtungszeitpunkt der aktuellen Beobachtungswerte.
Aus dieser propagierten Kovarianz der Kalibrierungszustände des zusätzlichen Sensors und den entsprechend propagierten, mit dem zusätzlichen Sensor assoziierten Kreuzkovarianz und der propagierten Kovarianz der Kernzustandsgrößen wird dann, vorzugsweise in der Kernlogikkomponente 20, die Kovarianzmatrix gebildet, welche dann, ebenfalls durch die Kernlogikkomponente 20, gegebenenfalls mit dem Eigenwert-Verfahren korrigiert wird, um eine positive semidefinite Kovarianzmatrix zu erhalten, die dann an die jeweilige Sensorkomponente 24, 26 weitergegeben wird. Danach wird die Kovarianzmatrix mit den neuesten Beobachtungen des jeweiligen zusätzlichen Sensors in der mit diesem assoziierten Sensorkomponente 24, 26 aktualisiert/korrigiert. Vorteilhafterweise können dabei zusätzliche statische Tests wie beispielsweise ein x²-Test von der jeweiligen Sensorkomponente 24, 26 durchgeführt werden. Die in der jeweiligen Sensorkomponente 24, 26 aktualisierte Kovarianzmatrix bzw. deren (Kovarianz- und Kreuzkovarianz-)Segmente werden danach via den Sensormanager 30 an die Kernlogikkomponente 20 übermittelt, welche die Segmente an den Zwischenspeicher 22 zur Speicherung weiterleitet.
Ferner ist eine Filterkomponente 36 vorgesehen, welche einen rekursiven Bayesschen Filter repräsentiert bzw. instantiiert. Beispielhaft repräsentiert die Filterkomponente 36 einen Kalman-Filter, insbesondere einen Extended-Kalman-Filter. Durch die modulare Implementierung/Instantiierung des rekursiven Bayesschen Filters kann dessen konkrekte Ausgestaltung auf einfache und effiziente Weise beispielsweise von einem Extended-Kalman-Filter in einen anderen rekursiven Bayesschen Filter, beispielsweise einen sogenannten Unscented Kalman-Filter, geändert werden.
Zur Schätzung der Kernzustandsgrößen werden die Sensorkomponenten 24, 26, 28 und die Filterkomplemente 34, vorzugsweise softwaretechnisch, aus einer Abstrakte-SensorKomponente 34 abgeleitet. Dies hat den Vorteil, dass einfach und effizient Sensorkomponenten für zusätzliche Sensoren hinzugefügt, entfernt und ersetzt werden können.
Die in 5 gezeigte Implementierung des erfindungsgemäßen Verfahrens hat den Vorteil, dass die Kernzustandslogikkomponente 20 keinerlei Kenntnisse bezüglich der Sensoren zu haben braucht, sozusagen unabhängig von diesen implementiert sein kann. Alles „Sensorenwissen“, wie z.B. mathematische Definitionen der jeweiligen Sensormodelle und die für die jeweiligen Sensoren einzusendenden Verfahren zur Initialisierung/Kalibrierung, Erzeugung und Handhabung von deren Kalibrierungszuständen einschließlich deren Propagieren, Aktualisieren und Korrektur, ist in den Sensorkomponenten 24, 26, 28 enthalten. Dies erlaubt vorteilhafterweise das effiziente und elegante Hinzufügen bzw. Zuschalten von zusätzlichen Sensoren auch nach dem Aufstarten. Jede Sensorkomponente ist eigenständig und berechnet selbständig die mit ihr assoziierten Kovarianz und Kreuzvarianzen sowohl bei der Initialisierung, als auch beim Propagieren, als auch beim Aktualisieren/Korrigieren, wobei insbesondere bei der Initialisierung die zu deren Zeitpunkt geltenden Kernzustandsgrößen via die Kernlogikkomponente 20 erhalten und somit berücksichtigt werden können.
ZITATE ENTHALTEN IN DER BESCHREIBUNG
Diese Liste der vom Anmelder aufgeführten Dokumente wurde automatisiert erzeugt und ist ausschließlich zur besseren Information des Lesers aufgenommen. Die Liste ist nicht Bestandteil der deutschen Patent- bzw. Gebrauchsmusteranmeldung. Das DPMA übernimmt keinerlei Haftung für etwaige Fehler oder Auslassungen.
Zitierte Patentliteratur

WO 2015/105597 A1 [0012]
US 9031782 B1 [0012]
US 7181323 B1 [0012]
US 10274318 B1 [0012]

Claims

Computer-implementiertes Verfahren zur Schätzung von Zustandsgrößen eines beweglichen Objekts, gekennzeichnet durch die folgenden Schritte: a) Initialisieren eines rekursiven Bayesschen Filters zum Schätzen von vorab definierten Kernzustandsgrößen des beweglichen Objekts, b) Beobachten von technischen Eigenschaften des beweglichen Objekts mit Hilfe von einem oder mehreren Sensoren unter Bildung von Beobachtungswerten, c) zeitliches Propagieren der Kernzustandsgrößen des beweglichen Objekts und einer Kovarianz der Kernzustandsgrößen mittels eines Zustandsgrößenmodells des rekursiven Bayesschen Filters unter Verwendung derjenigen Beobachtungswerte, welche mit Hilfe von einem oder mehreren Sensoren gebildet worden sind, die seit Aufstarten des beweglichen Objekts eingesetzt werden, d) Feststellen, ob Beobachtungswerte mit Hilfe eines nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors gebildet werden, e) falls in Schritt d) festgestellt wurde, dass Beobachtungswerte mit Hilfe eines nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors gebildet werden, e1) Initialisieren einer Kovarianz von Kalibrierungszustandsgrößen des zusätzlichen Sensors und von Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe der zu einem ersten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte, e2) bei Bilden von Beobachtungswerten durch den zusätzlichen Sensor an einem nach dem ersten Zeitpunkt liegenden zweiten Zeitpunkt: Formen einer Kovarianzmatrix des rekursiven Bayesschen Filters aus der auf einen einen Zeitschritt vor dem zweiten Zeitpunkt liegenden Zeitpunkt propagierten Kovarianz der Kernzustandsgrößen des beweglichen Objekts, der neuesten Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und den neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors, e3) Aktualisieren der Kovarianzmatrix mit Hilfe der zu dem zweiten Zeitpunkt von dem zusätzlichen Sensor gebildeten Beobachtungswerte, e4) Ermitteln der Kernzustandsgrößen des beweglichen Objekts mit Hilfe der aktualisierten Kovarianzmatrix des rekursiven Bayesschen Filters, e5) Separieren der Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und der Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors von der Kovarianz der Kernzustandsgrößen des zusätzlichen Sensors, e6) Wiederholen der Schritte e2) bis e5) für Beobachtungswerte des zusätzlichen Sensors, die an nach dem zweiten Zeitpunkt liegenden spätere Zeitpunkten gebildet werden, wobei in Schritt e2) die Kovarianz der Kernzustandsgrößen des beweglichen Objekts auf einen einen Zeitschritt vor dem jeweiligen späteren Zeitpunkt liegenden Zeitpunkt propagiert wird.
Verfahren nach Anspruch 1, wobei die Schritte e1) bis e6) für jeden nach dem Aufstarten des beweglichen Objekts hinzugefügten zusätzlichen Sensors durchgeführt werden.
Verfahren nach Anspruch 1, wobei in Schritt e2) die neueste Kovarianz der Kalibrierungszustandsgrößen des zusätzlichen Sensors und die neuesten Kreuzkovarianzen der Kernzustandsgrößen des beweglichen Objekts und der Kalibrierungszustandsgrößen des zusätzlichen Sensors mit Hilfe einer Reihe aus einer oder mehreren Zustandsübergangsmatrizen auf denselben Zeitpunkt wie die Kovarianz der Kernzustandsgrößen des beweglichen Objekts propagiert werden.
Verfahren nach Anspruch 3, wobei vor Ausführung von Schritt e3) die gebildete Kovarianzmatrix in eine positiv semidefinite Kovarianzmatrix korrigiert wird.
Verfahren nach Anspruch 4, wobei die gebildete Kovarianzmatrix mit Hilfe eines Eigenwert-Verfahrens in eine positiv semidefinite Kovarianzmatrix korrigiert wird, bei welchem die Kovarianzmatrix i) in ihre Eigenwerte und Eigenvektoren zerlegt wird, ii) gegebenenfalls negative Eigenwerte korrigiert werden und iii) die Kovarianzmatrix mit den korrigierten Eigenwerten und den Eigenvektoren rekonstruiert wird.
Verfahren nach Anspruch 5, wobei die negativen Eigenwerte durch die Absolute-Eigenwert-Korrektur, die Null-Eigenwert-Korrektur oder die Delta-Eigenwert-Korrektur korrigiert werden.
Verfahren nach einem der vorhergehenden Ansprüche, wobei der rekursive Bayessche Filter als Kalman-Filter, insbesondere als Extended-Kalman-Filter, ausgestaltet ist.
Computerlesbares Speichermedium, umfassend Befehle, welche bei der Ausführung durch einen Computer diesen veranlassen, ein Verfahren nach einem der vorhergehenden Ansprüche auszuführen.
System zur Datenverarbeitung, umfassend Mittel zur Ausführung eines Verfahrens nach einem der vorhergehenden Ansprüche 1 bis 7, wobei die Mittel ausgestaltet sind, eine oder mehrere Sensorkomponenten zu instantiieren, welche einen oder mehrere nach Aufstarten eines beweglichen Objekts hinzugefügte zusätzliche Sensoren repräsentieren, und eine Filterkomponente zu instantiieren, welche einen rekursiven Bayesschen Filter repräsentiert, wobei die Filterkomponente abhängig von der Ausführung des rekursiven Bayesschen Filter ist.