Titel

Verfahren zur Sensordatenverarbeitung in einem Sensorsystem und zugehöriges Sensorsystem

Stand der Technik

Die vorliegende Erfindung betrifft ein Verfahren zur Sensordatenverarbeitung in einem Sensorsystem, in welchem eine Relativposition eines Objektes zu dem Sensorsystem durch eine Kinematik verändert wird. Die vorliegende Erfindung betrifft ferner ein zugehöriges Sensorsystem.

Aktuelle Methoden zur Sensordatenfusion und Totzeitkompensation von bildbasierten Regelungssystemen erweitern bestehende klassische Algorithmen wie z.B. den Kalman-Filter. Die damit erzielten Ergebnisse sind sehr genau, aber nur mit großem Aufwand an veränderliche Umgebungsbedingungen anpassbar. Dabei wird versucht, eine Totzeitkompensation und eine Sensordatenfusion in einem einzelnen Schritt zu lösen.

Offenbarung der Erfindung

Das erfindungsgemäße Verfahren zur Sensordatenverarbeitung betrifft ein Verfahren zur Sensordatenverarbeitung in einem Sensorsystem, in welchem eine Relativpose von einem oder mehreren Objekten zu dem Sensorsystem durch eine Kinematik verändert wird. Das Verfahren umfasst ein Erfassen von Sensordaten eines ersten Umgebungssensors, wobei die Sensordaten des ersten Umgebungssensors eine Lage des Objektes gegenüber dem Sensorsystem zu zumindest einem ersten Zeitpunkt beschreiben, ein Ausführen einer Zeitverzugskompensation mittels einer ersten Zustandsschätzung basierend auf den Sensordaten des ersten Umgebungssensors, wobei dem ersten Umgebungssensor zugehörige zustandskorrigierte Sensordaten errechnet werden, wobei die zustandskorrigierten Sensordaten eine erste korrigierte Lage und/oder Bewegung des Objektes gegenüber dem Sensorsystem zu zumindest einem zweiten Zweitpunkt beschreibt, wobei der zweite Zeitpunkt auf den ersten Zeitpunkt folgt, und wobei die erste Zustandsschätzung mittels zumindest eines ersten Bewegungsmodells erfolgt, welches eine Bewegung des Objektes gegenüber dem Sensorsystem beschreibt, ein Ausführen einer Sensordatenfusion mittels einer zweiten Zustandsschätzung basierend auf den bei der ersten Zustandsschätzung errechneten zustandskorrigierten Sensordaten und weiteren Sensordaten, wobei ein dem Objekt zugehöriger Zustandsvektor errechnet wird, wobei der Zustandsvektor ein Datensatz ist, der eine zweite korrigierte Lage und/oder Bewegung des Objektes gegenüber dem Sensorsystem beschreibt, und wobei die zweite Zustandsschätzung auf zumindest einem zweiten Bewegungsmodell basiert, welches eine Bewegung des Objektes gegenüber dem Sensorsystem beschreibt, und ein Bereitstellen des errechneten Zustandsvektors.

Es wird somit ein zweistufiges Verfahren geschaffen, wobei in einer ersten Stufe das Ausführen der Zeitverzugskompensation erfolgt und in einer zweiten Stufe das Ausführen der Sensordatenfusion erfolgt. Es erfolgt somit eine getrennte Betrachtung von Sensordatenfusion und Zeitverzugskompensation.

Es erfolgt somit eine getrennte Betrachtung von Sensordatenfusion und Zeitverzugskompensation in zwei Ebenen. Dadurch wird u.a. erreicht, dass eine Abtastrate und die Zeitverzugskompensation unabhängig voneinander eingestellt und variiert werden können, was insbesondere auch bei großen Zeitverzügen möglich ist. Die Abtastrate ist dabei insbesondere die Rate, mit welcher der Zustandsvektor jeweils neu bereitgestellt wird. Durch die Funktionstrennung wird eine Modularität innerhalb der Softwarearchitektur geschaffen. Weitere Messgrößen, die aus zusätzlichen Sensoren kommen, können der Struktur modular zugefügt werden. Es kann schnell auf bauliche Veränderungen reagiert werden, da nur kleine Teile der Implementierung angepasst werden müssen. Die verschiedenen Sensormessungen können trotz unterschiedlicher Frequenzen ohne Steuerungslogik miteinander fusioniert werden. Jede Messgröße erhält eine eigene Zeitverzugskompensation in der ersten Ebene und beeinflusst die restliche Implementierung nicht. Die Zeitverzugskompensation in der ersten Ebene wird vom jeweiligen Umgebungssensor getriggert, wodurch eine sehr effiziente Möglichkeit geschaffen wird, um die Architektur auf Mikrocontrollern laufen zu lassen. Durch die Kinematik wird eine Relativposition oder Pose eines Objektes oder mehrerer Objekte zu dem Sensorsystem verändert. Dabei wird insbesondere die Relativposition eines Objektes gegenüber dem ersten Umgebungssensor verändert. Die Relativposition des Objektes zu dem Sensor wird dabei beispielsweise dadurch verändert, dass der erste Umgebungssensor durch die Kinematik bewegt wird, wobei der erste Umgebungssensor an einem Element der Kinematik angeordnet ist. Alternativ oder zusätzlich wird die Relativposition des Objektes zu dem Sensorsystem verändert, indem das Objekt durch die Kinematik gegenüber dem ortsfest angeordneten ersten Umgebungssensor bewegt wird. So ist beispielsweise der erste Umgebungssensor an einer festen Position angeordnet und das Objekt wird durch die Kinematik bewegt. Alternativ oder zusätzlich wird die Relativposition des Objektes zu dem Sensorsystem verändert, indem sowohl das Objekt als auch der Umgebungssensor durch die Kinematik bewegt wird. So ist beispielsweise der erste Umgebungssensor an einem ersten Element der Kinematik befestigt und das Objekt wird von einem zweiten Element der Kinematik gehalten, wobei die beiden Elemente durch eine zugehörige Motorik gegeneinander bewegt werden.

