DE102019214008A1

DE102019214008A1 - Verfahren und Vorrichtung zur Lokalisierung eines mobilen Agenten in einer Umgebung mit dynamischen Objekten

Info

Publication number: DE102019214008A1
Application number: DE102019214008.4A
Authority: DE
Inventors: Sebastian Scherer; Edgar Klenske; Michael Herman
Original assignee: Robert Bosch GmbH
Current assignee: Robert Bosch GmbH
Priority date: 2019-09-13
Filing date: 2019-09-13
Publication date: 2021-03-18

Abstract

Die Erfindung betrifft ein computer-implementiertes Verfahren zur Posenbestimmung eines mobilen Agenten (1) in einer Umgebung mit dynamischen Objekten (4) mit einem Faktorgraphen, wobei der Faktorgraph in aufeinanderfolgenden Zeitschritten aktualisiert und ausgewertet wird, wobei in mindestens zwei aufeinanderfolgenden Zeitschritten folgende Schritte ausgeführt werden:- Bereitstellen oder Erweitern des Faktorgraphen in einem aktuellen Zeitschritt, indem für eine absolute Poseninformation über eine aktuelle Pose des mobilen Agenten (1) ein Agenten-Knoten (21) eingefügt wird und für eine absolute Poseninformation über eine aktuelle Pose eines dynamischen Objekts (4) ein Dynamisches-Objekt-Knoten (24) eingefügt wird, wobei mindestens zwei Dynamisches-Objekt-Knoten (24) mit einer Kante verbunden sind, denen ein Bewegungsmodell-Faktorknoten zugeordnet ist, wobei der Bewegungsmodell-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell ergibt;- Ermitteln der Verbundverteilung in dem Faktorgraph (20);- Auswerten (S9) des Faktorgraphen mithilfe der Verbundverteilung, um die Pose des mobilen Agenten (1) in dem aktuellen Zeitschritt zu ermitteln.

Description

Technisches Gebiet
Die Erfindung betrifft Verfahren zur Lokalisation eines mobilen Agenten in einer Umgebung mit statischen und dynamischen Objekten.
Technischer Hintergrund
Die Lokalisation eines mobilen Agenten in einer Umgebung erfolgt in der Regel mithilfe von Messungen mit Sensoreinrichtungen, wie beispielsweise LiDAR, einer Laserscan-Vorrichtung, einer Kamera zur Erfassung eines Kamerabildes oder dergleichen. Messungen mit entsprechenden Sensoreinrichtungen werden dazu mit vorangehenden Messungen oder einer Umgebungskarte oder beidem verglichen, um die Verortung des mobilen Agenten durchzuführen. Dabei wird davon ausgegangen, dass die Umgebung mehr oder weniger statisch ist, um eine ausreichende Referenz für die Verortung des mobilen Agenten zu haben.
Eine ausreichende Bezugnahme auf örtliche Strukturen und Merkmale als Referenz ist jedoch bei mobilen Agenten, wie beispielsweise Fahrzeugen, Fußgängern, mobilen Robotern oder dergleichen nicht immer möglich, da sich der mobile Agent bewegt und sich bei häufigen Anwendungen in dessen Umgebung auch dynamische Objekte, d.h. potenziell bewegliche Objekte, befinden. Die meisten Verfahren versuchen dabei, die dynamischen Objekte zur Lokalisierung des mobilen Agenten auszublenden und lediglich die Informationen über die statischen Objekte zu nutzen.
Offenbarung der Erfindung
Erfindungsgemäß sind ein Verfahren zur Lokalisation eines mobilen Agenten in einer Umgebung mit dynamischen Objekten gemäß Anspruch 1 sowie durch die die Vorrichtung und den mobilen Agenten gemäß den nebengeordneten Ansprüchen vorgesehen.
Weitere Ausgestaltungen sind in den abhängigen Ansprüchen angegeben.
Gemäß einem ersten Aspekt ist ein Verfahren zur Posenbestimmung eines mobilen Agenten in einer Umgebung mit dynamischen Objekten mithilfe eines Faktorgraphen vorgesehen, wobei der Faktorgraph in aufeinanderfolgenden Zeitschritten aktualisiert und ausgewertet wird, wobei in mindestens zwei aufeinanderfolgenden Zeitschritten folgende Schritte ausgeführt werden:

