DE112006003363T5

DE112006003363T5 - Verfahren und Vorrichtung zur Identifizierung der Eigenposition, und Verfahren und Vorrichtung zur Messung einer dreidimensionalen Gestalt

Info

Publication number: DE112006003363T5
Application number: DE112006003363T
Authority: DE
Inventors: Toshihiro Hayashi; Yukihiro Kawano; Hideo Terada
Original assignee: IHI Corp
Current assignee: IHI Corp
Priority date: 2005-12-16
Filing date: 2006-12-15
Publication date: 2009-01-02
Anticipated expiration: 2026-12-16
Also published as: JP4650752B2; US20090167761A1; WO2007069726A1; CN101331379B; CN101331379A; JPWO2007069726A1; DE112006003363B4; US8121399B2

Abstract

Eigenpositions-Identifizierungsverfahren zum Identifizieren einer Eigenposition durch Aufnehmen von dreidimensionalen Gestalten aus dem Außenraum, umfassend:
einen Dateneingabeschritt zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt an einer neuen Messposition in einen Computer; und
einen Modell-Strukturierungsschritt zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von Voxeln, die aus rechteckigen Körpern gebildet sind, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel speichert;
einen Abgleichungs- bzw. Zuordnungsschritt zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon, innerhalb des Voxels, das dem Koordinatenwert entspricht;
wobei: wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, eine neue Messposition als eine Eigenposition identifiziert wird; und
wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition existieren: ein Feinzuordnungsschritt zur Positionszuordnung so ausgeführt wird, dass die Summierung der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen...

Description

HINTERGRUND DER ERFINDUNG
Technisches Gebiet der Erfindung
Die vorliegende Erfindung betrifft ein Verfahren und eine Vorrichtung zur Identifizierung der Eigenposition sowie ein Verfahren und eine Vorrichtung zur Messung einer dreidimensionalen Gestalt durch Empfangen von externer dreidimensionaler Information.
Beschreibung des Stands der Technik
GPS, DGPS, magnetische Marker, magnetische Methoden, Funkpeilungs-, bzw. Radiomethoden, Bildmarkermethoden, optische/elektromagnetische Markermethoden und dergleichen sind als Einrichtungen oder Verfahren zur direkten Identifizierung einer Eigenposition bekannt. Während diese Einrichtungen den Vorteil aufweisen, zum Identifizieren stabiler Positionen fähig zu sein, weisen sie jedoch die Probleme auf, dass sie eine Infrastruktur erfordern, eine begrenzte Reichweite aufweisen, über welche Positionen identifiziert werden können, Arbeitsschritte bei der Installation benötigen, und so weiter.
Im Gegensatz dazu gab es eine Vielzahl von Vorschlägen für Eigenpositions-Identifizierungseinrichtungen, welche die Eigenposition durch Vergleichen von neu erfassten gemessenen Daten mit Umgebungsinformationen, die durch Messen der umliegenden Umgebung unter Verwendung von Sensoren, ohne Infrastruktur, erfasst worden sind, berechnen.
Dreidimensionale Sensoren, welche für diesen Zweck geeignet sind, schließen Ultraschallsensoren, Bildsensoren und Entfernungssensoren ein. Allerdings besitzen Ultraschallsensoren die Probleme, einen schmalen Messbereich und eine geringe Genauigkeit aufzuweisen, wohingegen Bildsensoren das Problem aufweisen, dass sie gegenüber dem Einfluss der Lichtbedingungen anfällig sind.
Im Gegensatz zu diesen sind Entfernungssensoren, im Vergleich zu Ultraschallsensoren, zum exakten Messen von Entfernungsdaten über einen weiten Bereich fähig und sind nicht gegenüber dem Einfluss von Änderungen in der Beleuchtung, wie bei einer Bildverarbeitung, anfällig, sodass sie den Vorteil besitzen, zum Erstellen stabiler Messungen fähig zu sein, ohne durch externe Rauschstörungen beeinflusst zu werden.
Einrichtungen für die Positionsidentifizierung unter Anwendung von Entfernungssensoren werden im Nicht-Patentdokument 1, Patentdokument 1 und dergleichen vorgeschlagen.
Die Einrichtungen des Nicht-Patentdokuments 1 vollführen eine Abgleichung, bzw. Zuordnung, unter Anwendung eines ICP-Algorithmus, durch Vergleichen von sequenziell erhaltenen gemessenen Daten mit einem Umgebungsmodell, das durch Messungen der umliegenden Umgebung unter Verwendung eines Lasersensor-Typs mit dreidimensionaler Abtastung erhalten wird.
Der ICP-Algorithmus ist eine Vorgehensweise zur Zuordnung, wenn es keine im Voraus bekannten korrespondierenden Punkte gibt, wobei die nahesten Punkte in den später gemessenen Daten, im Verhältnis zu Daten, welche früher gemessen wurden, berechnet werden, und Rotationen und Translationen ausgeführt werden, um eine Lösung zu finden, in welcher der Zustand mit der minimalen Summierung von Distanzen der übereinstimmende Zustand ist (Nicht-Patentdokument 2).
Die "Eigenpositions-Identifizierungsvorrichtung" von Patentdokument 1 weist im Besonderen keinen Laufweg auf und ist mit keinerlei äußeren Orientierungspunkten oder dergleichen versehen, und speichert eine Karte der äußeren Umgebung automatisch durch manuelles Betätigen einer sich bewegenden Einheit, im Voraus, entlang eines Ziel-Laufwegs (Lernen, bzw. "Teaching").
In diesem Patentdokument 1 wird, wie in 1 gezeigt, eine Karte der äußeren Umgebung verwendet, wobei die Positions-Beziehung einer sich bewegenden Einheit 101, welche sich in einer Ebene bewegt, zu einem umliegenden, externen Umgebungsobjekt 102 innerhalb dieser Ebene in Form diskreter Daten in einem Speicher abgespeichert wird. Diese Daten verwenden als einen Ursprung 103 einen Punkt, welcher auf dem sich bewegenden Objekt stationär ist, und kombinieren Richtungsdaten 104 von der sich bewegenden Einheit, verteilt über ein vorbestimmtes Intervall, mit Entfernungsdaten 105 bis zu Objekten der umliegenden äußeren Umgebung, die in dieser Richtung im vorbestimmten Abstand verteilt sind.
Weiterhin wird das Nicht-Patentdokument 3 als eine Technologie, welche sich auf die vorliegende Erfindung bezieht, offenbart.
(Nicht-Patentdokument 1)