Es erfolgt ein Erfassen von Sensordaten des ersten Umgebungssensors, wobei die Sensordaten des ersten Umgebungssensors eine Lage des Objektes gegenüber dem Sensorsystem zu zumindest einem ersten Zeitpunkt beschreiben. Dabei wird die Lage des Objektes als mehreren ersten Zeitpunkten zugehörig angesehen, wenn diese beispielsweise durch eine Mittelwertbildung über mehrere Zeitpunkte hinweg ermittelt wird. Die Sensordaten sind dabei bevorzugt solche Daten, welche basierend auf von dem Umgebungssensor bereitgestellten Messwerten ermittelt werden. Ist der erste Umgebungssensor beispielsweise eine Kamera, so repräsentieren die Sensordaten nicht zwingend ein Kamerabild, sondern beschreiben beispielsweise Positionsdaten des Objektes, die durch eine Bildverarbeitung aus den Bilddaten des Kamerabildes extrahiert wurden. So sind die Sensordaten insbesondere Informationen, die aus den Messwerten des ersten Umgebungssensors extrahiert wurden.

Es folgt ein Ausführen einer Zeitverzugskompensation mittels der ersten Zustandsschätzung basierend auf den Sensordaten des ersten Umgebungssensors. Dabei werden dem ersten Umgebungssensor zugehörige zustandskorrigierte Sensordaten errechnet. Die Zeitverzugskompensation ist dabei eine Totzeitkompensation. So ergibt sich das Problem, dass eine gewisse Zeit verstreicht, zwischen dem Zeitpunkt, zu dem durch den ersten Umgebungssensor eine Messung ausgeführt wurde und einem Zeitpunkt, zu dem die Sensordaten des ersten Umgebungssensors tatsächlich für eine Verarbeitung bereitstehen. Dies beruht beispielsweise darauf, dass eine gewisse Rechenarbeit notwendig ist, um die Sensordaten aus den Messwerten des ersten Umgebungssensors zu extrahieren. Somit sind die Sensordaten des ersten Umgebungssensors in gewisser Weise schon veraltet, also nicht aktuell, wenn diese bereitgestellt werden. Aus diesem Grund wird mittels der Zeitverzugskompensation eine Korrektur der Sensordaten des ersten Umgebungssensors ausgeführt, wobei prädiziert wird, wie diese gewählt sein müssten, wenn keine Verzögerung, also keine Totzeit, durch den ersten Umgebungssensor und die zugehörige Extraktion der Sensordaten vorliegen würde. Dies erfolgt mittels der ersten Zustandsschätzung. Dabei wird abgeschätzt, wie eine Lage und/oder Bewegung des Objektes ist, wenn keine Totzeit des ersten Umgebungssensors existieren würde. Dazu wird eines oder mehrere erste Bewegungsmodelle herangezogen. Durch das Bewegungsmodell wird modelliert, wie sich die Relativposition des Objektes gegenüber dem Sensorsystem mit hoher Wahrscheinlichkeit verändert. Dabei greif das Bewegungsmodell auf die Sensordaten des ersten Umgebungssensors zurück.

Durch das Ausführen der Zeitverzugskompensation werden zustandskorrigierte Sensordaten errechnet. Diese beschreiben eine erste korrigierte Lage und/oder Bewegung des Objektes gegenüber dem Sensorsystem zu zumindest einem zweiten Zeitpunkt, welcher auf den ersten Zeitpunkt folgt. Dabei wird die Lage des Objektes als mehreren zweiten Zeitpunkten zugehörig angesehen, wenn diese beispielsweise durch eine Mittelwertbildung über mehrere Zeitpunkte hinweg ermittelt wird. Es wird somit mit der korrigierten Lage und/oder Bewegung des Objektes eine Lage und/oder Bewegung des Objektes ermittelt, die zu dem zweiten Zeitpunkt vorliegt, obwohl die zugehörigen Messdaten des ersten Umgebungssensors zu einem ersten Zeitpunkt erfasst wurden.

Es erfolgt ferner das Ausführen der Sensordatenfusion mittels der zweiten Zustandsschätzung, basierend auf den bei der ersten Zustandsschätzung errechneten zustandskorrigierten Sensordaten und weiteren Sensordaten. Die weiteren Sensordaten sind dabei insbesondere weitere zustandskorrigierte Sensordaten, die aus einer vorangehenden Zeitverzugskompensation resultieren oder sind Sensordaten, die unmittelbar für die Sensordatenfusion bereitgestellt werden. Es werden somit in der zweiten Stufe Sensordaten unterschiedlicher Sensoren zusammengeführt, wobei zumindest die Sensordaten des ersten Umgebungssensors einer Zeitverzugskompensation unterliegen. Die der Sensordatenfusion zugrundeliegenden Sensordaten sind daher bereits hinsichtlich ihrer Totzeit korrigiert, wenn diese von einem Umgebungssensor stammen.

Bei der zweiten Zustandsschätzung wird auf eines oder mehrere zweite Bewegungsmodelle zugegriffen, um eine zweite korrigierte Lage und/oder Bewegung des Objektes zu ermitteln. Die zweite korrigierte Lage und/oder Bewegung des Objektes ist dabei eine Lage, die das Objekt zu einem dritten Zeitpunkt aufweist, der auf den zweiten Zeitpunkt folgt. Alternativ ist die zweite korrigierte Lage und/oder Bewegung die mittels weiterer Sensordaten weiter präzisierte erste korrigierte Lage. So ist es beispielsweise für eine weitere Datenverarbeitung notwendig, dass die Lage und/oder Bewegung des Objektes für einen Zeitpunkt ermittelt wird, zu dem eine folgende Datenverarbeitung beendet ist. So wird beispielsweise eine Relativposition des Objektes gegenüber dem Sensorsystem durch die Kinematik angefahren. Die Berechnung eines Bewegungsablaufes der Kinematik muss vorausberechnet werden. Mit anderen Worten wird somit mit der zweiten korrigierten Lage und/oder Bewegung des Objektes eine Lage und/oder Bewegung des Objektes ermittelt, die gegenüber dem zweiten Zeitpunkt in der Zukunft liegt. Durch das Ausführen der Zeitverzugskompensation und das Ausführen der Sensordatenfusion werden somit insbesondere auch zwei separate Zeitverzugskompensationen ausgeführt. Abhängig von einer Reaktionszeit des Gesamtsystems kann das Ausführen der Sensordatenfusion jedoch auch auf eine Zeitverzugskompensation verzichten. Bevorzugt basiert die zweite Zustandsschätzung dazu auf einem zweiten Bewegungsmodell.