- Bereitstellen oder Erweitern des Faktorgraphen in einem aktuellen Zeitschritt, indem für eine absolute Poseninformation über eine aktuelle Pose des mobilen Agenten ein Agenten-Knoten eingefügt wird und für eine absolute Poseninformation über eine aktuelle Pose eines dynamischen Objekts ein Dynamisches-Objekt-Knoten eingefügt wird, wobei mindestens zwei Dynamisches-Objekt-Knoten mit einer Kante verbunden sind, denen ein Bewegungsmodell-Faktorknoten zugeordnet ist, wobei der Bewegungsmodell-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell ergibt;
- Ermitteln der Verbundverteilung in dem Faktorgraph;
- Auswerten des Faktorgraphen durch Maximieren der Verbundverteilung, um die Pose des mobilen Agenten in dem aktuellen Zeitschritt zu ermitteln.

Weiterhin kann die absolute und/oder die relative Poseninformation einer Posenangabe und einer zugehörigen probabilistischen Verteilung angeben, die die Zuverlässigkeit der zugrundeliegenden Posenangabe angibt.
Während herkömmliche Verfahren dynamische Objekte in der Umgebung des mobilen Agenten nicht zur Lokalisation bzw. Posenermittlung des mobilen Agenten nutzen, sieht obiges Verfahren vor, den Informationsgehalt der Detektion eines oder mehrerer dynamischer Objekte für die Lokalisation des mobilen Agenten zu nutzen. Durch die Berücksichtigung eines probabilistischen Bewegungsmodells für jedes der dynamischen Objekte kann in Verbindung mit dem Informationsgehalt aus einer dem betreffenden dynamischen Objekt zugeordneten Messung eine Trajektorie der betreffenden dynamischen Objekte mit einer entsprechenden Unsicherheit bestimmt werden. Diese kann dazu verwendet werden, die Genauigkeit der Posenermittlung des mobilen Agenten und die Robustheit der Lokalisierung zu verbessern, insbesondere, wenn dieser von vielen dynamischen Objekten umgeben ist, die das Erfassen einer für die geforderte Genauigkeit ausreichenden Anzahl statischer Objekte verhindert oder die Erfassung der statischen Objekte durch die Sensoreinrichtungen nicht mit genügender Präzision erfolgen kann.
Vor allem, wenn die Unsicherheit der probabilistischen Bewegungsmodelle gering ist, erhöht die Kombination der Berücksichtigung der einem jeweiligen dynamischen Objekt zugeordneten Messung mit dem betreffenden Bewegungsmodell die Nachverfolgungsgenauigkeit der Trajektorie des betreffenden dynamischen Objekts und somit die Genauigkeit der Lokalisierung des mobilen Agenten.
Eine Schwierigkeit bei der Posenermittlung des mobilen Agenten besteht darin, dass eine einzelne Sensormessung möglicherweise keine ausreichende Information für eine genaue Bestimmung der Pose des mobilen Agenten liefert. Die Genauigkeit der Posenermittlung kann jedoch durch Integrieren der Informationen über mehrere zeitlich nacheinander erfolgende Sensormessungen deutlich erhöht werden. Obwohl man durch verschiedene Sensoreinrichtungen, wie beispielsweise Lidar, Laserscanner, Kamera oder Radar, nur eine unvollständige und rauschbehaftete Information über die Positionen statischer und dynamischer Objekte in der Umgebung erhalten kann, ermöglichen sie eine Inferenz über mögliche Posen des mobilen Agenten. So können beispielsweise bei einer Lidar-Messung nur an manchen Positionen eine Übereinstimmung mit einer Umgebungskarte erkannt werden, bei einer Kamerabildauswertung die Schlüsselstellen in der aufgezeichneten Abbildung nur von einzelnen Betrachtungspunkten bzw. Posen des mobilen Agenten beobachtet werden und bei einer Radarmessung die Abstände zu bestimmten statischen Objekten nur abhängig von manchen Posen des mobilen Agenten bestimmt werden.
Um die Sensormessungen über die Zeit auszuwerten, kann ein Bewegungsmodell verwendet werden, das in der Regel rauschbehaftet ist. Durch die Nutzung von Faktorgraphen ist es möglich, ein probabilistisches graphisches Modell zu erzeugen, das die Abhängigkeit zwischen den Beobachtungen und den Posen codiert. Dadurch kann die Pose eines mobilen Agenten ermittelt werden.
Der Faktorgraph modelliert die Verbundverteilung über Zufallsvariablen von Posen über der Zeit und den einzelnen Sensormessungen als Beobachtungen. Die Pose des mobilen Agenten wird zu verschiedenen Zeitpunkten geschätzt. Relative Posen und Faktoren des Bewegungsmodells können mithilfe bekannter Verfahren wie Odometrie-Übereinstimmungssuche, Scan-Matching, visuelle Odometrie oder Kombinationen davon ermittelt werden. Auch absolute Posenfaktoren, die beispielsweise aus GPS-Messungen erhalten werden können, können verwendet werden.
Es wird also ein Beobachtungsmodell beschrieben, das die Charakteristika einer bestimmten Beobachtung, z. B. einer Verteilung über die Abstände einer Radarmessung bei einer bestimmten Pose, spezifiziert.
Um den Informationsgehalt der Erfassung von dynamischen Objekten durch die Sensormessungen zur Verbesserung der Lokalisierung des mobilen Agenten in der Umgebung zu nutzen, kann das probabilistische Bewegungsmodell verwendet werden, die eine wahrscheinliche Bewegung eines dynamischen Objekts prädiziert. Wenn z. B. das Bewegungsmodell für ein dynamisches Objekts bekannt ist, kann das dynamische Objekt detektiert und nachverfolgt werden und somit auch dann für die Posenermittlung des mobilen Agenten verwendet werden, wenn es sich bewegt. Dies ist besonders dann hilfreich, wenn in Umgebungen sehr viele dynamische und wenige statische Objekte vorhanden sind, wie beispielsweise beim Fahren eines autonom fahrenden Fahrzeugs durch einen Tunnel mit umgebenden, sich ebenfalls bewegenden Fahrzeugen und wenigen statischen Orientierungspunkten an der Tunnelwand oder beim Bewegen eines mobilen Roboters in einer Umgebung mit vielen den Roboter umgebenden Personen.
Dies wird durchgeführt, indem ein probabilistisches graphisches Modell in Form eines Faktorgraphen erzeugt wird, wie es für die Modellierung von Beobachtungen von statischen Objekten bei herkömmlichen Lokalisierungsverfahren bekannt ist. Beim Erkennen von potenziell dynamischen Objekten in der Umgebung des mobilen Agenten durch die Sensormessungen werden diese jedoch separat in dem Modell behandelt. Wenn ein potenziell dynamisches Objekt zum ersten Mal durch eine Messung erfasst wird, wird seine Pose als ein Knoten zu dem graphischen Modell hinzugefügt. Seine relative Pose zum mobilen Agenten kann dann aus der Messung abgeleitet werden und im grafischen Modell als ein Faktor zwischen den mobilen Agenten und der Pose des dynamischen Objekts verwendet werden. Jede weitere Detektion desselben dynamischen Objekts zu einem späteren Zeitpunkt führt zu einem weiteren solchen relativen Posenfaktor, jedoch können zusätzlich aufeinanderfolgenden Posen desselben dynamischen Objekts zu verschiedenen Zeitpunkten durch Faktorknoten verbunden werden, die einem dem betreffenden dynamischen Objekt zugeordneten probabilistischen Bewegungsmodell entsprechen.
Jedoch erfordert diese Vorgehensweise probabilistische Bewegungsmodelle, die als ein generisches Bewegungsmodell, ein Bewegungsmodell basierend auf Domänenwissen oder ein aus Daten trainiertes Bewegungsmodell vorgegeben werden können. Die Verbundverteilung über Zufallsvariablen im Graphen kann dann aus dem Faktorgraph in an sich bekannter Weise abgeleitet werden. Durch Maximieren der Verbundwahrscheinlichkeit erhält man die aktuelle Pose des mobilen Agenten.
Es kann vorgesehen sein, dass zum Auswerten des Faktorgraphen die Verbundverteilung in dem Faktorgraph ermittelt wird und der Faktorgraph über die letzte Pose oder letzte Posen aus dem erstellten Faktorgraph inferiert oder die Pose mit der höchsten Wahrscheinlichkeit ermittelt wird.
Weiterhin kann das probabilistische Bewegungsmodell einem generischen Bewegungsmodell, einem Bewegungsmodell basierend auf Domänenwissen oder einem aus Daten trainierten Bewegungsmodell entsprechen.
Es kann vorgesehen sein, dass in jedem Zeitschritt mithilfe einer Umgebungssensorik das dynamische Objekt erfasst wird und dessen relative Pose zu dem mobilen Agenten ermittelt wird, wobei das probabilistische Bewegungsmodell dem dynamischen Objekt zugeordnet wird, indem das dynamische Objekt, insbesondere durch ein Mustererkennungsverfahren, klassifiziert wird und das Bewegungsmodell entsprechend der erkannten Klasse ausgewählt wird.
Somit kann vorgesehen sein, dass die dynamischen Objekte in der Umgebung eines mobilen Agenten klassifiziert werden und ein probabilistisches Bewegungsmodell entsprechend einer Klassenzugehörigkeit des betreffenden dynamischen Objekts zugeordnet wird. Dadurch kann ein Bewegungsmodell den dynamischen Objekten einer Klasse gemeinsam zugeordnet werden.
Gemäß einer Ausführungsform können ein Agenten-Knoten und ein Dynamisches-Objekt-Knoten mit einer Kante verbunden sein, denen ein Relativposen-Faktorknoten zugeordnet ist, wobei der Relativposen-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einer Beobachtung ergibt.
Weiterhin können mindestens zwei Agenten-Knoten verbunden sein, denen ein Agentenbewegungsmodell-Faktorknoten zugeordnet ist, wobei der Agentenbewegungsmodell-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell für den mobilen Agenten ergibt.
Gemäß einem weiteren Aspekt ist eine Vorrichtung zur Posenbestimmung eines mobilen Agenten in einer Umgebung mit dynamischen Objekten mit einem Faktorgraphen vorgesehen, wobei der Faktorgraph in aufeinanderfolgenden Zeitschritten aktualisiert und ausgewertet wird, wobei die Vorrichtung ausgebildet ist, um in mindestens zwei aufeinanderfolgenden Zeitschritten:

- den Faktorgraphen in einem aktuellen Zeitschritt bereitzustellen oder zu erweitern, indem für eine absolute Poseninformation über eine aktuelle Pose des mobilen Agenten ein Agenten-Knoten eingefügt wird und für eine absolute Poseninformation über eine aktuelle Pose eines dynamischen Objekts ein Dynamisches-Objekt-Knoten eingefügt wird, wobei mindestens zwei Dynamisches-Objekt-Knoten mit einer Kante verbunden sind, denen ein Bewegungsmodell-Faktorknoten zugeordnet ist, wobei der Bewegungsmodell-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell ergibt;
- eine Verbundverteilung in dem Faktorgraph zu ermitteln; und
- den Faktorgraphen durch Maximieren der Verbundverteilung auszuwerten, um die Pose des mobilen Agenten in dem aktuellen Zeitschritt zu ermitteln.

Gemäß einem weiteren Aspekt ist ein Mobiler Agent mit einer Bewegungsaktuatorik, mit einer Umgebungssensorik zum Erfassen von statischen und dynamischen Objekten in der Umgebung und mit der obigen Vorrichtung vorgesehen.
Figurenliste
Ausführungsformen werden nachfolgend anhand der beigefügten Zeichnungen näher erläutert. Es zeigen:

1 eine schematische Darstellung eines mobilen Agenten in einer Umgebung mit statischen und dynamischen Objekten;
2 ein Faktorgraph zur Berücksichtigung von dynamischen Objekten; und
3 ein Flussdiagramm zur Veranschaulichung eines Verfahrens zur Lokalisation des mobilen Agenten in der Umgebung.