Andreas Nuchter et al., "6D SLAM with an Application in Autonomous Mine Mapping", IEEE 2004 International Conference an Robotics and Automation (ICRA' 04), April 2004.

(Nicht-Patentdokument 2)

Paul J. Besl, "A Method for Registration of 3-D Shapes", IEEE Transactions of Pattern Analysis and Mechanical Intelligence, Band 14, Nr. 2, Februar 1992.

(Nicht-Patentdokument 3)

SEKIMOTO Kiyohide, et al., "Development of a Three-Dimensional Laser Radar", Ishikawajima-Harima Engineering Review, Band 43, Nr. 4 (2003-7).

(Patentdokument 1)
Japanische ungeprüfte Patentanmeldungs-Veröffentlichung 2003-15739 , "External Enviroment Map, Self-Position Identifying Device and Guide Controller".
Wenn Entfernungssensoren verwendet werden, wie beim dreidimensionalen Laserradar, bilden die gemessenen Punkte auf dem dreidimensionalen Objekt, das vermessen wird, eine Punktegruppe, welche in der horizontalen Richtung und in der vertikalen Richtung verteilt ist. In Hinsicht auf den Zwischenabstand dieser Punktegruppe wird, wenn die Distanz vom Messpunkt zum Beispiel 50 m beträgt, der Zwischenabstand der zu messenden Punkte beispielsweise etwa 315 mm in der horizontalen Richtung und etwa 525 mm in der vertikalen Richtung erreichen.
Wenn eine nicht-bewegliche dreidimensionale Gestalt aus einer Vielzahl von Messpunkten vermessen wird, dann sind außerdem die Positionen der zu messenden Punkte für jeden Messpunkt in einem Entfernungssensor, wie einem dreidimensionalen Laserradar, normalerweise unterschiedlich.
Ferner besteht bei diesem Typ von Entfernungssensor typischerweise ein Fehler von etwa ±20 cm in den gemessenen Distanzen.
Folglich bestehen die folgenden einschränkenden Umstände A bis C, wenn ein Entfernungssensor, wie eine dreidimensionale Laser-Radarvorrichtung, verwendet wird.
Umstand A: Die Anzahl an Punkten in den gemessenen Daten ist gering (zum Beispiel 166 × 50 Punkte in einem Einzelbild).
Umstand B: Es besteht ein Fehler in den gemessenen Daten (zum Beispiel etwa ±20 cm in den gemessenen Distanzen).
Umstand C: Die gemessenen Daten werden nicht auf das Messen des gleichen gemessenen Punkts eingeschränkt.
Das heißt, die Entfernungsdaten, welche erhalten werden, bestehen aus einer Gruppe von Punkten, welche in der horizontalen Richtung und der vertikalen Richtung verteilt ist, und die Position ist für jede Messung unterschiedlich, sodass es keine korrespondierenden Positionen gibt, und die gemessene Distanz weist einen relativ großen Fehler auf.
Es bestehen die folgenden Probleme, wenn dieser Typ von diskreten Daten unter Anwendung des oben beschriebenen "ICP-Algorithmus" verarbeitet wird:
(1) Kumulativer Fehler
Der ICP-Algorithmus ist die Methode zum Überlagern von zwei Entfernungsdaten, und selbst obwohl die früheren Daten und die späteren Daten wiederholt verglichen und die Differenzen integriert werden, gibt es im Wesentlichen keine korrespondierenden Punkte, sodass der Fehler kumulativ ist.
(2) Großer Rechenaufwand
Der ICP-Algorithmus ist eine iterative Berechnung, sodass der Rechenaufwand groß ist. Das heißt, der ICP-Algorithmus erfordert das Suchen nach Modelldaten, entsprechend jedem der Datenpunkte in den gemessenen Daten, sodass der Rechenaufwand steigt, wenn die Anzahl an Modelldatenpunkten und gemessenen Datenpunkten steigt. Spezifisch gesagt beläuft sich, wenn die Anzahl an Modelldatenpunkten M ist, und die Anzahl an gemessenen Datenpunkten N ist, die Berechnungsgrößenordnung für die gesamte Suchzeit dann auf O(M × N).
(3) Mangelnde Eignung, wenn es wenige gemessene Punkte gibt
Der ICP-Algorithmus wird für dichte Entfernungsdaten verwendet, sodass, wenn die Punktegruppe verstreut und räumlich spärlich ist, der Algorithmus zu unkorrekten Ergebnissen konvergiert.
Deshalb ist es notwendig, beim Durchführen von Positionsidentifizierung unter Verwendung diskreter Sensoren auf diese Weise, die folgenden Bedingungen zu erfüllen:
1) Datenstruktur der umliegenden Umgebung mit guter Speichereffizienz
Ein Verfahren, das alle gemessenen Daten abspeichert, welche sequenziell akkumuliert werden, erfordert unbegrenzten Speicher. Folglich ist es notwendig, dass eine Datenstruktur vorliegt, welche die Ergebnisse der Messung der umliegenden Umgebung in effizienter Weise speichert.
2) Stabilität in Hinsicht auf ungenügende Punktanzahlen und Fehler in den gemessenen Daten
Es ist notwendig, dass eine so kleine Verringerung der Positionsidentifizierungsgenauigkeit wie möglich vorliegt, selbst wenn die gemessenen Daten wenige Punkte aufweisen und Fehler enthalten.
3) Erhöhte Effizienz bei Berechnungen zur Identifizierung von Positionen
Die Eigenpositionsidentifizierung wird durch Vergleichen von gemessenen Daten mit Umgebungsdaten, welche durch Messen der umliegenden Umgebung erhalten worden sind, durchgeführt, und dieser Vergleichsprozess erfordert viele Berechnungen.
ZUSAMMENFASSUNG DER ERFINDUNG
Die vorliegende Erfindung wurde zur Lösung der oben beschriebenen Probleme erfunden. Das heißt, das Ziel der vorliegenden Erfindung besteht darin, ein Verfahren und eine Vorrichtung zur Identifizierung der Eigenposition und ein Verfahren und eine Vorrichtung zum Messen einer dreidimensionalen Gestalt bereitzustellen, fähig zur Ausführung der Berechnungen zur Positionsidentifikation in effizienter Weise, aufweisend eine Datenstruktur für die umliegende Umgebung mit hervorragender Speichereffizienz, und stabil in Hinsicht auf ungenügende Punktanzahlen und hinsichtlich Fehler(n) in den gemessenen Daten. Das Verfahren und die Vorrichtung zur Identifizierung der Eigenposition und das Verfahren und die Vorrichtung zur Messung der dreidimensionalen Gestalt sind in der Lage, eine exakte Zuordnung bei geringem Rechenaufwand, ohne kumulativen Fehler, selbst bei Vorliegen einer Diskrepanz hinsichtlich der Positionen in jeder Messung, ohne korrespondierende Punkte und bei relativ großem Ausmaß des inbegriffenen Fehlers, auszuführen, wobei es sich bei den externen dreidimensionalen Daten um eine räumlich verteilte Punktegruppe handelt.
Die vorliegende Erfindung liefert ein Eigenpositions-Identifizierungsverfahren zum Identifizieren einer Eigenposition durch Aufnehmen von dreidimensionalen Gestalten aus dem Außenraum, umfassend: einen Dateneingabeschritt zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt an einer neuen Messposition in einen Computer; einen Modell-Strukturierungsschritt zum Aufbauen eines Umgebungsmodells, der eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel speichert; und einen Zuordnungsschritt zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon, innerhalb des Voxels, der dem Koordinatenwert entspricht; wobei: wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, eine neue Messposition als eine Eigenposition identifiziert wird; wenn die dreidimensionalen Gestaltdaten in einer früheren Messposition existieren: ein Feinzuordnungsschritt zur Positionszuordnung so ausgeführt wird, dass ein Prüfwert (zum Beispiel die Summierung der Distanzen) bezüglich der Distanz zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird; und ein Eigenpositions-Identifizierungsschritt zum Identifizieren der Eigenposition aus einem Rotationsbetrag und einem Translationsbetrag in einem Feinzuordnungsschritt durchgeführt wird. Außerdem kann auch ein Ausgabeschritt zum Ausgeben der Eigenposition an eine Ausgabevorrichtung durchgeführt werden.
Ferner sieht die vorliegende Erfindung ein dreidimensionales Gestalt-Messverfahren zum Reproduzieren einer dreidimensionalen Gestalt aus Koordinatenwerten von gemessenen Punkten auf einer externen dreidimensionalen Gestalt und zum Ausgeben von dreidimensionalen Gestaltdaten vor, umfassend: einen Dateneingabeschritt zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt an einer neuen Messposition in einen Computer; einen Modell-Strukturierungsschritt zum Aufbauen eines Umgebungsmodells, der eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel speichert; und einen Zuordnungsschritt zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon, innerhalb des Voxels, der dem Koordinatenwert entspricht; wobei: wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, eine neue Messposition mit einer Eigenposition identifiziert wird; und wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition existieren: ein Feinzuordnungsschritt zur Positionszuordnung so ausgeführt wird, dass ein Prüfwert bezüglich der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird; ein Eigenpositions-Identifizierungsschritt zum Identifizieren der Eigenposition aus einem Rotationsbetrag und einem Translationsbetrag in dem Feinzuordnungsschritt durchgeführt wird; wobei das Verfahren zur Messung der dreidimensionalen Gestalt ferner einen Ausgabeschritt zum Ausgeben von wenigstens einem der Eigenposition und des repräsentativen Punkts, der Voxelposition und der Fehlerverteilung, basierend auf der Eigenposition, an eine Ausgabevorrichtung umfasst.
Es ist anzumerken, dass in dem oben beschriebenen Verfahren zum Identifizieren der Eigenposition und Verfahren zum Messen der dreidimensionalen Gestalt, anstatt der Summierung der Distanzen, der Prüfwert bezüglich der Distanz der Mittelwert der Distanzen, die Summe der Quadrate der Distanzen, oder der Maximumwert für die Distanzen sein kann oder ein anderer geeigneter Prüfwert sein kann.
Das Verfahren zur Identifizierung der Eigenposition und das Verfahren zur Messung der dreidimensionalen Gestalt, wie oben beschrieben, können das Folgende einschließen.
In einer bevorzugten Ausführungsform der vorliegenden Erfindung wird im Eigenpositions-Identifizierungsschritt, wie oben beschrieben, eine Position in sechs Freiheitsgraden für die neue Messposition aus der Position und Orientierung einer früheren Messposition identifiziert.
Im Zuordnungsschritt, wird ein Wahrscheinlichkeitswert, der die Wahrscheinlichkeit der Existenz eines Objekts innerhalb des Voxels angibt, ebenfalls, zusätzlich zu einem repräsentativen Punkt und einer Fehlerverteilung, innerhalb des Voxels festgelegt und abgespeichert.
Zusätzlich dazu gibt es, vor dem Feinzuordnungsschritt, einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, sodass ein Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen gemessenen Daten und benachbarten Voxeln, und zwischen Fehlerverteilungen und benachbarten Voxeln, wobei die Voxel repräsentative Punkte aufweisen, minimiert wird, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue gemessene Position relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen Voxeln mit repräsentativen Punkten zu minimieren.
Es ist anzumerken, dass, anstatt der Summierung der Distanzen, der Prüfwert bezüglich der Distanz zwischen gemessenen Daten und benachbarten Voxeln, und zwischen Fehlerverteilungen und benachbarten Voxeln, wobei die Voxel repräsentative Punkte aufweisen, der Mittelwert der Distanzen, die Summe der Quadrate der Distanzen oder der Maximumwert für die Distanzen sein kann, oder ein anderer geeigneter Prüfwert sein kann.
Darüber hinaus kann der Prüfwert bezüglich der Distanzen zwischen Voxeln mit repräsentativen Punkten ein Mittelwert der Distanzen, die Summe der Quadrate der Distanzen oder der Maximumwert der Distanzen, oder ein anderer geeigneter Prüfwert, anstatt der Summierung der Distanzen, sein.
Umgekehrt gibt es dann, wenn im Zuordnungsschritt Einrichtungen zum Erstellen und Abspeichern eines Wahrscheinlichkeitswerts, der die Wahrscheinlichkeit der Existenz eines Objekts innerhalb des Voxels ausdrückt, in einem Speicher vorhanden sind, vor dem Feinzuordnungsschritt, einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition zum Maximieren eines Prüfwerts (wie der Summierung der Wahrscheinlichkeitswerte) bezüglich der Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten- und Fehlerverteilungen benachbart sind, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition zum Minimieren eines Prüfwerts (wie der Summierung der Differenzen von Wahrscheinlichkeitswerten) bezüglich der Differenzen in Wahrscheinlichkeitswerten von benachbarten Voxeln.
Es ist anzumerken, dass, anstatt der Summierung der Wahrscheinlichkeitswerte, der Prüfwert bezüglich des Wahrscheinlichkeitswerts eines Voxels mit einem repräsentativen Punkt, das zu einer gemessenen Daten- und Fehlerverteilung benachbart ist, der Mittelwert der Wahrscheinlichkeitswerte, die Summe von Quadraten der Wahrscheinlichkeitswerte oder der Minimumwert für die Wahrscheinlichkeitswerte sein kann, oder ein anderer geeigneter Prüfwert sein kann.
Außerdem kann es sich bei dem Prüfwert bezüglich der Differenz in den Wahrscheinlichkeitswerten benachbarter Voxel, anstatt um die Summierung der Differenzen der Wahrscheinlichkeitswerte, um die Summe von Quadraten von Differenzen von Wahrscheinlichkeitswerten oder den Maximumwert für die Differenz von Wahrscheinlichkeitswerten, oder um einen anderen geeigneten Prüfwert, handeln.
Weiterhin gibt es nach dem Dateneingabeschritt einen den Suchbereich beschränkenden Schritt zum Beschränken des Umfangs der Prüfung durch Erhalten der derzeitigen Messposition durch Rückschluss aus einer Änderung in der Messposition der Vergangenheit, oder aus einem Sensor, welcher fähig zum Erhalten der derzeitigen Messposition ist, oder durch die Verwendung eines Reflexionsstärkewerts zusätzlich zum Distanzwert der gemessenen Daten.
Darüber hinaus wird im Feinzuordnungsschritt der Fall, worin sich Fehlerverteilungen schneiden, als ein einzelner gemessener Punkt behandelt, und die Distanz zwischen Fehlerverteilungen wird/werden berechnet durch Multiplizieren der Distanzwerte dieses Falls mit einer Gewichtung, die aus den Koinzidenzgraden der Verteilungen berechnet wird.
Darüber hinaus wird im Modell-Strukturierungsschritt das größte Voxel auf eine Größe, entsprechend der minimal erforderlichen Auflösung, eingestellt, und wenn eine Vielzahl von gemessenen Punkten innerhalb eines einzelnen Voxels existiert, wird das Voxel hierarchisch weiter in eine Vielzahl von Voxeln unterteilt, sodass nur ein einzelner gemessener Punkt innerhalb eines einzelnen Voxels existiert.
Ferner gibt es nach dem Eigenpositions-Identifizierungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells, wobei der Modell-Auffrischungsschritt ein Voxel abruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und unter der Annahme, dass kein Objekt zwischen dem Ursprung und dem gemessenen Punkt existiert, den repräsentativen Punkt und die Fehlerverteilung in einem dazwischen positionierten Voxel neu einstellt, bzw. nachjustiert, oder eliminiert.
Ferner gibt es, nach dem Eigenpositions-Identifizierungsschritt, einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells, wobei der Modell-Auffrischungsschritt ein Voxel abruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und sofern es keinen repräsentativen Punkt innerhalb des Voxels gibt, den Koordinatenwert und die Fehlerverteilung als den Koordinatenwert und die Fehlerverteilung des repräsentativen Punkts festlegt.
Ferner gibt es nach dem Eigenpositions-Identifizierungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells, wobei der Modell-Auffrischungsschritt ein Voxel abruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und wenn es einen repräsentativen Punkt gibt, der bereits innerhalb des Voxels festgelegt worden ist, eine neu erhaltene Fehlerverteilung und die bereits innerhalb des Voxels festgelegte Fehlerverteilung vergleicht; wenn sich die Fehlerverteilungen gegenseitig überlappen, eine neue Fehlerverteilung und einen neuen repräsentativen Punkt aus den beiden Fehlerverteilungen, oder aus den beiden Fehlerverteilungen und dem bereits innerhalb des Voxels festgelegten repräsentativen Punkt und den Koordinatenwerten des neu eingegebenen, gemessenen Punkts neu einstellt; und dann, wenn die Fehlerverteilungen nicht gegenseitig überlappen, das Voxel hierarchisch in eine Vielzahl von Voxeln weiter unterteilt, sodass nur ein einziger repräsentativer Punkt innerhalb eines einzelnen Voxels existiert. Ein "Octree" oder ein K-D-Baum wird für die Voxelunterteilung verwendet.
Zusätzlich dazu wird, neben dem Identifizieren der Eigenposition, eine Fehlerverteilung für die Eigenposition identifiziert, und die Eigenposition und die Fehlerverteilung werden durch Kalman-Filterung aus der derzeitigen Eigenposition und Fehlerverteilung, und aus der Eigenposition und Fehlerverteilung, welche identifiziert worden sind, korrigiert, und es erfolgt eine Ausgabe davon als die Eigenposition und Fehlerverteilung.
Im Modell-Auffrischungsschritt werden die neu erhaltene Fehlerverteilung und die Fehlerverteilung, die bereits innerhalb des Voxels festgelegt worden ist, verglichen, und falls die Fehlerverteilungen gegenseitig überlappen, wenn, als Ergebnis der Neu-Einstellung einer neuen Fehlerverteilung und eines neuen repräsentativen Punkts aus den beiden Fehlerverteilungen, der neue repräsentative Punkt in ein anderes Voxel bewegt worden ist, dann: wenn es keinen repräsentativen Punkt in dem anderen Voxel gibt, dann werden die neue Fehlerverteilung und der neue repräsentative Punkt in das andere Voxel festgelegt; und wenn es einen repräsentativen Punkt gibt, der bereits in dem anderen Voxel festgelegt worden ist, dann werden die neue Fehlerverteilung und die Fehlerverteilung, welche bereits in das andere Voxel festgelegt worden ist, verglichen; und (A) wenn die Fehlerverteilungen gegenseitig überlappen, dann werden eine neue Fehlerverteilung und ein neuer repräsentativer Punkt aus beiden Fehlerverteilungen oder aus beiden Fehlerverteilungen und dem repräsentativen Punkt, der bereits innerhalb des Voxels festgelegt worden ist, und den Koordinatenwerten des neu eingegebenen gemessenen Punkts, neu eingestellt; oder (B) wenn die Fehlerverteilungen nicht gegenseitig überlappen, wird das Voxel weiter hierarchisch in eine Vielzahl von Voxeln unterteilt, sodass nur ein einziger repräsentativer Punkt innerhalb eines einzelnen Voxels existiert.
Fernerhin gibt es, nach dem Eigenpositions-Identifizierungsschritt, einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells; wobei der Modell-Auffrischungsschritt einen neuen repräsentativen Punkt und Fehlerverteilung durch einen Kalman-Filter aus dem Koordinatenwert des neu eingegebenen gemessenen Punkts und der Fehlerverteilung davon sowie dem repräsentativen Punkt und der Fehlerverteilung davon, welche bereits in dem Voxel festgelegt worden sind, erhält und neu einstellt.
In einer anderen Ausführungsform, wie sie in der vorliegenden Erfindung dargestellt ist, werden im Feinzuordnungsschritt eine neue gemessene Daten- und Fehlerverteilung relativ zu dem Umgebungsmodell für die frühere Messposition rotiert und translatiert, oder ein Umgebungsmodell für eine neue Messposition wird relativ zu dem Umgebungsmodell für die frühere Messposition rotiert und translatiert, zum Maximieren des Prüfwerts (wie der Multiplikation des Koinzidenzgrads) bezüglich des Koinzidenzgrads, erstellt durch Maximum-Likelihood-Schätzungen, basierend auf der Distanz zwischen benachbarten Fehlerverteilungen, anstatt einer Positionszuordnung zum Minimieren eines Prüfwerts, basierend auf der Distanz zwischen benachbarten Fehlerverteilungen.
Die Gleichung zur Berechnung des Prüfwerts für den Koinzidenzgrad ist in der folgenden Gleichung (1) ausgedrückt: (Gleichung 1)
In dieser Gleichung wird eine Korrelation (Assoziation) zwischen den gemessenen Punkten j und den repräsentativen Punkten i im Umgebungsmodell aufgestellt, wobei die Wahrscheinlichkeit des Erhaltens von gemessenen Daten, d. h. dem gemessenen Punkt j, durch EM(i, j) angegeben ist, worin ω(j) gleich 1 ist, wenn es einen repräsentativen Punkt, der dem gemessenen Punkt j entspricht, innerhalb des Umgebungsmodells gibt, und ansonsten gleich 0 ist.
Im Ausgabeschritt wird nicht nur wenigstens eines von der Position des Voxels, der Position des repräsentativen Punkts und der Position der Fehlerverteilung an die Ausgabevorrichtung als der dreidimensionale Gestalt-Messwert ausgegeben, sondern auch ein Index, der die Zuverlässigkeit oder Genauigkeit des Messwerts anzeigt, basierend auf der Größenordnung der Fehlerverteilung innerhalb des Voxels, wird an die Ausgabevorrichtung ausgegeben.
Wenn wenigstens eines von der Position des Voxels, der Position des repräsentativen Punkts und der Position der Fehlerverteilung als dreidimensionaler Gestalt-Messwert an die Ausgabevorrichtung ausgegeben wird, dann wird in diesem Ausgabeschritt, sofern die Größenordnung der Fehlerverteilung innerhalb des Voxels größer als ein spezifischer Referenzwert ist, die Zuverlässigkeit oder Genauigkeit des Messwerts niedriger als eine spezifische Referenz eingestuft, und der Messwert für das Voxel wird nicht an die Ausgabevorrichtung ausgegeben.
Nach dem Zuordnungsschritt gibt es einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells, wobei der Modell-Auffrischungsschritt ein Voxel abruft, das dem Koordinatenwert des neu eingegebenen gemessenen Punkts entspricht; und wenn der repräsentative Punkt und/oder die Fehlerverteilung innerhalb dieses Voxels neu festgelegt, oder nachjustiert, wird, oder dieses Voxel weiter hierarchisch in eine Vielzahl von Voxeln unterteilt wird, dann wird im Ausgabeschritt die Position des repräsentativen Punkts dieses Voxels an die Ausgabevorrichtung als dreidimensionaler Gestalt-Messwert ausgegeben.
Im Ausgabeschritt wird die Position eines repräsentativen Punkts für ein Voxel im Umgebungsmodell in einem Bereich, für welchen die Position von der Position des Entfernungssensors aus gemessen werden kann, an die Ausgabevorrichtung als ein dreidimensionaler Gestalt-Messwert ausgegeben.
Weiterhin sieht die vorliegende Erfindung eine Eigenpositions-Identifizierungsvorrichtung zum Identifizieren einer Eigenposition durch Aufnehmen dreidimensionaler Gestalten aus dem Außenraum vor, umfassend: eine Dateneingabevorrichtung zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt in einen Computer; eine Modell-Strukturierungsvorrichtung zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von Voxeln, die aus rechteckigen Körpern gebildet sind, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel abspeichert; eine Zuordnungsvorrichtung zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb eines Voxels, das einem Koordinatenwert entspricht; und eine Datenübertragungsvorrichtung zum Ausgeben der Eigenposition an eine Ausgabevorrichtung; wobei: an einer neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition nicht existieren, eine neue Messposition als Eigenposition identifiziert wird; und an der neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition existieren: ein Umgebungsmodell für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition rotiert und translatiert wird, um eine Positionszuordnung auszuführen, sodass ein Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanz zwischen benachbarten Fehlerverteilungen minimiert wird; und die Eigenposition aus dem Rotationsbetrag und Translationsbetrag in der Positionszuordnung identifiziert wird.
Darüber hinaus sieht die vorliegende Erfindung eine Vorrichtung zur Messung einer dreidimensionalen Gestalt zum Reproduzieren einer dreidimensionalen Gestalt aus Koordinatenwerten von gemessenen Punkten auf einer dreidimensionalen Gestalt und zum Ausgeben von dreidimensionalen Gestaltdaten vor, umfassend: eine Dateneingabevorrichtung zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt in einen Computer; eine Modell-Strukturierungsvorrichtung zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel abspeichert; eine Zuordnungsvorrichtung zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb eines Voxels, das einem Koordinatenwert entspricht; wobei: an einer neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition nicht existieren, eine neue Messposition als Eigenposition identifiziert wird; an der neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition existieren: ein Umgebungsmodell für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition rotiert und translatiert wird, um eine Positionszuordnung auszuführen, sodass ein Prüfwert bezüglich der Distanz zwischen benachbarten Fehlerverteilungen minimiert wird; und die Eigenposition aus dem Rotationsbetrag und Translationsbetrag in der Positionszuordnung identifiziert wird; und das dreidimensionale Gestalt-Messverfahren ferner eine Datenübertragungsvorrichtung zum Ausgeben, an eine Ausgabevorrichtung, von wenigstens einem von der Eigenposition, der Voxelposition, dem repräsentativen Punkt und der Fehlerverteilung, basierend auf der Eigenposition, umfasst.
Die oben beschriebene Eigenpositions-Identifizierungsvorrichtung und dreidimensionale Gestalt-Messvorrichtung können Folgendes einschließen.
Die Zuordnungsvorrichtung legt auch einen Wahrscheinlichkeitswert, der die Wahrscheinlichkeit der Existenz eines Objekts innerhalb des Voxels angibt, innerhalb des Voxels fest, zusätzlich zu einem repräsentativen Punkt und einer Fehlerverteilung, und speichert diese ab.
Vor der Positionszuordnung (dem Feinzuordnungsschritt) vollführt die Eigenpositions-Identifizierungsvorrichtung oder die Vorrichtung zum Messen der dreidimensionalen Gestalt einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen gemessenen Daten und benachbarten Voxeln und zwischen Fehlerverteilungen und benachbarten Voxeln, wobei die Voxel repräsentative Punkte besitzen, zu minimieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen Voxeln mit repräsentativen Punkten zu minimieren.
Vor dem Feinzuordnungsschritt, vollführt die Eigenpositions-Identifizierungsvorrichtung oder die Vorrichtung zum Messen der dreidimensionalen Gestalt einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Wahrscheinlichkeitswerte) bezüglich der Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten- und Fehlerverteilungen benachbart sind, zu maximieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Differenzen von Wahrscheinlichkeitswerten) bezüglich der Differenzen in Wahrscheinlichkeitswerten von benachbarten Voxeln, zu minimieren.
Darüber hinaus limitiert, nach dem Dateneingabeschritt, die Eigenpositions- Identifizierungsvorrichtung oder Vorrichtung zum Messen der dreidimensionalen Gestalt den Prüfbereich durch Erhalten der derzeitigen Messposition durch Rückschluss aus einer Änderung in der Messposition der Vergangenheit, oder aus einem Sensor, der in der Lage zum Erhalten der derzeitigen Messposition ist, oder durch die Verwendung eines Reflexionsstärkenwerts zusätzlich zu dem Distanzwert der gemessenen Daten.
Die Eigenpositions-Identifizierungsvorrichtung oder Vorrichtung zum Messen der dreidimensionalen Gestalt identifiziert eine Position in sechs Freiheitsgraden, für die neue Messposition, aus der Position und Orientierung einer früheren Messposition.
Darüber hinaus behandelt die Eigenpositions-Identifizierungsvorrichtung oder Vorrichtung zum Identifizieren der dreidimensionalen Gestalt den Fall, worin sich Fehlerverteilungen schneiden, als einen einzelnen gemessenen Punkt, und berechnet die Distanz zwischen den Fehlerverteilungen durch Multiplizieren der Distanzwerte dieses Falls mit einer Gewichtung, die aus den Koinzidenzgraden der Verteilungen berechnet wird.
In der Modell-Strukturierungsvorrichtung wird das größte Voxel auf eine Größe, entsprechend der erforderlichen Minimum-Auflösung, festgelegt, und wenn eine Vielzahl von gemessenen Punkten innerhalb eines einzelnen Voxels existiert, wird das Voxel hierarchisch weiter in eine Vielzahl von Voxeln unterteilt, sodass nur ein einzelner gemessener Punkt innerhalb eines einzelnen Voxels existiert.
Die Eigenpositions-Identifizierungsvorrichtung oder dreidimensionale Gestalt-Messvorrichtung besitzt eine Modellauffrischungs-Vorrichtung zum Auffrischen des Umgebungsmodells nach der Eigenpositions-Identifizierung (dem Eigenpositions-Identifizierungsschritt), wobei die Modellauffrischungs-Vorrichtung: ein Voxel abruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und annimmt, dass kein Objekt zwischen dem Ursprung und dem gemessenen Punkt existiert, um den repräsentativen Punkt und die Fehlerverteilung in einem dazwischen positionierten Voxel nachzujustieren oder zu eliminieren.
Die Eigenpositions-Identifizierungsvorrichtung oder Vorrichtung zur Messung der dreidimensionalen Gestalt umfasst eine Modellauffrischungs-Vorrichtung zum Auffrischen des Umgebungsmodells nach dem Eigenpositions-Identifizierungsschritt, wobei: die Modellauffrischungs-Vorrichtung ein Voxel aufruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und sofern es keinen repräsentativen Punkt innerhalb des Voxels gibt, den Koordinatenwert und die Fehlerverteilung als Koordinatenwert und Fehlerverteilung des repräsentativen Punkts festlegt.
Die Eigenpositions-Identifizierungsvorrichtung oder Vorrichtung zum Messen der dreidimensionalen Gestalt umfasst eine Modellauffrischungs-Vorrichtung zum Auffrischen des Umgebungsmodells nach dem Eigenpositions-Identifizierungsschritt; wobei: der Modell-Auffrischungsschritt ein Voxel abruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und sofern es einen repräsentativen Punkt gibt, der bereits innerhalb des Voxels festgelegt wurde, eine neu erhaltene Fehlerverteilung und die bereits innerhalb des Voxels festgelegte Fehlerverteilung vergleicht; wenn die Fehlerverteilungen gegenseitig überlappen, eine neue Fehlerverteilung und einen neuen repräsentativen Punkt aus den beiden Fehlerverteilungen oder aus den beiden Fehlerverteilungen und dem, bereits innerhalb des Voxels festgelegten, repräsentativen Punkt und den Koordinatenwerten des neu eingegebenen, gemessenen Punkts neu einstellt; und dann, wenn die Fehlerverteilungen nicht gegenseitig überlappen, das Voxel hierarchisch weiter in eine Vielzahl von Voxeln unterteilt, so dass innerhalb eines einzelnen Voxels nur ein einziger repräsentativer Punkt existiert.
Die Eigenpositions-Identifizierungsvorrichtung oder Vorrichtung zum Messen der dreidimensionalen Gestalt, identifiziert im Eigenpositions-Identifizierungsschritt eine Fehlerverteilung für die Eigenposition, neben der Identifizierung der Eigenposition, und korrigiert vor dem Ausgabeschritt die Eigenposition und Fehlerverteilung durch Kalman-Filterung aus der derzeitigen Eigenposition und Fehlerverteilung, und aus der Eigenposition und Fehlerverteilung, welche identifiziert worden sind; wobei die Datenübertragungsvorrichtung die Eigenposition und die Fehlerverteilung an die Ausgabevorrichtung ausgibt.
Die Modellauffrischungs-Vorrichtung vergleicht die neu erhaltene Fehlerverteilung und die Fehlerverteilung, die bereits innerhalb des Voxels festgelegt worden ist; sofern die Fehlerverteilungen gegenseitig überlappen, wenn sich, als Ergebnis des Festlegens einer neuen Fehlerverteilung und eines neuen repräsentativen Punkts aus den beiden Fehlerverteilungen, der neue repräsentative Punkt in ein anderes Voxel hinein bewegt hat, dann: falls es keinen repräsentativen Punkt in dem anderen Voxel gibt, setzt sie dann die neue Fehlerverteilung und den neuen repräsentativen Punkt in das andere Voxel fest; und, falls es einen repräsentativen Punkt gibt, der bereits in dem anderen Voxel festgelegt worden ist, vergleicht sie dann die neue Fehlerverteilung und die Fehlerverteilung, welche bereits in das andere Voxel gesetzt worden ist; und (A), wenn sich die Fehlerverteilungen gegenseitig überlappen, dann stellt sie eine neue Fehlerverteilung und einen neuen repräsentativen Punkt entweder aus beiden Fehlerverteilungen oder aus beiden Fehlerverteilungen und dem repräsentativen Punkt, der bereits innerhalb des Voxels festgelegt worden ist, und den Koordinatenwerten für den neu eingegebenen gemessenen Punkt neu ein; oder (B) wenn die Fehlerverteilungen nicht gegenseitig überlappen, dann unterteilt sie die Voxel hierarchisch weiter in eine Vielzahl von Voxeln, sodass nur ein einziger repräsentativer Punkt innerhalb eines einzelnen Voxels existiert.
Die Eigenpositions-Identifizierungsvorrichtung oder Vorrichtung zur Messung der dreidimensionalen Gestalt umfasst eine Modellauffrischungs-Vorrichtung zum Auffrischen des Umgebungsmodells nach dem Eigenpositions-Identifizierungsschritt, worin: die Modellauffrischungs-Vorrichtung einen neuen repräsentativen Punkt und eine Fehlerverteilung durch einen Kalman-Filter aus dem Koordinatenwert des neu eingegebenen gemessenen Punkts sowie dem repräsentativen Punkt und der Fehlerverteilung davon, welche bereits in dem Voxel festgelegt worden sind, erhält und neu einstellt.
In dem Feinzuordnungsschritt rotiert und translatiert die Eigenpositions-Identifizierungsvorrichtung oder die Vorrichtung zum Messen der dreidimensionalen Gestalt eine neue gemessene Daten- und Fehlerverteilung relativ zu dem Umgebungsmodell für die frühere Messposition, oder rotiert und translatiert ein Umgebungsmodell für eine neue Messposition relativ zu dem Umgebungsmodell für die frühere Messposition, um einen Prüfwert (wie die Multiplikation des Koinzidenzgrads) bezüglich des Koinzidenzgrads, welcher durch Maximum-Likelihood-Schätzungen aufgestellt wurde, basierend auf den Distanzen zwischen benachbarten Fehlerverteilungen, zu maximieren, anstatt einer Positionszuordnung zum Minimieren eines Prüfwerts (wie eine Summierung von Distanzen) bezüglich der Distanz zwischen benachbarten Fehlerverteilungen. In diesem Fall ist die Gleichung zur Berechnung des Prüfwerts bezüglich des Koinzidenzgrads in der obigen Gleichung (1) ausgedrückt.
Die Datenübertragungsvorrichtung gibt nicht nur wenigstens eines von der Position des Voxels, der Position des repräsentativen Punkts und der Position der Fehlerverteilung an die Ausgabevorrichtung als dreidimensionalen Gestalt-Messwert aus, sondern gibt ebenfalls einen Index, der die Zuverlässigkeit oder Genauigkeit des Messwerts, basierend auf der Größenordnung der Fehlerverteilung innerhalb des Voxels, anzeigt, an die Ausgabevorrichtung aus.
Wenn die Datenübertragungsvorrichtung wenigstens eines von der Position des Voxels, der Position des repräsentativen Punkts und der Position der Fehlerverteilung, an die Ausgabevorrichtung als dreidimensionalen Gestalt-Messwert ausgibt, dann wird, sofern die Größenordnung der Fehlerverteilung innerhalb des Voxels größer als ein spezifischer Referenzwert ist, die Zuverlässigkeit oder Genauigkeit des Messwerts niedriger als eine spezifische Referenz eingestuft, und der Messwert für das Voxel wird nicht an die Ausgabevorrichtung ausgegeben.
Darüber hinaus verwendet die Dateneingabevorrichtung einen Entfernungssensor, um, als Entfernungsdaten mit einer willkürlichen Messposition als dem Ursprung, Koordinatenwerte auf der dreidimensionalen Gestalt sequenziell zu erhalten, während der Ursprung bewegt wird.
Es gibt eine Modellauffrischungs-Vorrichtung zum Auffrischen des Umgebungsmodells nach dem Zuordnungsschritt, wobei die Modellauffrischungs-Vorrichtung ein Voxel abruft, das dem Koordinatenwert des neu eingegebenen gemessenen Punkts entspricht; und wenn der repräsentative Punkt und/oder die Fehlerverteilung innerhalb dieses Voxels neu festgelegt oder nachjustiert wird, oder dieses Voxel hierarchisch ferner in eine Vielzahl von Voxeln unterteilt wird, dann gibt die Ausgabevorrichtung die Position des repräsentativen Punkts dieses Voxels als einen dreidimensionalen Gestalt-Messwert an eine Ausgabevorrichtung aus.
Die Datenübertragungsvorrichtung gibt die Position eines repräsentativen Punkts für ein Voxel im Umgebungsmodell in einem Bereich, für welchen die Position von der Position des Entfernungssensors aus gemessen werden kann, als einen dreidimensionalen Gestalt-Messwert an eine Ausgabevorrichtung aus.
Das Verfahren und die Vorrichtung, wie in der vorliegenden Erfindung dargestellt, wie oben beschrieben, unterteilen eine räumliche Region, in der eine dreidimensionale Gestalt existiert, in eine Vielzahl von Voxeln und zeichnen die Position jedes Voxels auf, so dass, wenn das zu messende Objekt groß ist, die Datengröße in Proportion zur Anzahl an Voxeln klein gehalten wird.
Zusätzlich dazu werden ein repräsentativer Punkt und seine Fehlerverteilung innerhalb eines Voxels, das einem Koordinatenwert entspricht, festgelegt und aufgezeichnet, was die Anzeige von Daten bei mehr als der Voxel-Auflösung ermöglicht.
Folglich kann die Datenstruktur, wie in der vorliegenden Erfindung dargestellt, in eine festgelegte Größe hinein, gemessene Daten aus einer Vielzahl von Beobachtungspunkten integrieren.
Zusätzlich dazu ermöglicht das Festlegen und Speichern eines Wahrscheinlichkeitswerts, der die Wahrscheinlichkeit der Existenz eines Objekts innerhalb des Voxels anzeigt, eine einfache Ermittlung hinsichtlich des Wahrscheinlichkeitswerts für das Voxel, ohne Notwendigkeit, ein Voxel mit einem repräsentativen Punkt zu finden, welcher repräsentiert, ob ein Objekt in dem Voxel existiert, oder nicht, und ohne Notwendigkeit, eine Neuberechnung aus der Fehlerverteilung vornehmen zu müssen, selbst wenn die Fehlerverteilung breiter als das Voxel mit dem repräsentativen Punkt ist, wodurch es möglich gemacht wird, die Suchzeit zu verringern, bzw. zu beschränken.
Außerdem begrenzt das Erhalten der derzeitigen Messposition durch Rückschluss aus einer Änderung in der Messposition der Vergangenheit, oder aus einem Sensor, welcher in der Lage zum Erhalten der derzeitigen Messposition ist, oder durch die Verwendung eines Reflexionsstärkenwerts zusätzlich zu dem Distanzwert der gemessenen Daten, den Bereich der Prüfung (Zuordnung), wodurch ermöglicht wird, dass die Suchzeit beschränkt wird.
Darüber hinaus kann, in einem Grobzuordnungsschritt, eine Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen gemessenen Daten und benachbarten Voxeln, und zwischen Fehlerverteilungen und benachbarten Voxeln, wobei die Voxel repräsentative Punkte aufweisen, zu minimieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zum Umgebungsmodell für die vorherige Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen Voxeln mit repräsentativen Punkten, zu minimieren, oder durch Ausführen der Positionszuordnung durch Rotieren und Translatieren einer neuen Messdaten- und Fehlerverteilung relativ zu dem Umgebungsmodell für die vorherige Messposition, um einen Prüfwert (wie die Summierung der Wahrscheinlichkeitswerte) bezüglich der Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten und Fehlerverteilungen benachbart sind, zu maximieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zum Umgebungsmodell für die vorherige Messposition, um einen Prüfwert (wie die Summierung der Differenzen in den Wahrscheinlichkeitswerten) bezüglich der Differenzen in Wahrscheinlichkeitswerten von benachbarten Voxeln zu minimieren, durchgeführt werden, wodurch es möglich gemacht wird, die Positionszuordnung von Voxeln mit repräsentativen Punkten zueinander durchzuführen, und zwar in einer kurzen Zeitdauer, während ein kumulativer Fehler verhindert wird.
Beim Durchführen der Positionszuordnung durch Rotieren und Translatieren des Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für die vorherige Messposition, sodass ein Prüfwert (wie die Summierung von Differenzen von Wahrscheinlichkeitswerten) bezüglich der Differenz in den Wahrscheinlichkeitswerten von benachbarten Voxeln, minimiert wird, ist außerdem die Genauigkeit verbessert, weil die Daten des Falls, bei welchem kein Objekt existiert, ebenfalls berücksichtigt werden.
Im Anschluss daran kann, in einem Feinzuordnungsschritt, weil die Positionszuordnung so durchgeführt wird, dass ein Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird, die Fein-Positionszuordnung von Fehlerverteilungen zueinander deshalb in einer kurzen Zeitdauer ausgeführt werden.
Folglich kann die vorliegende Erfindung Hochpräzisions-Gestalten erhalten, während ein kumulativer Fehler durch das Verfahren der Integration von Daten aus einer Vielzahl von Beobachtungspunkten verhindert wird.
Weil die Datenstruktur der vorliegenden Erfindung eine Struktur ist, worin die Voxel-Struktur erweitert, bzw. vergrößert wird, ist außerdem die Datengröße kleiner, wenn sie mit der Punktegruppe verglichen wird. Mit anderen Worten kann bei der in der vorliegenden Erfindung vorgeschlagenen Datenstruktur, weil ein einziger repräsentativer Punkt innerhalb eines Voxels existiert, die Berechnungsgrößenordnung für das Suchen nach einem Modellpunkt, der einem gemessenen Punkt entspricht, auf 1 gesetzt werden, und daher kann die gesamte Berechnungsgrößenordnung auf O(N) reduziert werden.
Folglich wird, bei der Durchführung der Zuordnung (dem ICP-Algorithmus) der umliegenden Umgebung in den gemessenen Daten, die Datengröße, welche der Suche unterworfen wird, verringert, was eine verbesserte Effizienz in den Berechnungen ermöglicht.
Während der herkömmliche ICP-Algorithmus fehlerhafte Ergebnisse für spärliche Daten ausgibt, weist das Umgebungsmodell der vorliegenden Erfindung außerdem repräsentative Punkte und Fehlerverteilungen innerhalb von Voxeln auf, und daher ist eine Positionszuordnung für spärliche Daten möglich.
Darüber hinaus kann die Genauigkeit der Eigenpositionierung durch Korrigieren der Eigenposition und der Fehlerverteilung durch Kalman-Filterung aus der derzeitigen Eigenposition und Fehlerverteilung und aus der Eigenposition und Fehlerverteilung, welche identifiziert worden sind, verbessert werden.
Folglich besteht, angesichts des Verfahrens und der Vorrichtung, wie in der vorliegenden Erfindung dargestellt, nicht nur eine Funktion zum Korrigieren, um Daten, und zwar Daten, die einen Fehler enthalten, zu berichtigen, sondern auch zum Konvergieren zu einer hochgenauen Gestalt in Messungen über längere Zeitperioden hinweg. Weiterhin ist, im Positionsidentifizierungsverfahren, das Ausmaß an Berechnungen zum Auffrischen, mittels der gemessenen Punkte, der repräsentativen Punkte und Fehlerverteilungen für die Voxel, entsprechend jedem der Voxel, klein, und die Berechnung ist innerhalb eines Voxels abgeschlossen, wobei sie keinen Einfluss auf die umliegenden Voxel besitzt. Dies ermöglicht eine Hochgeschwindigkeits-Verarbeitung. Darüber hinaus können die gemessenen Daten sequenziell in die Voxel integriert werden, und die Größe des Speichers für die Daten der umliegenden Umgebung, welche erhalten werden, geht nicht über eine spezielle Größe hinaus.
Ferner werden im Modell-Auffrischungsschritt ein(e) neuer repräsentativer Punkt und Fehlerverteilung durch einen Kalman-Filter aus einem Koordinatenwert des neu eingegebenen gemessenen Punkts und dessen Fehlerverteilung und dem repräsentativen Punkt und dessen Fehlerverteilung, welche bereits im Voxel festgelegt worden sind, erhalten und neu eingestellt, was das Erhalten einer Gestalt ermöglicht, welche dem tatsächlichen Wert näher kommt.
Durch Wiederholen des Modell-Auffrischungsschritts, welcher den Kalman-Filter verwendet, erzeugt die Wirkung des Kalman-Filters im Besonderen eine hochgenaue Gestalt, welche zum tatsächlichen Wert hin konvergiert, selbst wenn die Daten Fehler enthalten.
Im Feinzuordnungsschritt werden darüber hinaus eine neue gemessene Daten- und Fehlerverteilung relativ zu dem Umgebungsmodell für die frühere Messposition rotiert und translatiert, oder ein Umgebungsmodell für eine neue Messposition wird relativ zu dem Umgebungsmodell für die frühere Messposition rotiert und translatiert, um den Prüfwert (wie die Multiplikation der Koinzidenzgrade) bezüglich des Koinzidenzgrad, der durch Maximum-Likelihood-Schätzungen aufgestellt wurde, basierend auf der Distanz zwischen benachbarten Fehlerverteilungen, zu maximieren, anstatt einer Positionszuordnung zum Minimieren eines Prüfwerts (wie der Summierung von Distanzen) bezüglich der Distanz zwischen benachbarten Fehlerverteilungen, was ermöglicht, dass die Positionszuordnung den Fehler sowohl im Umgebungsmodell als auch in den gemessenen Daten berücksichtigt.
Weiterhin wird, im Ausgabeschritt, die Position des repräsentativen Punkts des Voxels als der dreidimensionale Gestalt-Messwert an die Ausgabevorrichtung ausgegeben, und ein Index, der die Zuverlässigkeit oder Genauigkeit des Messwerts, basierend auf der Größenordnung der Fehlerverteilung innerhalb des Voxels, anzeigt, wird an die Ausgabevorrichtung ausgegeben, sodass dem Anwender ermöglicht wird, das Löschen von gemessenen Werten, die eine geringe Zuverlässigkeit besitzen, abhängig von den Einzelheiten der Anwendung, zu entscheiden, wenn die Messvorrichtung verwendet wird.
Wenn die Position des repräsentativen Punkts des Voxels als dreidimensionaler Gestalt-Messwert an die Ausgabevorrichtung ausgegeben wird, dann wird darüber hinaus in diesem Ausgabeschritt, sofern die Größenordnung der Fehlerverteilung innerhalb des Voxels größer als ein spezifischer Referenzwert ist, die Zuverlässigkeit oder Genauigkeit des Messwerts niedriger als eine spezifische Referenz eingestuft, sodass der Messwert für das Voxel nicht an die Ausgabevorrichtung ausgegeben wird, was zu einer Verringerung der Größe der Daten, mit denen umzugehen ist, und zu einer Erhöhung der Zuverlässigkeit führt, weil es beim Verwenden der Messvorrichtung möglich ist, zunächst nur die Messwerte zu behandeln, die eine hohe Zuverlässigkeit aufweisen.
Im Dateneingabeschritt ist, durch Verwendung des Entfernungssensors zum sequenziellen Erhalten von, als Entfernungsdaten mit einer willkürlichen Messposition als Ursprung, dreidimensionalen Gestalt-Koordinatenwerten während des Bewegens des Ursprungs, und insbesondere durch Erhalten von Entfernungsdaten aus unterschiedlichen Richtungen, außerdem die Integration von Entfernungsdaten von Fehlerverteilungen mit unterschiedlichen Verteilungsorientierungen möglich, wodurch eine erhöhte Genauigkeit ermöglicht wird.
Weiterhin gibt es, nach dem Zuordnungsschritt, einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells; wobei der Modell-Auffrischungsschritt ein Voxel aufruft, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und nur dann, wenn der repräsentative Punkt und/oder die Fehlerverteilung innerhalb dieses Voxels neu eingestellt, oder nachjustiert werden, oder dieses Voxel hierarchisch in eine Vielzahl von Voxeln weiter unterteilt wird, im Ausgabeschritt dann wenigstens eines von der Position des repräsentativen Punkts, der Fehlerverteilung und der Position dieses Voxels, als ein dreidimensionaler Gestalt-Messwert an die Ausgabevorrichtung ausgegeben wird, was zum Ausgeben des Werts eines repräsentativen Punkts oder dergleichen innerhalb eines Voxels führt, der durch den vom Entfernungssensor neu erhaltenen, gemessenen Punkt beeinflusst worden ist. Deswegen ist der Anwender, während eine Bedienung beabsichtigt wird, welche identisch zu jener ist, die herkömmlich ist, in der Lage, das Gerät so zu verwenden, dass die ursprünglichen, aus dem Entfernungssensor erhaltenen Messwerte durch solche ersetzt werden können, welche Messwerte höherer Genauigkeit zu sein scheinen. Dies ermöglicht dreidimensionale Gestaltmessungen mit erhöhter Genauigkeit.
Ferner ermöglicht, im Ausgabeschritt, das Ausgeben der Position eines repräsentativen Punkts für ein Voxel im Umgebungsmodell in einem Bereich, für den die Position von der Position des Entfernungssensors aus gemessen werden kann, als dreidimensionalen Gestalt-Messwert an eine Ausgabevorrichtung den Einsatz von etwas, was als Entfernungssensor mit höherer Genauigkeit und höherer Auflösung wirkt, selbst bei der groben Auflösung der Messwerte eines herkömmlichen Entfernungssensors. Dies gestattet dreidimensionale Gestaltmessungen mit erhöhter Genauigkeit.
Andere Ziele und vorteilhafte Aspekte der vorliegenden Erfindung werden aus der nachstehenden Beschreibung unter Bezugnahme auf die Zeichnungen klar werden.
KURZE BESCHREIBUNG DER ZEICHNUNGEN
1 ist ein schematisches Diagramm einer "Eigenpositions-Identifizierungsvorrichtung" von Patentdokument 1.
2 ist ein Diagramm des Gesamtaufbaus der Eigenpositions-Identifizierungsvorrichtung, wie in der vorliegenden Erfindung dargestellt.
3 ist eine Strukturzeichnung eines im Nicht-Patentdokument 2 offenbarten dreidimensionalen Laserradars.
4A ist ein Diagramm, welches die Beziehung zwischen Fehler und Polarkoordinaten-Daten veranschaulicht, die unter Verwendung eines Entfernungssensors gemessen werden.
4B veranschaulicht einen Fall der Näherung der Fehlerverteilung als ein Ellipsoid, das in einem rechteckigen Körper enthalten ist.
5 ist ein Vorrichtungs-Strukturdiagramm zum Ausführen eines Verfahrens, wie in der vorliegenden Erfindung dargestellt.
6 ist ein Flussdiagramm, welches ein Verfahren, wie in der vorliegenden Erfindung dargestellt, veranschaulicht.
7 ist ein schematisches Diagramm eines Modell-Strukturierungsschritts.
8 ist ein schematisches Diagramm des entworfenen Umgebungsmodells.
9A ist ein Diagramm, welches die Datenstruktur von Voxel-Daten in der vorliegenden Erfindung veranschaulicht, wobei ein Speicher-Auslegungsbeispiel für einzelne Voxel-Daten veranschaulicht ist.
9B ist ein Diagramm, welches die Datenstruktur von Voxel-Daten in der vorliegenden Erfindung veranschaulicht, wobei ein Beispiel veranschaulicht ist, worin ein Level-2(1, 1, 0)-Voxel einen repräsentativen Punkt aufweist.
10 ist ein Datenverarbeitungs-Flussdiagramm für den Grobzuordnungsschritt S6 und den Feinzuordnungsschritt S7.
11 ist ein schematisches Diagramm des Grobzuordnungsschritts S6.
12 ist ein schematisches Diagramm des Feinzuordnungsschritts S7.
13 ist ein Datenverarbeitungs-Flussdiagramm im Modell-Auffrischungsschritt.
14 ist ein schematisches Diagramm des Falls, bei welchem ein repräsentativer Punkt bereits in dem zutreffenden Voxel festgelegt worden ist.
15 veranschaulicht den Fall, worin sich der neue repräsentative Punkt in einen anderen Voxel hinein bewegt hat, und zwar als Ergebnis des Neu-Einstellens einer neuen Fehlerverteilung und der Mitte der neuen Fehlerverteilung aus beiden Fehlerverteilungen, wenn sich die Fehlerverteilungen gegenseitig überlappen.
16 ist ein schematisches Diagramm des Falls, bei welchem sich die Fehlerverteilungen gegenseitig überlappen.
17 ist ein schematisches Diagramm, welches den Fall veranschaulicht, worin die Genauigkeit des repräsentativen Punkts durch Verkleinern der Fehlerverteilung des repräsentativen Punkts durch Integrieren von Entfernungsdaten mit einer Vielzahl von Messpositionen als Ursprüngen erhöht wird.
18 veranschaulicht das durch den Modell-Auffrischungsschritt unter Verwendung des Kalman-Filters erhaltene Ergebnis.
19 ist eine teilweise vergrößerte Ansicht von 18.
20 veranschaulicht die Korrelationen unter Berücksichtigung des Fehlers.
AUSFÜHRLICHE BESCHREIBUNG DER BEVORZUGTEN AUSFÜHRUNGSFORMEN
Eine bevorzugte Ausführungsform der vorliegenden Erfindung wird nachstehend unter Bezugnahme auf die Zeichnungen erläutert. Es ist anzumerken, dass in jeder Zeichnung identischen Teilen identische Codes zugewiesen werden, und dass redundante Erklärungen ausgelassen werden.
2 ist ein Gesamtstrukturdiagramm der Eigenpositions-Identifizierungsvorrichtung, wie in der vorliegenden Erfindung dargestellt. In dieser Figur ist die Eigenpositions-Identifizierungsvorrichtung, wie in der vorliegenden Erfindung dargestellt, eine Einrichtung zur Identifizierung bei sechs Freiheitsgraden, eine Vorrichtung, welche die Eigenposition durch Aufnahme von dreidimensionaler Information aus dem Außenraum von einem Entfernungssensor, einem Wegmesser, einer Kamera, einem GPS-Gerät und einem Orientierungssensor identifiziert. Es ist anzumerken, dass in der vorliegenden Erfindung "Eigenposition" eine Position und Orientierung in sechs Freiheitsgraden in der äußeren Umgebung der Eigenpositions-Bestimmungsvorrichtung bedeutet.
Es ist anzumerken, dass in der vorliegenden Erfindung der Wegmesser, die Kamera, die GPS-Vorrichtung und der Orientierungssensor zusätzlich zu dem Entfernungssensor nicht unverzichtbar sind, sondern nach Bedarf wahlfrei eingesetzt werden können. Nachstehend ist ein Beispiel erläutert, in welchem der Entfernungssensor verwendet wird.
3 ist ein Strukturdiagramm eines dreidimensionalen Laserradars als einem Beispiel für einen Entfernungssensor. Ein dreidimensionales Laserradar wird zum Beispiel im Nicht-Patentdokument 3 offenbart.
Wie in der Zeichnung veranschaulicht, ist das dreidimensionale Laserradar 10 aus einem Radarkopf 12 und einer Steuervorrichtung 20 aufgebaut. Ein von einer Laserdiode 13 erzeugter Puls-Laserstrahl 1 wird durch eine Projektionslinse 14 zu einem kollimierten Strahl 2 geformt, und wird in zweidimensionalen Richtungen durch die Spiegel 18a und 18b und den Polygonspiegel 15, der rotiert und schwingt, gerastert, um das zu messende Objekt zu beleuchten. Der Puls-Laserstrahl 3, der von dem zu messenden Objekt reflektiert wird, wird durch die Foto-Empfängerlinse 16 über den Polygonspiegel 15 fokussiert, um durch einen Fotodetektor 17 in ein elektrisches Signal umgewandelt zu werden.
Ein Zeitintervall-Zähler 21 einer Steuervorrichtung 20 misst das Zeitintervall eines Startpulses 4, der mit der Pulsemissions-Zeitgebung der Laserdiode 13 synchronisiert ist, und eines Stopp-Pulses 5, der aus dem Fotodetektor 17 ausgegeben wird. Eine Signalverarbeitungsplatine 22 gibt das Zeitintervall t und den Rotationswinkel θ und den Schwenkwinkel φ des Polygonspiegels als Polarkoordinaten-Daten (r, θ, φ) zu dem Zeitpunkt, an welchem der reflektierte Strahl detektiert wird, aus.
r ist die Entfernung (Distanz) unter Behandlung der Messposition (der Aufstellungsposition des Radarkopfs) als Ursprung, die mittels der Formel r = c × t/2 berechnet wird. Hierbei ist c die Lichtgeschwindigkeit.
Die Ermittlungs-Verarbeitungseinheit 23 wandelt die Polarkoordinaten-Daten von der Signalverarbeitungsplatine in dreidimensionale Raumdaten (x, y, z) um, wobei die Aufstellungsposition des Radarkopfs der Ursprung ist, und führt ein Detektionsverfahren aus. Es ist anzumerken, dass 24 in dieser Figur eine Antriebseinheit ist.
Die Messbereich des oben beschriebenen dreidimensionalen Laserradars 10 ist zum Beispiel ein Horizontalfeld-Winkel von 60°, ein Vertikalfeld-Winkel von 30° und eine maximale Messentfernung von 50 m. Darüber hinaus beläuft sich die Positions-Nachweisgenauigkeit zum Beispiel auf etwa 20 cm.
Wenn die gemessenen Daten als ein Entfernungsbild angezeigt werden, aufweisend Entfernungswerte in der Tiefenrichtung für jedes der Bildelemente, werden außerdem, falls die Anzahl an Messpunkten in einem Einzelbild 166 Punkte in der Breitenrichtung und 50 Punkte in der Abtastrichtung beträgt, in einem Einzelbild dann 166 × 50 = 8300 Punkte angezeigt. In diesem Fall beläuft sich die Einzelbildrate zum Beispiel auf etwa zwei Einzelbilder pro Sekunde.
Die gemessenen Punkte auf der dreidimensionalen Gestalt, welche durch den Dreidimensional-Laserradar 10 gemessen werden, bilden eine Gruppe von Punkten, welche gegenseitig um Δθ × r in der Breitenrichtung, sowie um Δφ × r in der vertikalen Richtung, verteilt sind. Zum Beispiel beläuft sich, im Fall von Δθ = 60/166 × π/180 = 6,3 × 10^-3 rad, Δφ = 30/50 × π/180 = 10,5 × 10^-3 rad, und r = 50 m, das Intervall zwischen den gemessenen Punkten auf ungefähr 315 mm in der Horizontalrichtung und ungefähr 525 mm in der Vertikalrichtung, sogar für die gemessenen Punkte, welche am nächsten liegen.
In der vorliegenden Erfindung wird zum Beispiel ein dreidimensionales Laserradar 10, wie oben beschrieben, als der Entfernungssensor verwendet. Jedoch ist der Entfernungssensor nicht darauf beschränkt, sondern kann vielmehr einen Entfernungssensor, der Parallaxe(n) verwendet, oder einen anderen bekannten Entfernungssensor einsetzen.
4A und 4B sind Diagramme, welche die Beziehung zwischen dem Fehler und den Polarkoordinaten-Daten, die unter Verwendung eines Entfernungssensors gemessen werden, veranschaulichen.
Wie in 4A gezeigt, werden die Polarkoordinatenwerte (r, θ, φ), mit einer willkürlichen Messposition als dem Ursprung, als die Messergebnisse gemessen. Normalerweise gibt es eine Fehlerverteilung, wie in der Figur gezeigt, in den Messergebnissen aus dem Entfernungssensor.
Wenn diese Fehlerverteilung die Existenzwahrscheinlichkeit P(r_s, θ_s, φ_s) bei r_s, θ_s, φ_s der Fehlerverteilung ist, dann weist die Fehlerverteilung Normalverteilungen in axialer r-Richtung, θ-Richtung und φ-Richtungen der Messungen auf, und kann beispielsweise als Formel (1) repräsentiert werden. Hierbei sind r, θ und φ durch den Sensor gemessene Werte, σ_r, σ_θ und σ_φ sind Standardabweichungen, wobei A eine Normierungskonstante ist.
Wie in 4B gezeigt wird, ist die Fehlerverteilung normalerweise innerhalb eines Scheitel-geschnittenen Kegels enthalten, der sich in der r-Richtung der Länge nach erstreckt (Figur auf der linken Seite), und die Differenz zwischen a und b an der entfernt liegenden Stelle ist gering. Folglich kann die Fehlerverteilung mit Sicherheit als ein in einem rechteckigen Körper enthaltenes Ellipsoid angenähert werden. (Gleichung 2)
5 ist ein Vorrichtungs-Strukturdiagramm zur Ausführung eines Verfahrens, wie in der vorliegenden Erfindung dargestellt. Wie in dieser Figur gezeigt, ist die Vorrichtung mit einer Dateneingabevorrichtung 32, einer externen Speichervorrichtung 33, einer internen Speichervorrichtung 34, einer zentralen Verarbeitungsvorrichtung 35 und einer Ausgabevorrichtung 36 ausgestattet.
Die Dateneingabevorrichtung 32 weist den oben beschriebenen Entfernungssensor auf und gibt in die Speichervorrichtungen die Koordinatenwerte auf der dreidimensionalen Gestalt ein. Darüber hinaus können die Position/Lage und der Bewegungsbereich des Entfernungssensors zum Beispiel durch die Verwendung eines Goniometers und eines Wegmessers eingegeben werden. Es ist anzumerken, dass die Dateneingabevorrichtung 32 auch eine normale Eingabevorrichtung, wie eine Tastatur, aufweisen kann.
Die externe Speichervorrichtung 33 ist beispielsweise eine Festplatte, eine Floppy^®-Diskette, ein Magnetband, eine Compact-Disc, bzw. CD oder dergleichen. Wenn die Größe des Umgebungsmodells groß wird, und die Koordinatenwerte auf der dreidimensionalen Gestalt, die Voxelpositionen und die repräsentativen Punkte und deren Fehlerverteilungen nicht vollständig in der internen Speichervorrichtung 34, wie nachstehend beschrieben, gehalten werden können, dann speichert die externe Speichervorrichtung 33 einen Teil oder die Gesamtheit der eingegebenen Dreidimensional-Gestalt-Koordinatenwerte, Voxelpositionen und repräsentativen Punkte und deren Fehlerverteilungen für ein partielles oder vollständiges Umgebungsmodell, und speichert ein Programm zum Ausführen des Verfahrens der vorliegenden Erfindung.
Die interne Speichervorrichtung 34 ist zum Beispiel ein RAM, ein ROM oder dergleichen, und speichert einen Teil oder die Gesamtheit der eingegebenen dreidimensionalen Gestalt-Koordinatenwerte, Voxelpositionen und repräsentativen Punkte und deren Fehlerverteilungen für ein partielles oder vollständiges Umgebungsmodell.
Die zentrale Verarbeitungsvorrichtung (CPU) 35 wirkt als eine Modell-Strukturierungsvorrichtung, eine Zuordnungsvorrichtung, eine Positionszuordnungsvorrichtung für Grob- und Feinzuordnung, eine Modellauffrischungs-Vorrichtung und eine Datenübertragungsvorrichtung, und verarbeitet zentral Berechnungen, Eingabe-Ausgabe und dergleichen, und führt zusammen mit der internen Speichervorrichtung 34 Programme aus. Die Modell-Strukturierungsvorrichtung ist eine Vorrichtung zur Durchführung des Modell-Strukturierungsschritts, wie nachstehend beschrieben; die Zuordnungsvorrichtung ist ein Vorrichtung zur Durchführung des Zuordnungsschritts; die Positionszuordnungsvorrichtung ist eine Vorrichtung zur Durchführung des Grobzuordnungsschritts und des Feinzuordnungsschritts, welche nachstehend beschrieben sind; die Modellauffrischungs-Vorrichtung ist eine Vorrichtung zur Durchführung des Modell-Auffrischungsschritts, welcher nachstehend beschrieben ist; und die Datenübertragungsvorrichtung ist eine Vorrichtung zum Ausgeben von Daten an eine Ausgabevorrichtung 36.
Die Ausgabevorrichtung 36 ist zum Beispiel eine Anzeigevorrichtung, ein Drucker, eine externe Vorrichtung oder dergleichen, und ist in der Lage, wenigstens die Programmausführungsergebnisse und die Daten, welche in der internen Speichervorrichtung 34 und externen Speichervorrichtung 33 gespeichert sind, auszugeben. Die Schnittstelle mit der externen Vorrichtung ist ein LAN, eine USB-Vorrichtung und IEEE 1394 oder dergleichen, und gibt in Antwort auf eine Anfrage die Ergebnisse der Gesamtheit des Umgebungsmodells oder eines Abschnitts des Umgebungsmodells, die Ergebnisse des Anwendens der repräsentativen Punkte, Fehlerverteilungen, Voxelpositionen und dergleichen, innerhalb der betreffenden Voxel, entsprechend den eingegebenen Koordinatenwerten auf der dreidimensionalen Gestalt, und dergleichen, aus.
Die oben beschriebene Vorrichtung der vorliegenden Erfindung kann eine Kombination aus dem oben beschriebenen Entfernungssensor mit einem normalen PC (Computer) sein, oder kann eine integrierte Vorrichtung sein. Darüber hinaus kann die Vorrichtung in eine Gerätschaft eingebunden sein, welche sich unabhängig bewegen kann.
6 ist ein Flussdiagramm, welches ein Verfahren, wie in der vorliegenden Erfindung dargestellt, veranschaulicht.
Das Verfahren, wie in der vorliegenden Erfindung dargestellt, ist ein Eigenpositions-Identifizierungsverfahren, welches eine Eigenposition durch Aufnehmen von dreidimensionalen Gestaltsdaten aus dem Außenraum identifiziert, sowie ein dreidimensionales Gestalt-Messungsverfahren, umfassend einen Dateneingabeschritt S1, einen Datenkorrekturschritt S2, einen Suchbereich-Beschränkungsschritt S3, einen Modell-Strukturierungsschritt S4, einen Zuordnungsschritt S5, die Eigenpositions-Identifizierungsschritte S7 und S10, einen Grobzuordnungsschritt S8, einen Feinzuordnungsschritt S9, einen Modell-Auffrischungsschritt S11 und einen Ausgabeschritt S12.
Es ist anzumerken, dass diese Abfolge von Prozessen, S1, S2, S3 und S5 bis S12, jedesmal ausgeführt werden, wenn gemessene Daten erhalten werden, und S4 nur ausgeführt wird, wenn zuerst gemessene Daten erhalten werden.
Im Dateneingabeschritt S1 wird der Entfernungssensor verwendet, um Koordinatenwerte auf der dreidimensionalen Gestalt in die Speichervorrichtung eines Computers einzugeben. Weiterhin können die Position/Lage und der Bewegungsbereich des Entfernungssensors beispielsweise durch die Verwendung eines Goniometers und eines Wegmessers eingegeben werden.
Ferner kann, in diesem Dateneingabeschritt S1, ein Dreidimensional-Laserradar 10 verwendet werden, um sequenziell, als Entfernungsdaten mit einer willkürlichen Messposition als dem Ursprung, Koordinatenwerte auf der dreidimensionalen Gestalt zu erhalten, während der Ursprung bewegt wird.
Wenn ein Dreidimensional-Laserradar 10 als der Entfernungssensor verwendet wird, sind die Koordinatenwerte auf der dreidimensionalen Gestalt Entfernungsdaten mit einer willkürlichen Messposition als dem Ursprung, und werden als Polarkoordinatenwerte (r, θ, φ) ausgedrückt. Darüber hinaus werden die Fehlerverteilungen von jedem der Koordinatenwerte entweder durch Berechnung aus den Polarkoordinatenwerten (r, θ, φ) gefunden oder im Voraus durch separate Eingabeeinrichtungen (zum Beispiel eine Tastatur) eingegeben.
Im Datenkorrekturschritt S2 wird ein Entfernungsdaten-Korrekturprozess ausgeführt, um die Genauigkeit der Entfernungsdaten zu verbessern. Darüber hinaus können die Polarkoordinaten-Daten und die Wegmesserdaten in dreidimensionale Raumdaten (x, y, z) umgewandelt werden, wobei eine willkürlich festgelegte Position als der Ursprung herangezogen wird.
Im Korrekturprozess für die Entfernungsdaten werden isolierte Punkte (Acnodes) gelöscht, eine statistische Verarbeitung durchgeführt, und dergleichen. Ein "Acnode" ist ein Punkt, welcher in Isolation von den umliegenden Punkten existiert, und weil die gemessenen Daten eine Vielzahl von proximalen, bzw. naheliegenden Punkten umfassen, können "Acnodes" als Messfehler betrachtet und eliminiert werden. Die statistischen Verfahren berücksichtigen die in den statistischen Daten eingeschlossene Fehlerverteilung, um die Entfernungen durch Ausführen einer statistischen Verarbeitung (wie Mittelung oder dergleichen) an mehreren wiederholten Messungen zu korrigieren.
Sofern die dreidimensionale Zielgestalt unter Verwendung einer linearen Näherung oder einer planaren Näherung approximiert werden kann, kann außerdem eine derartige Näherung durchgeführt werden.
Der Suchbereich des Entfernungssensors wird durch den Suchbereichs-Beschränkungsschritt S3 limitiert.
Es können mehrere Lösungen (zu messende Punkte) erhalten werden, wenn ein Zuordnungsverfahren für die gemessenen Daten für das Umgebungsmodell ohne Beschränken des Suchbereichs durchgeführt wird. Angesichts dieser Tatsache (1) wird die derzeitige Sensorposition basierend auf der Änderung zu einer vergangenen Sensorposition abgeschätzt, um das Suchen in der Nachbarschaft des Sensorpositions-Schätzungsergebnisses durchzuführen; (2) wird die Sensorposition unter Verwendung eines Wegmessers geschätzt, um den Suchbereich zu beschränken; (3) werden die Suchergebnisse unter Verwendung eines Reflexionsstärkenwerts und des Distanzwerts der Distanzdaten und dergleichen, eingeengt.
7 ist ein schematisches Diagramm des Modell-Strukturierungsschritts, wenn ein Octree in der Voxel-Unterteilung verwendet wird.
Im Modell-Strukturierungsschritt S4, wie er in dieser Figur gezeigt ist, wird ein Umgebungsmodell aufgebaut, wobei eine räumliche Region, in der eine dreidimensionale Gestalt existiert, in eine Vielzahl von Voxeln 6, die aus rechteckigen Körpern gebildet sind, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt wird und die Position der einzelnen Voxel abgespeichert wird.
Die Form der Voxel 6 kann in einem Kubus bestehen, bei welchem die Länge jeder der Seiten identisch ist, oder kann in einem Rechteckskörper bestehen, bei welchem die Längen der Seiten unterschiedlich sind.
Darüber hinaus können die Längen von jeder der Kanten des Voxels 6 auf eine Größe eingestellt werden, entsprechend der minimal erforderlichen Auflösung des größten Voxels 6. Nachfolgend wird das größte Voxel 6 als das "Level 1-Voxel" bezeichnet.
Wenn eine Vielzahl von gemessenen Punkten innerhalb eines einzelnen Voxels existiert, wird dieses einzelne Voxel weiterhin hierarchisch in eine Vielzahl von Voxeln unterteilt, zum Beispiel unter Anwendung einer Octree-Teilung, falls ein Octree ausgewählt ist, sodass es innerhalb eines einzelnen Voxels nur einen einzigen gemessenen Punkt gibt. Im Untenstehenden wird eine räumliche Region, an welcher eine Octree-Teilung des Maximum-Voxels 6 einmal durchgeführt worden ist, als ein "Level 2-Voxel" bezeichnet, und eine räumliche Region, an welcher eine Octree-Teilung k-mal durchgeführt worden ist, wird als ein "Level k + 1-Voxel" bezeichnet.
8 ist ein schematisches Diagramm des aufgestellten Umgebungsmodells.
Im Zuordnungsschritt S5, wie er in der Figur gezeigt ist, werden ein repräsentativer Punkt 7 und dessen Fehlerverteilung 8 in das Voxel 6, das einem Koordinatenwert auf einer dreidimensionalen Gestalt entspricht, hinein festgelegt und werden abgespeichert. Das letztendliche Voxel kann nur einen einzigen repräsentativen Punkt für einen gemessenen Wert aufweisen. Jedes Voxel weist einen repräsentativen Punkt für einen gemessenen Wert und dessen Fehlerverteilung auf, um die Gestalt des Objekts darzustellen. Darüber hinaus kann das Voxel auch einen Wahrscheinlichkeitswert aufweisen, der die Existenzwahrscheinlichkeit des Objekts angibt.
Im Zuordnungsschritt S5 wird die absolute Position des repräsentativen Punkts durch die Gleichungsformel (2) der Gleichung 3 angegeben. Hierbei sind (x, y, z) relative Koordinaten des Voxels des repräsentativen Punkts; Sx, Sy und Sz sind die Größen der Seiten des Voxels bei Level 1; n_x(k), n_y(k) und n_z(k) sind die Voxel-Adressen bei Level k; und L ist das Level, bzw. die Stufe, auf dem/der der zu berechnende repräsentative Punkt existiert. (Gleichung 3)
9A und 9B sind Figuren, welche die Datenstrukturen für Voxeldaten in der vorliegenden Erfindung veranschaulichen.
In dieser Figur ist 9A ein Beispiel der Speicherauslegung für individuelle Voxeldaten. In dieser Figur zeigen die Pfeilmarkierungen Sprungmarken zu Daten, und Werte enthalten Hinweise zu Daten.
9B veranschaulicht ein Beispiel eines Falls, worin ein Level 2(1, 1, 0)-Voxel einen repräsentativen Punkt aufweist. Es ist anzumerken, dass in dieser Figur null eine leere Menge anzeigt.
Ein Umgebungsmodell, welches die oben beschriebene Datenstruktur aufweist, besitzt die folgenden Charakteristika:

(1) Inhalt: Ein Raum wird durch kleine rechteckige Körper unterteilt, sodass jedes Voxel einen repräsentativen Punkt und eine Fehlerverteilung für einen Messpunkt aufweist.
(2) Genauigkeit: Die repräsentativen Werte entsprechen den Messpunkten für jedes Voxel.
(3) Vorkommen: Es ist möglich, auszudrücken, ob ein Objekt existiert, oder nicht.
(4) Datengröße: Speicher wird proportional zur Anzahl an Voxeln erfordert, aber die Größe ist konstant.
(5) Umwandlung aus einer Punktegruppe: Geeignet, mit geringem Rechenaufwand.
(6) Zugriffgeschwindigkeit: Der Zugriff auf Elemente erfolgt auf Grund der einfachen Struktur rasch.

Angesichts dieser Merkmale erfüllt das oben beschriebene Umgebungsmodell weiterhin alle der nachfolgenden Effekte A bis C:

Effekt A: Ermöglichen einer Ausdrucksweise unter Berücksichtigung des Fehlers.
Effekt B: Die erforderliche Speichergröße und der erforderliche Rechenaufwand sind geringer als ein spezifischer Betrag.
Effekt C: Es wird nicht nur zum Ausdruck gebracht, dass ein Objekt existiert, sondern dass ein Objekt nicht existiert.