Mittels der zweiten Zustandsschätzung wird der Zustandsvektor berechnet, welcher ein Datensatz ist, der die zweite korrigierte Lage und/oder Bewegung des Objektes gegenüber dem Sensorsystem beschreibt. Wurde der Zustandsvektor errechnet, so erfolgt ein Bereitstellen des errechneten Zustandsvektors. Die Unteransprüche zeigen bevorzugte Weiterbildungen der Erfindung.

Es ist vorteilhaft, wenn das Verfahren ein Erfassen von Sensordaten mehrerer Umgebungssensoren umfasst, welche den ersten Umgebungssensor umfassen, und ein Ausführen einer Zeitverzugskompensation mittels einer ersten Zustandsschätzung für jeden der Umgebungssensoren basierend auf den von dem jeweiligen Umgebungssensor erfassten Sensordaten umfasst, wobei jeweils zustandskorrigierte Sensordaten errechnet werden, wobei die erste Zustandsschätzung für jeden der Umgebungssensoren auf einem zugehörigen ersten Bewegungsmodell basiert, welches eine Bewegung des Objektes gegenüber dem Sensorsystem beschreibt Dabei basiert das Ausführen der zweiten Zustandsschätzung auf den zustandskorrigierten Sensordaten der Umgebungssensoren, wobei der dem Objekt zugehörige Zustandsvektor errechnet wird. Es wird somit in der ersten Stufe bevorzugt für mehrere Umgebungssensoren jeweils separat eine Zeitverzugskompensation ausgeführt. Somit wird es ermöglicht, dass Umgebungssensoren, welche unterschiedliche Totzeiten aufweisen, miteinander kombiniert werden können.

Dabei ist es vorteilhaft, wenn die zustandskorrigierten Sensordaten der Umgebungssensoren oder daraus resultierende Filterparameter in einer Speichereinheit gespeichert werden und bei der zweiten Zustandsschätzung genutzt werden, wobei insbesondere die zuletzt gespeicherten zustandskorrigierten Sensordaten der Umgebungssensoren oder die daraus resultierenden Filterparameter genutzt werden. So werden in der zweiten Stufe bei der Sensordatenfusion insbesondere die bei der ersten Zustandsschätzung errechneten zustandskorrigierten Sensordaten genutzt, um Filterparameter zu setzen, basierend auf denen eine Lage und/oder Bewegung des Objektes korrigiert wird. Dadurch, dass die zustandskorrigierten Sensordaten und alle Umgebungssensoren in der Speichereinheit abgelegt werden, kann auf diese zugegriffen werden, was insbesondere dann auch gilt, wenn für einen aktuellen Zeitpunkt keine aktuellen zustandskorrigierten Sensordaten für einen der Umgebungssensoren bereitstehen. In diesem Fall kann auf die zuletzt gespeicherten zustandskorrigierten Sensordaten des jeweiligen Umgebungssensors zugegriffen werden. Bevorzugt werden die Zustandskorrigierten Sensordaten in einem first-in- / first-out-Speicher hinterlegt, wodurch sich für jeden der Umgebungssensoren ein FiFo-System ergibt. Auch ist es vorteilhaft, wenn die zustandskorrigierten Sensordaten mehrerer Umgebungssensoren in zueinander unterschiedlichen ersten Frequenzen für die zweite Zustandsschätzung bereitgestellt werden. So kann jeder der Umgebungssensoren in der für den jeweiligen Umgebungssensor maximalen Arbeitsfrequenz betrieben werden. In der zweiten Stufe wird bei der Sensordatenfunktion der Zustandsvektor unabhängig von den ersten Frequenzen bereitgestellt.

Bevorzugt erfolgt die erste Zustandsschätzung basierend auf Sensordaten einer Bewegungssensorik, wobei das erste Bewegungsmodell die Bewegung des Objektes gegenüber dem Sensorsystem, basierend auf Sensordaten der Bewegungssensorik errechnet. Alternativ oder zusätzlich erfolgt die zweite Zustandsschätzung basierend auf Sensordaten einer Bewegungssensorik, wobei das zweite Bewegungsmodell die Bewegung des Objektes gegenüber dem Sensorsystem, basierend auf den Sensordaten der Bewegungssensorik errechnet. Die Bewegungssensorik ist dabei insbesondere keine bildbasierte Sensorik, sondern eine Sensorik, bei welcher Informationen über die Lage und/oder Bewegung des Objektes relativ zu der Kinematik ohne aufwendige Bildverarbeitung bereitgestellt werden können. So ist die Bewegungssensorik insbesondere eine Sensorik, welche einen Beschleunigungssensor oder einen abtastenden Positionssensor umfasst. Die Bewegungssensorik ist somit insbesondere eine Sensorik bei der ein von einem Sensor der Bewegungssensorik erfasster Messwert gleich den Sensordaten des jeweiligen Sensors entspricht. Die Bewegungssensorik erfasst einen Zustand der

