DE102008008499A1

DE102008008499A1 - Verfahren zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten

Info

Publication number: DE102008008499A1
Application number: DE200810008499
Authority: DE
Inventors: Wendelin Dr. Feiten; Michael Fiegert
Original assignee: Siemens AG
Current assignee: Siemens Healthcare GmbH
Priority date: 2008-02-11
Filing date: 2008-02-11
Publication date: 2009-08-27
Anticipated expiration: 2028-02-12
Also published as: DE102008008499B4; WO2009101030A1; US20100318320A1; CN101939666B; JP5210396B2; US8798957B2; JP2011511943A; CN101939666A

Abstract

Die Erfindung betrifft ein Verfahren zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten eines auf dem Objekt befindlichen Sensors, wobei die Sensordaten zu verschiedenen Zeitpunkten erfasste Messpunktmengen aus Messpunkten der Umgebung des Objekts enthalten und wobei Objektbewegung zwischen einer ersten, zu einem ersten Zeitpunkt erfassten Messpunktmenge und einer zweiten, zu einem zweiten Zeitpunkt erfassten Messpunktmenge ermittelt wird. Das Verfahren zeichnet sich dadurch aus, dass aus der ersten und zweiten Messpunktmenge zunächst Strukturinformationen in der Form von Strukturelementen, wie z. B. Linien, Kreise und dergleichen, extrahiert werden und zusammen mit den keinem Strukturtyp zuordenbaren Messpunkten gespeichert werden. Anschließend werden Assoziationen zunächst zwischen den Strukturelementen vom gleichen Strukturtyp bestimmt und eine entsprechende Transformation zur Abbildung der Strukturelemente aufeinander ermittelt. Schließlich erfolgt in einem nächsten Schritt eine Assoziation der unzuordenbaren Messpunkte mit Strukturelementen und eine entsprechende Transformation der assoziierten Messpunkte und Strukturelemente aufeinander. Durch das erfindungsgemäße Verfahren wird ein hybrider Scan geschaffen, auf dessen Basis die Eigenbewegung des Objekts bestimmt werden kann. Die Assoziation der Strukturelemente bzw. Messpunkte erfolgt dabei vorzugsweise mittels statistischer Verfahren, bei denen das Messrauschen des Sensors berücksichtigt ...

Description