In 6 wird nach dem Zuordnungsschritt S5 dann im Schritt S6 eine Überprüfung dahingehend durchgeführt, ob die identischen dreidimensionalen Gestaltdaten für einen früheren Messpunkt vorliegen, oder nicht. Wenn in dieser Prüfung die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, wird eine neue Messposition als die Eigenposition identifiziert.
Vorzugsweise wird an der Anfangsposition einer beweglichen Einheit, welche sich in sequenzieller Weise bewegt, diese Identifikation an einer Position in sechs Freiheitsgraden (zum Beispiel dem Ursprung in einem Global-Koordinatensystem) durchgeführt, welche bereits bekannt ist. Außerdem kann in dieser Identifikation eine Position mit sechs Freiheitsgraden, einschließlich der Position (drei Freiheitsgrade) und der Orientierung, bzw. Ausrichtung (drei Freiheitsgrade) der neuen Messposition, identifiziert werden.
Wenn, in der Prüfung im Schritt S6, die dreidimensionalen Gestaltdaten für eine frühere Messposition vorliegen, dann werden, in 6, außerdem der Grobzuordnungsschritt S8 und der Feinzuordnungsschritt S9 nach dem Zuordnungsschritt S5 durchgeführt.
10 ist ein Datenverarbeitungs-Flussdiagramm für den Grobzuordnungsschritt S8 und den Feinzuordnungsschritt S9; 11 ist ein schematisches Diagramm des Grobzuordnungsschritts S8; und 12 ist ein schematisches Diagramm des Feinzuordnungsschritts S9.
In 10 kann im Grobzuordnungsschritt S8, wie er in 11 gezeigt ist, die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition zum Minimieren eines Prüfwerts (wie der Summierung der Distanzen) bezüglich der Distanzen von gemessenen Daten- und Fehlerverteilungen zu benachbarten Voxeln mit repräsentativen Punkten, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu dem Umgebungsmodell für die frühere Messposition zum Minimieren eines Prüfwerts (wie der Summierung der Distanzen) bezüglich der Distanzen zwischen Voxeln mit repräsentativen Punkten,
oder durch Ausführen einer Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition zum Maximieren eines Prüfwerts (wie der Summierung der Wahrscheinlichkeitswerte) bezüglich der Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten- und Fehlerverteilungen benachbart sind, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition zum Minimieren eines Prüfwerts (wie der Summierung der Differenzen in Wahrscheinlichkeitswerten) bezüglich der Differenzen in Wahrscheinlichkeitswerten von benachbarten Voxeln, durchgeführt werden.
Die Positionszuordnung im Grobzuordnungsschritt S8 wird durch Ausdrücken eines Umgebungsmodells und gemessener Daten in einem Voxel-Raum oder durch Ausdrücken eines Umgebungsmodells auf einem Voxel-Raum und Ausdrücken repräsentativer Punkte und Fehlerverteilungen als gemessene Daten erzielt. Da die derzeitigen gemessenen Daten an einer Position (x, y, z) und einer Lage (θ, φ, ψ) gemessen werden, werden die gemessenen Daten in Weltkoordinaten umgewandelt, und der Koinzidenzgrad zwischen den gemessenen Daten und dem Umgebungsmodell wird berechnet.
Das Minimal-Distanzverfahren kann zum Beispiel verwendet werden, um den Koinzidenzgrad zu berechnen. Die Distanz zwischen Voxeln, wenn das Minimal-Distanzverfahren angewandt wird, kann durch die Gleichungsformel (3) von Gleichung 4 definiert werden, wobei zwei Voxel-Räume als x⁽¹⁾ und x⁽²⁾ definiert sind, die Gesamtanzahl an Voxeln als I definiert ist und der Voxel-Wert als x_i ⁽ⁿ⁾ definiert ist.
Die optimale Position und Orientierung (Lage) für die gemessenen Daten kann durch die Methode der kleinsten Quadrate durch Minimieren von ε durch Variieren der Position (x, y, z) und der Orientierung (θ, φ, ψ) berechnet werden.
Weiterhin kann der Koinzidenzgrad einen Prüfwert (zum Beispiel die Summierung von Differenzen von Wahrscheinlichkeitswerten) bezüglich der Differenz von Wahrscheinlichkeitswerten von zwei benachbarten Voxeln in zum Beispiel dem Umgebungsmodell und gemessenen Daten für zwei Voxel verwenden. In diesem Fall wird die optimale Position und Orientierung (Lage) der gemessenen Daten variiert, um den Koinzidenzgrad zu minimieren.
Wenn das Umgebungsmodell im Voxel-Raum ist, und die gemessenen Daten repräsentative Werte und Fehlerverteilungen ausdrücken, kann ferner ein Prüfwert (zum Beispiel die Summierung der Wahrscheinlichkeitswerte) bezüglich der Wahrscheinlichkeitswerte von Voxeln in einem Umgebungsmodell, worin die repräsentativen Werte und Fehlerverteilungen der gemessenen Daten benachbart sind, als Koinzidenzgrad verwendet werden. In diesem Fall wird die optimale Position und Orientierung der gemessenen Daten variiert, um den Koinzidenzgrad zu maximieren. (Gleichung 4)
In 10 wird im Feinzuordnungsschritt S9, wie er in 12 gezeigt ist, eine Positionszuordnung so durchgeführt, dass ein Prüfwert (zum Beispiel die Summierung der Distanzen) bezüglich der Distanz zwischen benachbarten Fehlerverteilungen minimiert wird, durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition.
Bei der Positionszuordnung von Umgebungsmodellen und gemessenen Daten im Feinzuordnungsschritt S9 wird ein ICP-Algorithmus zur Positionszuordnung zwischen einer Punktegruppe und einer Punktegruppe angewandt, wobei die Fehlerverteilung berücksichtigt wird. Bei der Positionszuordnung werden die durch Grob- Positionszuordnung erhaltene Position/Orientierung als die Anfangswerte verwendet.
Bei der Berechnung der Distanz zwischen Fehlerverteilungen unter Anwendung des ICP-Algorithmus werden Fehlerverteilungen, welche einander schneiden, als Einzelmesspunkt angesehen, und die Distanzwerte in diesem Fall werden mit einer Gewichtung multipliziert, die aus den Koinzidenzgraden der Verteilungen errechnet wird, um die Berechnungen durchzuführen. Bei der Koinzidenz der Verteilung kann eine Distanzskala, wie zum Beispiel die generalisierte Mahalanobis-Distanz, verwendet werden.
Die Distanz(en) der Umgebungsmodelle und der gemessenen Daten ist in diesem Falle in der Formel (4) von Gleichung 5 definiert, worin die Umgebungsmodell-Daten als p_Mi definiert sind, die Fehlerabweichung der Umgebungsmodell-Daten als Σ_Mi definiert ist, die gemessenen Daten als P_Di definiert sind, die Fehlerverteilung der Messdaten als Σ_Di definiert ist, die zusammengesetzte Funktion für die Fehlerverteilung als w definiert ist, und die Anzahl an Umgebungsmodell-Daten, welche den gemessenen Daten entspricht, als N definiert ist. Hierbei gibt T eine Transposition an.
Die optimale Position und Orientierung für die gemessenen Daten kann durch die Methode der kleinsten Quadrate durch Minimieren von ε durch Variieren der Position (x, y, z) und Orientierung (θ, φ, ψ), welche für die gemessenen Daten gemessen werden, und Bewegen von P_Di berechnet werden.
Zusätzlich dazu wird zusammen mit dem Identifizieren der Eigenposition/Orientierung eine Fehlerverteilung für die Eigenposition identifiziert, und die Eigenposition und die Fehlerverteilung werden durch Kalman-Filterung aus der derzeitigen Eigenposition und Fehlerverteilung und der Eigenposition und Fehlerverteilung, welche identifiziert worden sind, korrigiert. (Gleichung 5)
Darüber hinaus wird, in 6, der Modell-Auffrischungsschritt S11 nach dem Eigenpositions-Identifizierungsschritt S10 durchgeführt, um das Umgebungsmodell, welches im Modell-Strukturierungsschritt S4 erstellt wurde, aufzufrischen.
13 ist ein Datenverarbeitungs-Flussdiagramm des Modell-Auffrischungsschritts S11. Wie in dieser Figur gezeigt, wird im Schritt ST1 ein Voxel, das einem Koordinatenwert eines neu eingegebenen Messpunkts entspricht, abgerufen, und wenn es im Schritt ST2 keinen repräsentativen Punkt innerhalb des betreffenden Voxels gibt (d. h. das Voxel leer ist), dann werden im Schritt ST3 der Koordinatenwert und die Fehlerverteilung des neu eingegebenen Messpunkts als der Koordinatenwert und die Fehlerverteilung des repräsentativen Punkts festgelegt (neue Eintragung).
Im Prinzip sollte im Schritt ST3 außerdem kein Objekt zwischen der neuen Messposition (dem Ursprung) und dem gemessenen Punkt (dem zu messenden Punkt) existieren. Folglich werden der repräsentative Punkt und die Fehlerverteilung innerhalb des Voxels, das zwischen der neuen Messposition (Ursprung) und dem gemessenen Punkt positioniert ist, nachjustiert oder eliminiert.
14 ist ein schematische Diagramm des Falls, worin ein repräsentativer Punkt bereits im betreffenden Voxel festgelegt worden ist.
Im Schritt ST2 von 13 werden, wenn es einen bereits innerhalb des betreffenden Voxels festgelegten, repräsentativen Punkt gibt, im Schritt ST4 dann die neu erhaltene Fehlerverteilung und die bereits innerhalb des Voxels festgelegte Fehlerverteilung verglichen (mit anderen Worten wird eine Entscheidung getroffen, ob es sich bei ihnen um unterschiedliche Punkte oder identische Punkte handelt).
Wenn sich in diesem Vergleich die Fehlerverteilungen gegenseitig überlappen ((A) von 14), dann werden im Schritt ST5 eine neue Fehlerverteilung und ein neuer repräsentativer Punkt aus den beiden Fehlerverteilungen, oder aus beiden Fehlerverteilungen, dem repräsentativen Punkt, der bereits innerhalb des Voxels festgelegt ist, und dem Koordinatenwert des neu eingegebenen Messpunkts nachjustiert, bzw. neu eingestellt (d. h. die Fehlerverteilungen werden kombiniert).
Weiterhin wird, wenn sich, in diesem Vergleich, die Fehlerverteilungen nicht gegenseitig überlappen ((B) von 14), in den Schritten ST6 und ST7 das betreffende Voxel dann hierarchisch gemäß einem "Octree" weiter in eine Vielzahl von Voxeln unterteilt, welche dann neu eingetragen werden, sodass es nur einen einzigen repräsentativen Punkt in einem einzelnen Voxel gibt.
Die Bestimmungsgröße für das Unterteilen und Kombinieren wird zum Beispiel basierend auf dem Koinzidenzgrad der Fehlerverteilungen ermittelt. Im Koinzidenzgrad der Fehlerverteilung kann eine Distanzskala, wie zum Beispiel die generalisierte Mahalanobis-Distanz, Verwendung finden. Darüber hinaus kann durch statistisches Testen, basierend auf den zwei Fehlerverteilungen, bestimmt werden, ob die zwei den identischen Punkt ausdrücken oder nicht.
Im Schritt ST5 kehrt, wenn das Ergebnis des Festlegens einer neuen Fehlerverteilung und eines neuen Fehlerverteilungs-Zentrums aus beiden Fehlerverteilungen darin besteht, dass der neue repräsentative Punkt sich in ein neues Voxel bewegt hat (d. h., JA im Schritt ST8), die Verarbeitung zum Schritt ST2 zurück, und der oben genannte Prozess wird wiederholt.
Es ist anzumerken, dass 15 den Fall veranschaulicht, worin der neue repräsentative Punkt sich in das Innere eines anderen Voxel bewegt hat, und zwar als Ergebnis des Neueinstellens einer neuen Fehlerverteilung und eines neuen Fehlerverteilungs-Mittelpunkts aus beiden der Fehlerverteilungen im Schritt ST5, oder aus beiden der Fehlerverteilungen und dem repräsentativen Punkt, der bereits innerhalb des Voxels festgelegt war, und den Koordinatenwerten des neu eingegebenen Messpunkts.
Wenn ein Wahrscheinlichkeitswert, der die Wahrscheinlichkeit des Vorliegens eines Objekts darstellt, in einem Voxel festgelegt wird, dann werden im Modell-Auffrischungsschritt S11 ein repräsentativer Punkt und eine Fehlerverteilung innerhalb des Voxels neu eingetragen oder neu eingestellt oder gelöscht, oder der Wahrscheinlichkeitswert innerhalb des Voxels wird, abhängig von dem Verfahren zur Neu-Eintragung nach dem Unterteilen, durch ein statistisches Verfahren neu eingetragen oder nachjustiert oder gelöscht, oder nach dem Unterteilen neu eingetragen.
16 ist ein anderes schematisches Diagramm des Falls ((A) von 14), worin sich Fehlerverteilungen gegenseitig überlappen. Im Schritt ST5 kann ein Kalman-Filter als Einrichtung zum Festlegen des neuen repräsentativen Punkts und der Fehlerverteilung durch Kombinieren zweier repräsentativer Punkte und Fehlerverteilungen verwendet werden. Im zweidimensionalen Fall, wie in dieser Figur gezeigt, ist zum Beispiel, wenn die zwei repräsentativen Punkte jeweils x(1) und x'(2) sind, und die zwei Fehlerverteilungen Σ(1) und Σ'(2) sind, und der kombinierte repräsentative Punkt x(2) ist und die Fehlerverteilung Σ(2) ist, ein schematisches Diagramm einer Berechnung des repräsentativen Punkts x(2) und der Fehlerverteilung Σ(2) wie in 16 beschaffen.
In 6 wird, im Ausgabeschritt S12, neben der Eigenposition, wenigstens eines von der Voxelposition, dem repräsentativen Punkt und der Fehlerverteilung, basierend auf der Eigenposition, an die Ausgabevorrichtung ausgegeben. Für den Fall, bei welchem die Ausgabevorrichtung 36 eine Anzeigevorrichtung (zum Beispiel eine Kathodenstrahlröhre, bzw. ein Bildschirm) ist, wird die Eigenposition vorzugsweise in einem dreidimensionalen Bild auf einer dreidimensionalen Anzeige dargestellt. Darüber hinaus können die Eigenpositions-Daten zu einer anderen Vorrichtung (zum Beispiel einem Steuergerät/Computer) übertragen oder an einen Drucker ausgegeben werden.
Weiterhin wird, im Ausgabeschritt S12, nicht nur die Position des repräsentativen Punkts des Voxels an die Ausgabevorrichtung 36 als der dreidimensionale Gestalt-Messwert ausgegeben, sondern auch ein Index (wie ein numerischer Wert), der die Zuverlässigkeit oder Genauigkeit des Messwerts anzeigt, wird, basierend auf der Größenordnung oder Größe der Fehlerverteilung innerhalb des Voxels, an die Ausgabevorrichtung 36 ausgegeben. Außerdem ist, beim Ausgeben der Position des repräsentativen Punkts des Voxels an die Ausgabevorrichtung 36 als dreidimensionaler Gestalt-Messwert, in diesem Ausgabeschritt S12 dann, wenn die Größenordnung (Breite) der Fehlerverteilung innerhalb des Voxels größer als ein spezifischer Referenzwert ist, die Zuverlässigkeit oder Genauigkeit des Messwerts geringer als eine spezifische Referenz, und der Messwert für das Voxel (d. h. die Position des repräsentativen Punkts für das Voxel) wird nicht an die Ausgabevorrichtung 36 ausgegeben.
Fernerhin kann es nach dem Zuordnungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells geben; wobei der Modell-Auffrischungsschritt ein Voxel abruft, das dem Koordinatenwert des neu eingegebenen gemessenen Punkts entspricht; und wenn der repräsentative Punkt und/oder die Fehlerverteilung innerhalb dieses Voxels neu festgelegt oder nachjustiert wird, oder dieses Voxel ferner hierarchisch in eine Vielzahl von Voxeln unterteilt wird, dann wird im Ausgabeschritt die Position des repräsentativen Punkts dieses Voxels an die Ausgabevorrichtung als ein dreidimensionaler Gestalt-Messwert ausgegeben.
Darüber hinaus wird, wenn die Position und Orientierung des Entfernungssensors erhalten werden, im Ausgabeschritt S6 die Position eines repräsentativen Punkts für ein Voxel im Umgebungsmodell in einem Bereich, für welchen die Position erfasst werden kann, an die Ausgabevorrichtung als ein dreidimensionaler Gestalt-Messwert ausgegeben. Der Bereich, welcher von der Position des Entfernungssensors aus erfasst werden kann, ist der Bereich, über welchen der Entfernungssensor die Position messen kann, und kann einen Winkelbereich einschließen, über welchen der Entfernungssensor von der Position des Entfernungssensors aus eine Position messen kann (das Blickfeld), und kann einen Distanzbereich einschließen, über welchen der Entfernungssensor von der Position des Entfernungssensors aus eine Position messen kann.
Die in 6 veranschaulichte Abfolge von Prozessen wird an einer neuen Messposition jedes Mal wiederholt, wenn neue gemessene Daten erhalten werden können, und die Ergebnisse werden in der internen Speichervorrichtung 34 und/oder externen Speichervorrichtung 33 abgespeichert. Um die Geschwindigkeit der Verarbeitung zu erhöhen, sollten die Ergebnisse in der internen Speichervorrichtung abgespeichert werden, sofern die Kapazität dies erlaubt.
Das Verfahren und die Vorrichtung, wie in der vorliegenden Erfindung, wie oben beschrieben, dargestellt, unterteilen eine räumliche Region, in der eine dreidimensionale Gestalt vorliegt, in eine Vielzahl von Voxeln 6 und speichern die Position jedes Voxels in einer internen Speichervorrichtung 34 und/oder einer externen Speichervorrichtung 33, sodass, wenn das zu messende Objekt groß ist, die Datengröße im Verhältnis zur Anzahl an Voxeln klein gehalten wird.
Zusätzlich dazu werden ein repräsentativer Punkt 7 und dessen Fehlerverteilung 8 innerhalb eines Voxels 6, das einem Koordinatenwert entspricht, festgelegt und aufgezeichnet, was die Anzeige von Daten bei mehr als der Voxel-Auflösung ermöglicht.
Folglich kann die Datenstruktur, wie in der vorliegenden Erfindung dargestellt, gemessene Daten von einer Vielzahl von Beobachtungspunkten in eine fixierte Größe hinein integrieren.
Darüber hinaus ermöglicht das Festlegen und Abspeichern eines Wahrscheinlichkeitswerts, der die Wahrscheinlichkeit des Vorliegens eines Objekts innerhalb des Voxels angibt, eine leichte Bestimmung hinsichtlich des Wahrscheinlichkeitswerts für das Voxel, ohne die Notwendigkeit, ein Voxel finden zu müssen, das einen repräsentativen Punkt aufweist, der repräsentiert, ob ein Objekt in dem Voxel existiert, oder nicht, und ohne die Notwendigkeit einer Neuberechnung aus der Fehlerverteilung, selbst wenn die Fehlerverteilung breiter als das Voxel mit dem repräsentativen Punkt ist, wodurch es ermöglicht wird, die Suchzeit zu beschränken.
Darüber hinaus wird durch Erhalten der derzeitigen Messposition durch Rückschluss aus einer Änderung in der Messposition der Vergangenheit, oder aus einem Sensor, der in der Lage zum Erfassen der derzeitigen Messposition ist, oder durch die Verwendung eines Reflexionsstärkewerts zusätzlich zum Distanzwert der gemessenen Daten, der Bereich der Prüfung limitiert, wodurch es ermöglicht wird, dass die Suchzeit beschränkt wird.
Zusätzlich dazu kann, in einem Grobzuordnungsschritt S8, die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen gemessenen Daten und benachbarten Voxeln, und zwischen Fehlerverteilungen und benachbarten Voxeln, wobei die Voxel repräsentative Punkte aufweisen, zu minimieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen Voxeln mit repräsentativen Punkten zu minimieren, oder Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Wahrscheinlichkeitswerte) bezüglich der Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten- und Fehlerverteilungen benachbart sind, zu maximieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert (wie die Summierung der Differenzen in den Wahrscheinlichkeitswerten) bezüglich der Differenzen in Wahrscheinlichkeitswerten von benachbarten Voxeln zu minimieren, durchgeführt werden, wodurch es möglich gemacht wird, die Positionszuordnung von Voxeln mit repräsentativen Punkten zueinander durchzuführen, und zwar in einer kurzen Zeitdauer, während ein kumulativer Fehler verhindert wird.
Außerdem ist, beim Rotieren und Translatieren des Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für die frühere Messposition, zur Ausführung der Positionszuordnung, sodass ein Prüfwert (wie die Summierung von Differenzen von Wahrscheinlichkeitswerten) bezüglich der Differenz in Wahrscheinlichkeitswerten von benachbarten Voxeln minimiert wird, die Genauigkeit deswegen verbessert, weil der Fall, bei welchem kein Objekt existiert, ebenfalls berücksichtigt wird.
Im Anschluss hieran wird in einem Feinzuordnungsschritt S7 die Positionszuordnung so durchgeführt, dass ein Prüfwert (wie die Summierung der Distanzen) bezüglich der Distanzen zwischen benachbarten Fehlerverteilungen, durch Rotieren und Translatieren einer neuen gemessen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird, sodass die Fein-Positionszuordnung von Fehlerverteilungen zueinander in einer kurzen Zeitdauer ausgeführt werden kann.
Folglich kann die vorliegende Erfindung Hochpräzisions-Gestalten erhalten, während ein kumulativer Fehler verhindert wird, und zwar durch den Prozess des Integrierens von Daten aus einer Vielzahl von Beobachtungspunkten.
Ferner kann, weil die Datenstruktur der vorliegenden Erfindung eine Struktur ist, bei welcher im Voxel die Struktur vergrößert, bzw. erweitert wird, die Datengröße kleiner sein, wenn mit der Punktegruppe verglichen wird. Bei der Durchführung der Zuordnung der umliegenden Umgebung in den gemessenen Daten (dem ICP-Algorithmus), ist folglich die Datengröße, welche der Suche unterzogen wird, reduziert, was eine verbesserte Effizienz in den Berechnungen ermöglicht.
Während der herkömmliche ICP-Algorithmus fehlerhafte Ergebnisse für spärliche Daten ausgibt, besitzt das Umgebungsmodell der vorliegenden Erfindung außerdem repräsentative Punkte und Fehlerverteilungen innerhalb von Voxeln, und somit wird eine Positionszuordnung für spärliche Daten möglich.
Darüber hinaus wird, im Modell-Strukturierungsschritt S4, das größte Voxel 9 auf eine Größe, entsprechend der minimalen erforderlichen Auflösung, eingestellt, und wenn eine Vielzahl gemessener Punkte innerhalb eines einzelnen Voxels existiert, wird das Voxel hierarchisch gemäß einem "Octree" weiter in eine Vielzahl von Voxeln unterteilt, sodass innerhalb eines einzelnen Voxels nur ein einziger gemessener Punkt existiert, was es möglich macht, gleichzeitig die Datengröße zu beschränken, und die Auflösung weiter zu erhöhen, und zwar durch die Verwendung der Voxel und repräsentativen Punkte nach der Unterteilung.
Insbesondere können durch Erhalten einer Vielzahl von Koordinatenwerten auf einer dreidimensionalen Gestalt als Entfernungsdaten mit einer Vielzahl von Messpositionen als dem Ursprung, und Verwenden der Koordinatenwerte der Entfernungsdaten als Koordinatenwerte der repräsentativen Punkte und Verwenden des Messfehlers der Koordinatenwerte der Entfernungsdaten als Fehlerverteilung der repräsentativen Punkte, präzise Koordinatenwerte und Fehlerverteilungen eingesetzt werden, um eine Vielzahl von Messungen statistisch zu kombinieren, was eine weiter erhöhte Genauigkeit ermöglicht.
17 veranschaulicht den Fall, worin die Genauigkeit des repräsentativen Punkts durch Reduzieren der Fehlerverteilung des repräsentativen Punkts erhöht wird, und zwar durch Integrieren von Entfernungsdaten, welche eine Vielzahl von Messpositionen als Ursprünge haben. Auf diese Weise besitzen die Entfernungsdaten, welche unter Verwenden verschiedener Messpositionen (mit anderen Worten, Positionen der Dreidimensional-Messvorrichtung, d. h. dem Entfernungssensor) erhalten werden, unterschiedliche Orientierungen in der Fehlerverteilung, sodass diese Entfernungsdaten sequenziell durch das Umgebungsmodell integriert werden, um die Fehlerverteilung des repräsentativen Punkts zu komprimieren, wodurch die Genauigkeit der Position des repräsentativen Punkts verbessert wird. Es ist anzumerken, dass in 17 die Zeichnung nach der dreidimensionalen Messung eine schematische Zeichnung ist, und einen zweidimensionalen Querschnitt einer Kaffeetasse zeigt, wobei die gepunktete Linie in der Zeichnung nach der dreidimensionalen Messung die tatsächliche Oberfläche der Kaffeetasse zeigt.
Weiterhin wird, zusammen der Identifizierung der Eigenposition, eine Fehlerverteilung für die Eigenposition identifiziert, und die Eigenposition und die Fehlerverteilung werden durch Kalman-Filterung aus der derzeitigen Eigenposition und Fehlerverteilung sowie der Eigenposition und Fehlerverteilung, welche identifiziert worden sind, korrigiert, was eine erhöhte Genauigkeit ermöglicht.
Unter der Annahme, dass kein Objekt zwischen dem Ursprung und dem gemessenen Punkt existiert, ermöglicht außerdem das Neu-Einstellen oder Eliminieren von repräsentativen Punkten und Fehlerverteilungen innerhalb der Voxel, welche dazwischen positioniert sind, die Eliminierung von fehlerhaften gemessenen Daten.
Darüber hinaus wird durch das Abrufen des Voxels, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht, und das Festlegen des Koordinatenwerts und der Fehlerverteilung als dem Koordinatenwert und der Fehlerverteilung des repräsentativen Punkts, wenn es keinen repräsentativen Punkt in dem Voxel gibt, ermöglicht, dass Koordinatenwert und Fehlerverteilung des repräsentativen Punkts leicht festgelegt werden können.
Falls es einen repräsentativen Punkt gibt, der bereits innerhalb des Voxels festgelegt worden ist, werden außerdem eine neu erhaltene Fehlerverteilung und die Fehlerverteilung, die bereits innerhalb des Voxels festgelegt ist, verglichen; und wenn die Fehlerverteilungen gegenseitig überlappen, werden eine neue Fehlerverteilung und ein neuer repräsentativer Punkt aus den beiden Fehlerverteilungen, oder aus den beiden Fehlerverteilungen und dem repräsentativen Punkt, der bereits innerhalb des Voxels festgelegt wurde, und den Koordinatenwerten des neu eingegebenen, gemessenen Punkts festgelegt; und wenn sich die Fehlerverteilungen nicht gegenseitig überlappen, dann wird das Voxel hierarchisch gemäß einem Octree weiter in eine Vielzahl von Voxeln unterteilt, sodass innerhalb eines einzelnen Voxels nur ein einziger repräsentativer Punkt existiert, wodurch es möglich gemacht wird, eine Konvergenz zu einer Hochgenauigkeits-Gestalt zu verursachen, während eine Fehlerakkumulation vermieden wird.
Angesichts des Verfahrens und der Vorrichtung, wie in der vorliegenden Erfindung dargestellt, gibt es folglich nicht nur eine Funktion zum Korrigieren, um Daten zu berichtigen, Daten, welche Fehler beinhalten, sondern auch zum Konvergieren zu einer hochgenauen Gestalt in Messungen über längere Zeitperioden hinweg. Ferner ist im Positionsidentifizierungsverfahren der Aufwand an Berechnungen zum Auffrischen der repräsentativen Punkte (entsprechend den jeweiligen Voxeln) und der Fehlerverteilungen für die Voxel durch die gemessenen Punkte gering, und die Berechnung wird innerhalb eines Voxels abgeschlossen, wobei sie keinen Einfluss auf die umliegenden Voxel hat. Dies ermöglicht eine Hochgeschwindigkeitsverarbeitung. Weiterhin können die gemessenen Daten sequenziell in die Voxel integriert werden, und die Größe des Speichers für die Daten der umliegenden Umgebung, welche erhalten werden, überschreitet nicht eine spezifische Größe.
Nun wird der Modell-Auffrischungsschritt unter Anwendung des Kalman-Filters ausführlich beschrieben.
Im Modell-Auffrischungsschritt unter Anwendung des Kalman-Filters werden ein neuer repräsentativer Punkt und eine Fehlerverteilung durch einen Kalman-Filter aus dem Koordinatenwert des neu eingegebenen gemessenen Punkts und dessen Fehlerverteilung, sowie dem repräsentativem Punkt und dessen Fehlerverteilung, welche bereits im Voxel festgelegt worden sind, erhalten und neu eingestellt.
Unter Behandlung jeder Position m(i) der behandelten Modell-Punktegruppe als eine Status-Größe, kann das Modell durch die folgende Gleichung 6, basierend auf der Position der Messpunkte des Entfernungssensors, ausgedrückt werden. Es ist anzumerken, dass in der vorliegenden Ausführungsform m(i) der repräsentative Punkt in dem Voxel ist (auch in der untenstehenden Beschreibung).
(Gleichung 6)