Kinematik, insbesondere ohne Berücksichtigung der Lage des Objekts. Durch die Bewegungssensorik wird eine Position des Objektes bzw. die Relativposition des Objektes zu dem Sensorsystem nicht direkt erfasst. Durch die Bewegungssensorik kann jedoch erfasst werden, wie die Kinematik sich bewegt. Dadurch kann das Bewegungsmodell verbessert werden, beispielsweise indem zugehörige Filterparameter angepasst werden. Wird beispielsweise durch die Bewegungssensorik eine lineare Bewegung der Kinematik erfasst, so kann davon ausgegangen werden, dass die Relativposition des Objektes gegenüber der Kinematik sich in der entsprechenden Achse verändert. Dies kann durch das Bewegungsmodell berücksichtigt werden. Die Präzision der Zustandsschätzung wird dadurch signifikant erhöht. Auch ist es vorteilhaft, wenn die erste Zustandsschätzung und/oder die zweite Zustandsschätzung mittels eines Kalman-Filters, eines bayesschen Filters oder mittels maschinelles Lernen ausgeführt wird. Dabei ist der Kalman-Filter insbesondere ein extended Kalman-Filter oder ein sonstiger bayesscher Filter. Durch die Verwendung eines Kalman-Filters wird eine besonders hohe Genauigkeit erreicht. Optional können alternative Verfahren zur Zustandsschätzung eingesetzt werden. Maschinelles Lernen kann dabei auch als Machine-Learning bezeichnet werden.

Es ist ferner vorteilhaft, wenn der Zustandsvektor mit einer zweiten Frequenz bereitgestellt wird, welche unterschiedlich zu einer ersten Frequenz ist, mit welcher die zustandskorrigierten Sensordaten bereitgestellt werden. Wird der Zustandsvektor mit der zweiten Frequenz bereitgestellt, so werden die Werte des Zustandsvektors entsprechen der zweiten Frequenz aktualisiert. Werden die zustandskorrigierten Sensordaten mit der ersten Frequenz bereitgestellt, so werden die zustandskorrigierten Sensordaten mit der ersten Frequenz aktualisiert. Die erste Frequenz kann somit von der zweiten Frequenz abweichen. Dies ermöglicht es, dass eine Verarbeitungsgeschwindigkeit der Umgebungssensoren und einer zugehörigen Verarbeitungselektronik unabhängig von einer Verarbeitungsgeschwindigkeit einer Elektronik gewählt werden kann, durch welche der Zustandsvektor ermittelt wird. Es wird somit u.a. ermöglicht, dass die Verarbeitungselektronik und/oder der erste Umgebungssensor getauscht werden kann, ohne dass dieses einen Einfluss auf eine Datenverarbeitung hat, durch welch der Zustandsvektor in der zweiten Stufe errechnet wird.

Auch ist es vorteilhaft, wenn die zweite Frequenz einstellbar ist. Dadurch wird es ermöglicht, dass der Zustandsvektor für unterschiedliche Verarbeitungsverfahren bereitgestellt werden kann, die eine Aktualisierung des Zustandsvektors mit unterschiedlichen Frequenzen erfordern.

Auch ist es vorteilhaft, wenn der erste Umgebungssensor eine Kamera, ein LiDAR-Sensor, ein Time-of-Flight-Sensor, ein Radar-Sensor oder ein Ultraschall- Sensor ist. Gerade für solche Umgebungssensoren ist oftmals eine aufwendige Signalverarbeitung notwendig, die zu längeren Totzeiten führen können. Abhängig von der Signalverarbeitung kann die Totzeit dabei variieren. Durch die separate Zeitverzugskompensation in der ersten Stufe können somit die Anforderungen an den ersten Umgebungssensor und die zugehörige Signalverarbeitung so erweitert werden, dass eine Veränderung der zugehörigen Signalverarbeitung ermöglicht ist, ohne dass eine Signalverarbeitung zum Ermitteln des Zustandsvektors bei der Sensordatenfusion dadurch betroffen ist.

Ein Sensorsystem, welches dazu eingerichtet ist, das erfindungsgemäße Verfahren auszuführen, weist alle Vorteile des Verfahrens auf.

Kurze Beschreibung der Zeichnungen

Nachfolgend werden Ausführungsbeispiele der Erfindung unter Bezugnahme auf die begleitende Zeichnung im Detail beschrieben. In der Zeichnung ist:

Figur 1 eine Darstellung eines Ablaufdiagrammes des erfindungsgemäßen Verfahrens zur Sensordatenverarbeitung,

Figur 2 eine schematische Darstellung einer beispielhaften Kinematik mit einem zugehörigen erfindungsgemäßen Sensorsystem,

Figur 3 ein dem erfindungsgemäßen Verfahren zugehöriges beispielhaftes Signalflussdiagramm gemäß einer Ausführungsform der Erfindung,

Figur 4 ein dem erfindungsgemäßen Verfahren zugehöriges

Signalflussdiagramm in einer weiteren Ausführungsform der Erfindung, und

Figur 5 ein dem Verfahren zugehöriges Signalflussdiagramm, welche eine Signalverarbeitung in einer zweiten Stufe beschreibt, in welcher die Sensordatenfusion erfolgt.

Ausführungsformen der Erfindung

Figur 1 zeigt ein Verfahren 100 zur Sensordatenverarbeitung in einem Sensorsystem 1 , in welchem eine Relativposition eines Objektes 20 zu dem Sensorsystem 1 durch eine Kinematik 2 verändert wird. In Figur 2 ist diesbezüglich eine beispielhafte Kinematik 2 dargestellt. Dabei ist die Kinematik 2 eine lineare Kinematik, welche mit drei linearen Bewegungen entlang unterschiedlicher Achsen arbeitet. Durch die Kinematik 2 wird beispielsweise ein Werkzeug 34 oder ein anderes Element gegenüber dem Objekt 20 bewegt.

Durch eine erste Bewegungseinheit 30 wird das Werkzeug 34 entlang einer Z- Richtung gegenüber dem Objekt 20 bewegt. Durch eine zweite Bewegungseinheit 31 wird das Werkzeug 34 entlang einer Y-Achse gegenüber dem Objekt 20 bewegt. Mittels einer dritten Bewegungseinheit 32 wird das Werkzeug 34 entlang einer X-Achse gegenüber dem Objekt 20 bewegt. Mittels einer Rotationseinheit 33 wird das Werkzeug 34 um die X-Achse gegenüber dem Objekt 20 rotiert.

