DE102018220581A1

DE102018220581A1 - Verfahren und Vorrichtung zum Prüfen einer Trajektorie für ein Fahrzeug

Info

Publication number: DE102018220581A1
Application number: DE102018220581.7A
Authority: DE
Inventors: Maxim Dolgov; Holger Andreas Banzhaf
Original assignee: Robert Bosch GmbH
Current assignee: Robert Bosch GmbH
Priority date: 2018-11-29
Filing date: 2018-11-29
Publication date: 2020-06-04

Abstract

Die vorliegende Erfindung bezieht sich auf ein Verfahren zum Prüfen einer Trajektorie (104) für ein Fahrzeug (100), das dadurch gekennzeichnet ist, dass zumindest ein Trajektorienpunkt (112) der Trajektorie (104) unter Verwendung einer Hüllkurve (114) auf Kollisionen (110) überprüft wird, um eine Sicherheit der Trajektorie (104) zu erkennen, wobei die Hüllkurve (114) einen möglichen Platzbedarf (118) des Fahrzeugs (100) an dem Trajektorienpunkt (112) umschließt und unter Verwendung einer erwarteten Posenungenauigkeit (120) des Fahrzeugs (100) und einer zu erreichenden Kollisionsunwahrscheinlichkeit (122) an dem Trajektorienpunkt (112) für den Trajektorienpunkt (112) erzeugt wird.

Description