L(j) = hm(Rr,tr,m(i)) + vL(j) = Rs -1(Rr -1(m(i) – tr) – ts) + vL(j) (J = 1, ..., N)

In Gleichung 6 gilt:
L(j) ist die vom Entfernungssensor gemessene Position. Zum Beispiel handelt es sich bei L(j) um L(j) = (x_L(j), y_L(j) z_L(j))^t für die gemessenen Punkte j (j = 1, ..., N) eines dreidimensionalen LRF (Laserentfernungsmesser) im Sensorkoordinatensystem des Entfernungssensors. Hierbei bezeichnet t die transponierte Matrix (ebenfalls in der untenstehenden Beschreibung).
h_m(R_r, t_r, m(i)) ist das Beobachtungssystem-Modell in Hinsicht auf L(j).
R_r ist die Rotationsmatrix R_r = (θ_x, θ_y, θ_z), welche relativ zum Weltkoordinatensystem die Lage der beweglichen Einheit (zum Beispiel eines sich bewegenden Roboters) angibt, auf welcher der Entfernungssensor montiert ist. Es ist anzumerken, dass θ_x, θ_y, θ_z die Rotationsgrade um die x-Achse, y-Achse, bzw. die z-Achse angeben (auch in der untenstehenden Beschreibung).
t_r ist der Translationsvektor t_r = (x, y, z)), welcher die Position der beweglichen Einheit (zum Beispiel eines sich bewegenden Roboters), auf welcher der Entfernungssensor montiert ist, relativ zum Weltkoordinatensystem angibt.
v_L(i) ist das Beobachtungsrauschen, welches zu dem gemessenen Wert L(j) des Entfernungssensors addiert wird.
R_s ist die Rotationsmatrix R_s = R (θ_x, θ_y, θ_z) des Sensorkoordinatensystems relativ zum Koordinatensystem der beweglichen Einheit.
t_s ist der Translationsvektor t_s = (x, y, z), welcher die Position des Sensorkoordinatensystems relativ zum Koordinatensystem der beweglichen Einheit ausdrückt.
Das zu messende Objekt ist stationär, falls die Position und Lage des zu messenden Objekts relativ zum Umgebungsmodell stationär sind.
Die vom Entfernungssensor gemessene Gruppe von Punkten wird mit den Punkten i (d. h., den repräsentativen Punkten) der Punktegruppe des Umgebungsmodells korreliert. Die Punkte i der Modell-Punktegruppe, für welche diese Korrelation durchgeführt wird, werden durch die folgende Gleichung aufgefrischt. Es ist anzumerken, dass nur die repräsentativen Punkte m(i) der Modell-Punktegruppe, welche mit der vom Entfernungssensor gemessenen Punktegruppe korreliert wird, durch die folgende Gleichung 7 aufgefrischt werden kann.
(Gleichung 7)