Beschreibung von Ausführungsformen
1 zeigt eine schematische Darstellung eines mobilen Agenten 1 in einer Umgebung 2 mit statischen und dynamischen Objekten 3, 4. Der mobile Agent 1 weist eine Umgebungssensorik 11 auf, um Informationen über die Verteilung der Objekte 3, 4 in der Umgebung des mobilen Agenten 1 zu erfassen.
Die Umgebungssensorik 11 kann eine Lidar-Sensorik, eine Radar-Sensorik, eine Ultraschallsensorik, eine Laserscan-Sensorik, eine Kamera und dergleichen umfassen, die jeweils Informationen über Objekte in der Umgebung 2 mit vorgegebener Genauigkeit erfassen kann. Beispielsweise kann bei einer Lidar-Messung mit Positionen in der Umgebung verglichen werden, um eine Pose des mobilen Agenten, d. h. eine Position und dessen Ausrichtung in der Umgebung, zu erkennen. Mithilfe einer Kamera können charakteristische Strukturen in der Umgebung erfasst werden, deren Position und Ausrichtung bekannt ist. Mithilfe einer Radarmessung können beispielsweise Distanzen zu Objekten in einer bestimmten Richtung erfasst werden. Obwohl diese Messungen in der Regel rauschbehaftet sind, ermöglichen sie, potenzielle Posen des mobilen Agenten 1 zu ermitteln. Eine Pose des mobilen Agenten 1 ist durch eine Position und eine Ausrichtung bezüglich eines Umgebungskoordinatensystems 5 der Umgebung 2 festgelegt.
Der mobile Agent 1 weist weiterhin eine Steuereinheit 12 auf, die die Sensormessungen der Umgebungssensorik 11 auswerten kann, um diesen erkannte Objekten zuzuordnen. Weiterhin führt die Steuereinheit 12 ein Verfahren zur Lokalisation bzw. zur Posenermittlung des mobilen Agenten 1 in der Umgebung 2 aus.
Mit Hilfe einer Bewegungsaktuatorik 13 kann eine Bewegung des mobilen Agenten zur Fortbewegung oder zum Bewegen eines Teils von diesem (Roboterarm) durchgeführt werden
Die Steuereinheit 12 erstellt dazu einen Faktorgraphen, wie er beispielhaft in 2 dargestellt ist. Ferner führt die Steuereinheit 12 ein Verfahren aus, wie es als Flussdiagramm in 3 dargestellt ist. Das Verfahren kann in Form eines Software- und/oder Hardware-Algorithmus in der Steuereinheit 12 implementiert sein.
Das Verfahren wird zyklisch ausgeführt und dadurch ein Faktorgraph erzeugt, der in jedem Zeitschritt ausgewertet werden kann, um die Pose des mobilen Agenten zu bestimmen. Der Faktorgraph ist ein Graph, der eine Faktorisierung einer Funktion darstellt. In der Wahrscheinlichkeitstheorie werden Faktorgraphen verwendet, um eine Faktorisierung einer Wahrscheinlichkeitsverteilungsfunktion darzustellen. Faktorgraphen werden mit Knoten, Kanten und den Kanten zugeordneten Faktorknoten gebildet.
Die Verwendung von Faktorgraphen für Lokalisationsprobleme ist prinzipiell bekannt. Mit ihnen werden die Verbundverteilung über Zufallsvariablen von Posen des mobilen Agenten über der Zeit und Beobachtungen modelliert. Zusätzlich können relative Poseninformationen des Bewegungsmodells des mobilen Agenten 1 auf Odometrie, Laserscanverfahren, visueller Odometrie oder Kombinationen davon berücksichtigt werden.
Das Verfahren zur Lokalisation des mobilen Agenten 1 in seiner Umgebung 2 nutzt die Informationen aus Messungen der Umgebungssensorik 11 und probabilistischen Bewegungsmodellen des mobilen Agenten 1 und von detektierten dynamischen Objekten sowie ggfs., wenn verfügbar, von absoluten Positionsmessungen.
In Schritt S1 werden zunächst Messungen mithilfe der Umgebungssensorik 11 vorgenommen.
Die Messungen werden in Schritt S2 in bekannter Weise ausgewertet, um Informationen von in der Umgebung 2 des mobilen Agenten 1 befindlichen statischen und dynamischen Objekten 3, 4 zu erhalten, d.h. es werden Objekte 3, 4 erkannt und ggfs als statisch oder dynamisch klassifiziert. Zusätzlich können die erkannten Objekte 3, 4, Klassen zugeordnet werden.
Weiterhin ermöglichen Sensormessungen in Schritt S3 eine Abschätzung der Pose des mobilen Agenten 1, z.B. indem statische Objekte in der Umgebung einer bekannten Position zugeordnet werden, wie es beispielsweise bei entsprechend bedateten Orts- oder Landschaftsmerkmalen der Fall sein kann.
Das beschriebene Verfahren beruht auf der Erstellung eines Faktorgraphen 20.
Für jeden Zeitschritt werden dem Faktorgraph 20 Agenten-Knoten 21 zugefügt, wobei jeder Agenten-Knoten 21 des Faktorgraphs 20 eine Poseninformation mit einer Posenangabe und einer zugehörigen probabilistischen Verteilung angibt, die die Zuverlässigkeit/Genauigkeit der zugrundeliegenden Posenangabe angibt.
In dem Faktorgraph stellen die Kreise Poseninformationen eines durch den Index bezeichneten Objekts dar. Der Index r bezeichnet den mobilen Agenten 1 und die dynamischen Objekte sind mit d1 und d2 angegeben. Der hochgestellte Index stellt den Zeitschritt dar, auf den sich die entsprechende Angabe bezieht, d.h. der Zeitschritt der Messung, die zum Ermitteln der entsprechenden Poseninformation verwendet wurde.
Relative Poseninformationen werden als Faktorknoten in die Kanten des Faktorgraphen eingefügt, die die durch die Knoten repräsentierten Zustände miteinander verbinden, d.h. die zeitlich aufeinanderfolgende Poseninformationen desselben Objekts bzw. Poseninformationen von verschiedenen Objekten miteinander verbinden. Die Faktorknoten werden mit einer entsprechenden relativen Poseninformation versehen, die ebenfalls eine Posenangabe mit einer entsprechenden probabilistischen Verteilung enthalten, die die Zuverlässigkeit/Genauigkeit der zugrundeliegenden Poseninformation angibt.
Beim Erstellen des Faktorgraphen wird zunächst für den aktuellen Zeitschritt von einer ermittelten Poseninformation des mobilen Agenten 1 ausgegangen und dieser als Agenten- Knoten 21 in den Faktorgraphen 20 eingefügt. Die Poseninformation kann beispielsweise durch Auswertung eines Kamerabildes basierend auf erkannten Strukturen der statischen Umgebung ermittelt werden.
Ferner können, wenn vorhanden, in Schritt S4 beispielsweise eine Information über eine absolute geografische Position in der Umgebung 2, die beispielsweise mithilfe eines GPS-Positionsbestimmungssystems oder dergleichen ermittelt wird, als Beobachtung o^t in dem Zeitschritt t in Form eines Absolutposition-Faktorknotens 22 dem dem gleichen Zeitschritt entsprechenden Agenten-Knoten 21 zugeordnet werden.
Ferner kann in Schritt S5 mithilfe eines vorgegeben oder geschätzten probabilistischen Bewegungsmodells des mobilen Agenten 1 eine relative Beobachtung $o_{r}^{t}$
dem entsprechenden Zeitschritt t zugeordnet werden und in der Kante zwischen den Agenten- Knoten 21 mit einer Agentenbewegungsmodell-Faktorknoten 23 berücksichtigt werden.
Man erkennt in 2 die den verschiedenen Zeitschritten t = 1, 2, 3 zugeordneten absoluten Poseninformationen x_r ^t, die diesen absoluten Posen zugeordneten absoluten Beobachtungen o¹, o³ und die in der Kante zwischen den Poseninformationen x_r ¹, x_r ², x_r ³ des mobilen Agenten 1 relativen Poseninformationen als relative Beobachtungen o_r ², o_r ³.
Das Bewegungsmodell für den mobilen Agenten 1 ist vorgegeben und in der Regel stark rauschbehaftet, da eine vollständig zuverlässige Prädiktion der künftigen Bewegung des mobilen Agenten 1 nur in seltenen Fällen möglich ist.
Weiterhin wird die Poseninformation jedes dynamischen Objekts 4, sobald dieses erstmalig erkannt wird, in Schritt S6 ebenfalls dem Faktorgraphen 20 hinzugefügt. Die Poseninformation des betreffenden dynamischen Objekts 4 wird als ein Zustand des Schätzproblems dem Faktorgraphen zugeordnet im Falle von zwei dynamischen Objekten x_d1 ¹, x_d1 ² sowie x_d2 ², x_d2 ³., wobei der entsprechende Knoten des Faktorgraphen als Dynamisches-Objekt- Knoten 24 eingefügt wird. Das dynamische Objekt d1 wird im dargestellten Beispiel dabei bereits beim ersten Zeitschritt erkannt, während das dynamische Objekt d2 erst ab dem zweiten Zeitschritt t=2 erkannt wird.
Die relative Poseninformation zwischem dem mobilen Agenten 1 und dem betreffenden dynamischen Objekt kann mithilfe der Messung mithilfe der Umgebungssensorik 11 ermittelt werden und als eine Relativposen-Faktorknoten 25 zwischen Agenten- Knoten 21 und dem Dynamisches-Objekt- Knoten 24 eingefügt werden, wobei die Faktoren o_d1 ¹, o_d1 ² für das erste dynamische Objekt sowie die Faktoren o_d2 ² und o_d2 ³ für das zweite dynamische Objekt 4 sind. Jede weitere Detektion desselben dynamischen Objekts 4 zu einem späteren Zeitschritt führt zu einem Einfügen einer weiteren solchen Relativposen-Kante 25, das die entsprechende relative Poseninformation mit der relativen Posenangabe und der zugehörigen probabilistischen Verteilung angibt, die die Zuverlässigkeit/Genauigkeit der zugrundeliegenden relativen Poseninformation angibt.
In Schritt S7 werden zeitlich aufeinanderfolgende Dynamisches-Objekt- Knoten 24, die aufeinanderfolgenden Posen desselben dynamischen Objekts entsprechen, durch Bewegungsmodell-Kanten 26 verbunden, die einem vorgegebenen probabilistischen Bewegungsmodell entsprechen und relative Poseninformation bezüglich der Fortbewegung des dynamischen Objekts mit einer zugehörigen probabilistischen Verteilung angibt.
Die probabilistischen Bewegungsmodelle für die detektierten dynamischen Objekte können einem generischen Bewegungsmodell (z.B. unter der Annahme einer konstanten Geschwindigkeit oder konstanter Beschleunigung), einem auf Domänenwissen basierenden Bewegungsmodell oder einem auf Trainingsdaten basierenden Bewegungsmodell entsprechen. Beispielsweise können bei einem autonom fahrenden Fahrzeug als mobilem Agenten andere Fahrzeuge als dynamische Objekte erkannt werden, wobei ein Bewegungsmodell dabei von einer Bewegung mit konstanter Geschwindigkeit ausgehen kann.
In Schritt S8 wird eine Verbundverteilung über die Zufallsvariablen in den Graphen ermittelt wie folgt: $p (x_{r}^{1}, x_{r}^{2},..., o^{1}, o^{2},..., x_{d 1}^{1},..., x_{d 2}^{1},...) = p (x_{r}^{1}) \prod_{t = 1}^{T - 1} p (o^{t} | x_{r}^{t}) p (x_{r}^{t + 1} | x_{r}^{t}) \prod_{d \in D (t)} p (o_{d}^{t} | x_{r}^{t}, x_{d}^{t}) p (x_{d}^{t + 1} | x_{d}^{t}) .$
wobei p() den jeweils angegebenen Wahrscheinlichkeitsverteilungen der Poseninformation und der Beobachtungen entsprechen und D der Liste von zum Zeitpunkt t erkannten dynamischen Objekten entspricht. Im Unterschied zu vorangehenden Ansätzen werden bei dem oben beschriebenen Verfahren Messungen von dynamischen Objekten 4 in die Lokalisierung des mobilen Agenten 1 einbezogen, indem man sowohl ihre relativen Poseninformation bezüglich des mobilen Agenten 1 als auch ihre probabilistischen Bewegungsmodelle nutzt, so dass deren jeweilige Trajektorien nachverfolgt werden können.
In Schritt S9 wird der in dem momentanen Zeitschritt erstellte Faktorgraph mithilfe der ermittelten Verbundverteilung ausgewertet, um die aktuelle Pose des mobilen Agenten 1 zu bestimmen.
Zum Ermitteln der aktuellen Position des mobilen Agenten 1 wird über die letzte Pose oder letzte Posen aus dem erstellten Faktorgraph 20 inferiert. Dafür werden die besten Werte mit der höchsten Wahrscheinlichkeit für die letzte Pose oder alle relevanten Knoten des Faktorgraphen 20 ermittelt: $x_{r}^{T} = arg max_{x_{r}^{T}} p (x_{r}^{T} | o^{1}, o^{2},..., x_{d 1}^{1},..., x_{d 2}^{1},...) .$
Dies wird auch als Optimierung (Maximierung) des Faktorgraphen 20 bezeichnet. Unter der Annahme von normalverteiltem Rauschen und Unsicherheiten entspricht diese Optimierung dem Lösen eines nichtlinearen Gleichungssystems über die Methode der kleinsten Quadrate (Nonlinear Least Squares) mit bekannten Verfahren.
Anschließend wird durch Rücksprung zu Schritt S1 der Faktorgraph 20 weiter fortentwickelt.
Weiterhin können dynamische Objekte verschiedenen Klassen zugeordnet werden. Die Klassen können basierend auf den Messungen, z. B. durch Form, Umrisserkennung oder dergleichen, oder durch Klassifizierung ihrer Bewegungsmuster zugeordnet werden. Es können somit identische Bewegungsmodelle von dynamischen Objekten, die der gleichen Klasse zugeordnet sind, verwendet werden, so dass die Vorgabe der Bewegungsmodelle für die dynamischen Objekte 4 vereinfacht ist. Diese Bewegungsmodelle können aus Domänenwissen trainierbaren Modellen oder Schätzungen entsprechen.