Gebiet der Erfindung
Die Erfindung betrifft ein Verfahren und eine Vorrichtung zum Prüfen einer Trajektorie für ein Fahrzeug.
Stand der Technik
Um eine geplante Trajektorie für ein Fahrzeug auf Kollisionen zu überprüfen, kann entlang der Trajektorie ein virtueller Fahrschlauch erzeugt werden, der so breit ist, wie das Fahrzeug. Zusätzlich kann rechts und links ein vordefinierter Sicherheitsabstand eingehalten werden, damit das Fahrzeug auch bei einer geringen Abweichung von der Trajektorie kollisionsfrei entlang der Trajektorie geführt werden kann. Wenn der Fahrschlauch ein Objekt berührt, kann eine Kollision erkannt werden.
Offenbarung der Erfindung
Vor diesem Hintergrund werden mit dem hier vorgestellten Ansatz ein Verfahren zum Prüfen einer Trajektorie für ein Fahrzeug, eine entsprechende Vorrichtung, sowie schließlich ein entsprechendes Computerprogrammprodukt und ein maschinenlesbares Speichermedium gemäß den unabhängigen Ansprüchen vorgestellt. Vorteilhafte Weiterbildungen und Verbesserungen des hier vorgestellten Ansatzes ergeben sich aus der Beschreibung und sind in den abhängigen Ansprüchen beschrieben.
Vorteile der Erfindung
Ausführungsformen der vorliegenden Erfindung können in vorteilhafter Weise ermöglichen, Positionsungenauigkeiten und/oder Orientierungsungenauigkeiten beziehungsweise Posenungenauigkeiten bei einer Überprüfung einer Trajektorie zu kompensieren. Zusätzlich kann je nach Anforderung an eine Kollisionswahrscheinlichkeit beziehungsweise Kollisionsunwahrscheinlichkeit der Trajektorie ein Raumbedarf der Trajektorie verringert oder vergrößert werden. Weiterhin kann ein Berechnungsaufwand zum Überprüfen auf Kollisionen verringert werden.
Es wird ein Verfahren zum Prüfen einer Trajektorie für ein Fahrzeug vorgeschlagen, das dadurch gekennzeichnet ist, dass zumindest ein Trajektorienpunkt der Trajektorie unter Verwendung einer Hüllkurve auf Kollisionen überprüft wird, um eine Sicherheit der Trajektorie zu erkennen, wobei die Hüllkurve einen möglichen Platzbedarf des Fahrzeugs an dem Trajektorienpunkt umschließt und unter Verwendung einer erwarteten Posenungenauigkeit des Fahrzeugs und einer zu erreichenden Kollisionsunwahrscheinlichkeit an dem Trajektorienpunkt für den Trajektorienpunkt erzeugt wird.
Ideen zu Ausführungsformen der vorliegenden Erfindung können unter anderem als auf den nachfolgend beschriebenen Gedanken und Erkenntnissen beruhend angesehen werden.
Eine Trajektorie wird hier insbesondere zweidimensional betrachtet, kann jedoch auch dreidimensional betrachtet werden. Aus der Trajektorie kann eine Vielzahl von hintereinander auf der Trajektorie angeordneten Trajektorienpunkten abgeleitet werden. Ein Trajektorienpunkt kann durch Koordinaten beschrieben werden. Bei der zweidimensionalen Betrachtungsweise kann der Trajektorienpunkt durch ein Koordinatenpaar und einen Winkel der Trajektorie in dem Koordinatenpunkt beschrieben werden.
Eine Hüllkurve kann mehrere überlappende Flächen umschließen und kann aus verschiedenen Abschnitten von Begrenzungslinien der Flächen zusammengesetzt sein.
Eine Kollision kann erkannt werden, wenn die Hüllkurve eine Begrenzungslinie eines Objekts berührt oder schneidet.
Ein Platzbedarf eines Fahrzeugs ist mindestens so groß, wie eine Projektion des Fahrzeugs auf einen Untergrund unter dem Fahrzeug. Der Platzbedarf kann durch Unsicherheiten und Toleranzen bei einer Positionsbestimmung des Fahrzeugs, der Trajektorie und der Objekte größer sein, als die Projektion.
Eine erwartete Posenungenauigkeit bildet erwartete Unsicherheiten und Toleranzen bei einer Positionsbestimmung und Orientierungsbestimmung des Fahrzeugs ab. Eine Kollisionsunwahrscheinlichkeit bildet eine Wahrscheinlichkeit ab, wie sicher das Fahrzeug den Trajektorienpunkt kollisionsfrei passieren kann. Die Kollisionsunwahrscheinlichkeit kann kleiner als 100 Prozent sein.
Zum Repräsentieren des Platzbedarfs kann beim Erzeugen der Hüllkurve in einer Bezugsebene ein Polygon mit zumindest drei Eckpunkten um den Trajektorienpunkt aufgespannt werden. An den Eckpunkten kann je zumindest eine im Wesentlichen an der Trajektorie ausgerichtete Kontur des Fahrzeugs in die Bezugsebene projiziert werden. Die Hüllkurve kann die in die Bezugsebene projizierten Konturen umschließen. Eine Größe des Polygons definiert einen Versatz der Konturen. Ein Mittelpunkt des Polygons kann an dem Trajektorienpunkt ausgerichtet sein. Das Polygon kann verzerrt sein. Das Polygon kann regelmäßige Abstände zwischen den Eckpunkten aufweisen. Durch eine Anzahl der Eckpunkte ist eine Anzahl der zu projizierenden Konturen begrenzt. Dadurch kann ein Berechnungsaufwand zum Erzeugen der Hüllkurve begrenzt werden.
An den Eckpunkten können je zumindest zwei Konturen des Fahrzeugs mit einem Winkelversatz zueinander in die Bezugsebene projiziert werden. Eine Winkelhalbierende des Winkelversatzes kann einem Winkel der Trajektorie in dem Trajektorienpunkt entsprechen. Durch den Winkelversatz kann eine Orientierungsungenauigkeit des Fahrzeugs in der Hüllkurve abgebildet werden, ohne den Berechnungsaufwand maßgeblich zu vergrößern.
Zumindest eine Abmessung des Polygons kann durch die Posenungenauigkeit und/oder die Kollisionsunwahrscheinlichkeit bestimmt werden. Das Polygon kann umso größer sein, je größer die Posenungenauigkeit und/oder die Kollisionsunwahrscheinlichkeit sind.
Das Polygon kann mit einer Projektionsfläche in der Bezugsebene aufgespannt werden, die größer ist, als eine Fläche um den Trajektorienpunkt, die einen der Kollisionsunwahrscheinlichkeit entsprechenden Anteil von aufgrund der Posenungenauigkeit um den Trajektorienpunkt verteilten möglichen Positionen des Fahrzeugs enthält. Die Positionen können mit einer Wahrscheinlichkeitsverteilung um den Trajektorienpunkt verteilt sein, wobei eine Dichte der Positionen mit zunehmendem Abstand von dem Trajektorienpunkt abnimmt. Die Fläche, die den Anteil an Trajektorienpunkten einschließt, der der Kollisionsunwahrscheinlichkeit entspricht, kann so groß sein, dass nur ein der Kollisionswahrscheinlichkeit entsprechender Anteil der möglichen Positionen außerhalb der Fläche liegt. Die Fläche kann beispielsweise kreisförmig oder ellipsenförmig sein. Die Eckpunkte des Polygons können so weit außerhalb der Fläche liegen, dass die Projektionsfläche des Polygons größer als die Fläche ist. Durch die größere Projektionsfläche liegt ein geringerer Anteil der Positionen außerhalb der Projektionsfläche, als außerhalb der Fläche.
Als Polygon kann ein Pentagon aufgespannt werden. Ein Pentagon weist fünf Eckpunkte auf und nähert eine Kreisform sehr gut an. Die geringe Anzahl der Eckpunkte im Verhältnis zu einer unvermeidbaren Abweichung des Polygons von der Kreisform ermöglicht einen geringen Rechenaufwand für die Hüllkurve bei einer hohen Genauigkeit der Kollisionsbetrachtung.
Die Hüllkurve kann konvex ausgeformt werden. Bei einer konvexen Hüllkurve liegen alle Mittelpunkte von Biegeradien der Hüllkurve innerhalb der Hüllkurve. Eine Sicherheit bei der Kollisionsbetrachtung kann gesteigert werden, da durch die konvexe Hüllkurve eine von der Hüllkurve eingeschlossene Fläche größer ist, als bei einer basierend auf den gleichen Stützpunkten geradlinig oder konkave ausgeformten Hüllkurve.
Das Verfahren kann beispielsweise in Software oder Hardware oder in einer Mischform aus Software und Hardware beispielsweise in einem Steuergerät implementiert sein.
Der hier vorgestellte Ansatz schafft ferner eine Vorrichtung, die dazu ausgebildet ist, um die Schritte einer Variante des hier vorgestellten Verfahrens in entsprechenden Einrichtungen durchzuführen, anzusteuern bzw. umzusetzen.
Die Vorrichtung kann ein elektrisches Gerät mit zumindest einer Recheneinheit zum Verarbeiten von Signalen oder Daten, zumindest einer Speichereinheit zum Speichern von Signalen oder Daten, und zumindest einer Schnittstelle und/oder einer Kommunikationsschnittstelle zum Einlesen oder Ausgeben von Daten, die in ein Kommunikationsprotokoll eingebettet sind, sein. Die Recheneinheit kann beispielsweise ein Signalprozessor, ein sogenannter System-ASIC oder ein Mikrocontroller zum Verarbeiten von Sensorsignalen und Ausgeben von Datensignalen in Abhängigkeit von den Sensorsignalen sein. Die Speichereinheit kann beispielsweise ein Flash-Speicher, ein EPROM oder eine magnetische Speichereinheit sein. Die Schnittstelle kann als Sensorschnittstelle zum Einlesen der Sensorsignale von einem Sensor und/oder als Aktorschnittstelle zum Ausgeben der Datensignale und/oder Steuersignale an einen Aktor ausgebildet sein. Die Kommunikationsschnittstelle kann dazu ausgebildet sein, die Daten drahtlos und/oder leitungsgebunden einzulesen oder auszugeben. Die Schnittstellen können auch Softwaremodule sein, die beispielsweise auf einem Mikrocontroller neben anderen Softwaremodulen vorhanden sind.
Von Vorteil ist auch ein Computerprogrammprodukt oder Computerprogramm mit Programmcode, der auf einem maschinenlesbaren Träger oder Speichermedium wie einem Halbleiterspeicher, einem Festplattenspeicher oder einem optischen Speicher gespeichert sein kann und zur Durchführung, Umsetzung und/oder Ansteuerung der Schritte des Verfahrens nach einer der vorstehend beschriebenen Ausführungsformen verwendet wird, insbesondere wenn das Programmprodukt oder Programm auf einem Computer oder einer Vorrichtung ausgeführt wird.
Es wird darauf hingewiesen, dass einige der möglichen Merkmale und Vorteile der Erfindung hierin mit Bezug auf unterschiedliche Ausführungsformen beschrieben sind. Ein Fachmann erkennt, dass die Merkmale der Vorrichtung und des Verfahrens in geeigneter Weise kombiniert, angepasst oder ausgetauscht werden können, um zu weiteren Ausführungsformen der Erfindung zu gelangen.
Figurenliste
Nachfolgend werden Ausführungsformen der Erfindung unter Bezugnahme auf die beigefügten Zeichnungen beschrieben, wobei weder die Zeichnungen noch die Beschreibung als die Erfindung einschränkend auszulegen sind.