Es wird darauf hingewiesen, dass die Kinematik 2 aus Figur 2 nur beispielhaft zu verstehen ist. Das erfindungsgemäße Verfahren kann in gleicher Weise auf andere Kinematiken angewendet werden, wobei das Verfahren insbesondere auch für die Verwendung von Roboterarmen geeignet ist. Es ist nicht relevant, ob das Werkzeug 34 oder das Objekt 20 gegenüber dem Sensorsystem 1 bewegt wird. So kann beispielsweise ein Umgebungssensor 10 des Sensorsystems 1 entweder an der Kinematik 2 oder unbeweglich angeordnet sein. Auch ist es möglich, dass ein Umgebungssensor 10 des Sensorsystems 1 an dem Objekt 20 angeordnet ist. Ebenso ist es möglich, dass sowohl das Objekt 20 als auch das Werkzeug 34 gegenüber dem Sensorsystem 1 bewegt werden. Das Werkzeug 34 ist optional und im weitesten Sinne des Begriffs „Werkzeug“ zu versehen. So kann beispielsweise auch ein weiterer Sensor als ein Werkzeug 34 verstanden werden, auch wenn dieser nicht direkt auf das Objekt 20 einwirkt.

Bei dem in Figur 2 gezeigten Beispiel ist das Werkzeug 34 beispielhaft ein Greifwerkzeug, welcher nach dem Objekt 20 greifen soll. Das Objekt 20 wird jedoch unabhängig von der Kinematik 2 gegenüber der Kinematik 2 und dem Sensorsystem 1 bewegt. Es ist somit die Herausforderung, das Werkzeug 34 so zu bewegen, dass dieses das Objekt 20 greifen kann, obwohl die von den Umgebungssensoren 10, 11 des Sensorsystems 1 erfassten Daten einer Totzeit unterliegen. Dies wird durch das erfindungsgemäße Verfahren 100 ermöglicht. In einem ersten Verfahrensschritt 101 folgt ein Erfassen von Sensordaten z1 eines ersten Umgebungssensors 10 des Sensorsystems 1. Die Sensordaten z1 des ersten Umgebungssensors 10 beschreiben dabei eine Lage des Objektes 20 gegenüber dem Sensorsystem 1 zu einem ersten Zeitpunkt. Der erste Umgebungssensor 10 ist dabei beispielsweise eine Kamera, welche an der Kinematik 2 angeordnet ist und mit dieser bewegt wird. In alternativen Ausführungsformen ist der erste Umgebungssensor 10 ein LiDAR, ein Time-of- Flight Sensor, einen Radar-Sensor oder ein Ultraschallsensor. Durch den ersten Umgebungssensor 10 wird in dieser Ausführungsform ein Videobild als Messdaten erfasst und für eine Datenbearbeitung bereitgestellt. Dabei wird das Videobild ausgewertet und eine Position des Objektes 20 gegenüber dem ersten Umgebungssensor 10 und somit gegenüber dem Sensorsystem 1 ermittelt. Diese Pose des ersten Umgebungssensors 10 wird in den Sensordaten z1 des ersten Umgebungssensors 10 für eine weitere Verarbeitung bereitgestellt.

Durch die Pose des ersten Umgebungssensors 10, welche durch die Sensordaten z1 des ersten Umgebungssensors 10 beschrieben wird, ist eine Lage des Objektes 10 gegenüber dem Sensorsystem 1 beschrieben. Würde die Kinematik 2 nun derart angesteuert werden, dass das Werkzeug 34 nach dem Objekt 20 greift, wobei angenommen wird, dass dieses sich an der Lage gegenüber dem Sensorsystem 1 befindet, die durch die Sensordaten z1 des ersten Umgebungssensors 10 beschrieben ist, so würde die Kinematik 2 das Werkzeug 34 nicht an die korrekte Stelle führen, an der das Objekt 20 gegriffen werden kann. Dies ist die Folge einer Totzeit des ersten Umgebungssensors 10. Das bedeutet, dass aufgrund der notwendigen Datenverarbeitung, durch welche die Lage des Objektes 10 aus dem Videobild des ersten Umgebungssensors 10 extrahiert wurde, die mit den Sensordaten z1 beschriebene Lage bereits veraltet ist. Das Objekt 20 hat sich innerhalb dieser Totzeit gegenüber dem Sensorsystem 1 bewegt und kann nicht gegriffen werden. Es wird daher ein zweiter Verfahrensschritt 102 ausgeführt, um die in den Sensordaten z1 beschriebene Lage des Objektes 20 zu präzisieren.

In dem zweiten Verfahrensschritt 102 erfolgt ein Ausführen einer Zeitverzugskompensation mittels einer ersten Zustandsschätzung basierend auf den Sensordaten z1 des ersten Umgebungssensors 10. Das Ausführen der Zeitverzugskompensation ist eine erste Stufe des Verfahrens 100. Dabei werden dem ersten Umgebungssensor 10 zugehörige zustandskorrigierte Sensordaten z1‘ errechnet. Die zustandskorrigierten Sensordaten z1‘ beschreiben eine korrigierte Lage und/oder Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 zu einem zweiten Zeitpunkt, welcher auf den ersten Zeitpunkt folgt. Die erste Zustandsschätzung erfolgt mittels eines ersten Bewegungsmodells, welches eine Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 beschreibt. Die korrigierte Lage und/oder Bewegung des Objektes 20, welche mit den zustandsangehörigen Sensordaten z1‘ beschrieben werden, sind totzeitkorrigierte Sensordaten. Das bedeutet, dass durch die Zeitverzugskompensation solche Sensordaten errechnet werden, die eine Lage und/oder Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 beschreiben, wobei die Lage und/oder Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 über die Totzeit des ersten Umgebungssensors 10 hinweg prädiziert wird.