Kmk(i) = Σmk,k-1(i)Hmk(j)t(Hmk(j)Σmk,k-1(i)Hmk(j)t + ΣLk(j))-1 m'k(i) = mk,k-1(i) + Kmk(i)(Lk(j) – hmk(Rrk,trk,mk,k-1(i))) Σ'mk(i) = Σmk,k-1(i) – Kmk(i)Hmk(j)Σmk,k-1(i)

In der Gleichung 7 gilt:
Das Suffix k weist auf den Wert an der "diskreten Zeit" k hin.
In Bezug auf m_k(i), gibt m'_k(i) den aufgefrischten Wert (den posteriori-Inferenzwert) von m_k(i) an, wobei m_k,k-1(i) den vorhergesagten Wert (den a-priori-Inferenzwert) von m_k(i) basierend auf m_k-1(i) angibt. Es ist anzumerken, dass die Umgebung (das zu messende Objekt) stationär ist, sodass m_k,k-1(i) = m'_k-1(i).
Σ_mk(i) ist die Fehlerkovarianzmatrix (d. h. die oben beschriebene Fehlerverteilung) des repräsentativen Punkts m_k(i) innerhalb eines Voxels. In Hinsicht auf Σ_mk(i), gibt Σ'_mk(i) außerdem den aufgefrischten Wert (den posteriori-Inferenzwert) von Σ_mk(i) an, wobei Σm_k,k-1(i) den vorhergesagten Wert (den a-priori-Inferenzwert) von Σ_mk(i), basierend auf Σ'_mk-1(i), angibt. In dem Sensorkoordinatensystem wird die Position des vom dreidimensionalen LRF gemessenen Punkts j (j = 1, ..., N) durch L(j) ausgedrückt, wobei die Fehlerkovarianzmatrix durch Σ_L(j) ausgedrückt wird. Hierbei ist das N die Gesamtanzahl gemessener Punkte, welche durch den dreidimensionalen LRF erhalten wird. Für das Fehlermodell für den dreidimensionalen LRF wird eine konstante Normalverteilung, welche unabhängig von der Messdistanz ist, angenommen. Wenn der Laser in der Richtung der x-Achse im Sensorkoordinatensystem emittiert wird, wird die Fehlerkovarianzmatrix Σ_s sein. Die Lage der Fehlerverteilung wird variieren, abhängig von der Richtung, in welcher der Laser emittiert wird. Σ_L(j) gibt den Σ_L(j) = R_L(j) Σ_sR_L ^t(j) an, unter Verwendung der Rotationsmatrix R_L(j) für die Richtung, in welcher der Laser, relativ zur Standardrichtung, emittiert wird. Die Position z(j) und die Fehlerkovarianzmatrix Σ_z(j) im Weltkoordinatensystem des gemessenen Punkts j können jeweilig ausgedrückt werden durch z(j) = Rr(RsL(j) + ts) + tr und Σz(j) = RrRsΣL(j)Rs tRr t.
K_mk(i) ist der Kalman-Zuwachs für m_k(i).
h_mk(R_rk, t_rk, m_k,k-1(i)) ist das Beobachtungssystemmodell in Hinsicht auf L_k(j), i = p_k(j). i = p_k(j) ist ein Punkt auf der Umgebungskarte (oder mit anderen Worten, dem Umgebungsmodell), der dem gemessenen Punkt j entspricht.
H_mk ist die Jacobi-Matrix für das Beobachtungssystemmodell für L_k(j) und i = p_k(j), und wird durch die folgende Gleichung 8 ausgedrückt. (Gleichung 8)
Das Auffrischungsverfahren unter Anwendung des Kalman-Filters aktualisiert das Umgebungsmodell unter Anwendung der folgenden Prozeduren bei der Stufe, an welcher der aufgefrischte Wert m'_k(i) von jedem Punkt (die repräsentativen Punkte für die Voxel) der Modellpunktegruppe und der aufgefrischte Wert Σ'_mk(i) der Fehlerkovarianzmatrix für die Umgebungskarte erhalten werden:

(1) Diese aufgefrischten Werte m'_k(i), Σ'_mk(i) werden als ein neuer repräsentativer Punkt und Fehlerverteilung neu eingestellt.
(2) Wenn das Ergebnis von (1) darin besteht, dass sich die Position des repräsentativen Punkts in ein anderes Voxel bewegt hat, und wenn das Voxel, in welches der repräsentative Punkt sich bewegt hat, nicht bereits einen repräsentativen Punkt besitzt, dann werden der repräsentative Punkt und die Fehlerkovarianzmatrix nach dem Bewegen im Ziel-Voxel behalten, und der repräsentative Punkt und so weiter werden aus dem Herkunfts-Voxel gelöscht. Wenn das Voxel, welches das Bewegungs-Ziel ist, bereits einen der repräsentativen Punkt aufweist, dann wird eine Entscheidung getroffen, ob die Fehlerverteilungen der zwei repräsentativen Punkte miteinander überlappen, oder nicht (dieselbe, wie die Entscheidung im oben genannten ST4). Danach ist das Verfahren das Gleiche wie die Verfahren in und nach ST4, in 13.
(3) Wenn an einem Punkt angelangt wird, der durch den Entfernungssensor gemessen werden soll, für welchen keine Korrelation mit einem repräsentativen Punkt m(i) einer Modellpunktegruppe definiert worden ist, werden, wenn das Voxel, das diesen gemessenen Punkt beinhaltet, keinen repräsentativen Wert aufweist, dieser repräsentative Punkt und die Fehlerverteilung hinzugefügt und als der repräsentative Punkt und die Fehlerverteilung für dieses Voxel beibehalten. Wenn bereits ein repräsentativer Punkt innerhalb des Voxels existiert, wird das Voxel so unterteilt, dass die existierenden repräsentativen Punkte und jeder der gemessenen Punkte, einschließlich einer anderen Vielzahl von Punkten, für welche keine Korrelation (oder Übereinstimmung) erstellt worden ist, sämtlich in verschiedenen Voxeln enthalten sind, sodass die Voxel die repräsentativen Punkte und dergleichen erben.

Das Wiederholen des Modell-Auffrischungsschritts unter Verwendung des Kaiman-Filters, wie oben beschrieben, verringert nicht nur schrittweise den Umfang der Fehlerkovarianzmatrix (d. h. die Fehlerverteilung), sondern macht es auch einfacher, die Voxel zu unterteilen. Die Unterteilung der Voxel ermöglicht den Ausdruck von Änderungen, welche von geringerer Größe als die anfänglichen Voxel sind.
18 veranschaulicht das Ergebnis, erhalten durch den Modell-Auffrischungsschritt unter Anwendung des Kalman-Filters. 19 ist eine teilweise vergrößerte Ansicht von 18. In diesen Figuren kann, wenn die Länge einer Seite eines anfänglichen Voxels 100 cm beträgt, die Unterteilung bis zu 6 Mal durchgeführt werden. In dem Areal, in welchem das Objekt existiert, wird das Objekt, als Resultat(e) der wiederholten Neu-Unterteilung von Voxeln, mit hervorragender Genauigkeit angezeigt. In Regionen, in welchen kein Objekt vorliegt, wird die Neu-Unterteilung von Voxeln nicht durchgeführt, was das Ausdrücken der Umgebung mit der erforderlichen und angemessenen Datengröße ermöglicht. Darüber hinaus sind Fehlerverteilungen der repräsentativen Punkte innerhalb der verschiedenen Voxel klein, und die Umgebungskarte kann mit hoher Genauigkeit ausgedrückt werden. Auf diese Weise konvergieren, selbst wenn die Daten Fehler enthalten, die Ergebnisse in günstiger Weise auf Grund der Auswirkung des Kalman-Filters. Ferner kann in diesem Verfahren die Anzahl an gemessenen Punkten erhöht werden, um die Standardabweichung zu verringern, wodurch die Genauigkeit erwartungsweise weiter gesteigert wird.
Weil die Position und die Lage des zu messenden Objekts stationär sind, kann das Auffrischen außerdem unabhängig von der Position und Lage des zu messenden Objekts durchgeführt werden. Es ist anzumerken, dass eine Auffrischung unter Anwendung des Kalman-Filters nur auf die repräsentativen Punkte m(i) der Modellpunktegruppe, für welche Korrelationen (oder Übereinstimmung) mit den durch den Entfernungssensor erhaltenen gemessenen Punktegruppen erstellt worden sind, den Rechenaufwand stark verringern kann.
Im Feinzuordnungsschritt werden eine neue gemessene Daten- und Fehlerverteilung relativ zu dem Umgebungsmodell für die frühere Messposition rotiert und translatiert, oder ein Umgebungsmodell für eine neue Messposition wird relativ zu dem Umgebungsmodell für die frühere Messposition rotiert und translatiert, um den Prüfwert (wie die Multiplikation des Koinzidenzgrads) des Koinzidenzgrads, erstellt durch Maximum-Likelihood-Schätzungen, basierend auf der Distanz zwischen benachbarten Fehlerverteilungen, zu maximieren, anstatt einer Positionszuordnung in der Weise, dass die Summierung der Distanzen zwischen benachbarten Fehlerverteilungen minimiert wird.
Dieser Fall wird nun ausführlich erläutert.
Weil sowohl in der Modellpunktegruppe, bei welcher es sich um die Umgebungskarte handelt (Umgebungsmodell), als auch der vom Sensor gemessenen Punktegruppe ein Fehlermodell betrachtet wird, können beide Fehler in der Berechnungsformel (nachstehend bezeichnet als die "Prüffunktion") für den Prüfwert (zum Beispiel die Multiplikation des Koinzidenzgrads) bezüglich des Koinzidenzgrads verwendet werden. In der vorliegenden Ausführungsform wird, anstatt einfach eine Korrelation mit dem nächstliegenden Punkt zu erzeugen, das Konzept der Wahrscheinlichkeit eingebunden, um die Fehlerverteilung in der Umgebungskarte und die Fehlerverteilung der gemessenen Punkte in die Prüffunktion so einzubinden, dass die Prüffunktion so bestimmt wird, dass, falls die Wahrscheinlichkeit des Auftretens jedes gemessenen Punkts in der Umgebungskarte beim derzeitigen Zeitpunkt bei einem Maximum ist, die Prüffunktion ebenfalls bei einem Maximum sein wird.
Insbesondere wird angenommen, dass die Position des Punkts i = p(j) auf der Umgebungskarte, welche dem gemessenen Punkt j entspricht, einer Normalverteilung des Mittelwerts (repräsentativer Punkt) m(i) und der Fehlerkovarianzmatrix Σ_m(i) folgt, wobei die Prüffunktion in der nachstehenden Gleichung 9 bestimmt ist, sodass das Ergebnis der Messung unter Einsatz eines dreidimensionalen LRF darin besteht, dass der Wahrscheinlichkeitswert Pr(L(j)|m(i), Σ_m(i)) die Prüffunktion EM(i, j) für Punkt i und Punkt j sein wird, und die Multiplikation davon maximiert sein wird. (Gleichung 9)
Es ist anzumerken, dass ω(j) gleich 1 ist, wenn es einen repräsentativen Punkt gibt, der dem gemessenen Punkt j innerhalb des Umgebungsmodells entspricht, und ansonsten gleich 0 ist.
Wenn das Pr(L(j)|q) den Wert des Wahrscheinlichkeitswerts repräsentiert, dass die gemessenen Daten, bei welchen es sich um L(j) handelt, erhalten werden, falls der Punkt auf der Umgebungskarte an der Position von q liegt, und wenn Pr(q|m(i), Σ_m(i)) den Wert des Wahrscheinlichkeitswerts repräsentiert, dass der Punkt auf der Umgebungskarte q ist, wird hierin, unter der Annahme, dass der Durchschnittswert m(i) und die Fehlerkovarianzmatrix Σ_m(i) einer Normalverteilung folgen, die Gleichung 10 aufgestellt.
(Gleichung 10)

Pr(L(j)|m(i),Σm(i)) = ∫{Pr(L(j)|q)·Pr(q|m(i),Σm(i)}}dq

Wenn angenommen wird, dass Pr(q|m(i), Σ_m(i)) eine Normalverteilung aufweist, ist das Ergebnis die folgende Gleichung 11: (Gleichung 11)
Andererseits kann, durch Substituieren als z(j) für L(j), dieses Pr(L(j)|q) durch die folgende Gleichung 12 angenähert werden: (Gleichung 12)
Hierbei ist z_k(j) abhängig von der Position t_r und der Orientierung R_r der beweglichen Einheit, auf welcher der Entfernungssensor montiert ist. Eigentlich sind die Richtung von q und die Richtung des gemessenen Punkts L(j), bei Betrachtung vom Zentrum des Sensorkoordinatensystems des dreidimensionalen LRF aus, unterschiedlich, wie es in 20 gezeigt ist, sodass es ebenfalls notwendig ist, eine Rotationstransformation an der Fehlerkovarianzmatrix Σ_z(i) durchzuführen, um der Richtung von q zu entsprechen, aber weil die Existenzwahrscheinlichkeit von q, das an einer Stelle liegt, die vom Punkt i auf der Umgebungskarte deutlich getrennt ist, für welchen die Korrelation definiert worden ist, niedrig ist, kann davon ausgegangen werden, dass die Näherung eine geeignete Genauigkeit aufweist. Folglich kann Pr(L(j)|m(i), Σ_m(i)) durch die folgende Gleichung 13 wiedergegeben werden: (Gleichung 13)
Die folgende Gleichung 14 kann durch eine einfache Berechnung erhalten werden: (Gleichung 14)
Es ist anzumerken, dass α(j) und β(j) durch die folgende Gleichung 15 ausgedrückt werden können:
(Gleichung 15)

α(j) = (Σz(j)-1 + Σm(i)-1)-1 × (Σm(i)-1m(i) + Σz(j)-1z(j)) β(j) = (m(i) – z(j))t(Σz(j) + Σm(i))-1(m(i) – z(j))

Folglich kann die Prüffunktion EM (i, j), welche den Koinzidenzgrad für die Korrelation von Punkt i in der Modellpunktegruppe und Punkt j in der gemessenen Punktegruppe ausdrückt, dem Wert der Wahrscheinlichkeit angenähert werden, dass z(j) in der Normalverteilung des Durchschnittswerts m(i) und der Fehlerkovarianzmatrix Σ_m(i) + Σ_z(j) erhalten wird. Die Anwendung dieser Prüffunktion ermöglicht die Definition der Korrelation unter Berücksichtigung des Fehlers sowohl in der Umgebungskarte als auch den gemessenen Daten.
Eine zusätzliche Erklärung wird in Bezug auf die Aufstellung von Korrelationen zwischen den gemessenen Punkten und der Umgebungskarte (d. h. dem Umgebungsmodell) angegeben. Weil in der oben beschriebenen Ausführungsform eine statistische Prüffunktion, welche die Fehlerverteilung berücksichtigt, angewandt wird, kann der korrespondierende Punkt nicht ohne Berechnung des Werts der Prüffunktion bestimmt werden. Angesichts dieser Tatsache, wird der korrespondierende Punkt basierend auf den Werten der Prüffunktion aus Kandidaten, nach einer im Voraus erfolgenden Einengung auf zu korrelierende Kandidaten, aus der auf der Umgebungskarte festgelegten Modellpunktegruppe, ermittelt. Im Genaueren kann dies wie folgend durchgeführt werden:

(1) Das höchste Level bzw. die höchste Stufe von Voxeln, welche den Bereich der Fehlerkovarianzmatrix Σ_L(j) für den Messpunkt j von Interesse (zum Beispiel ein Bereich von dem 3-fachen der Standardabweichung) schneiden, und das höchste Level von Voxeln, welche zu diesen Voxeln benachbart sind, werden berechnet, und die repräsentativen Punkte, welche innerhalb dieser Voxel existieren, einschließlich der Voxel auf niedrigeren Levels, werden als Kandidaten für korrespondierende Punkte definiert. Weil die Voxel eine hierarchische Struktur bilden, erfolgt das Abrufen der Kandidatenpunkte im Wesentlichen ohne Rechenaufwand. Falls es, zu dieser Zeit, keine repräsentativen Kandidatenpunkte gibt, dann gibt es keine korrespondierenden Punkte. Der Grund für das Hinzufügen der benachbarten Voxel zu den Kandidaten besteht darin, dass es Fälle gibt, in welchen sich, abhängig von der Position des repräsentativen Punkts innerhalb des Voxels, der Bereich der Fehlerkovarianzmatrix in das benachbarte Voxel hinein erstreckt.
(2) Der repräsentative Punkt i und die Fehlerkovarianzmatrix des Kandidatenvoxels werden verwendet, um den Wert der Prüffunktion EM(i, j) zu berechnen.
(3) Der repräsentative Punkt i mit dem Maximalwert für die Prüffunktion EM(i, j) wird als der korrespondierende Punkt definiert. Wenn jedoch der Wert der Prüffunktion geringer als ein Schwellenwert ist, wird definiert, dass kein korrespondierender Punkt vorliegt.

In der vorliegenden Ausführungsform wird, als die Prüffunktion EM(i, j) für das Definieren der Korrelation, eine Gleichung verwendet, welche auf Wahrscheinlichkeit basiert, und weil es statistisch einen klaren Entscheidungsmaßstab dahingehend gibt, ob ein korrespondierender Punkt vorliegt, oder nicht, wird es in dem Fall keine erzwungene Korrelation geben, in welchem der korrespondierende Punkt als nicht-existent angesehen werden kann. Es ist anzumerken, dass, wenn kein korrespondierender Punkt vorliegt, der betreffende gemessene (Ziel-)Punkt als ein Punkt interpretiert wird, der einem Abschnitt entspricht, welcher noch nicht vermessen worden ist, und der Umgebungskarte hinzugefügt wird.
Als anwendbare Ausführungsformen wurden Erläuterungen in Bezug auf ein Verfahren und eine Vorrichtung zum Identifizieren einer dreidimensionalen Position und in Bezug auf ein Verfahren und eine Vorrichtung zum Messen einer dreidimensionalen Gestalt zum Identifizieren einer dreidimensionalen Eigenposition durch Aufnehmen von dreidimensionaler Information aus dem Außenraum angegeben; wenn jedoch eine zweidimensionale Gestalt als ein Spezialfall einer dreidimensionalen Gestalt angesehen wird, kann die vorliegende Erfindung als ein Verfahren und eine Vorrichtung zur Identifizierung einer zweidimensionalen Position und ein Verfahren und eine Vorrichtung zum Messen einer zweidimensionalen Gestalt, zum Identifizieren einer zweidimensionalen Eigenposition durch Aufnehmen von dreidimensionaler Information aus dem Außenraum, ausgeführt sein.
Ferner müssen, im oben beschriebenen Ausgabeschritt, die Position des Voxels, der repräsentative Punkt und die Fehlerverteilung nicht alle ausgegeben werden, sondern es kann eher, zum Beispiel in einem Fall, bei welchem es möglich ist, die dreidimensionale Gestalt ohne alle von diesen zu verstehen, oder wenn von diesen nur eines oder zwei notwendig sind, dann wenigstens eines von der Voxel-Position, dem repräsentativen Punkt und der Fehlerverteilung an die Ausgabevorrichtung ausgegeben werden.
Es ist anzumerken, dass die vorliegende Erfindung nicht auf die oben beschriebenen Ausführungsformen beschränkt ist, sondern eher, selbstverständlich, verschiedene Modifikationen vorgenommen werden können, ohne vom Zweck der vorliegenden Erfindung abzuweichen.
ZUSAMMENFASSUNG DER OFFENBARUNG
Die Erfindung umfasst einen Schritt S1 zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt in einen Computer; einen Schritt S4 zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der eine dreidimensionale Gestalt existiert, in eine Vielzahl von Voxeln von rechteckigen Körpern unterteilt und jede Position abspeichert; und einen Schritt S5 zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb des Voxels, das dem Koordinatenwert entspricht. Wenn in einer früheren Messposition keine Daten vorhanden sind, wird die Positionszuordnung in einem Fein-Positionszuordnungsschritt S7 so durchgeführt, dass ein Prüfwert bezüglich der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen Messdaten- und Fehlerverteilung für das Umgebungsmodell für eine frühere Messposition, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird.
ZITATE ENTHALTEN IN DER BESCHREIBUNG
Diese Liste der vom Anmelder aufgeführten Dokumente wurde automatisiert erzeugt und ist ausschließlich zur besseren Information des Lesers aufgenommen. Die Liste ist nicht Bestandteil der deutschen Patent- bzw. Gebrauchsmusteranmeldung. Das DPMA übernimmt keinerlei Haftung für etwaige Fehler oder Auslassungen.
Zitierte Patentliteratur

- JP 2003-15739 [0012]

Zitierte Nicht-Patentliteratur