1 zeigt eine Darstellung eines Fahrzeugs mit einer Vorrichtung gemäß einem Ausführungsbeispiel;
2 zeigt eine Darstellung eines Merkmalsraums für eine Zustandsschätzung;
3 zeigt eine Darstellung einer Vielzahl von Fahrzeugkonturen basierend auf einer Zustandsschätzung;
4 zeigt eine Darstellung eines um einen Trajektorienpunkt aufgespannten Polygons gemäß einem Ausführungsbeispiel;
5 zeigt eine Darstellung von zwei mit einem Winkelversatz an einem Eckpunkt eines Polygons angeordneten Konturen gemäß einem Ausführungsbeispiel; und
6 zeigt eine Darstellung einer Hüllkurve um einen Trajektorienpunkt gemäß einem Ausführungsbeispiel.

Die Figuren sind lediglich schematisch und nicht maßstabsgetreu. Gleiche Bezugszeichen bezeichnen in den Figuren gleiche oder gleichwirkende Merkmale.
Ausführungsformen der Erfindung
1 zeigt eine Darstellung eines Fahrzeugs 100 mit einer Vorrichtung 102 zum Prüfen einer Trajektorie 104 für das Fahrzeug 100 gemäß einem Ausführungsbeispiel. Die Trajektorie 104 ist für das Fahrzeug 100 vorgeplant worden. Die Trajektorie 104 kann beispielsweise von einem Navigationssystem 106 des Fahrzeugs 100 berechnet und als eine Trajektorieninformation 108 bereitgestellt werden. Die Trajektorie 104 ist entsprechend einer Oberfläche eines Untergrunds im Wesentlichen zweidimensional. Die Trajektorie 104 ist hier auf einen Mittelpunkt einer Hinterachse des Fahrzeugs 100 bezogen. Koordinaten der Trajektorie 104 können also auf einen Koordinatenursprung bezogen sein, der im Mittelpunkt der Hinterachse liegt. Der Koordinatenursprung kann jedoch auch an einem beliebigen anderen Ort des Fahrzeugs 100 oder einem beliebigen anderen Bezugspunkt der Umgebung liegen.
Die Vorrichtung 102 ist dazu ausgebildet, die Trajektorie 104 auf Kollisionen 110 zu überprüfen. Dazu wird zumindest ein Trajektorienpunkt 112 der Trajektorie 104 ausgewählt und unter Verwendung einer Hüllkurve 114 überprüft, ob eine Kollision 110 mit einem Objekt 116 oder mehreren Objekten 116 vorliegt. Die Überprüfung kann auch für viele Trajektorienpunkte 112 parallel oder als Abfolge einer Vielzahl von Überprüfungen nacheinander erfolgen.
Die Hüllkurve 114 zu dem Trajektorienpunkt 112 umschließt dabei einem möglichen Platzbedarf 118 des Fahrzeugs 100 an dem Trajektorienpunkt 112. Der Platzbedarf 118 ist dabei abhängig von einer Posenungenauigkeit 120 des Fahrzeugs 100 an dem Trajektorienpunkt 112 und einer gewünschten Kollisionsunwahrscheinlichkeit 122 an dem Trajektorienpunkt 112. Die Posenungenauigkeit 120 wird der Vorrichtung 102 als Posenungenauigkeitsinformation 124 beispielsweise von dem Navigationsgerät 106 bereitgestellt. Die Kollisionsunwahrscheinlichkeit 122 kann voreingestellt sein oder auch situationsabhängig variabel sein.
Mit anderen Worten wird durch den hier vorgestellten Ansatz eine robuste Kollisionsüberprüfung für die Trajektorienplanung in SE(2) vorgestellt.
Um für Fahrzeuge und Roboter (nachfolgend als „Agenten“ bezeichnet) Trajektorien für sichere, also kollisionsfreie, Manöver planen zu können, ist es notwendig, die i.d.R. probabilistisch quantifizierte Unsicherheit der Zustandsschätzung entlang der geplanten Trajektorie zu berücksichtigen. Eine Trajektorie beschreibt dabei den Verlauf des Zustands eines Systems (z.B. Position mit Orientierung und Längsgeschwindigkeit) über die Zeit und kann entweder parametrisch beispielsweise in Form eines Polynoms oder nichtparametrisch z.B. als eine Menge von Punkten in der Raumzeit angegeben werden. Die vorgenannten Unsicherheiten beinhalten unter anderem Fehler bei der Lokalisierung und der Ableitung anderer Systemgrößen wie z.B. der Längsgeschwindigkeit aus Sensormessungen. Da im probabilistischen Formalismus für viele Darstellungsarten der Zustandsschätzungsunsicherheit eine Kollision immer möglich ist, kann die Kollisionswahrscheinlichkeit als ein Maß für die Sicherheit einer Trajektorie verwendet werden.
Außer in einigen sehr eingeschränkten Fällen, kann die Kollisionswahrscheinlichkeit nicht analytisch bestimmt werden. Alternativ können aufwändige numerische Berechnungen durchgeführt werden. Eine vorteilhafte Alternative sind approximative Verfahren. Beispielsweise kann die Zustandsschätzung des Agenten, die in Form einer multivariaten Gauß'schen Verteilung vorliegt, durch einen Ellipsoid approximiert werden, der die für die Einhaltung der geforderten Schranke an die Kollisionswahrscheinlichkeit notwendige Wahrscheinlichkeitsmasse der Zustandsverteilung einschließt. Anschließend wird die Planung robust durchgeführt, was bedeutet, dass keines solcher Ellipsoide entlang der geplanten Trajektorie eine Kollision verursacht.
Ein alternativer Ansatz zur Auswertung der Kollisionswahrscheinlichkeit besteht darin, Samples (Stichproben) aus den Verteilungen, die die Zustandsschätzungen entlang der geplanten Trajektorien darstellen, zu ziehen. Die Trajektorie kann dann so geplant werden, dass die Anzahl der Samples, die eine Kollision aufweisen, relativ zur Gesamtzahl der Samples kleiner ist, als das geforderte Maß an die Kollisionsfreiheit der Trajektorie. Die mindestens notwendige Anzahl an zufällig gezogenen Samples kann hergeleitet werden.
Für die Trajektorienplanung für Fahrzeuge und Roboter kann berücksichtigt werden, dass der Agent sowie statische und dynamische Hindernisse, mit denen er kollidieren kann, eine geometrische Ausprägung haben. Bei der Trajektorienplanung unter Berücksichtigung der Kollisionswahrscheinlichkeit, wie sie für allgemeine punktförmige Systeme definiert ist, kann die Geometrie von Agenten und Hindernissen ebenfalls berücksichtigt werden, indem diese als ein Teil des Zustands betrachtet wird. Dadurch, dass die Geometrie bekannt ist, bewegt sich der punktförmige Zustand anschließend auf einer Mannigfaltigkeit. Eine effizientere Überprüfung auf Kollision kann z.B. dadurch erreicht werden, dass auf die Samples aus den Zustandsschätzungen, wobei für Fahrzeuge oft nur die Position und die Orientierung betrachtet werden, die Kontur des Agenten entsprechend der durch das jeweilige Sample vorgegebenen Konfiguration aufgeprägt wird, und diese Konturen auf Kollision geprüft werden. Eine Überprüfung auf Kollision kann ebenfalls in 3D erfolgen.
Eine Methode für die Evaluation der Kollisionswahrscheinlichkeit ist, die für die Einhaltung der Kollisionswahrscheinlichkeit notwendige Wahrscheinlichkeitsmasse in einem Polyeder, am besten einem platonischen, einzuschließen. Anschließend kann die Fahrzeugkontur an die Ecken des Polyeders entsprechend der Konfiguration an diesen Stellen transformiert werden und auf Kollision geprüft werden. Die Verbundmenge der transformierten Fahrzeugkonturen wird als Beliefprint bezeichnet. Bei dem hier vorgestellten Ansatz wird eine robuste Approximation dieser Methode vorgeschlagen, d.h. der wahre Beliefprint wird stets konservativ eingeschlossen.
Hier wird ein Verfahren für die Evaluation der Kollisionswahrscheinlichkeit bei der Trajektorienplanung vorgeschlagen, welche Unsicherheiten der Zustandsschätzung berücksichtigt. Betrachtet wird die Planung in SE(2), also für Position des Agenten in der Ebene sowie seine Orientierung. Das vorgeschlagene Verfahren kann eingesetzt werden, um Trajektorien zu überprüfen, die mit diversen Algorithmen berechnet wurden. Die Algorithmen können nominelle Trajektorien ausgeben, also Trajektorien, die keine Unsicherheit der Zustandsschätzung angeben.
Der hier vorgestellte Ansatz ist für die Trajektorienplanung für Fahrzeuge im realen Einsatz und Roboter geeignet, da keine Reformulierung des Planungsproblems als ein Problem für ein punktförmiges System erforderlich ist. Weiterhin wird nur eine geringe Anzahl an Samples verwendet, um die Kollisionswahrscheinlichkeit mit einer vorgegebenen Konfidenz korrekt einzuhalten
Bei dem hier vorgestellten Ansatz wird eine alternative Methode für die Überprüfung auf Kollision verwendet und in einer robusten Approximation ausgeführt, die die Auswertung der Kollisionswahrscheinlichkeit zusätzlich beschleunigt. Hierzu wird die Konditionierung der Orientierung in den jeweiligen Samples, die sich in diesem Fall aus den Ecken eines Polyeders ergeben, auf die Position durchgeführt, um eine robuste Approximation für die Orientierung zur Einhaltung der Einschränkung an die Kollisionswahrscheinlichkeit zu bestimmen.
Bei der Überprüfung auf Kollision wird angenommen, dass die Zustandsschätzung entlang einer Nominaltrajektorie in Form von Gauß'schen Verteilungen vorliegt. Um nun zu überprüfen, ob für eine dieser Verteilungen eine Kollision vorliegt, werden diese durch Polyeder, am besten durch platonische Polyeder wie das Ikosaeder, approximiert. Dazu wird zunächst für die vorgegebene Kollisionswahrscheinlichkeit, d.h. mit der Wahrscheinlichkeit liegt keine Kollision vor, ein Polyeder bestimmt, das die Wahrscheinlichkeitsmasse einer multivariaten Standardnormalverteilung einschließt. Diese Berechnung wird offline durchgeführt.
Danach wird dieser Polyeder auf die Form der tatsächlich vorliegenden Gauß'schen Zustandsschätzung transformiert. Anschließend wird die Fahrzeugkontur (Projektion des Fahrzeugs auf die Bodenebene) auf die Ecken des Polyeders entsprechend ihrer Konfiguration (Position und Orientierung) transformiert. Die konvexe Hülle der transformierten Fahrzeugkonturen wird als Beliefprint bezeichnet.
Schließlich wird geprüft, ob alle der so transformierten Fahrzeugkonturen kollisionsfrei sind. Ist das nicht der Fall, so wurde die geforderte Schranke an die Kollisionswahrscheinlichkeit nicht eingehalten.
In 2 ist erkennbar, dass viele der abgebildeten Fahrzeugkonturen nicht zum Beliefprint beitragen, da sie innen liegen. Ihre Berechnung kann dank dem hier beschriebenen Verfahren entfallen, sodass die numerische Auswertung der Kollisionswahrscheinlichkeit weiter beschleunigt werden kann.
Für die nachfolgende Beschreibung wird angenommen, dass der Zustand des Agenten wie folgt ist x = [x y ϕ]^T , wobei [x y] die Position des Agenten ist und ϕ seine Orientierung. Damit kann die Gauß'sche Zustandsschätzung des Agenten bestimmt durch den Mittelwert und Kovarianz geschrieben werden als $μ = [\begin{matrix} μ_{x} \\ μ_{ϕ} \end{matrix}], C = [\begin{matrix} C_{x} & C_{x ϕ} \\ C_{x ϕ}^{T} & C_{ϕ} \end{matrix}]$
Mit µ_x = E{[x y]^T}, µ_ϕ = E{ϕ}, C_x = Cov{[x y]}, C_xϕ = Cov{[x y], ϕ}, C_ϕ = Cov{ϕ}
Offline wird zunächst ein zweidimensionales Standard-Polyeder mit dem Innenradius r_2D so festgelegt, dass es die Wahrscheinlichkeitsmasse 1 - β der Standardnormalverteilung einschließt. Nachfolgend wird exemplarisch ein Ikosaeder verwendet. Prinzipiell werden platonische Körper oder Fullerene aufgrund ihrer Symmetrie empfohlen. $\int_{Ω} p ([\begin{matrix} x & y \end{matrix}]) d [\begin{matrix} x & y \end{matrix}] = 1 - β$
Die Konstante β entspricht der erlaubten 2D (Positions-) Kollisionswahrscheinlichkeit (z.B. mit der 3*Sigma Regel: 1 - β = 98.8%
Alternativ kann der Radius des zweidimensionalen Standard-Polyeders so gewählt werden, dass sein Innenradius dem Innenradius des dreidimensionalen Standard-Polyeders, welcher die Wahrscheinlichkeitsmasse 1 - α einschließt, entspricht.
Um den Beliefprint nun auf Kollision zu prüfen, wird der zweidimensionale Standardpolyeder auf die aktuelle Zustandsschätzung transformiert. Es wird der Mittelwert und die Kovarianz der Orientierung bedingt auf eine beliebige Position [x y] berechnet gemäß $μ_{ϕ | x} = μ_{ϕ} + C_{x ϕ}^{T} C_{x}^{- 1} (x - μ_{x})$
$C_{ϕ | x} = C_{ϕ} - C_{x ϕ}^{T} C_{x}^{- 1} C_{x ϕ}$
Nun wird die Schranke δ bestimmt, sodass gilt µ_ϕ|x - δ < ϕ < µ_ϕ|x + δ . Dazu wird folgende Gleichung erfüllt, $\int_{x} p ([\begin{matrix} x & y \end{matrix}], ϕ) d x = \iint_{Ω, Φ} p (ϕ | [\begin{matrix} x & y \end{matrix}]) p ([\begin{matrix} x & y \end{matrix}]) d ϕ d [\begin{matrix} x & y \end{matrix}] = 1 - α$
wobei der Wert α die erlaubte 3D Kollisionswahrscheinlichkeit angibt (z.B. mit der 3*Sigma Regel: 1 - α = 97,1%). Das Integral kann nun nach δ aufgelöst werden gemäß $\begin{array}{l} \int_{Ω} p ([\begin{matrix} x & y \end{matrix}]) \int_{μ_{ϕ | x^{- δ}}}^{μ_{ϕ | x^{+ δ}}} N (ϕ, μ_{ϕ | x, C_{ϕ | x}}) d ϕ d [\begin{matrix} x & y \end{matrix}] \\ = \int_{Ω} p ([\begin{matrix} x & y \end{matrix}]) \int_{- δ}^{+ δ} N (ϕ,0, C_{ϕ | x}) d ϕ d [\begin{matrix} x & y \end{matrix}] = e r f (\frac{δ}{\sqrt{2} C_{ϕ | x}}) (1 - β) \end{array}$
Nun wird δ so bestimmt, dass das Produkt aus den beiden Ergebnissen zu 1 - α wird.
Durch Iteration über alle Ecken des transformierten 2D-Polyeders wird die auf die Position bedingte minimale und maximale Ausrichtung µ_ϕ|x,min und µ_ϕ|x,max bestimmt.
Die Fahrzeugkontur wird nun auf µ_ϕ|x,min - δ und µ_ϕ|x,max + δ transformiert und alle Rotationen dazwischen mittels eines Polygons oder einer Oriented Bounding Box konservativ approximiert.
Das so erhaltene Polygon (Umrandung in 3), das alle möglichen Rotationen der Fahrzeugkontur an der gegebenen Zustandsschätzung einschließt, wird nun an alle Ecken des transformierten 2D-Polyeders verschoben und eine konvexe Hülle berechnet.
Die konvexe Hülle der Fahrzeugkonturen wird auf Kollision geprüft und soll für die Einhaltung der probabilistischen Schranke 1 - α kollisionsfrei sein.
Falls die Zustandsschätzungen nicht Gauß-förmig sind, kann das vorgeschlagene Verfahren ebenso verwendet werden, wenn das erste und das zweite zentrale Moment der vorliegenden Zustandsschätzungen berechnet werden kann. Dies entspricht der Approximation der Zustandsschätzung mit einer Gauß.
2 zeigt eine Darstellung eines Merkmalsraums 200 für eine Zustandsschätzung 202. Der Merkmalsraums 200 ist dreidimensional und durch eine Koordinatenabweichung in x und y sowie eine Winkelabweichung ϑ aufgespannt. Die Zustandsschätzung 202 ist als dreidimensionale Punktewolke in dem Merkmalsraum 200 abgebildet. Ein Mittelpunkt der Zustandsschätzung 202 liegt im Koordinatenursprung x0, y0, ϑ0 des Merkmalsraums 200. Der Koordinatenursprung x0, y0, ϑ0 ist dabei der Trajektorienpunkt 112.
Die Zustandsschätzung 202 folgt in allen Dimensionen des Merkmalsraums 200 einer Wahrscheinlichkeitsverteilung, beispielsweise einer Gauß-Verteilung. Eine Ausdehnung der Zustandsschätzung 202 entspricht der Posenungenauigkeit.
In dem Merkmalsraum 200 ist ferner ein Ikosaeder 204 dargestellt, in dem Ikosaeder 204 ist ein der Kollisionsunwahrscheinlichkeit entsprechender Anteil der Punktewolke eingeschlossen.
Mit anderen Worten sind in 2 ein auf die Form einer Zustandsschätzung 202 transformierter Ikosaeder 204 (Gerüst) und zufällig aus der Zustandsschätzung 202 gezogene Samples (Punkte) dargestellt. Es wird die geforderte Anzahl an Samples eingeschlossen.
3 zeigt eine Darstellung einer Vielzahl von Fahrzeugkonturen 300 basierend auf einer Zustandsschätzung. Die Fahrzeugkonturen 300 sind entsprechend der durch das Ikosaeder eingeschlossenen Punkte der Zustandsschätzung in x und y angeordnet und in ϑ ausgerichtet. Alle dargestellten Fahrzeugkonturen 300 sind durch eine Hüllkurve 114 umschlossen. Die Hüllkurve 114 ist hier konvex und weist keine Einbuchtungen auf.
Mit anderen Worten zeigt 3 den gemäß dem Algorithmus entstandenen Beliefprint. Die konvexe Hülle (schwarzer Rahmen) approximiert die Konfidenzanforderungen an die Kollisionswahrscheinlichkeit.
4 zeigt eine Darstellung eines um einen Trajektorienpunkt 112 aufgespannten Polygons 400 gemäß einem Ausführungsbeispiel. Das Polygon 400 ist in einem zweidimensionalen Merkmalsraum 200 aus den Koordinaten x und y dargestellt. Das Polygon 400 umschließt entsprechend dem dreidimensionalen Ikosaeder in 2 zumindest den Anteil der Punktewolke der Zustandsschätzung 202, die der geforderten Kollisionsunwahrscheinlichkeit entspricht. Dabei ist hier der umschlossene Anteil etwas größer, als ein durch eine Ellipse 402 gekennzeichneter exakter Anteil.
An Eckpunkten 404 des Polygons 400 werden bei dem hier vorgestellten Ansatz Fahrzeugkonturen wie in 3 in die xy-Ebene projiziert und die Hüllkurve berechnet. Die Fahrzeugkonturen sind dabei im Wesentlichen so ausgerichtet, wie die Trajektorie im Trajektorienpunkt 112.
In einem Ausführungsbeispiel werden an jedem Eckpunkt 404 zwei Fahrzeugkonturen in die xy-Ebene projiziert. Die beiden Fahrzeugkonturen eines Eckpunkts 404 werden dabei jeweils um einen entgegengesetzten Winkelbetrag 406 zu einem Winkel 408 der Trajektorie gedreht projiziert.
Das Polygon 400 ist mit anderen Worten eine Transformation eines 2D Standard-Polyeders, der die Unsicherheit des Agenten in der Position einschließt auf die Gauß'sche Verteilung, die diese Unsicherheit beschreibt.
5 zeigt eine Darstellung von zwei mit einem Winkelversatz 500 an einem Eckpunkt 404 eines Polygons angeordneten Fahrzeugkonturen 300 gemäß einem Ausführungsbeispiel. Der Eckpunkt 404 entspricht dabei einem der Eckpunkte in 4. Der Winkelversatz 500 entspricht zweimal dem Winkelbetrag 406.
Mit anderen Worten zeigt 5 eine Approximation der rotierten Fahrzeugkonturen 300 zwischen µ_ϕ|x,min - δ und µ_ϕ|x,max + δ durch ein weiteres Polygon 502.
6 zeigt eine Darstellung einer Hüllkurve 114 um einen Trajektorienpunkt 112 gemäß einem Ausführungsbeispiel. Die Hüllkurve 114 ist sehr ähnlich zu dem weiteren Polygon in 5. Die Hüllkurve 114 umschließt für jeden Eckpunkt des Polygons aus 4 ein Polygon wie in 5.
In einem Ausführungsbeispiel ist die Hüllkurve 600 konvex ausgeführt. Alle Seitenkanten der Hüllkurve 600 sind von dem Trajektorienpunkt 112 geringfügig weg gebogen. Dadurch weist die Hüllkurve 600 keine Einbuchtungen auf.
Mit anderen Worten zeigt 6 eine Faltung des in 5 erhaltenen Polygons mit dem 2D Standard-Polyeder zum robusten Beliefprint.
Für weiterführende Informationen kann folgende Literatur herangezogen werden.
D. van Hessem and O. Bosgra, „A Conic Reformulation of Model Predictive Control Including Bounded and Stochastic Disturbances Under State and Input Constraints," in Proceedings of the 41st IEEE Conference on Decision and Control, vol. 4, 2002, pp. 4643-4648.
G. C. Calafiore and M. C. Campi, „The Scenario Approach to Robust Control Design," IEEE Transactions on Automatic Control, vol. 51, no. 5, pp. 742-753, 2006.
A. Nemirovski, „On Safe Tractable Approximations of Chance Constraints," European Journal of Operational Research, vol. 219, no. 3, pp. 707-718, 2012.
D. Lyons, J.-P. Calliess, and U. D. Hanebeck, „Chance Constrained Model Predictive Control for Multi-Agent Systems with Coupling Constraints," in Proceedings of the 2012 American Control Conference (ACC 2012), Montreal, Canada, Jun. 2012.
M. Dolgov, G. Kurz, and U. D. Hanebeck, „Chance-constrained Model Predictive Control based on Box Approximations," in Proceedings of the 54th IEEE Conference on Decision and Control (CDC 2015), Osaka, Japan, Dec. 2015.
A. Lambert, D. Gruyer and G. Saint Pierre, „A fast Monte Carlo algorithm for collision probability estimation," 2008 10th International Conference on Control, Automation, Robotics and Vision, Hanoi, 2008, pp. 406-411.
A. Lee et al., „Sigma hulls for Gaussian belief space planning for imprecise articulated robots amid obstacles," 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, 2013, pp. 5660-5667.
S. J. Julier and J. K. Uhlmann, „Unscented Filtering and Nonlinear Estimation". Proceedings of the IEEE, 92(3):401-422, Mar 2004.
M. Werling, J. Ziegler, S. Kammel and S. Thrun, „Optimal trajectory generation for dynamic street scenarios in a Frenet Frame," 2010 IEEE International Conference on Robotics and Automation, Anchorage, AK, 2010, pp. 987-993.
Steven M. Lavalle, „Rapidly-Exploring Random Trees: A New Tool for Path Planning, " 1998.
A. Bry and N. Roy, „Rapidly-exploring random belief trees for motion planning under uncertainty," IEEE International Conference on Robotics and Automation, 2011.
W. Li and E. Todorov. Iterative Linearization Methods for Approximately Optimal Control and Estimation of Non-linear Stochastic System. International Journal of Control, 80(9):1439-1453, 2007.
J. van den Berg, S. Patil, and R. Alterovitz. Motion Planning under Uncertainty using Differential Dynamic Programming in Belief Space. In Proceedings of the 15th International Symposium on Robotics Research (ISRR 2011), 2011.
Abschließend ist darauf hinzuweisen, dass Begriffe wie „aufweisend“, „umfassend“, etc. keine anderen Elemente oder Schritte ausschließen und Begriffe wie „eine“ oder „ein“ keine Vielzahl ausschließen. Bezugszeichen in den Ansprüchen sind nicht als Einschränkung anzusehen.
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 Nicht-Patentliteratur

D. van Hessem and O. Bosgra, „A Conic Reformulation of Model Predictive Control Including Bounded and Stochastic Disturbances Under State and Input Constraints,“ in Proceedings of the 41st IEEE Conference on Decision and Control, vol. 4, 2002, pp. 4643-4648 [0069]
G. C. Calafiore and M. C. Campi, „The Scenario Approach to Robust Control Design,“ IEEE Transactions on Automatic Control, vol. 51, no. 5, pp. 742-753, 2006 [0070]
A. Nemirovski, „On Safe Tractable Approximations of Chance Constraints,“ European Journal of Operational Research, vol. 219, no. 3, pp. 707-718, 2012 [0071]
D. Lyons, J.-P. Calliess, and U. D. Hanebeck, „Chance Constrained Model Predictive Control for Multi-Agent Systems with Coupling Constraints,“ in Proceedings of the 2012 American Control Conference (ACC 2012), Montreal, Canada, Jun. 2012 [0072]
M. Dolgov, G. Kurz, and U. D. Hanebeck, „Chance-constrained Model Predictive Control based on Box Approximations,“ in Proceedings of the 54th IEEE Conference on Decision and Control (CDC 2015), Osaka, Japan, Dec. 2015 [0073]
A. Lambert, D. Gruyer and G. Saint Pierre, „A fast Monte Carlo algorithm for collision probability estimation,“ 2008 10th International Conference on Control, Automation, Robotics and Vision, Hanoi, 2008, pp. 406-411 [0074]
A. Lee et al., „Sigma hulls for Gaussian belief space planning for imprecise articulated robots amid obstacles,“ 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, 2013, pp. 5660-5667 [0075]
S. J. Julier and J. K. Uhlmann, „Unscented Filtering and Nonlinear Estimation“. Proceedings of the IEEE, 92(3):401-422, Mar 2004 [0076]
M. Werling, J. Ziegler, S. Kammel and S. Thrun, „Optimal trajectory generation for dynamic street scenarios in a Frenet Frame,“ 2010 IEEE International Conference on Robotics and Automation, Anchorage, AK, 2010, pp. 987-993 [0077]
Steven M. Lavalle, „Rapidly-Exploring Random Trees: A New Tool for Path Planning, “ 1998 [0078]
A. Bry and N. Roy, „Rapidly-exploring random belief trees for motion planning under uncertainty,“ IEEE International Conference on Robotics and Automation, 2011 [0079]
W. Li and E. Todorov. Iterative Linearization Methods for Approximately Optimal Control and Estimation of Non-linear Stochastic System. International Journal of Control, 80(9):1439-1453, 2007 [0080]
J. van den Berg, S. Patil, and R. Alterovitz. Motion Planning under Uncertainty using Differential Dynamic Programming in Belief Space. In Proceedings of the 15th International Symposium on Robotics Research (ISRR 2011), 2011 [0081]

Claims

Verfahren zum Prüfen einer Trajektorie (104) für ein Fahrzeug (100), dadurch gekennzeichnet, dass zumindest ein Trajektorienpunkt (112) der Trajektorie (104) unter Verwendung einer Hüllkurve (114) auf Kollisionen (110) überprüft wird, um eine Sicherheit der Trajektorie (104) zu erkennen, wobei die Hüllkurve (114) einen möglichen Platzbedarf (118) des Fahrzeugs (100) an dem Trajektorienpunkt (112) umschließt und unter Verwendung einer erwarteten Posenungenauigkeit (120) des Fahrzeugs (100) und einer zu erreichenden Kollisionsunwahrscheinlichkeit (122) an dem Trajektorienpunkt (112) für den Trajektorienpunkt (112) erzeugt wird.
Verfahren gemäß Anspruch 1, bei dem zum Repräsentieren des Platzbedarfs (118) beim Erzeugen der Hüllkurve (114) in einer Bezugsebene ein Polygon (400) mit zumindest drei Eckpunkten (404) um den Trajektorienpunkt (112) aufgespannt wird, wobei an den Eckpunkten (404) je zumindest eine im Wesentlichen an der Trajektorie (104) ausgerichtete Kontur (300) des Fahrzeugs (100) in die Bezugsebene projiziert wird und die Hüllkurve (114) die in die Bezugsebene projizierten Konturen (300) umschließt.
Verfahren gemäß Anspruch 2, bei dem an den Eckpunkten (404) je zumindest zwei Konturen (300) des Fahrzeugs (100) mit einem Winkelversatz (500) zueinander in die Bezugsebene projiziert werden, wobei eine Winkelhalbierende (408) des Winkelversatzes (500) einem Winkel der Trajektorie (104) in dem Trajektorienpunkt (112) entspricht.
Verfahren gemäß einem der Ansprüche 2 bis 3, bei dem zumindest eine Abmessung des Polygons (400) durch die Posenungenauigkeit (120) und/oder die Kollisionsunwahrscheinlichkeit (122) bestimmt wird.
Verfahren gemäß Anspruch 4, bei dem das Polygon (400) mit einer Projektionsfläche in der Bezugsebene aufgespannt wird, die größer ist, als eine Fläche um den Trajektorienpunkt (112), die einen der Kollisionsunwahrscheinlichkeit (122) entsprechenden Anteil von aufgrund der Posenungenauigkeit (120) um den Trajektorienpunkt (112) verteilten möglichen Positionen (202) des Fahrzeugs (100) enthält.
Verfahren gemäß einem der Ansprüche 2 bis 5, bei dem als Polygon (400) ein Pentagon aufgespannt wird.
Verfahren gemäß einem der vorhergehenden Ansprüche, bei dem die Hüllkurve (114) konvex ausgeformt wird.
Vorrichtung (102) zum Prüfen einer Trajektorie (104) für ein Fahrzeug (100), wobei die Vorrichtung (102) dazu ausgebildet ist, das Verfahren gemäß einem der vorhergehenden Ansprüche in entsprechenden Einrichtungen auszuführen, umzusetzen und/oder anzusteuern.
Computerprogrammprodukt, das dazu eingerichtet ist, das Verfahren gemäß einem der Ansprüche 1 bis 7 auszuführen, umzusetzen und/oder anzusteuern.
Maschinenlesbares Speichermedium, auf dem das Computerprogrammprodukt gemäß Anspruch 9 gespeichert ist.