Claims

Computer-implementiertes Verfahren zur Posenbestimmung eines mobilen Agenten (1) in einer Umgebung mit dynamischen Objekten (4) mit einem Faktorgraphen, wobei der Faktorgraph in aufeinanderfolgenden Zeitschritten aktualisiert und ausgewertet wird, wobei in mindestens zwei aufeinanderfolgenden Zeitschritten folgende Schritte ausgeführt werden: - Bereitstellen oder Erweitern des Faktorgraphen in einem aktuellen Zeitschritt, indem für eine absolute Poseninformation über eine aktuelle Pose des mobilen Agenten (1) ein Agenten-Knoten (21) eingefügt wird und für eine absolute Poseninformation über eine aktuelle Pose eines dynamischen Objekts (4) ein Dynamisches-Objekt-Knoten (24) eingefügt wird, wobei mindestens zwei Dynamisches-Objekt-Knoten (24) mit einer Kante verbunden sind, denen ein Bewegungsmodell-Faktorknoten (23) zugeordnet ist, wobei der Bewegungsmodell-Faktorknoten (23) jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell ergibt; - Ermitteln der Verbundverteilung in dem Faktorgraph (20); - Auswerten (S9) des Faktorgraphen mithilfe der Verbundverteilung, um die Pose des mobilen Agenten (1) in dem aktuellen Zeitschritt zu ermitteln.
Verfahren nach Anspruch 1, wobei der Faktorgraph (20) über die letzte Pose oder letzte Posen aus dem erstellten Faktorgraph (20) inferiert oder die Pose mit der höchsten Wahrscheinlichkeit ermittelt wird.
Verfahren nach Anspruch 1 oder 2, wobei die absolute und/oder die relative Poseninformation einer Posenangabe und einer zugehörigen probabilistischen Verteilung angeben, die die Zuverlässigkeit der zugrundeliegenden Posenangabe angibt.
Verfahren nach einem der Ansprüche 1 bis 3, wobei das probabilistische Bewegungsmodell einem generischen Bewegungsmodell, einem Bewegungsmodell basierend auf Domänenwissen oder einem aus Daten trainierten Bewegungsmodell entspricht.
Verfahren nach einem der Ansprüche 1 bis 4, wobei in jedem Zeitschritt mithilfe einer Umgebungssensorik das dynamische Objekt (4) erfasst wird und deren relative Pose zu dem mobilen Agenten (1) ermittelt wird, wobei das probabilistische Bewegungsmodell dem dynamischen Objekt (4) zugeordnet wird, indem das dynamische Objekt (4), insbesondere durch ein Mustererkennungsverfahren, klassifiziert wird und das Bewegungsmodell entsprechend der erkannten Klasse ausgewählt wird.
Verfahren nach einem der Ansprüche 1 bis 5, wobei ein Agenten-Knoten (21) und ein Dynamisches-Objekt-Knoten (24) mit einer Kante verbunden sind, denen ein Relativposen-Faktorknoten zugeordnet ist, wobei der Relativposen-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einer Beobachtung ergibt.
Verfahren nach einem der Ansprüche 1 bis 6, wobei mindestens zwei Agenten-Knoten (21) mit einer Kante verbunden sind, denen ein Agentenbewegungsmodell-Faktorknoten (23) zugeordnet ist, wobei der Agentenbewegungsmodell-Faktorknoten (23) jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell für den mobilen Agenten (1) ergibt.
Vorrichtung, insbesondere Steuereinheit (12), zur Posenbestimmung eines mobilen Agenten (1) in einer Umgebung mit dynamischen Objekten (4) mit einem Faktorgraphen (20), wobei der Faktorgraph (20) in aufeinanderfolgenden Zeitschritten aktualisiert und ausgewertet wird, wobei die Vorrichtung ausgebildet ist, um in mindestens zwei aufeinanderfolgenden Zeitschritten: - den Faktorgraphen (20) in einem aktuellen Zeitschritt bereitzustellen oder zu erweitern, indem für eine absolute Poseninformation über eine aktuelle Pose des mobilen Agenten (1) ein Agenten-Knoten (21) eingefügt wird und für eine absolute Poseninformation über eine aktuelle Pose eines dynamischen Objekts (4) ein Dynamisches-Objekt-Knoten (24) eingefügt wird, wobei mindestens zwei Dynamisches-Objekt-Knoten (24) mit einer Kante verbunden sind, denen ein Bewegungsmodell-Faktorknoten zugeordnet ist, wobei der Bewegungsmodell-Faktorknoten jeweils eine relative Poseninformation zugeordnet ist, die sich aus einem vorgegebenen probabilistischen Bewegungsmodell ergibt; - eine Verbundverteilung in dem Faktorgraph (20) zu ermitteln; - den Faktorgraphen (20) mithilfe der Verbundverteilung auszuwerten, um die Pose des mobilen Agenten (1) in dem aktuellen Zeitschritt zu ermitteln.
Mobiler Agent mit einer Bewegungsaktuatorik (13), mit einer Umgebungssensorik (11) zum Erfassen von statischen (3) und dynamischen Objekten (4) in der Umgebung und mit einer Vorrichtung (12) nach Anspruch 8.
Computerprogramm mit Programmcodemitteln, das dazu eingerichtet ist, ein Verfahren nach einem der Ansprüche 1 bis 7 auszuführen, wenn das Computerprogramm auf einer Recheneinheit ausgeführt wird.
Maschinenlesbares Speichermedium mit einem darauf gespeicherten Computerprogramm nach Anspruch 10.