Die Erfindung betrifft ein Verfahren zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten sowie eine entsprechende Vorrichtung und ein entsprechendes Computerprogrammprodukt.
Die Erfindung betrifft das technische Gebiet der Ermittlung der Eigenbewegung von mobilen Objekten, wie z. B. Fahrzeugen, Flugzeugen, Robotern und dergleichen. Bei der autonomen Bewegung solcher Objekte ist es erforderlich, dass die Position und Orientierung des Objekts im Raum, z. B. in einer zweidimensionalen Ebene der Bewegung, bestimmt wird. Zur Positionsbestimmung werden auf dem Objekt befindliche Messsensoren, beispielsweise Laserscanner, eingesetzt. Diese Sensoren erfassen die Umgebung des Objekts und erzeugen hierdurch eine Menge von Messpunkten, die sich beispielsweise aus der Laufzeit von an Gegenständen der Umgebung reflektierten Laserstrahlen eines Laserscanners ergeben.
Werden Laserscanner zur Positionsbestimmung verwendet, sind diese meistens so montiert, dass sie parallel zum Boden einen Lichtstrahl kreisen lassen und den Abstand zum nächsten reflektierenden Punkt messen. Auf diese Weise erhält man eine Folge zweidimensionaler Messpunkte in den Koordinaten des autonomen Objekts, wobei diese Folge auch als Scan bezeichnet wird. Um aus diesen Daten die Eigenbewegung des Objekts zu bestimmen, ist es aus dem Stand der Technik bekannt, einen zu einem früheren Zeitpunkt vorgenommenen Scan mit einem aktuellen Scan zu vergleichen. Es wird hierbei eine Transformation gesucht, welche diese beiden Scans möglichst gut aufeinander abbildet.
Im Stand der Technik gibt es verschiedene Standardverfahren zur Bestimmung der Eigenbewegung von Objekten basierend auf dem Vergleich von Messpunktmengen zu verschiedenen Zeitpunkten. Ein Überblick über gängige Verfahren findet sich hierbei in der Druckschrift [1].
Die bekannten Verfahren lassen sich im Wesentlichen in zwei Gruppen einteilen. Die erste Gruppe der Verfahren extrahiert Strukturelemente, insbesondere Linien, aus den Messpunktmengen und sucht nach einer Transformation, welche diese Strukturelemente möglichst gut aufeinander abbildet. Die zweite Gruppe verarbeitet Rohdaten in der Form der erfassten Messpunkte. Die erste Gruppe arbeitet genau und zuverlässig in strukturierten Umgebungen, wie z. B. in Innenräumen. Die zweite Gruppe ist hingegen universeller einsetzbar, jedoch ungenau und rechenaufwändiger. Beide Verfahrensklassen funktionieren schlecht, wenn die Umgebung nur teilweise strukturiert ist, beispielsweise wenn nur eine einzelne Wand extrahiert werden kann. In diesem Fall kann ein Verfahren gemäß der ersten Gruppe eine Verschiebung des Objekts parallel zur Wand nicht extrahieren, während ein Verfahren der zweiten Gruppe dazu neigt, sich an zufällig, jedoch falsch zugeordneten Messpunktpaaren der miteinander verglichenen Messpunktmengen festzuhalten.
Aufgabe der Erfindung ist es, ein Verfahren zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten zu schaffen, welches einfach und zuverlässig die Eigenbewegung des Objekts bestimmt.
Diese Aufgabe wird durch die Merkmale des unabhängigen Anspruchs 1, des Anspruchs 26 oder des Anspruchs 27 gelöst. Weiterbildungen der Erfindung sind in den abhängigen Ansprüchen definiert.
In dem erfindungsgemäßen Verfahren werden Sensordaten eines auf einem Objekt befindlichen Sensors verarbeitet, wobei die Sensordaten zu verschiedenen Zeitpunkten erfasste Messpunktmengen aus Messpunkten der Umgebung des Objekts enthalten und wobei die Objektbewegung zwischen einer ersten, zu einem ersten Zeitpunkt erfassten Datenmenge und einer zweiten, zu einem zweiten Zeitpunkt erfassten Datenmenge ermittelt wird.
In dem erfindungsgemäßen Verfahren werden in einem ersten Schritt a) Strukturelemente von einem oder mehreren Strukturtypen aus der ersten Messpunktmenge extrahiert, wodurch eine erste Datenmenge erhalten wird. Analog werden aus der zweiten Messpunktmenge Strukturelemente extrahiert, wodurch eine zweite Datenmenge erhalten wird. Die erste und zweite Datenmenge enthalten dabei jeweils die extrahierten Strukturelemente und unzuordenbare Messpunkte, welche keinem Strukturelement zugewiesen werden können.
In einem Schritt b) werden dann die Strukturelemente vom gleichen Strukturtyp der zweiten Datenmenge mit den entsprechenden Strukturelementen der ersten Datenmenge assoziiert. Sofern Assoziationen aufgefunden werden, wird basierend auf den Assoziationen eine Transformationsfolge (welche gegebenenfalls nur eine einzige Transformation oder bei nicht vorhandener Assoziation eine Null-Transformation ohne Lageveränderung umfassen kann) zur Abbildung der Strukturelemente der zweiten Datenmenge auf die Strukturelemente der ersten Datenmenge durchgeführt, wodurch eine dritte Datenmenge erhalten wird.
Anschließend werden in einem Schritt c) die Strukturelemente der dritten Datenmenge mit den unzuordenbaren Messpunkten der ersten Datenmenge assoziiert. Sofern Assoziationen aufgefunden werden, wird basierend auf den Assoziationen eine zweite Transformationsfolge (welche gegebenenfalls nur eine einzige Transformation oder bei nicht vorhandenen Assoziationen eine Null-Transformation ohne Lageveränderung enthalten kann) zur Abbildung der Strukturelemente der dritten Datenmenge auf die unzuordenbaren Messpunkte der ersten Datenmenge durchgeführt, wodurch eine vierte Datenmenge erhalten wird.
Basierend auf zumindest der ersten und zweiten Transformationsfolge wird schließlich die Bewegung des Objekts zwischen dem ersten und zweiten Zeitpunkt bestimmt.
Das erfindungsgemäße Verfahren zeichnet sich dadurch aus, dass in dem Verfahren in geeigneter Weise sowohl Strukturinformationen in der Form von Strukturelementen als auch nicht zuordenbare Messpunkte verarbeitet werden. Das Verfahren vereint somit die Vorteile der beiden eingangs beschriebenen Verfahren der ersten und zweiten Gruppe, wobei zunächst nach Übereinstimmungen von extrahierten Strukturelementen gesucht wird und anschließend die sich daraus ergebende Transformation durch den Vergleich von unzuordenbaren Messpunkten mit Strukturelementen verfeinert wird. Auf diese Weise wird ein Verfahren geschaffen, welches einfach und mit wenigen oder keinen Fehlassoziationen die Eigenbewegung eines Objekts durch den Vergleich von zwei erfassten Messpunktmengen bestimmen kann.
In einer bevorzugten Ausführungsform des erfindungsgemäßen Verfahrens sind die extrahierten Strukturelemente zumindest vom Typ Linie. Vorzugsweise werden im Schritt b) des Verfahrens als erstes die Strukturelemente vom Typ Linie extrahiert, wobei zur Extraktion bekannte Verfahren verwendet werden können, welche beispielsweise in dem Dokument [1] beschrieben sind.
In einer bevorzugten Variante des erfindungsgemäßen Verfahrens umfassen die extrahierten Strukturelemente auch weitere Strukturtypen, insbesondere den Strukturtyp Kreissegment. Durch die Verwendung von weiteren Strukturtypen wird eine genauere Assoziation zwischen Strukturelementen und somit eine genauere Abbildung der Datenmengen aufeinander erreicht.
In einer bevorzugten Variante des erfindungsgemäßen Verfahrens werden in Schritt a) zunächst Zusammenhangskomponenten umfassend aufeinander folgende Messpunkte ermittelt, wobei benachbarte, zu einer Zusammenhangskomponente gehörende Messpunkte einen maximalen Punktabstand nicht überschreiten. Besonders einfach und effizient kann dabei aus einer Zusammenhangskomponente ein Strukturelement vom Strukturtyp Kreissegment wie folgt extrahiert werden:
Zunächst wird ein Kreis berechnet, auf dem die an den jeweiligen Enden der Zusammenhangskomponente liegenden Randmesspunkte sowie ein Messpunkt zwischen den Randmesspunkten der Zusammenhangskomponente, insbesondere im Wesentlichen in der Mitte zwischen den Randmesspunkten, angeordnet sind. Falls der minimale Abstand jedes Messpunkts der Zusammenhangskomponente zum Kreis kleiner als ein vorgegebener Schwellwert ist, werden die Messpunkte der Zusammenhangskomponente dem berechneten Kreis zugeordnet, und ansonsten wird die Zusammenhangskomponente am Messpunkt der Zusammenhangskomponente mit dem größten minimalen Abstand von dem Kreis in zwei Zusammenhangskomponenten aufgeteilt.
Das obige Verfahren liefert durch einfache geometrische Betrachtungen in kurzer Zeit geeignete Kreissegmente, welche als Strukturelemente verarbeitet werden können.
In einer besonders bevorzugten Ausführungsform des erfindungsgemäßen Verfahrens wird in den Schritten b) und c) mittels geeigneter statistischer Verfahren auch das Messrauschen des Sensors berücksichtigt, wodurch die Genauigkeit der Bestimmung der Eigenbewegung des Objekts erhöht wird. Vorzugsweise fließt dieses Messrauschen in der Form einer Kovarianzmatrix sowie der entsprechenden Mahalanobis-Distanz ein, wobei diese Größen dem Fachmann hinlänglich bekannt sind. Die erste Transformationsfolge des Schritts b) wird dabei insbesondere durch eine Iteration wie folgt bestimmt:
In einem Schritt b.i) werden für eine Menge von Strukturelement-Paaren, welche jeweils ein Strukturelement einer transformierten zweiten Datenmenge und ein Strukturelement der ersten Datenmenge umfassen, die von einer momentanen Kovarianzmatrix abhängigen Mahalanobis-Distanzen der Transformationen berechnet, welche jeweils die Strukturelemente eines Strukturelement-Paars aufeinander abbilden, wobei unter Berücksichtigung der Mahalanobis-Distanzen ein Strukturelement-Paar ausgewählt wird und eine Transformation zur Abbildung der Strukturelemente des ausgewählten Strukturelement-Paars aufeinander ausgeführt wird, wodurch eine neue transformierte zweite Datenmenge erhalten wird.
In einem Schritt b.ii) wird aus der Menge der Strukturelement-Paare zumindest das ausgewählte Strukturelement-Paar gestrichen, die Kovarianz wird aktualisiert und es wird zu Schritt b.i) zurückgegangen.
Bei dem obigen iterativen Verfahren werden bei der Auswahl der Transformationen in Schritt b.i) vorzugsweise Transformationen mit kleinen Mahalanobis-Distanzen und/oder Transformationen zwischen langen Strukturelementen und/oder Anhäufungen von Transformationen bevorzugt.
Die in Schritt b.i) durchgeführte Transformation wird in einer bevorzugten Ausführungsform der Erfindung geschätzt, insbesondere basierend auf dem Kalman-Filter. Die Aktualisierung der Kovarianzmatrix in Schritt b.ii) erfolgt besonders effizient ebenfalls durch die Verwendung des Kalman-Filters, wobei die Kalman-Filter-Update-Formel zur Neuschätzung der Kovarianzmatrix eingesetzt wird.
Vorzugsweise werden in Schritt b.ii) aus der Menge der Strukturelemente auch solche Strukturelement-Paare gestrichen, deren Strukturelemente durch eine Transformation aufeinander abgebildet werden, deren von der aktualisierten Kovarianz matrix abhängige Mahalanobis-Distanz größer als ein vorgegebener Schwellwert ist.
In einer weiteren Ausgestaltung des erfindungsgemäßen Verfahrens entspricht zu Beginn der Iteration zur Bestimmung der ersten Transformationsfolge die transformierte zweite Datenmenge der in Schritt a) erhaltenen zweiten Datenmenge und die Menge der Strukturelement-Paare beinhaltet alle möglichen Paare, welche ein Strukturelement der ersten Datenmenge und der zweiten Datenmenge umfassen, bzw. eine Vorauswahl von Strukturelement-Paaren, wobei die Vorauswahl solche Strukturelement-Paare umfasst, deren Strukturelemente durch eine Transformation aufeinander abgebildet werden, deren von einer anfänglichen Kovarianzmatrix abhängige Mahalanobis-Distanz kleiner oder gleich einem vorgegebenen Schwellwert ist. In einer besonders bevorzugten Ausführungsform wird die anfängliche Kovarianzmatrix dabei basierend auf dem Messrauschen einer odometrischen Messung oder aus einem Bewegungsmodell bestimmt.
In einer weiteren Ausgestaltung des erfindungsgemäßen Verfahrens wird die zweite Transformationsfolge des Schritts c) durch eine Iteration wie folgt bestimmt:
In einem Schritt c.i) werden für eine Menge von Strukturelement-Messpunkt-Paaren, welche jeweils ein Strukturelement einer transformierten dritten Datenmenge und einen unzuordenbaren Messpunkt der ersten Datenmenge umfassen, die von einer Kovarianzmatrix abhängigen Mahalanobis-Distanzen der Transformationen berechnet, welche jeweils das Strukturelement auf den unzuordenbaren Messpunkt eines Strukturelement-Messpunkt-Paars abbilden, und unter Berücksichtigung der Mahalanobis-Distanzen wird ein Strukturelement-Messpunkt-Paar ausgewählt und eine Transformation des Strukturelements auf dem Messpunkt des ausgewählten Strukturelement-Messpunkt-Paars ausgeführt, wodurch eine neue transformierte dritte Datenmenge erhalten wird.
In einem Schritt c.ii) wird aus der Menge der Strukturelement-Messpunkt-Paare zumindest das ausgewählte Strukturelement-Messpunkt-Paar gestrichen, die Kovarianzmatrix wird aktualisiert und es wird zu Schritt c.i) zurückgegangen.
Vorzugsweise werden in dem obigen iterativen Verfahren bei der Auswahl der Transformationen in Schritt c.i) Transformationen mit kleinen Mahalanobis-Distanzen und/oder Anhäufungen von ähnlichen Transformationen bevorzugt. Die in Schritt c.i) durchgeführte Transformation wird in einer bevorzugten Ausführungsform der Erfindung geschätzt, insbesondere basierend auf dem Kalman-Filter. Auch die Aktualisierung der Kovarianzmatrix kann wiederum mit Hilfe des Kalman-Filters durchgeführt werden.
In einer bevorzugten Variante des iterativen Verfahrens werden in Schritt c.ii) Strukturelement-Messpunkt-Paare, bei denen das Strukturelement auf einen unzuordenbaren Messpunkt durch eine Transformation abgebildet wird, deren von der aktualisierten Kovarianzmatrix abhängige Mahalanobis-Distanz größer als ein vorgegebener Schwellwert ist, auch aus der Menge der Strukturelement-Messpunkt-Paare gestrichen.
In einer weiteren Variante des erfindungsgemäßen Verfahrens entspricht zu Beginn der Iteration zur Bestimmung der zweiten Transformationsfolge die transformierte dritte Datenmenge der in Schritt b) erhaltenen dritten Datenmenge und die Menge der Strukturelement-Messpunkt-Paare beinhaltet alle möglichen Paare, welche ein Strukturelement der dritten Datenmenge und einen unzuordenbaren Messpunkt der ersten Datenmenge umfassen, oder eine Vorauswahl von Strukturelement-Messpunkt-Paaren, wobei die Vorauswahl solche Strukturelement-Messpunkt-Paare umfasst, bei denen das Strukturelement auf einen unzuordenbaren Messpunkt durch eine Transformation abgebildet wird, deren von der aktuellen Kovarianzmatrix abhän gige Mahalanobis-Distanz kleiner oder gleich einem vorgegebenen Schwellwert ist.
Das erfindungsgemäße Verfahren umfasst in einer bevorzugten Variante ferner nach Durchführung des Schritts c) auch eine Suche nach Assoziationen von noch übrigen nicht zuordenbaren Messpunkten, wodurch eine noch präzisere Bestimmung der Abbildung der Datenmengen aufeinander erreicht wird. Die verbleibenden unzuordenbaren Messpunkte sind dabei in der oben definierten vierten Datenmenge enthalten und diese Messpunkte werden mit den unzuordenbaren Messpunkten der ersten Datenmenge assoziiert, sofern geeignete Assoziationen gefunden werden. Basierend auf den Assoziationen wird dann eine dritte Transformationsfolge (welche gegebenenfalls nur eine einzige Transformation oder bei nicht vorhandenen Assoziationen eine Null-Transformation ohne Lageveränderung umfassen kann) zur Abbildung der unzuordenbaren Messpunkte der vierten Datenmenge auf die unzuordenbaren Messpunkte der ersten Datenmenge durchgeführt, wodurch eine fünfte Datenmenge erhalten wird, wobei die Bewegung des Objekts zwischen dem ersten und zweiten Zeitpunkt basierend auf der ersten und zweiten und dritten Transformationsfolge bestimmt wird.
In der oben beschriebenen Variante des erfindungsgemäßen Verfahrens wird zur Bestimmung der dritten Transformationsfolge insbesondere eine Iteration mit folgenden Schritten durchgeführt:
In einem Schritt i) werden für eine Menge an Messpunkt-Paaren, welche jeweils einen unzuordenbaren Messpunkt einer transformierten fünften Datenmenge und einen unzuordenbaren Messpunkt der ersten Datenmenge umfassen, die von einer Kovarianzmatrix abhängigen Mahalanobis-Distanzen der Transformationen berechnet, welche die unzuordenbaren Messpunkte eines Messpunkt-Paars aufeinander abbilden, und unter Berücksichtigung der Mahalanobis-Distanzen wird ein Messpunkt-Paar ausgewählt und eine Transformation zur Abbildung der Messpunkte des ausgewählten Messpunkt-Paars aufeinander ausgeführt, wodurch eine neue transformierte fünfte Datenmenge erhalten wird.
In einem Schritt ii) wird aus der Menge der Messpunkt-Paare zumindest das ausgewählte Messpunkt-Paar gestrichen, die Kovarianzmatrix wird aktualisiert und es wird zu Schritt i) zurückgegangen. Bei der Auswahl der Transformationen im obigen Schritt i) werden vorzugsweise Transformationen mit kleinen Mahalanobis-Distanzen und/oder Anhäufungen von ähnlichen Transformationen bevorzugt.
Bei der obigen Bestimmung der fünften Datenmenge wird vorzugsweise wiederum mit Hilfe von statistischen Verfahren das Messrauschen des Sensors berücksichtigt. Die in Schritt i) durchgeführte Transformation wird in einer bevorzugten Ausführungsform der Erfindung geschätzt, insbesondere basierend auf dem Kalman-Filter. Auch die Aktualisierung der Kovarianzmatrix erfolgt vorzugsweise wiederum mit dem Kalman-Filter.
In einer weiteren Variante des erfindungsgemäßen Verfahrens werden Sensordaten in der Form von Laserscans verarbeitet, welche mit einem Laserscanner auf dem Objekt erfasst wurden. Ein bevorzugtes Anwendungsgebiet des erfindungsgemäßen Verfahrens ist die Bestimmung der Bewegung eines Roboters und/oder eines Krans und/oder eines Fahrzeugs, insbesondere zur autonomen Navigation solcher Objekte.
Neben dem oben beschriebenen Verfahren umfasst die Erfindung ferner eine Vorrichtung zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten eines auf dem Objekt befindlichen Sensors, wobei die Sensordaten zu verschiedenen Zeitpunkten erfasste Messpunktmengen aus Messpunkten in der Umgebung des Objekts enthalten, wobei die Vorrichtung ein Mittel zur Bestimmung der Objektbewegung zwischen einer ersten, zu einem ersten Zeitpunkt erfassten Messpunktmenge und einer zweiten, zu einem zweiten Zeitpunkt erfassten Mess punktmenge aufweist. Dieses Mittel ist derart ausgestaltet, dass im Betrieb der Vorrichtung das oben beschriebene erfindungsgemäße Verfahren durchgeführt wird.
Die Erfindung betrifft darüber hinaus ein Computerprogrammprodukt mit einem auf einem maschinenlesbaren Träger gespeicherten Programmcode zur Durchführung des oben beschriebenen erfindungsgemäßen Verfahrens, wenn das Programm auf einem Rechner abläuft.
Ausführungsbeispiele der Erfindung werden nachfolgend anhand der beigefügten Figuren detailliert beschrieben.
Es zeigen:
1 eine schematische Darstellung, welche die Extraktion von Zusammenhangskomponenten gemäß einer Ausführungsform des erfindungsgemäßen Verfahrens verdeutlicht;
2 eine schematische Darstellung, welche die Extraktion von Kreisen gemäß einer Ausführungsform des erfindungsgemäßen Verfahren zeigt; und
3 bis 7 schematische Darstellungen, welche den Ablauf des erfindungsgemäßen Verfahrens zur Assoziation von Strukturelementen und Messpunkten gemäß einer Ausführungsform des erfindungsgemäßen Verfahrens wiedergibt.
In der hier beschriebenen Ausführungen des erfindungsgemäßen Verfahrens werden Sensordaten in der Form von zweidimensionalen Punkten betrachtet, welche mit einem Laserscanner als sog. Laserscans erfasst wurden. Die einzelnen Punkte in den Scans ergeben sich dabei durch die Reflexionen des sich wäh rend des Scans bewegenden Laserstrahls an den Gegenständen in der Umgebung des Objekts, auf dem der Laserscanner angeordnet ist. Ziel des nachfolgend beschriebenen Verfahrens ist es, die Eigenbewegung des Objekts dadurch zu bestimmen, dass zu verschiedenen Zeitpunkten ermittelte Laserscans aufeinander transformiert werden und dadurch die Bewegung des Objekts zwischen diesen Zeitpunkten bestimmt wird.
Um überhaupt zwei unterschiedliche Laserscans miteinander vergleichen zu können, werden in der hier beschriebenen Ausführungsform des erfindungsgemäßen Verfahrens Strukturelemente in der Form von Linien und Kreisen aus den einzelnen Messpunkten des jeweiligen Scans ermittelt. Zur Extraktion dieser Strukturelemente werden zunächst Zusammenhangskomponenten in der Form von Punktefolgen ermittelt, und anschließend werden entsprechende Strukturelemente aus den Zusammenhangskomponenten bestimmt.
Das Verfahren der Erfindung zeichnet sich dadurch aus, dass bei der Transformation von zwei Scans aufeinander nicht nur die extrahierten Strukturelemente, sondern auch nicht zuordenbare Messpunkte, d. h. Messpunkte, denen kein Strukturelement zugewiesen werden kann, verarbeitet werden. Somit werden hybride Scans umfassend Strukturelemente und Messpunkte verarbeitet, wobei die Elemente in einem solchen hybriden Scan in der hier beschriebenen Ausführungsform durch die Menge der Punkte P, der Linien L und der Kreise C charakterisiert ist. Für einen Scan S mit Elementen e_i gilt somit: S = {ei|ei ∊ {P, L, C}}i=1,...,n
Wie bereits oben erwähnt, müssen zur Extraktion von Linien und Kreisen zunächst zusammenhängende Stücke von Messpunkten in dem Scan bestimmt werden. 1 verdeutlicht hierbei einen entsprechenden Algorithmus zur Aufteilung des Scans in zusammenhängende Stücke anhand von Messpunkten, welche entlang einer Wand W angeordnet sind. Zusammenhängende Bereiche von Messpunkten werden dabei durch einen maximalen Punktabstand derart charakterisiert, dass Messpunkte nur dann zu einer zusammenhängenden Komponente gehören, wenn ihr Abstand kleiner oder gleich dem maximalen Punktabstand ist. Der maximale Punktabstand zwischen zwei Punkten 1 und 2 an der Wand W ist in 1 mit amax bezeichnet. Das Sensorsystem S mit den Achsen x und y erfasst die Punkte 1 bzw. 2 über die benachbarten Strahlen B1 und B2. Der Abstand a benachbarter Messpunkte auf der Wand W ergibt sich nach dem Sinussatz aus der Winkeldistanz δ / 2 zwischen dem linken Strahl B1 bzw. dem rechten Strahl B2 und dem hypothetischen, in 1 gestrichelt gezeichneten Mittelstrahl B3 und dem Winkel γ zwischen diesem Mittelstrahl und der Wand W sowie aus den beiden mit dem Sensorsystem S gemessenen Abständen von dem Ursprung des Sensorsystems zu dem Punkt 1 (als d₁ bezeichnet) und von dem Ursprung des Sensorsystems S zu dem Punkt 2 (als d₂ bezeichnet).
Insbesondere gilt:
Der Winkel γ zwischen Mittelstrahl B3 und der Wand W wird als Parameter gewählt und hierdurch erhält man für jedes benachbarte Punktepaar einen oberen Grenzwert amax des Abstands zwischen benachbarten Punkten auf der Wand.
Mit Hilfe des Abstands amax erfolgt dann die Extraktion von zusammenhängigen Komponenten basierend auf folgendem Verfahren:
Zunächst wird der erste Messpunkt des Scans in die erste, aktuelle Zusammenhangskomponente eingeordnet. Die aktuelle Zusammenhangskomponente wird dann um den nächsten Punkt erweitert, wenn der Abstand zwischen dem letzten Punkt der Zusam menhangskomponente und dem nächsten Punkt kleiner oder gleich dem maximalen Abstand amax ist. Wenn der nächste Punkt jedoch einen Abstand größer als der maximale Abstand amax zum letzten Punkt aufweist, wird der nächste Punkt einer neuen, dann aktuellen Zusammenhangskomponente zugeordnet. Nicht zugeordnete Messpunkte bleiben im Scan und werden nicht verworfen. Zusammenhangskomponenten, die weniger als eine minimale Anzahl von einzelnen Punkten enthalten, werden wieder gelöscht und als nicht zuordenbare Messpunkte in den Scan rücksortiert.
Nach der Ermittlung aller Zusammenhangskomponenten werden schließlich die geometrischen Merkmale in der Form von Linien und Kreisen aus den einzelnen Zusammenhangskomponenten extrahiert. Dabei wird versucht, jede der Zusammenhangskomponenten zum einen durch Linien und zum anderen durch Kreise zu approximieren. Ferner kann man auch andere Elemente, wie z. B. rechtwinklige Ecken, aus dem Scan extrahieren. Die Extraktion von Linien aus einem Scan ist dem Fachmann hinlänglich bekannt und wird beispielsweise in der Druckschrift [1] beschrieben. In der hier beschriebenen Ausführungsform werden zunächst Linien mit einem gängigen Verfahren aus dem Scan extrahiert. Anschließend erfolgt eine rekursive Extraktion von Kreisen, wobei diese Extraktion nachfolgend anhand von 2 erläutert wird.
2 zeigt im oberen Teil eine Zusammenhangskomponente aus Messpunkten P1, P2, ..., P12. Zur Extraktion von Kreisen bzw. Kreissegmenten aus dieser Menge an Punkten erfolgt eine Zerlegung der jeweiligen Zusammenhangskomponenten in Kreise, wobei im Falle, dass eine betrachtete Zusammenhangskomponente zu kurz wird, d. h. wenn ihre Anzahl an Punkten unterhalb eines Schwellenwerts liegt, die Messpunkte dieser Komponente wieder in den Scan zurücksortiert werden. Zur Extraktion von Kreisen aus der Zusammenhangskomponente der 2 werden zunächst die Randpunkte P1 und P12 sowie ein mittlerer Punkt P8 der Zusammenhangskomponente betrachtet. Aus diesen drei Punk ten lässt sich eindeutig ein Kreis bestimmen, dessen Umfang durch alle drei Punkte läuft. Diese Bestimmung erfolgt durch das jeweilige Verbinden der Randpunkte P1 bzw. P12 mit dem mittleren Punkt P8 über eine Gerade und das Bestimmen der Mittelsenkrechten der jeweiligen Geraden, wobei der Schnittpunkt der Mittelsenkrechten der Mittelpunkt des Kreises ist.
Es ergibt sich somit zunächst ein Kreis mit der entsprechenden Kreislinie C, auf der die Punkte P1, P8 und P12 liegen. Von diesem Kreis ist der Radius r und dessen Mittelpunkt c = (c_x c_y) bekannt. In einem nächsten Schritt wird dann das Quadrat des Abstands jedes Punktes der Zusammenhangskomponente von dem Kreis berechnet, d. h. es wird folgender Abstand berechnet:
p_x und p_y bezeichnen hierbei die x- bzw. y-Koordinate des jeweiligen Punktes der Zusammenhangskomponente. Wenn nunmehr alle Abstände der Punkte der Zusammenhangskomponente unter einem vorgegebenen Schwellwert liegen, wird das Segment als Kreissegment klassifiziert.
In der in 2 gezeigten Ausführungsform sind die einzelnen Abstände d(p, c, r) als senkrechte Verbindungslinien zwischen den Punkten und der Kreislinie angedeutet, wobei in diesem Beispiel der Abstand zwischen dem Punkt P6 und der Kreislinie den vorgegebenen Schwellwert überschreiten, was dadurch angedeutet wird, dass der Punkt P6 schwarz gefüllt wiedergegeben ist. Die Zusammenhangskomponente kann somit nicht als Kreis klassifiziert werden, und es erfolgt deshalb in einem nächsten Schritt eine Aufteilung der Zusammenhangskomponente am Punkt des maximalen Abstands, d. h. in 2 am Punkt P6. Dies ist im unteren Teil der 2 angedeutet, wobei nunmehr zwei Zusammenhangskomponenten vorliegen. Die erste Zusammenhangskomponente besteht dabei aus den Punkten P1 bis P6 und die zweite Zusammenhangskomponente aus den Punkten P6 bis P12. Nunmehr wird für jede Zusammenhangskomponente analog wie oben beschrieben verfahren, d. h. basierend auf den Randpunkten P1 und P6 sowie dem mittleren Punkt P3 der ersten Zusammenhangskomponente wird ein entsprechender Kreis bestimmt und basierend auf den Randpunkten P6 und P12 sowie dem mittleren Punkt P9 der zweiten Zusammenhangskomponente wird ein Kreis bestimmt. In dem Beispiel der 2 liegen die Abstände der einzelnen Punkte von der Kreislinie für beide Zusammenhangskomponenten unterhalb des vorgegebenen Schwellwerts, so dass die beiden Segmente als Kreise klassifiziert werden.
Der oben beschriebene Algorithmus neigt zur Übersegmentierung, d. h., dass statt eines Kreisbogens zwei aneinander stoßende Kreisbögen mit etwa gleichen Mittelpunkten und Radius gefunden werden. Diese können anschließend wieder zusammengefasst werden. Ebenso wie bei der Linienextraktion können anschließend Mittelpunkte und Radius des Kreises entsprechend der Zusammenhangskomponente genau geschätzt werden, beispielsweise durch Bestimmung des folgenden Minimums:
Dabei bezeichnet Z die Menge der Punkte der Zusammenhangskomponente. Es können hierbei aus dem Stand der Technik bekannte Optimierungsverfahren, wie das Newton-Verfahren, das Quasi-Newton-Verfahren oder konjugierte Gradienten zum Auffinden des Minimums eingesetzt werden.
Wenn Punkte in einem Scan sowohl in einer extrahierten Linie als auch in einem extrahierten Kreissegment auftauchen, wird der Konflikt im Überlappungsbereich zwischen Linie und Kreissegment aufgrund der Länge der einzelnen Segmente und der Summe der jeweiligen Abweichungen im Überlappungsbereich aufgelöst. Ein längeres Segment (gemessen durch die Anzahl der Punkte) wird dabei bevorzugt.
Nachdem ein hybrider Scan, d. h. ein Scan mit extrahierten Linien, Kreisen und nicht zuordenbaren Messpunkten, erhalten wurde, wird schließlich ein Scan-Matching-Algorithmus zur Abbildung der Strukturelemente und Messpunkte von zwei Scans aufeinander durchgeführt. Dieser Scan-Matching-Algorithmus wird zunächst allgemein anhand von 3 bis 7 beschrieben und anschließend erfolgt eine detaillierte Erläuterung des Algorithmus und der dabei verwendeten mathematischen Verfahren.
3 zeigt einen ersten Scan, der auch als Referenzscan bezeichnet wird und durch Punkte in der Form von Kreuzen angedeutet ist, wobei einige der Punkte mit Bezugszeichen M1 bis M5 versehen sind. Ferner ist ein zweiter Scan wiedergegeben, der durch Punkte in der Form von Kreisen angedeutet ist, wobei einige der Punkte mit Bezugszeichen M1' bis M5' versehen sind. Ziel ist es nunmehr, eine Transformation zu bestimmen, durch welche die Punkte des zweiten Scans auf den Referenzscan abgebildet werden. Die Abbildung der Scans aufeinander erfolgt dabei unter Berücksichtigung des Messrauschens des Sensors, wodurch die Wahrscheinlichkeit einer Fehlinterpretation entsprechender Assoziationen von Punkten und Strukturelementen zwischen den einzelnen Scans vermindert wird.
In der hier beschriebenen Ausführungsform erfolgt die Bestimmung der Transformation, welche die beiden Scans aufeinander abbildet, schrittweise durch Ermittlung einzelner Transformationen einer Transformationsfolge, wobei das Messrauschen in der Form der Bewegungskovarianzmatrix der einzelnen Transformationen der Transformationsfolge berücksichtigt wird. Eine Transformation wird hierbei durch einen Vektor beschrieben, der als x- und y-Komponente die Translation gemäß der Transformation und als weitere Komponente die Rotation der Transformation in der Form eines Winkels β enthält. Für eine solche Transformation lässt sich die Kovarianzmatrix bestimmen, wobei die Komponenten der Transformation als Zufallsvariablen betrachtet werden und jeder Eintrag der Kovarianzmatrix die Kovarianzen der entsprechenden Einträge beschreibt. Die Kovarianz ist dabei definiert als der Erwartungswert aus dem Produkt der Differenz zwischen einer ersten Zufallsvariablen und deren Erwartungswert und der Differenz zwischen einer zweiten Zufallsvariablen und deren Erwartungswert. Das Messrauschen des Sensors basierend auf den Kovarianzen wird dabei in der hier beschriebenen Ausführungsform basierend auf der bekannten Mahalanobis-Distanz berücksichtigt, welche weiter unten noch näher erläutert wird.
In 3 bis 7 wird die Position des Sensorsystems als Ursprung der beiden Achsen A1 und A2 wiedergegeben. Die aktuelle Bewegungskovarianz bzw. die entsprechende Mahalanobis-Distanz wird durch Kreise bzw. Ellipsen angedeutet. Hierdurch wird die Wahrscheinlichkeit einer Bewegung basierend auf dem Messrauschen wiedergegeben. Im Falle einer Ellipse ist somit eine Bewegung in Richtung der kleinen Halbachse weniger wahrscheinlich als in Richtung der großen Halbachse. Bei einem Kreis sind alle Bewegungsrichtungen gleich wahrscheinlich.
Zu Beginn des Verfahrens wird zunächst eine initiale Kovarianzmatrix vorgegeben, welche beispielsweise aus odometrischen Daten bestimmt wird, d. h. aus der Bewegung der Räder des Objekts, auf dem der Scanner angeordnet ist. Im Beispiel der 3 ist für die anfängliche Kovaranzmatrix jede Transformationsrichtung gleich wahrscheinlich, so dass die Bewegungskovarianz als Kreis C wiedergegeben ist. Zunächst erfolgt in den Scans der 3 die oben beschriebene Extraktion von Strukturelementen. Es wird hierbei für jeden Scan eine Linie extrahiert, welche im Referenzscan die Scanpunkte M1 und M2 und alle dazwischen liegenden Punkte und im zweiten Scan die Punkte M1' und M2' und alle dazwischen liegenden Punkte umfasst.
Nachfolgend werden neben den Linien auch Kreise extrahiert, sofern solche vorhanden sind. In dem Beispiel der 3 finden sich keine Kreise und es liegen lediglich drei weitere, keinem Strukturelement zuordenbare Messpunkte M3, M4 und M5 für den Referenzscan und M3', M4' und M5' für den zweiten Scan vor. Nach dieser Extraktion erfolgt eine Assoziation der extrahierten Linien der beiden Scans zueinander. Hierbei wird die Bewegungskovarianz und die Mahalanobis-Distanz der Transformation der Linien aufeinander berücksichtigt. In dem Beispiel der 3 findet sich je Scan genau eine Linie, wobei die Linien auch so zueinander liegen, dass die Transformation, welche die Linien aufeinander abbildet, gut verträglich mit der Bewegungskovarianz sind, d. h. die entsprechende Transformation weist eine kleine Mahalanobis-Distanz auf. Entsprechend wird der zweite Scan zur Abbildung der Linien aufeinander bewegt, wie in 4 wiedergegeben ist. Nach der Transformation erfolgt eine Aktualisierung der Kovarianzmatrix, wobei hierzu der aus dem Stand der Technik bekannte Kalman-Filter verwendet wird.
Wie sich aus 4 ergibt, sind nunmehr nicht mehr alle Bewegungen gleich wahrscheinlich, wie durch die entsprechende Ellipse E verdeutlicht wird. Insbesondere sind Bewegungen in Richtung der Achse A1 unwahrscheinlicher als in Richtung der Achse A2, d. h. in Richtung der Achse A1 ergeben sich größere Mahalanobis-Distanzen als in Richtung der Achse A2. In 4 erkennt man ferner die extrahierten und aufeinander abgebildeten Linien L sowie die nicht zuordenbaren Messpunkte M3 bis M5 des Referenzscans und M3' bis M5' des zweiten Scans.
Es erfolgt nunmehr in einem nächsten Schritt die Suche nach Assoziationen der nicht zuordenbaren Messpunkte M3' bis M5' zu der Linie L. Da eine weitere Bewegung in Richtung der Achse A1 aufgrund der sehr großen Mahalanobis-Distanz sehr unwahrscheinlich ist, wird durch den Algorithmus keine Assoziation zwischen den nicht zuordenbaren Messpunkten und der Linie L akzeptiert. Deshalb erfolgt in dem Algorithmus nunmehr in einem nächsten Schritt eine Assoziation der nicht zuordenbaren Messpunkte der jeweiligen Scans zueinander. Wie sich aus 4 ergibt, ist hierbei die günstigste Richtung der Transformation die Richtung entlang der Achse A2. Zur Ermittlung der Assoziation zwischen den Messpunkten werden wiederum die möglichen Transformationen zwischen Punktepaaren betrachtet und basierend auf der Mahalanobis-Distanz der Transformationen werden die Punkte M3 und M3' miteinander assoziiert und durch eine erste Transformation aufeinander abgebildet, wobei diese Transformation in 5 durch eine entsprechende Gerade G1 angedeutet ist. Nach Durchführung der Transformation wird wiederum die Kovarianzmatrix aktualisiert.
Die Punkt-zu-Punkt-Assoziation wird schließlich für weitere Punktepaare wiederholt, wobei als nächstes die Messpunkte M4 und M4' miteinander assoziiert werden, wie durch die Gerade G2 in 6 angedeutet ist. Nach Durchführung einer entsprechenden Transformation ergibt sich eine verkleinerte Kovarianzmatrix, was durch eine entsprechende Verkleinerung der Ellipse E in 6 gegenüber der Ellipse E in 5 ersichtlich wird. Abschließend erfolgt eine entsprechende Transformation durch Assoziation der Messpunkte M5' und M5 zueinander, so dass sich insgesamt nach Durchführung der einzelnen Transformationen der Punktepaare insgesamt eine Transformationsfolge ergibt, welche die beiden Scans sehr gut aufeinander abbildet, wie aus 7 ersichtlich ist. Die Kovarianzmatrix ist dabei klein genug, so dass der Scan-Match mit hinreichender Sicherheit als richtig betrachtet werden kann.
Aus der Gesamttransformation aller zuvor durchgeführten Transformationen zur Abbildung einer Linie auf eine Linie und der Messpunkte aufeinander ergibt sich schließlich die Eigenbewegung des Objekts.
Nachfolgend werden die einzelnen Schritte eines Scan-Matching-Algorithmus zur Abbildung von zwei hybriden Scans detailliert erläutert.
Ein Scan S = {e₁, ... e_n} aus Punkten bzw. Strukturelementen e₁ bis e_n, z. B. ein zuvor aufgenommener Referenzscan S^r, wird als fest betrachtet und ein anderer, insbesondere der aktuelle Scan S a / 0, als beweglich. Die Elemente können hierbei Punkte, Geraden oder Kreissegmente sein. Gleichzeitig wird nach Aufruf des Scan-Matching-Algorithmus eine anfängliche Kovarianz Σ₀ der Wahrscheinlichkeitsverteilung der Konfiguration der T_k = (x₀ y₀ β₀)^T = (0 0 0)^T des beweglichen Scans übergeben.
In einem nächsten Schritt werden zunächst in den hybriden Scans die Linien miteinander verglichen. Wenn es in einem der Scans keine Linien gibt, geht es weiter zum Vergleich von Linien mit Punkten, bzw. zum Vergleich von Kreissegmenten, sofern in den hybriden Scans Kreissegmente enthalten sind.
Es werden zum Vergleich von Linien mit Linien pro Linien-Paar (Lai , Lrj ), Lai ∊ Sak , Lrj ∊ Sr eine Menge von Transformationen {Ti,j|Ti,j(Lai ) kollinear Lrj } bestimmt. Anschaulich sind dies die Transformationen, welche die bewegliche Linie parallel zur Referenzlinie im Referenzscan ausrichten und dann auf diese verschieben. Im Raum der Transformationen bildet diese Menge selbst wieder eine Gerade. Jeder Transformation T = (x y β)^T wird die von der momentanen Bewegungskovarianz Σ abhängige Mahalanobis-Distanz
zugeordnet.
Diese Mahalanobis-Distanz enthält für ein Linien-Paar als Parameter die Verschiebung entlang der Linie.
Aus den möglichen Transformationen zwischen Linien wird nun eine Transformation und hierdurch das zugeordnete Linienpaar ausgewählt. Dazu werden folgende Kriterien verwendet:

– Transformationen zwischen langen Linien sind wertvoller als zwischen kurzen;
– Transformationen mit kleiner Norm sind gut;
– Cluster von Transformationen sind gut.

Basierend auf diesen Kriterien lassen sich verschiedene Strategien ableiten. In einer Variante der Erfindung wird die Transformation zwischen der längsten Linie mit einer hinreichenden kleinen Norm ausgewählt. Hierdurch wird das Risiko von Fehlassoziationen minimiert.
In einem nächsten Schritt wird der Scan durch die Transformation T_k+1 verschoben und gedreht, wodurch sich folgender neuer Scan ergibt: Sak+1 = Tk+1(Sak )
Die Transformation wird in der hier beschriebenen Ausführungsform mit einem Schätzverfahren bestimmt. Die geschätzte Transformation stellt dabei eine spezielle Variante der Transformationen bzw. Transformationsfolgen dar, welche gemäß den Ansprüchen durchgeführt bzw. ausgeführt werden. Im Folgenden wird das Schätzverfahren an einem Beispiel erläutert, bei dem eine Linie L a / i des aktuellen Scans auf eine Linie L r / j des Referenzscans geschoben und gedreht wird. Die Lage der Endpunkte der Linie sollte nicht zur Schätzung der Transformation verwendet werden, da sie sehr unsicher ist. Daher werden die Linien entsprechend der Geraden, auf denen sie liegen, durch Länge und Richtung des Lots auf diese Gerade, die sog. Hessesche Normalform L ~ (dφ ) parametrisiert. In der Druckschrift [2] wird die Herleitung dieser Darstellung aus den zur Linie gehörenden Scanpunkten beschrieben. Diese Darstellung wird als „Messung" im Sinn des Kalman-Mechanismus aufgefasst.
In der Druckschrift [2] wird außerdem die Kovarianz einer solchen Linie aus der Kovarianz der Scanpunkte bestimmt. Diese Bestimmung wird weiter unten bei der Ermittlung der Kovarianzen der Referenzlinie Σ_r und der beweglichen Linie Σ_a verwendet.
Der zu schätzende Zustand im Sinne des Kalman-Filters ist die Transformation zwischen aktuellem Scan und Referenzscan. Da mit nimmt
die Rolle der Prädiktion ein und
wird als „Messung" verwendet. Die Kovarianz der Prädiktion setzt sich zusammen aus der Kovarianz der Linienextraktion Σ_a und der bisherigen Kovarianz der Transformation.
Durch die Transformation T_(xyβ) des Objekts wird die Prädiktion ( d / φ) überführt in
Die Jacobische davon ist
Die Kovarianz der Prädiktion ergibt sich aus der Bewegungskovarianz Σ_G = G·Σ_k·G^T. Dazu wird die Kovarianz Σ_a aus der Messunsicherheit addiert.
Damit wird der Kalman-Filter-Gain berechnet zu K_k = Σ_k·G^T·(G·Σ_k·G^T + Σ_a + Σ_r)^–1.
Die Transformation aus dieser Landmarkenassoziation berechnet sich als
Die Bewegungskovarianz Σ_k+1 wird ebenfalls nach der Kalman-Filter-Update-Formel neu geschätzt, und zwar wie folgt: Σ_k+1 = (Id – K_k·G)·Σ_k.
Mit Hilfe der soeben bestimmten Transformation T_k+1 wird der vorhergehende aktuelle Scan transformiert und der resultierende Scan wird zum neuen vorhergehenden Scan, d. h. es gilt: S a / k+1 = T_k+1(S a / k).
Wenn es noch nicht berücksichtigte Paare von Geraden gibt, setzt man k ← k + 1 und geht zurück zu dem obigen Schritt, bei dem ein Linienpaar basierend auf der Mahalanobis-Distanz aus gewählt wird. Es wird hierbei das soeben berücksichtigte Linien-Paar nicht mehr betrachtet.
Nach Durchführung des Vergleichs der Linien werden in dem hybriden Scan Kreise mit Kreisen und schließlich Messpunkte mit Linien verglichen. Der Vergleich von Kreisen mit Kreisen läuft analog zum Vergleich von Linien mit Linien ab, weshalb das Verfahre nicht nochmals beschrieben wird. Beim Vergleich von Messpunkten mit Linien bilden die möglichen Transformationen, die einen Messpunkt auf einer Linie bewegen, eine zweidimensionale Mannigfaltigkeit. Aus praktischen Erwägungen wird immer eine geeignete Vorauswahl getroffen, in der hier beschriebenen Ausführungsform wird die Vorauswahl basierend auf der probabilistischen Norm mit der momentan gültigen Kovarianz bestimmt, d. h. basierend auf:
Die neue Kovarianz ergibt sich analog zum vorhergehenden Fall aus der Kovarianz der Linie, der Kovarianz des Messpunkts und der Funktion, die das Paar aus Messpunkt und Linie auf die gewählte Transformation abbildet.
Die Punkt-Linien-Paare werden wiederum sukzessive abgearbeitet, wobei unplausible Paarungen, d. h. Paarungen mit Mahalanobis-Distanzen oberhalb eines Schwellenwerts, weggelassen werden.
Schließlich werden noch Messpunkt-Paare behandelt, und zwar analog wie die Linien-Paare bzw. die Punkt-Linien-Paare.
Abschließend erhält man somit eine Transformationsfolge, mit der die Abbildung der hybriden Scans aufeinander beschrieben wird und woraus die Eigenbewegung des Objekts bestimmt werden kann.
Die soeben beschriebene Ausführungsform des erfindungsgemäßen Verfahrens weist eine Reihe von Vorteilen auf. Durch die sukzessive Verwendung der möglichen Assoziationen von Strukturelementen bzw. Punkten, sortiert nach dem Risiko einer Fehlassoziation, wird die Unsicherheit einer Fehlzuordnung verringert. Durch die Verwendung des Kalman-Filters innerhalb eines Scan-Matches werden in geeigneter Weise Messunsicherheiten statistisch berücksichtigt. Anstatt des Kalman-Filters können auch andere Beschreibungen der Unsicherheit, beispielsweise stichprobenbasierte Beschreibungen, wie z. B. auf Particle-Filter, eingesetzt werden. Gegebenenfalls kann das Verfahren auch derart ausgestaltet sein, dass die Bewegungsunsicherheit durch gewichtete Maße für die Assoziation von Strukturelementen bzw. Punkten berücksichtigt wird.
Literaturverzeichnis