Da die Totzeit vorliegt, sind die zustandskorrigierten Sensordaten zT somit abgeschätzte Werte, da in die Zukunft geblickt werden muss, um die tatsächlich aktuell korrigierte Lage und/oder Bewegung des Objektes 20 zu beschreiben. Um dies zu ermöglichen, erfolgt die erste Zustandsschätzung mittels des ersten Bewegungsmodells. Das erste Bewegungsmodell beschreibt die Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 und gegenüber der Kinematik 2 in einem mathematischen Modell. Im einfachsten Fall ist das Bewegungsmodell ein lineares Bewegungsmodell. Bewegt sich das Objekt 20 mit einer konstanten Geschwindigkeit in eine konstante Richtung, so kann mittels des linearen Bewegungsmodells vorhergesagt werden, so sich das Objekt 20 nach Ablauf der Totzeit des ersten Umgebungssensors 10 befindet. Das erste Bewegungsmodell basiert somit bevorzugt aus einer Analyse von früheren Werten der Sensordaten des ersten Umgebungssensors 10. Alternativ oder zusätzlich basiert das erste Bewegungsmodell zudem auf den Sensordaten einer Bewegungssensorik der Kinematik 2. So wird beispielsweise innerhalb der ersten Bewegungseinheit 30, der zweiten Bewegungseinheit 31 , der dritten Bewegungseinheit 32 und der Rotationseinheit 33 mittels geeigneter Sensoren, wie z.B. Positions- und/oder Beschleunigungssensoren, der aktuelle räumliche Zustand der Kinematik 2 erfasst. Werden diese Bewegungsdaten q der Bewegungssensorik der Kinematik 2 in dem Bewegungsmodell, hier dem ersten Bewegungsmodell, bereitgestellt, so kann eine Abschätzung der Bewegung des Objektes gegenüber dem Sensorsystem 1 präzisiert werden. So kann beispielsweise aus den Bewegungsdaten q der Bewegungssensorik ermittelt werden, wie sich das Werkzeug 34 innerhalb der Totzeit gegenüber dem Objekt 20 bewegt hat. Somit kann eine Abweichung, die aus der Bewegung der Kinematik 2 resultiert, rechnerisch in das Ermitteln der zustandskorrigierten Sensordaten zT einfließen. Dies ist insbesondere deswegen vorteilhaft, da die Bewegungsdaten q der Bewegungssensorik zumeist einer deutlich geringeren Totzeit unterliegen, als die Sensordaten der Umgebungssensoren 10, 11. Die zustandskorrigierten Sensordaten zT werden für eine weitere Verarbeitung bereitgestellt.

In einem dritten Verfahrensschritt 103 erfolgt ein Ausführen einer Sensordatenfusion mittels einer zweiten Zustandsschätzung, basierend auf den bei der ersten Zustandsschätzung errechneten korrigierten Sensordaten zT und weiteren Sensordaten, wobei ein dem Objekt 20 zugehöriger Zustandsvektor x errechnet wird. Das Ausführen der Sensordatenfusion ist eine zweite Stufe des Verfahrens 100.

Der Zustandsvektor x ist ein Datensatz, der eine zweite korrigierte Lage und/oder Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 beschreibt. Die zweite Zustandsschätzung basiert auf einem zweiten Bewegungsmodell, welches eine Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 beschreibt. Bei der Sensordatenfusion werden die Sensordaten mehrerer Sensoren zusammengeführt. So werden beispielsweise die zustandskorrigierten Sensordaten z1 des ersten Umgebungssensors 10 mit den Bewegungsdaten q der Bewegungssensorik der Kinematik 2 und/oder mit zustandskorrigierten Sensordaten z2 weiterer Umgebungssensoren zusammengeführt, die ebenfalls einer Zeitverzugskompensation in der ersten Stufe unterzogen wurden.

Bei der Zustandsschätzung in dem dritten Verfahrensschritt 103 erfolgt optional ebenfalls eine Zeitverzugskompensation. Durch diese optionale weitere Zeitverzugskompensation wird die Lage und/oder Bewegung des Objektes 20 zu einem dritten Zeitpunkt beschrieben, welcher auf den zweiten Zeitpunkt folgt. Dabei erfolgt jedoch keine Totzeitkompensation von Sensoren, sondern es wird eine Vorhersage über ein Zeitintervall geschaffen, welches durch die bei der Sensordatenfusion ausgeführten Schritte und/oder durch folgende Datenverarbeitungsschritte verursacht wird. Im einfachsten Fall ist die Zustandsschätzung so gewählt, dass ein Zustand ausgegeben wird, der für einen Zeitpunkt korrekt ist, zu dem auch der Zustandsvektor x ausgegeben wird. Wie auch bei der ersten Zustandsschätzung basiert auch die zweite Zustandsschätzung optional auf den Bewegungsdaten q der Bewegungssensorik.

In einem vierten Verfahrensschritt 104 erfolgt ein Bereitstellen des errechneten Zustandsvektors x. Der Zustandsvektor x wird bevorzugt für eine Steuerung der Kinematik 2 genutzt.

Figur 3 ist eine Darstellung eines Signalflussdiagramms, welches einen Signalfluss in dem Sensorsystem 1 beschreibt. In Figur 3 links ist der erste Umgebungssensor 10 schematisch dargestellt. Der erste Umgebungssensor 10 umfasst dabei eine Recheneinheit, durch welche die Sensordaten z1 des ersten Umgebungssensor 10 errechnet werden. Durch den ersten Umgebungssensor 10 wird eine Pose als Sensordaten z1 des Umgebungssensors 10 an einen Zustandsschätzer 40 übertragen, durch welchen das Verfahrens 100 ausgeführt wird.

Durch eine Zeitverzugskompensationseinheit 41 des Zustandsschätzers 40 wird die Zeitverzugskompensation gemäß dem zweiten Verfahrensschritt 102 ausgeführt. Dies erfolgt insbesondere durch einen Extended Kalman-Filter 42. Die Sensordaten z1 des ersten Umgebungssensors 10 werden dabei insbesondere dazu insbesondere dazu genutzt, um Filterparameter des extended Kalman-Filters zu setzen. Die Ausgangssignale des Extended Kalman-Filters 42 werden an eine Sensordatenfusionseinheit 43 ausgegeben.