- Andreas Nuchter et al., "6D SLAM with an Application in Autonomous Mine Mapping", IEEE 2004 International Conference an Robotics and Automation (ICRA' 04), April 2004 [0011]
- Paul J. Besl, "A Method for Registration of 3-D Shapes", IEEE Transactions of Pattern Analysis and Mechanical Intelligence, Band 14, Nr. 2, Februar 1992 [0011]
- SEKIMOTO Kiyohide, et al., "Development of a Three-Dimensional Laser Radar", Ishikawajima-Harima Engineering Review, Band 43, Nr. 4 (2003-7) [0011]

Claims

Eigenpositions-Identifizierungsverfahren zum Identifizieren einer Eigenposition durch Aufnehmen von dreidimensionalen Gestalten aus dem Außenraum, umfassend: einen Dateneingabeschritt zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt an einer neuen Messposition in einen Computer; und einen Modell-Strukturierungsschritt zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von Voxeln, die aus rechteckigen Körpern gebildet sind, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel speichert; einen Abgleichungs- bzw. Zuordnungsschritt zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon, innerhalb des Voxels, das dem Koordinatenwert entspricht; wobei: wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, eine neue Messposition als eine Eigenposition identifiziert wird; und wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition existieren: ein Feinzuordnungsschritt zur Positionszuordnung so ausgeführt wird, dass die Summierung der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird; und ein Eigenpositions-Identifizierungsschritt zum Identifizieren der Eigenposition aus einem Rotationsbetrag und einem Translationsbetrag im Feinzuordnungsschritt durchgeführt wird; und ein Ausgabeschritt zum Ausgeben der Eigenposition an eine Ausgabevorrichtung durchgeführt wird.
Eigenpositions-Identifizierungsverfahren, wie dargestellt in Anspruch 1, welches im Zuordnungsschritt Folgendes umfasst: Hinzufügen eines repräsentativen Punkts und der Fehlerverteilung davon zum Inneren eines Voxels; und Festlegen und Abspeichern eines Wahrscheinlichkeitswerts, der die Wahrscheinlichkeit der Existenz eines Objekts innerhalb des Voxels ausdrückt.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das vor dem Feinzuordnungsschritt, umfasst: einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um die Summierung der Distanzen von gemessenen Daten- und Fehlerverteilungen zu benachbarten Voxeln mit repräsentativen Punkten zu minimieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um die Summierung der Distanzen zwischen Voxeln mit repräsentativen Punkten, zu minimieren.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das vor dem Feinzuordnungsschritt umfasst: einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um die Summierung der Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten- und Fehlerverteilungen benachbart sind, zu maximieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um die Summierung der Differenzen von Wahrscheinlichkeitswerten von benachbarten Voxeln zu minimieren.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das nach dem Dateneingabeschritt umfasst: einen Suchbereich-beschränkenden Schritt zum Beschränken des Prüfungsbereichs durch Erhalten der derzeitigen Messposition durch Rückschluss aus einer Änderung in der Messposition der Vergangenheit oder aus einem Sensor, welcher fähig zum Erhalten der derzeitigen Messposition ist, oder durch die Verwendung eines Reflexionsstärkewerts zusätzlich zum Distanzwert der gemessenen Daten.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das im Eigenpositions-Identifizierungsschritt umfasst: Identifizieren einer Position in sechs Freiheitsgraden einer neuen Messposition aus der Position und Orientierung einer früheren Messposition.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das im Feinzuordnungsschritt umfasst: wenn sich Fehlerverteilungen schneiden, Behandlung des Falls als einzelnen gemessenen Punkt; und Berechnen der Distanz zwischen Fehlerverteilungen durch Multiplizieren der Distanzwerte dieses Falls mit einer Gewichtung, die aus den Koinzidenzgraden der Verteilungen berechnet wird.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das im Modell-Strukturierungsschritt umfasst: Festlegen des größten Voxels auf eine Größe, entsprechend der minimal erforderten Auflösung; und wenn eine Vielzahl von gemessenen Punkten innerhalb eines einzelnen Voxels existiert, weiteres hierarchisches Unterteilen des Voxels in eine Vielzahl von Voxeln, sodass nur ein einzelner gemessener Punkt innerhalb eines einzelnen Voxels existiert.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das nach dem Eigenpositions-Identifizierungsschritt umfasst: einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells, wobei der Modell-Auffrischungsschritt umfasst: Abrufen eines Voxels, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und unter der Annahme, dass kein Objekt zwischen dem Ursprung und dem gemessenen Punkt existiert, Neueinstellen oder Eliminieren der repräsentativen Punkte und Fehlerverteilungen innerhalb der dazwischen positionierten Voxel.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das nach dem Eigenpositions-Identifizierungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells umfasst, wobei der Modell-Auffrischungsschritt umfasst: Abrufen eines Voxels, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und sofern es keinen repräsentativen Punkt innerhalb des Voxels gibt, Festlegen des Koordinatenwerts und der Fehlerverteilung als Koordinatenwert und Fehlerverteilung des repräsentativen Punkts.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das nach dem Eigenpositions-Identifizierungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells umfasst, wobei der Modell-Auffrischungsschritt umfasst: Abrufen eines Voxels, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; sofern es einen repräsentativen Punkt gibt, der bereits innerhalb des Voxels festgelegt worden ist, Vergleichen einer neu erhaltenen Fehlerverteilung und der bereits innerhalb des Voxels festgelegten Fehlerverteilung; wenn sich die Fehlerverteilungen gegenseitig überlappen, Neueinstellen einer neuen Fehlerverteilung und eines neuen repräsentativen Punkts aus beiden Fehlerverteilungen; und dann, wenn sich die Fehlerverteilungen nicht gegenseitig überlappen, weiteres hierarchisches Unterteilen des Voxels in eine Vielzahl von Voxeln, sodass nur ein einziger repräsentativer Punkt innerhalb eines einzelnen Voxels existiert.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das im Eigenpositions-Identifizierungsschritt umfasst: Identifizieren einer Fehlerverteilung für die Eigenposition zusammen mit dem Identifizieren der Eigenposition; vor dem Ausgabeschritt Korrigieren der Eigenposition und der Fehlerverteilung durch Kalman-Filterung aus der derzeitigen Eigenposition und Fehlerverteilung, sowie der Eigenposition und Fehlerverteilung, welche identifiziert worden sind; und Ausgeben der Eigenposition und der Fehlerverteilung in einem Ausgabeschritt.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das im Modell-Auffrischungsschritt folgendes umfasst: Vergleichen der neu erhaltenen Fehlerverteilung und der Fehlerverteilung, die bereits innerhalb des Voxels festgelegt worden ist; falls die Fehlerverteilungen gegenseitig überlappen, wenn, als Ergebnis der Neu-Einstellung einer neuen Fehlerverteilung und eines neuen repräsentativen Punkts aus den beiden Fehlerverteilungen, sich der neue repräsentative Punkt in ein anderes Voxel bewegt hat: wenn es keinen repräsentativen Punkt in dem anderen Voxel gibt, dann Festlegen der neuen Fehlerverteilung und des neuen repräsentativen Punkts in das andere Voxel; und wenn es einen repräsentativen Punkt gibt, der bereits in dem anderen Voxel festgelegt worden ist, dann Vergleichen der neuen Fehlerverteilung und der Fehlerverteilung, welche bereits in das andere Voxel festgelegt worden ist; und (A) wenn die Fehlerverteilungen sich gegenseitig überlappen, dann Neu-Einstellung einer neuen Fehlerverteilung und eines neuen repräsentatives Punkts aus beiden Fehlerverteilungen, oder aus beiden Fehlerverteilungen und dem repräsentativen Punkt, der bereits innerhalb des Voxels festgelegt worden ist, und dem Koordinatenwert des neu eingegebenen gemessenen Punkts; oder (B) wenn die Fehlerverteilungen nicht gegenseitig überlappen, weiteres hierarchisches Unterteilen des Voxels in eine Vielzahl von Voxeln, sodass nur ein einziger repräsentativer Punkt innerhalb eines einzelnen Voxels existiert.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das nach dem Eigenpositions-Identifizierungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells umfasst; wobei der Modell-Auffrischungsschritt folgendes umfasst: Erhalten und Neueinstellung eines neuen repräsentativen Punkts und einer Fehlerverteilung durch einen Kalman-Filter aus dem Koordinatenwert des neu eingegebenen gemessenen Punkts und der Fehlerverteilung davon sowie dem repräsentativen Punkt und der Fehlerverteilung davon, welche bereits in dem Voxel festgelegt worden sind.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 1 dargestellt, das im Feinzuordnungsschritt umfasst: Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu dem Umgebungsmodell für die frühere Messposition zum Maximieren des Prüfwerts für den Koinzidenzgrad, erstellt durch Maximum-Likelihood-Schätzungen, basierend auf der Distanz zwischen benachbarten Fehlerverteilungen, anstatt Positionszuordnung zum Minimieren eines Prüfwerts für die Distanz zwischen benachbarten Fehlerverteilungen.
Verfahren zum Zuordnen von dreidimensionalen Gestalt-Daten, wie in Anspruch 15 dargestellt, wobei die Gleichung zur Berechnung des Prüfwerts für den Koinzidenzgrad in der folgenden Gleichung (16) ausgedrückt wird: (Gleichung 16)
wobei in dieser Gleichung eine Korrelation zwischen den gemessenen Punkten j und den repräsentativen Punkten i im Umgebungsmodell aufgestellt wird, die Wahrscheinlichkeit des Erhaltens von gemessenen Daten, d. h. dem gemessenen Punkt j, durch EM(i, j) angegeben ist, ω(j) gleich 1 ist, wenn es einen repräsentativen Punkt, der dem gemessenen Punkt j entspricht, innerhalb des Umgebungsmodells gibt, und ansonsten gleich 0 ist.
Eigenpositions-Identifizierungsverfahren zum Identifizieren einer Eigenposition durch Aufnehmen von dreidimensionalen Gestalten aus dem Außenraum, umfassend: einen Dateneingabeschritt zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt an einer neuen Messposition in einen Computer; einen Modell-Strukturierungsschritt zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel speichert; und einen Zuordnungsschritt zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon, innerhalb des Voxels, das dem Koordinatenwert entspricht; wobei: wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, eine neue Messposition mit einer Eigenposition identifiziert wird; und wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition existieren: ein Feinzuordnungsschritt zur Positionszuordnung so ausgeführt wird, dass ein Prüfwert bezüglich der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird; und ein Eigenpositions-Identifizierungsschritt zum Identifizieren der Eigenposition aus einem Rotationsbetrag und einem Translationsbetrag im Feinzuordnungsschritt durchgeführt wird.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 17 dargestellt, das ferner einen Ausgabeschritt zum Ausgeben der Eigenposition an eine Ausgabevorrichtung umfasst.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 17 dargestellt, das im Zuordnungsschritt folgendes umfasst: Hinzufügen eines repräsentativen Punkts und der Fehlerverteilung davon zum Inneren eines Voxels; und Festlegen und Abspeichern eines Wahrscheinlichkeitswerts, welcher die Wahrscheinlichkeit der Existenz eines Objekts innerhalb des Voxels ausdrückt.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 17 dargestellt, das vor dem Feinzuordnungsschritt umfasst: einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert für die Distanzen von gemessenen Daten- und Fehlerverteilungen zu benachbarten Voxeln mit repräsentativen Punkten zu minimieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert für die Distanzen zwischen Voxeln mit repräsentativen Punkten zu minimieren.
Eigenpositions-Identifizierungsverfahren, wie in Anspruch 17 dargestellt, das vor dem Feinzuordnungsschritt umfasst: einen Grobzuordnungsschritt für die Positionszuordnung durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert für die Wahrscheinlichkeitswerte von Voxeln mit repräsentativen Punkten, die zu gemessenen Daten- und Fehlerverteilungen benachbart sind, zu maximieren, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition, um einen Prüfwert für die Differenzen von Wahrscheinlichkeitswerten von benachbarten Voxeln zu minimieren.
Verfahren zur Messung einer dreidimensionalen Gestalt zum Reproduzieren einer dreidimensionalen Gestalt aus Koordinatenwerten von gemessenen Punkten auf einer externen dreidimensionalen Gestalt und für die Ausgabe der dreidimensionalen Gestaltdaten, umfassend: einen Dateneingabeschritt zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt an einer neuen Messposition in einen Computer; einen Modell-Strukturierungsschritt zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel speichert; einen Zuordnungsschritt zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb des Voxels, das dem Koordinatenwert entspricht; wobei: wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition nicht existieren, eine neue Messposition mit einer Eigenposition identifiziert wird; und wenn die dreidimensionalen Gestaltdaten für eine frühere Messposition existieren: ein Feinzuordnungsschritt zur Positionszuordnung so ausgeführt wird, dass ein Prüfwert bezüglich der Distanzen zwischen benachbarten Fehlerverteilungen durch Rotieren und Translatieren einer neuen gemessenen Daten- und Fehlerverteilung, oder Rotieren und Translatieren eines Umgebungsmodells für eine neue Messposition, relativ zu einem Umgebungsmodell für eine frühere Messposition, minimiert wird; ein Eigenpositions-Identifizierungsschritt zum Identifizieren der Eigenposition aus einem Rotationsbetrag und einem Translationsbetrag im Feinzuordnungsschritt durchgeführt wird; und ein Ausgabeschritt zum Ausgeben von wenigstens einem von der Eigenposition, der Voxelposition, dem repräsentativen Punkt und der Fehlerverteilung, basierend auf der der Eigenposition, an eine Ausgabevorrichtung durchgeführt wird.
Verfahren zur Messung einer dreidimensionalen Gestalt, wie in Anspruch 22 dargestellt, wobei der Ausgabeschritt umfasst: Ausgeben von wenigstens einem von der Position des Voxels, der Position eines repräsentativen Punkts und der Position einer Fehlerverteilung als der dreidimensionale Gestalt-Messwert an die Ausgabevorrichtung; und Ausgeben eines Index, der die Zuverlässigkeit oder Genauigkeit des Messwerts, basierend auf der Größenordnung der Fehlerverteilung innerhalb des Voxels, anzeigt, an die Ausgabevorrichtung.
Verfahren zur Messung einer dreidimensionalen Gestalt, wie in Anspruch 22 dargestellt, wobei: im Ausgabeschritt, wenn wenigstens eines von der Position des Voxels, der Position des repräsentativen Punkts und der Position der Fehlerverteilung als dreidimensionaler Gestalt-Messwert an die Ausgabevorrichtung ausgegeben wird, dann, wenn die Größenordnung der Fehlerverteilung innerhalb des Voxels größer als ein spezifischer Referenzwert ist, die Zuverlässigkeit oder Genauigkeit des Messwerts niedriger als eine spezifische Referenz eingestuft wird, und der Messwert für das Voxel nicht an die Ausgabevorrichtung ausgegeben wird.
Verfahren zur Messung einer dreidimensionalen Gestalt, wie in Anspruch 22 dargestellt, das nach dem Zuordnungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells umfasst, wobei der Modell-Auffrischungsschritt umfasst: Abrufen eines Voxels, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; und sofern es keinen repräsentativen Punkt innerhalb des Voxels gibt, Festlegen des Koordinatenwerts und der Fehlerverteilung als Koordinatenwert und Fehlerverteilung des repräsentativen Punkts.
Verfahren zur Messung einer dreidimensionalen Gestalt, wie in Anspruch 22 dargestellt, das nach dem Zuordnungsschritt einen Modell-Auffrischungsschritt zum Auffrischen des Umgebungsmodells umfasst, wobei der Modell-Auffrischungsschritt umfasst: Abrufen eines Voxels, das dem Koordinatenwert eines neu eingegebenen gemessenen Punkts entspricht; wenn es einen repräsentativen Punkt gibt, der bereits innerhalb des Voxels festgelegt worden ist, Vergleichen einer neu erhaltenen Fehlerverteilung und der bereits innerhalb des Voxels festgelegten Fehlerverteilung; wenn sich die Fehlerverteilungen gegenseitig überlappen, Neu-Einstellung einer neuen Fehlerverteilung und eines neuen repräsentativen Punkts aus den beiden Fehlerverteilungen, oder aus den beiden Fehlerverteilungen und dem bereits innerhalb des Voxels festgelegten repräsentativen Punkt und den Koordinatenwerten des neu eingegebenen, gemessenen Punkts; und dann, wenn sich die Fehlerverteilungen nicht gegenseitig überlappen, weiteres hierarchisches Unterteilen des Voxels in eine Vielzahl von Voxeln, sodass nur ein einziger repräsentativer Punkt innerhalb eines einzelnen Voxels existiert.
Verfahren zur Messung einer dreidimensionalen Gestalt, wie in Anspruch 22 dargestellt, das nach dem Zuordnungsschritt einen Modell-Auffrischungs schritt zum Auffrischen des Umgebungsmodells umfasst, wobei der Modell-Auffrischungsschritt umfasst: Abrufen eines Voxels, das dem Koordinatenwert des neu eingegebenen gemessenen Punkts entspricht; und wenn wenigstens eines von dem repräsentativen Punkt und der Fehlerverteilung innerhalb dieses Voxels neu festgelegt, oder nachjustiert, wird, oder dieses Voxel weiter hierarchisch in eine Vielzahl von Voxeln unterteilt wird, dann im Ausgabeschritt Ausgeben der Position des repräsentativen Punkts dieses Voxels an die Ausgabevorrichtung als ein dreidimensionaler Gestalt-Messwert.
Verfahren zur Messung einer dreidimensionalen Gestalt, wie in Anspruch 22 dargestellt, das im Ausgabeschritt umfasst: Ausgeben der Position eines repräsentativen Punkts eines Voxels im Umgebungsmodell in einem Bereich, für welchen die Position von der Position des Entfernungssensors aus gemessen werden kann, an die Ausgabevorrichtung als dreidimensionaler Gestalt-Messwert.
Eigenpositions-Identifizierungsvorrichtung zum Identifizieren einer Eigenposition durch Aufnehmen von dreidimensionalen Gestalten aus dem Außenraum, umfassend: eine Dateneingabevorrichtung zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt in einen Computer; eine Modell-Strukturierungsvorrichtung zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel abspeichert; eine Zuordnungsvorrichtung zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb eines Voxels, das einem Koordinatenwert entspricht; und eine Ausgabevorrichtung zum Ausgeben der Eigenposition an eine Ausgabevorrichtung; wobei: an einer neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition nicht existieren, eine neue Messposition als eine Eigenposition identifiziert wird; und an der neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition existieren, ein Umgebungsmodell für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition rotiert und translatiert wird, um eine Positionszuordnung auszuführen, sodass die Summierung der Distanzen zwischen benachbarten Fehlerverteilungen minimiert wird; und die Eigenposition aus dem Rotationsbetrag und Translationsbetrag in der Positionszuordnung identifiziert wird.
Eigenpositions-Identifizierungsvorrichtung zum Identifizieren einer Eigenposition durch Aufnehmen von dreidimensionalen Gestalten aus dem Außenraum, umfassend: eine Dateneingabevorrichtung zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt in einen Computer; eine Modell-Strukturierungsvorrichtung zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel abspeichert; eine Zuordnungsvorrichtung zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb eines Voxels, das einem Koordinatenwert entspricht; und eine Ausgabevorrichtung zum Ausgeben der Eigenposition an eine Ausgabevorrichtung; wobei: an einer neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition nicht existieren, eine neue Messposition als eine Eigenposition identifiziert wird; und an der neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition existieren, ein Umgebungsmodell für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition rotiert und translatiert wird, um eine Positionszuordnung auszuführen, sodass ein Prüfwert bezüglich der Distanz zwischen benachbarten Fehlerverteilungen minimiert wird; und die Eigenposition aus dem Rotationsbetrag und Translationsbetrag in der Positionszuordnung identifiziert wird.
Vorrichtung zur Messung einer dreidimensionalen Gestalt zum Reproduzieren einer dreidimensionalen Gestalt aus Koordinatenwerten von gemessenen Punkten auf einer dreidimensionalen Gestalt und zum Ausgeben von dreidimensionalen Gestaltdaten, umfassend: eine Dateneingabevorrichtung zum Eingeben von Koordinatenwerten auf einer dreidimensionalen Gestalt in einen Computer; eine Modell-Strukturierungsvorrichtung zum Aufbauen eines Umgebungsmodells, das eine räumliche Region, in der die dreidimensionale Gestalt existiert, in eine Vielzahl von aus rechteckigen Körpern gebildeten Voxeln, deren Grenzflächen gegenseitig senkrecht stehen, unterteilt und die Positionen der einzelnen Voxel abspeichert; eine Zuordnungsvorrichtung zum Festlegen und Aufzeichnen eines repräsentativen Punkts und einer Fehlerverteilung davon innerhalb eines Voxels, das einem Koordinatenwert entspricht; wobei an einer neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition nicht existieren, eine neue Messposition als eine Eigenposition identifiziert wird; an der neuen Messposition, sofern die dreidimensionalen Gestalt-Daten für eine frühere Messposition existieren: ein Umgebungsmodell für eine neue Messposition relativ zu einem Umgebungsmodell für eine frühere Messposition rotiert und translatiert wird, um eine Positionszuordnung auszuführen, sodass ein Prüfwert bezüglich der Distanz zwischen benachbarten Fehlerverteilungen minimiert wird; und die Eigenposition aus dem Rotationsbetrag und Translationsbetrag in der Positionszuordnung identifiziert wird; und eine Datenübertragungsvorrichtung zum Ausgeben, an eine Ausgabevorrichtung, von wenigstens einem von der Eigenposition, der Voxelposition, dem repräsentativen Punkt und der Fehlerverteilung, basierend auf der Eigenposition.