[1] Jens-Steffen Gutmann: „Robuste Navigation autonomer mobiler Systeme", Dissertation, 2000, Albrecht-Ludwig-Universität Freiburg
[2] Michael Bosse: "ATLAS: A Framework for Large Scale Automated Mapping and Localization", Disseration MASSACHUSETTS INSTITUTE OF TECHNOLOGY, Seiten 57 bis 62, Feb 2004

Claims

Verfahren zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten eines auf dem Objekt befindlichen Sensors, wobei die Sensordaten zu verschiedenen Zeitpunkten erfasste Messpunktmengen aus Messpunkten (P1, ..., P12. M1, ..., M5') der Umgebung des Objekts enthalten, wobei die Objektbewegung zwischen einer ersten, zu einem ersten Zeitpunkt erfassten Messpunktmenge und einer zweiten, zu einem zweiten Zeitpunkt erfassten Messpunktmenge wie folgt ermittelt wird: a) es werden Strukturelemente (L, C) von einem oder mehreren Strukturtypen aus der ersten und zweiten Messpunktmenge extrahiert, wodurch eine erste und zweite Datenmenge erhalten wird, welche jeweils die extrahierten Strukturelemente (L, C) und unzuordenbare Messpunkte (M1, ..., M3') enthalten, die keinem Strukturelement (L, C) zugewiesen werden können; b) es wird nach Assoziationen der Strukturelemente (L, C) vom gleichen Strukturtyp der zweiten Datenmenge mit den Strukturelementen (L, C) der ersten Datenmenge gesucht und es wird basierend auf den aufgefundenen Assoziationen eine Transformationsfolge zur Abbildung der Strukturelemente (L, C) der zweiten Datenmenge auf die Strukturelemente (L, C) der ersten Datenmenge durchgeführt, wodurch eine dritte Datenmenge erhalten wird; c) es wird nach Assoziationen der Strukturelemente (L, C) der dritten Datenmenge mit den unzuordenbaren Messpunkten (M1, ..., M3') der ersten Datenmenge gesucht und es wird basierend auf den aufgefundenen Assoziationen eine zweite Transformationsfolge zur Abbildung der Strukturelemente (L, C) der dritten Datenmenge auf die unzuordenbaren Messpunkte (M1, ..., M3') der ersten Datenmenge durchgeführt, wodurch eine vierte Datenmenge erhalten wird; d) es wird die Bewegung des Objekts zwischen dem ersten und zweiten Zeitpunkt basierend auf zumindest der ersten und zweiten Transformationsfolge bestimmt.
Verfahren nach Anspruch 1, bei dem die extrahierten Strukturelemente (L, C) zumindest vom Typ Linie sind.
Verfahren nach Anspruch 2, bei dem in Schritt b) als erstes Strukturelemente vom Typ Linie extrahiert werden.
Verfahren nach Anspruch 2 oder 3, bei dem die extrahierten Strukturelemente ferner den Strukturtyp Kreissegment umfassen.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem in Schritt a) zunächst Zusammenhangskomponenten umfassend aufeinander folgende Messpunkte (P1, ..., P12) ermittelt werden, wobei benachbarte, zu einer Zusammenhangskomponente gehörende Messpunkte einen maximalen Punktabstand nicht überschreiten.
Verfahren nach Anspruch 4 und 5, bei dem ein Strukturelement vom Strukturtyp Kreissegment aus einer Zusammenhangskomponente wie folgt extrahiert wird: – es wird ein Kreis (C) berechnet, auf dem die an den jeweiligen Enden der Zusammenhangskomponente liegenden Randmesspunkte (P1, P12) sowie ein Messpunkt (P8) zwischen den Randmesspunkten (P1, P12) der Zusammenhangskomponente, insbesondere im Wesentlichen in der Mitte zwischen den Randmesspunkten (P1, P12), angeordnet sind; – falls der minimale Abstand jedes Messpunkts (P1, ..., P12) der Zusammenhangskomponente zum Kreis (C) kleiner als ein vorgegebener Schwellwert ist, werden die Messpunkte (P1, ..., P12) der Zusammenhangskomponente dem berechneten Kreis (C) zugeordnet, und ansonsten wird die Zusammenhangskomponente am Messpunkt (P6) der Zusammenhangskomponente mit dem größten minimalen Abstand von dem Kreis (C) in zwei Zusammenhangskomponenten aufgeteilt.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem in den Schritten b) und c) das Messrauchen des Sensors berücksichtigt wird.
Verfahren nach Anspruch 7, bei dem die erste Transformationsfolge des Schritts b) des Anspruchs 1 durch eine Iteration wie folgt bestimmt wird: b.i) für eine Menge von Strukturelement-Paaren, welche jeweils ein Strukturelement (L, C) einer transformierten zweiten Datenmenge und ein Strukturelement (L, C) der ersten Datenmenge umfassen, werden die von einer momentanen Kovarianzmatrix abhängigen Mahalanobis-Distanzen der Transformationen berechnet, welche jeweils die Strukturelemente (L, C) eines Strukturelement-Paars aufeinander abbilden, und unter Berücksichtigung der Mahalanobis-Distanzen wird ein Strukturelement-Paar ausgewählt und eine Transformation zur Abbildung der Strukturelemente (L, C) des ausgewählten Strukturelement-Paars aufeinander durchgeführt, wodurch eine neue transformierte zweite Datenmenge erhalten wird; b.ii) aus der Menge der Strukturelement-Paare wird zumindest das ausgewählte Strukturelement-Paar gestrichen, die Kovarianzmatrix wird aktualisiert und es wird zu Schritt b.i) zurückgegangen.
Verfahren nach Anspruch 8, bei dem bei der Auswahl der Transformationen in Schritt b.i) Transformationen mit kleinen Mahalanobis-Distanzen und/oder Transformationen zwischen langen Strukturelementen (L, C) und/oder Anhäufungen von ähnlichen Transformationen bevorzugt werden.
Verfahren nach Anspruch 8 oder 9, bei dem die in Schritt b.i) durchgeführte Transformation, insbesondere basierend auf dem Kalman-Filter, geschätzt wird und/oder in Schritt b.ii) die Aktualisierung der Kovarianzmatrix mit Hilfe des Kalman-Filters durchgeführt wird.
Verfahren nach einem der Ansprüche 8 bis 10, bei dem in Schritt b.ii) Strukturelement-Paare, deren Strukturelemente (L, C) durch eine Transformation aufeinander abgebildet werden, deren von der aktualisierten Kovarianzmatrix abhängige Mahalanobis-Distanz größer als ein vorgegebener Schwellwert ist, auch aus der Menge der Strukturelemente (L, C) gestrichen wird.
Verfahren nach einem der Ansprüche 8 bis 11, bei dem zu Beginn der Iteration zur Bestimmung der ersten Transformationsfolge die transformierte zweite Datenmenge der in Schritt a) erhaltenen zweiten Datenmenge entspricht und die Menge der Strukturelement-Paare alle möglichen Paare beinhaltet, welche ein Strukturelement (L, C) der ersten Datenmenge und der zweiten Datenmenge umfasst, oder eine Vorauswahl von Strukturelement-Paaren, wobei die Vorauswahl solche Strukturelement-Paare umfasst, deren Strukturelemente (L, C) durch eine Transformation aufeinander abgebildet werden, deren von einer anfänglichen Kovarianzmatrix abhängige Mahalanobis-Distanz kleiner oder gleich einem vorgegebenen Schwellwert ist.
Verfahren nach Anspruch 12, bei dem die anfängliche Kovarianzmatrix basierend auf dem Messrauschen einer odometrischen Messung und/oder aus einem Bewegungsmodell bestimmt wird.
Verfahren nach einem der vorhergehenden Ansprüche in Kombination mit Anspruch 7, bei dem die zweite Transformationsfolge des Schritts c) des Anspruchs 1 durch eine Iteration wie folgt bestimmt wird: c.i) für eine Menge von Strukturelement-Messpunkt-Paaren, welche jeweils ein Strukturelement (L, C) einer transformierten dritten Datenmenge und einen unzuordenbaren Messpunkt (M1, ..., M3') der ersten Datenmenge umfassen, werden die von einer Kovarianzmatrix abhängigen Mahalanobis-Distanzen der Transformationen berechnet, wel che jeweils das Strukturelement (L, C) auf den unzuordenbaren Messpunkt (M1, ..., M3') eines Strukturelement-Messpunkt-Paars abbilden, und unter Berücksichtigung der Mahalanobis-Distanzen wird ein Strukturelement-Messpunkt-Paar ausgewählt und eine Transformation zur Abbildung des Strukturelements (L, C) auf dem Messpunkt (M1, ..., M3') des ausgewählten Strukturelement-Messpunkt-Paars ausgeführt, wodurch eine neue transformierte dritte Datenmenge erhalten wird; c.ii) aus der Menge der Strukturelement-Messpunkt-Paare wird zumindest das ausgewählte Strukturelement-Messpunkt-Paar gestrichen, dessen Transformation in Schritt c.i) ausgewählt wurde, die Kovarianzmatrix wird aktualisiert und es wird zu Schritt c.i) zurückgegangen.
Verfahren nach Anspruch 14, bei dem bei der Auswahl der Transformationen in Schritt b.i) Transformationen mit kleinen Mahalanobis-Distanzen und/oder Anhäufungen von ähnlichen Transformationen bevorzugt werden.
Verfahren nach Anspruch 14 oder 15, bei dem die in Schritt c.i) durchgeführte Transformation, insbesondere basierend auf dem Kalman-Filter, geschätzt wird und/oder in Schritt c.ii) die Aktualisierung der Kovarianzmatrix mit Hilfe des Kalman-Filters durchgeführt wird.
Verfahren nach einem der Ansprüche 14 bis 16, bei dem in Schritt c.ii) Strukturelement-Messpunkt-Paare, bei denen das Strukturelement (L, C) auf einen unzuordenbaren Messpunkt (M1, ..., M3') durch eine Transformation abgebildet wird, deren von der aktualisierten Kovarianzmatrix abhängige Mahalanobis-Distanz größer als ein vorgegebener Schwellwert ist, auch aus der Menge der Strukturelement-Messpunkt-Paare gestrichen wird.
Verfahren nach einem der Ansprüche 14 bis 17, bei dem zu Beginn der Iteration zur Bestimmung der zweiten Transformati onsfolge die transformierte dritte Datenmenge der in Schritt b) erhaltenen dritten Datenmenge entspricht und die Menge der Strukturelement-Messpunkt-Paare alle möglichen Paare beinhaltet, welche ein Strukturelement (L, C) der dritten Datenmenge und einen unzuordenbaren Messpunkt (M1, ..., M3') des ersten Datenmenge umfassen, oder eine Vorauswahl von Strukturelement-Messpunkt-Paaren, wobei die Vorauswahl solche Strukturelement-Messpunkt-Paare umfasst, bei denen das Strukturelement (L, C) auf einen unzuordenbaren Messpunkt (M1, ..., M3') durch eine Transformation abgebildet wird, deren von der aktuellen Kovarianzmatrix abhängige Mahalanobis-Distanz kleiner oder gleich einem vorgegebenen Schwellwert ist.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem nach Durchführung des Schritts c) nach Assoziationen der verbleibenden unzuordenbare Messpunkte (M1, ..., M3') der vierten Datenmenge mit den unzuordenbaren Messpunkten (M1, ..., M3') der ersten Datenmenge gesucht wird und basierend auf den aufgefundenen Assoziationen eine dritte Transformationsfolge zur Abbildung der unzuordenbaren Messpunkte (M1, ..., M3') der vierten Datenmenge auf die unzuordenbaren Messpunkte (M1, ..., M3') der ersten Datenmenge durchgeführt wird, wodurch eine fünfte Datenmenge erhalten wird, wobei die Bewegung des Objekts zwischen dem ersten und zweiten Zeitpunkt basierend auf der ersten und zweiten und dritten Transformationsfolge bestimmt wird.
Verfahren nach Anspruch 19, bei dem bei der Bestimmung der fünften Datenmenge das Messrauchen des Sensors berücksichtigt wird.
Verfahren nach Anspruch 20, bei dem die dritte Transformationsfolge durch eine Iteration wie folgt bestimmt wird: i) für eine Menge von Messpunkt-Paaren, welche jeweils einen unzuordenbaren Messpunkt (M1, ..., M3') einer transformierten fünften Datenmenge und einen unzuordenbaren Messpunkt (M1, ..., M3') der ersten Datenmenge umfassen, werden die von einer Kovarianzmatrix abhängigen Mahalanobis-Distanzen der Transformationen berechnet, welche die unzuordenbaren Messpunkte (M1, ..., M3') eines Messpunkt-Paars aufeinander abbilden, und unter Berücksichtigung der Mahalanobis-Distanzen wird ein Messpunkt-Paar ausgewählt und eine Transformation zur Abbildung der Messpunkte (M1, ..., M3') des ausgewählten Messpunkt-Paars aufeinander ausgeführt, wodurch eine neue transformierte fünfte Datenmenge erhalten wird; ii) aus der Menge der Messpunkt-Paare wird zumindest das ausgewählte Messpunkt-Paar gestrichen, die Kovarianzmatrix wird aktualisiert und es wird zu Schritt i) zurückgegangen.
Verfahren nach Anspruch 21, bei dem bei der Auswahl der Transformationen in Schritt i) Transformationen mit kleinen Mahalanobis-Distanzen und/oder Anhäufungen von ähnlichen Transformationen bevorzugt werden.
Verfahren nach Anspruch 21 oder 22, bei dem die in Schritt i) ausgeführte Transformation, insbesondere basierend auf dem Kalman-Filter, geschätzt wird und/oder in Schritt ii) die Aktualisierung der Kovarianzmatrix mit Hilfe des Kalman-Filters durchgeführt wird.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem der auf dem Objekt befindliche Sensor ein Laserscanner ist und die Sensordaten Laserscans darstellen.
Verfahren nach einem der vorhergehenden Ansprüche, bei dem das Verfahren zur Bestimmung der Bewegung eines Roboters und/oder eines Krans und/oder eines Fahrzeugs eingesetzt wird.
Vorrichtung zur rechnergestützten Berechnung der Bewegung eines Objekts aus Sensordaten eines auf dem Objekt befindlichen Sensors, wobei die Sensordaten zu verschiedenen Zeit punkten erfasste Messpunktmengen aus Messpunkten (P1, ..., P12. M1, ..., M5') der Umgebung des Objekts enthalten, wobei die Vorrichtung ein Mittel zur Bestimmung der Objektbewegung zwischen einer ersten, zu einem ersten Zeitpunkt erfassten Messpunktmenge und einer zweiten, zu einem zweiten Zeitpunkt erfassten Messpunktmenge aufweist, wobei das Mittel derart ausgestaltet ist, dass es im Betrieb der Vorrichtung ein Verfahren nach einem der vorhergehenden Ansprüche durchführt.
Computerprogrammprodukt mit einem auf einem maschinenlesbaren Träger gespeicherten Programmcode zur Durchführung eines Verfahrens nach einem der vorhergehenden Ansprüche, wenn das Programm auf einem Rechner abläuft.