Durch die Sensordatenfusionseinheit 43 des Zustandsschätzers 40 wird die Sensordatenfusion gemäß dem dritten Verfahrensschritt 103 ausgeführt. Dies erfolgt insbesondere durch einen asynchronen extended Kalman-Filter 44, welcher als asynchron bezeichnet wird, da die Eingangsdaten asynchron bereitgestellt werden können.

An den Eingängen des asynchronen extended Kalman-Filters 44 in der Sensordatenfusionseinheit 43 können optional auch die Sensordaten von weiteren Umgebungssensoren bereitgestellt werden. So umfasst das Sensorsystem 1 optional einen, beispielsweise ortsfesten, zweiten Umgebungssensor 11. Die Zeitverzugskompensation gemäß dem zweiten Verfahrensschritt 102 wird für den zweiten Umgebungssensor 11 in entsprechender Weise ausgeführt. Die Zeitverzugskompensation für den zweiten Umgebungssensor 11 wird dabei insbesondere unabhängig von der für den ersten Umgebungssensor 10 ausgeführten Zeitverzugskompensation ausgeführt. So wird insbesondere für jeden der Umgebungssensoren 10, 11 eine eigene erste Zustandsabschätzung ausgeführt. Ein entsprechendes Signalflussdiagramm dazu ist in Figur 4 dargestellt.

Werden die Sensordaten mehrerer Umgebungssensoren benutzt, so erfolgt beispielsweise ein Erfassen von Sensordaten mehrerer Umgebungssensoren 10, 11, welche den ersten Umgebungssensor 10 umfassen. Für jeden der Umgebungssensoren 10, 11 erfolgt das Ausführen einer Zeitverzugskompensation mittels einer ersten Zustandsschätzung basierend auf den jeweiligen Sensordaten z1, z2 des zugehörigen Umgebungssensors 10, 11. Dabei werden jeweils zustandskorrigierte Sensordaten zT, z2‘ errechnet, wobei die erste Zustandsschätzung für jeden der Umgebungssensoren 10, 11 auf einem zugehörigen ersten Bewegungsmodell basiert, welches eine Bewegung des Objektes 20 gegenüber dem Sensorsystem 1 beschreibt. Bei dem Ausführen der zweiten Zustandsschätzung, basierend auf den zustandskorrigierten Sensordaten z1‘, z2‘ der Umgebungssensoren 10, 11 , wird der zugehörige Zustandsvektor x basierend auf den Sensordaten z1, z2, aller Umgebungssensoren 10, 11 errechnet.

Dem ersten Umgebungssensors 10 ist dabei die Zeitverzugskompensationseinheit 41 des ersten Umgebungssensors 10 zugehörig. Dem zweiten Umgebungssensors 11 ist dabei die ihm zugehörigen Zeitverzugskompensationseinheit 45 des zweiten Umgebungssensors 11 zugehörig. Die Zeitverzugskompensationseinheit 41 des ersten Umgebungssensors 10 und die Zeitverzugskompensationseinheit 45 des zweiten Umgebungssensors 11 stellen die von diesen ermittelten zustandskorrigierten Sensordaten zT, z2‘ der Sensordatenfusionseinheit 43 bereit.

Bei dem in Figur 4 dargestellten Signalflussdiagramm erfolgt die Sensordatenfusion in der Sensordatenfusionseinheit 43 basierend auf den bei der ersten Zustandsschätzung errechneten zustandskorrigierten Sensordaten z1‘, z2‘ und den Bewegungsdaten q der Bewegungssensorik der Kinematik 2, welche in den Bewegungseinheiten 30 bis 32 und der Rotationseinheit 33 integriert ist. Diese Bewegungsdaten q der Bewegungssensorik der Kinematik 2 können dabei als weitere Sensordaten z _n ‘ angesehen werden, auf welchen die zweite Zustandsschätzung basiert. Optional werden die Bewegungsdaten q der Bewegungssensorik auch für die Zeitverzugskompensation in der dem ersten Umgebungssensors 10 zugehörigen Zeitverzugskompensationseinheit 41 und oder für die Zeitverzugskompensation in der dem zweiten Umgebungssensors 11 zugehörigen Zeitverzugskompensationseinheit 45, bspw. durch den extended Kalman-Filter 42, 46, genutzt.

Bei dem in Figur 4 dargestellten Signalflussdiagramm sind die weiteren Sensordaten, welche mit den zustandskorrigierten Sensordaten z1 des ersten Umgebungssensors 10 in der Sensordatenfusion zusammengeführt werden, zumindest die zustandskorrigierten Sensordaten z2 des zweiten Umgebungssensors 11. Nach der Sensordatenfusion durch den extended Kalman-Filter 44 in der Sensordatenfusionseinheit 43 wird von diesem der Zustandsvektor x ausgegeben.

Durch das separate Ausführen der Zeitverzugskompensation in dem zweiten Verfahrensschritt 102, separat von dem Ausführen der Sensordatenfusion in dem dritten Verfahrensschritt 103, wird erreicht, dass neue Umgebungssensoren in einfacher Weise der Sensoreinheit 1 hinzugefügt oder bestehende Sensoren ersetzt werden können, ohne dass eine Umfangreiche Anpassung der Software notwendig ist. So ergibt sich beispielsweise, dass der erste Umgebungssensor 10 bzw. dessen zugehörige Verarbeitungselektronik, zu einer anderen Totzeit führt, als dies für den zweiten Umgebungssensor 11 der Fall ist. Für die beiden Umgebungssensoren 10, 11 wird jedoch unabhängig in der ersten Stufe eine Zeitverzugskompensation durch eine zugehörige Zeitverzugskompensationseinheit 41, 45 mit jeweils zugehörigem extended Kalman-Filter 42, 46 ausgeführt. Die jeweils totzeitkorrigierten und somit zustandskorrigierten Sensordaten z1, z2 der beiden Umgebungssensoren 10, 11 werden für die Sensordatenfusion durch die Sensordatenfusionseinheit 43 bereitgestellt. Wird einer der beiden Umgebungssensoren 10, 11 getauscht, so ist lediglich die zugehörige Zeitverzugskompensation an den neuen Umgebungssensor anzupassen.

Ferner ist zu berücksichtigen, dass die Sensordaten z1, z2 unterschiedlicher Umgebungssensoren oft in unterschiedlichen Frequenzen bereitgestellt werden. So werden beispielsweise die Sensordaten z1 des ersten Umgebungssensors 10 in einer Frequenz von 3 bis 5 Hz ausgegeben. Diese wird im Folgenden als erste Frequenz bezeichnet. Der Zustandsvektor x soll jedoch für eine folgende Datenverarbeitung in einer zweiten Frequenz ausgegeben werden, welche höher als die erste Frequenz ist. Zudem werden von dem zweiten Umgebungssensor 11 beispielsweise Sensordaten z2 mit einer dritten Frequenz bereitgestellt, welche ebenfalls im Bereich von 3 bis 5 Hz liegt, jedoch nicht zwingend gleich zu ersten Frequenz ist. Durch die Bewegungssensorik werden Bewegungsdaten q in einer vierten Frequenz ausgegeben, die jedoch höher als die erste oder dritte Frequenz ist, jedoch im Bereich der zweiten Frequenz liegt. Erfindungsgemäß ist es möglich, dass für die Sensordatenfusion alle durch die Sensordatenfusionseinheit 43 zu fusionierenden Sensordaten z1 bis zn in unterschiedlichen Frequenzen bereitgestellt werden und in der zweiten Frequenz ausgegeben werden, welche optional einstellbar ist. Es wird diesbezüglich auch auf Figur 5 verwiesen.

Figur 5 ist eine schematische Darstellung der Sensordatenfusionseinheit 43. Es ist ersichtlich, dass die zustandskorrigierten Sensordaten z1 , z2 der Sensordatenfusionseinheit bereitgestellt werden. Optional erfolgt eine Zustandskorrektur der zustandskorrigierten Sensordaten z1 , z2. Die erneut zustandskorrigierten Sensordaten z1“, z2“ werden einem globalen Speicher 51 zugeführt und dort jeweils in einem first-in- / first-out-Speicher abgelegt. Optional werden somit für jeden der Umgebungssensoren 10, 11 jeweils die einem Zeitpunkt zugehörigen erneut zustandskorrigierten Sensordaten z1“, z2“ hinterlegt. Diese werden als Filterparameter für den extended Kalman-Filter 44 der Sensordatenfusionseinheit 43 genutzt. Es wird somit immer der jeweils zuletzt verfügbare zustandskorrigierte Wert der Sensordaten z1, z2 als Filterwert genutzt. Mittels dieser Filterwerte wird eine zuvor ermittelte Position und/oder Beschleunigung des Objektes 20, hier als „u“ bezeichnet, prädiziert und die Prädiktion mittels der zusätzlichen Filterwerte korrigiert. Durch die in Figur 5 beispielhaft dargestellte schematische Zeiteinheit 52 kann gewählt werden, in welcher Frequenz der Zustandsvektor x aktualisiert und somit mit der zweiten Frequenz bereitgestellt wird. So wird durch die Zeiteinheit 52 beispielsweise eingestellt, mit welcher Frequenz der globale Speicher 51 ausgelesen wird.

In der Sensordatenfusionseinheit 43 werden somit die zustandskorrigierten Sensordaten z1‘, z2‘ der Umgebungssensoren 10, 11 oder daraus resultierende Filterparameter in einer Speichereinheit 51 gespeichert und bei der zweiten Zustandsschätzung genutzt, wobei insbesondere die zuletzt gespeicherten zustandskorrigierten Sensordaten der Umgebungssensoren 10, 11 oder die daraus resultierenden Filterparameter genutzt werden.

Ausgangspunkt für die Zeitverzugskompensation von Posenschätzungen in der ersten Ebene ist eine Pose, die die Kamerapose des ersten Umgebungssensors 10 relativ zum Objekt 2 beschreibt. Diese Pose ist Teil des Zustandsvektors x und auf Grund der 6D-Posen Extraktion zeitverzögert. Die Zeitverzugskompensation der ersten Stufe basiert auf der Vorwärtskinematik der Kinematik 2. Es wird davon ausgegangen, dass die Gelenk- oder Linearpositionen der Kinematik 2 mit einer im Vergleich zum Zeitverzug hohen Frequenz und ohne Zeitverzug vorliegen. Durch das Zwischenspeichern sämtlicher Gelenkpositionen ist es möglich, für jeden Zeitpunkt die Vorwärtskinematik zu bestimmen.

Die Zeitverzugskompensation der ersten Ebene läuft bevorzugt in drei Schritten ab.

1. In der Gegenwart liegt eine neue Messung als Messwert vor, die auf Grund des Zeitverzuges zu einem in Vergangenheit liegenden Zeitschrift gehört. Das Filter prädiziert deshalb aus der Vergangenheit in den zur Messung gehörenden Zeitpunkt.

2. Der prädizierte Zustand wird mit der Messung korrigiert.

3. Es wird vom Zeitpunkt der Messung in die Gegenwart t prädiziert. Schritt 3 stellt die eigentliche Zeitverzugskompensation dar.

Vorteilhaft bei der Sensordatenfusion ist der modulare Aufbau des Korrekturschritts im Filter. Durch eine globale Datenstruktur wird erreicht, dass nach jedem Prädiktionsschritt der Filterzustand und die Kovarianzmatrix an einer zentralen Stelle abgelegt werden. Erhält der Filter keine Messungen, wird der Zustand nur über die Prädiktion bereitgestellt. Dabei wächst die Unwissenheit über den Zustand, was sich in der Kovarianzmatrix durch unbegrenzt wachsende Einträge bemerkbar macht. Liegen während einer Filteriteration Messungen vor, werden diese genutzt, um den prädizierten Zustand zu korrigieren. Dabei besitzt jeder Sensor eine eigene Messgleichung, die nur auf die vom Sensor gemessenen Zustand wirkt. Nebst obenstehender schriftlicher Offenbarung wird explizit auf die Figuren 1 bis

5 verwiesen.