Verfahren zur Planung einer Soll-Trajektorie
Die Erfindung betrifft ein Verfahren zur Planung einer Soll-Trajektorie gemäß dem Oberbegriff des Patentanspruchs 1.
Aus der DE 102015208790 A1 sind ein Verfahren und ein System zum automatischen Bestimmen einer Trajektorie für ein Fahrzeug bekannt. Mit der Trajektorie wird ein Startpunkt, der der aktuellen Position des Fahrzeuges entspricht, mit einem Zielpunkt verbunden. Bei dem Verfahren werden mehrere Zwischenpunkte bestimmt, wobei zudem mindestens eine erste Teiltrajektorie bestimmt wird, welche den Startpunkt mit einem der Zwischenpunkte verbindet. Weiterhin werden mehrere zweite Teiltrajektorien bestimmt, welche den Zielpunkt mit jeweils einem der Zwischenpunkte verbinden. Darüber hinaus wird die Trajektorie bestimmt, indem eine der mindestens einen ersten Teiltrajektorie und eine der zweiten Teiltrajektorien gewählt werden, und mindestens eine Komponente des Fahrzeuges wird auf Basis der bestimmten Trajektorie angesteuert, wobei an jedem Zwischenpunkt mindestens zwei Teiltrajektorien enden.
Darüber hinaus beschreibt die WO 2019/223909 ein Verfahren zum zumindest teilautomatisierten Steuern eines Kraftfahrzeuges. Das Verfahren umfasst ein Empfangen von Umgebungssignalen, die eine mittels einer Umfeldsensorik des Kraftfahrzeuges erfasste Umgebung des Kraftfahrzeuges repräsentieren. Bei Detektion eines sich bezogen auf eine Fahrtrichtung des Kraftfahrzeuges vor dem Kraftfahrzeug befindlichen Objektes basierend auf den empfangenen Umgebungssignalen. Weiterhin sieht das Verfahren vor, dass ermittelt wird, ob innerhalb einer Überholtrajektorie zum Überholen des Objektes ein Straßenknotenpunkt liegt und ob für eine Dauer des Überholens ein Gegenverkehr des Kraftfahrzeuges blockiert wird. Wenn das Ermitteln ergeben hat, dass innerhalb einer Überholtrajektorie zum Überholen des Objektes kein Straßenknotenpunkt liegt und dass für eine Dauer des Überholens kein Gegenverkehr blockiert wird, dann
werden Steuersignale zum zumindest teilautomatisierten Steuern einer Quer- und Längsführung des Kraftfahrzeuges, basierend auf der Überholtrajektorie, ausgegeben.
Der Erfindung liegt die Aufgabe zugrunde, ein verbessertes Verfahren zur Planung einer Soll-Trajektorie, die von dem Fahrzeug automatisiert abgefahren werden soll, anzugeben.
Die Aufgabe wird erfindungsgemäß durch ein Verfahren gelöst, welches die in Anspruch 1 angegebenen Merkmale aufweist.
Vorteilhafte Ausgestaltungen der Erfindung sind Gegenstand der Unteransprüche.
Ein Verfahren zur Planung einer Soll-Trajektorie, die von einem Fahrzeug automatisiert abgefahren werden soll, sieht vor, dass eine diskrete Menge von Trajektorien als Kandidaten für die Soll-Trajektorie bestimmt werden, wobei jede dieser Trajektorien aus mehreren aneinandergereihten Trajektorienabschnitten zusammengesetzt ist. Das Verfahren sieht weiterhin vor, dass die Planung auf einer Auswahl einer der Trajektorien als Soll-Trajektorie beruht, wobei die Auswahl auf einer Bewertung der Trajektorien mit vorgegebenen Kostenfunktionen und einer Identifizierung derjenigen Trajektorie basiert, die am kostengünstigsten bewertet worden ist. Erfindungsgemäß wird jedem Trajektorienabschnitt eine Schar von Teiltrajektorien mit jeweils gleichen Ortsvorgaben und jeweils unterschiedlichen Dynamikvorgaben zugeordnet. Unter Ortsvorgaben sind hierbei Vorgaben zum Ortsverlauf zu verstehen, dem das Fahrzeug beim Abfahren des jeweiligen Trajektorienabschnitts folgen soll, und unter Dynamikvorgaben sind Vorgaben zur Dynamik des Fahrzeugs, insbesondere Vorgaben zur Beschleunigung und/oder Geschwindigkeit zu verstehen, mit der sich das Fahrzeug beim Abfahren des jeweiligen Trajektorienabschnitts bewegen soll. Bei Feststellung einer Änderung von einzuhaltenden Randbedingungen und/oder von auszuführenden Fahraufgaben wird eine Vorsteuerung der Auswahl vorgenommen, indem die Kostenfunktionen für die einzelnen Teiltrajektorien an die geänderten Randbedingungen und/oder Fahraufgaben angepasst werden, um Teiltrajektorien, die sich zur Einhaltung der geänderten Randbedingungen und/oder zur Ausführung der geänderten Fahraufgaben besser eignen als andere Teiltrajektorien, geringere Kosten zuzuweisen.
Durch Anwendung des Verfahrens können unterschiedliche Fahraufgaben des automatisiert fahrenden Fahrzeuges realisiert werden, wobei weitestgehend sichergestellt werden kann, dass die Fahraufgaben nur dann ausgeführt werden, wenn keine Verletzung sicherheitskritischer Kriterien vorliegt.
Die Fahraufgaben umfassen insbesondere ein Bilden einer Rettungsgasse, eine präventive Verringerung einer Fahrgeschwindigkeit des Fahrzeuges in bestimmten Fahrsituationen, ein Wechseln einer Fahrspur wegen bestimmter Fahrzeuge, wie z. B. der Polizei und/oder eines Rettungsdienstes, ein Abstellen des Fahrzeuges auf einem Standstreifen und/oder eine Berücksichtigung einer Degradation eines Lenk- oder Bremssystems des Fahrzeuges.
Mittels des Verfahrens wird ermöglicht, unterschiedliche Fahraufgaben eines automatisiert fahrenden Fahrzeuges durch gezielte Vorsteuerung einer Planung der Soll-Trajektorie in Echtzeit zu realisieren. Besteht das Risiko, dass Sicherheitsgrenzen durch Ausführen einer Fahraufgabe verletzt werden, kann die Planung der Soll-Trajektorie eine Vorgabe überstimmen und eine sicherere Soll-Trajektorie bereitstellen.
In einer Ausführung des Verfahrens umfasst jede als Kandidat bestimmte Trajektorie und mithin auch die aus der Menge dieser Kandidaten ausgewählte Soll-Trajektorie als Datensatz sowohl Informationen über einen Ortsverlauf, dem das Fahrzeug beim Abfahren der jeweiligen Trajektorie folgen soll, als auch weitere Informationen über eine Dynamik, insbesondere über eine Beschleunigung und/oder Fahrgeschwindigkeit, mit der sich das Fahrzeug beim Abfahren der jeweiligen Trajektorie bewegen soll. Mittels der aus der Menge der Trajektorien ausgewählten Soll-Trajektorie wird also nicht nur bestimmt, entlang welcher Ortskoordinaten das Fahrzeug im automatisierten Fährbetrieb fahren soll, sondern es wird zudem vorgegeben, wie dynamisch sich das Fahrzeug bewegen soll, d.h. zu welchen Zeitpunkten sich das Fahrzeug an der jeweiligen Ortskoordinate befinden soll. Das Verfahren ermöglicht somit das Auffinden eines optimalen Ortsverlaufs für die automatisierte Fahrzeugführung und gleichzeitig auch das Auffinden einer optimalen Dynamik des Fahrzeugs.
Darüber hinaus wird in einerweiteren Ausführungsform die Menge der Trajektorien, aus denen die Soll-Trajektorie ausgewählt wird, diskretisiert, indem in einem vorgebbaren Vorausschauhorizont eine vorbestimmte Menge von Trajektorienstützpunkten als mögliche Aufenthaltsorte des Fahrzeugs bestimmt wird, indem aus der Menge der Trajektorienstützpunkte mehrere in Fahrtrichtung verlaufende Punktereihen ausgewählt werden und indem die Trajektorien derart bestimmt werden, dass sie jeweils durch eine der Punktereihen verlaufen. Mit anderen Worten: die Trajektorienstützpunkte stellen Positionen innerhalb des Vorauschauhorizonts dar, durch die jeweils eine oder mehrere der Trajektorien geführt werden. Jede der Trajektorien wird demnach durch eine
vorgegebene Menge von Trajektorienstützpunkten geführt, wobei die Abschnitte zwischen zwei Trajektorienstützpunkten die erwähnten Trajektorienabschnitte bilden, denen jeweils die erwähnte Schar von Teiltrajektorien zugeordnet ist. Die einzelnen Trajektorien setzen sich somit aus Teiltrajektorien zusammen, die jeweils an einem der Trajektorienstützpunkte miteinander verbunden sind. Aufgrund der begrenzten Anzahl von Teiltrajektorien ist auch die Anzahl der aus diesen zusammengesetzten Trajektorien begrenzt. Die Menge der Trajektorien wird nachfolgend als Trajektorienschar bezeichnet. Da die Planung der Soll-Trajektorie auf die Auswahl einer Trajektorie aus Trajektorienschar zurückgeführt wird, kann die Soll-Trajektorie mit einem geringem Rechenaufwand geplant werden.
Für jede Trajektorie der Trajektorienschar werden in einer möglichen Weiterbildung mit den vorgegebenen Kostenfunktion Kosten ermittelt, wobei die Kostenfunktionen für die einzelnen Trajektorienabschnitte und die diesen jeweils zugeordneten Teiltrajektorien in Abhängigkeit der einzuhaltenden Randbedingungen bzw. der auszuführenden Fahraufgaben definiert werden. Die Kostenfunktionen berücksichtigen also Randbedingungen, wie z. B. dass die auszuwählende Soll-Trajektorie eine Fahrbahn des automatisiert fahrenden Fahrzeuges nicht verlassen darf und dass die Soll-Trajektorie physikalisch für das automatisiert fahrende Fahrzeug realisierbar ist.
In einer möglichen Weiterbildung des Verfahrens, werden für die verschiedenen T eiltrajektorien der einzelnen T rajektorienabschnitte jeweils eigene, für die verschiedenen Randbedingungen bzw. Fahraufgaben definierte Kostenfunktionen vorgegeben. Dabei geben die Kostenfunktionen an, wie gut eine jeweilige Randbedingung bzw. Fahraufgabe in Bezug auf die Soll-Trajektorie erfüllbar ist. Eine vergleichsweise gute Erfüllung wird mit geringen Kosten belohnt und eine vergleichsweise schlechte Erfüllung wird mit hohen Kosten bestraft.
Vorteilhafterweise werden für jeden der Trajektorienabschnitte Gesamtkosten ermittelt, indem die Kosten, die den Teiltrajektorien des jeweiligen Trajektorienabschnitts zugeordnet worden sind, gewichtet summiert werden.
Vorteilhafterweise werden die Kosten einer Trajektorie durch Summation der Gesamtkosten Ihrer Trajektorienabschnitte ermittelt.
Um die verhältnismäßig beste Soll-Trajektorie zu bestimmen, werden in einerweiteren möglichen Ausführung mittels einer gewichteten Summation der für die verschiedenen
Randbedingungen bestimmten Kosten eines Trajektorienabschnitts Gesamtkosten eines T rajektorienabschnitts ermittelt.
Darauffolgend werden in einer möglichen Weiterbildung Kosten einer Trajektorie mittels Summation der Gesamtkosten ihrer Trajektorienabschnitte ermittelt, wobei die Trajektorie aus der Trajektorienschar, welche die geringsten Kosten unter Berücksichtigung der Randbedingungen und/oder Fahraufgaben aufweist als Soll-Trajektorie ausgewählt wird, die das automatisiert fahrende Fahrzeug abfährt. .
Des Weiteren sieht das Verfahren vor, dass die Kostenfunktionen mittels einer Vorsteuerung modifiziert werden, wobei mittels der Vorsteuerung die Trajektorienplanung an eine aktuelle Fahraufgabe angepasst wird und bei mehreren Fahraufgaben eine Priorisierung vorgenommen wird. Die Vorsteuerung verfolgt das Ziel, die Planung der Trajektorien, insbesondere die Auswahl der Soll-Trajektorie, an eine aktuelle Fahraufgabe einschließlich der geforderten Randbedingungen anzupassen und bei mehreren Fahraufgaben die Priorisierung vorzunehmen. Die Auswahl der Soll-Trajektorie erfolgt somit unter Berücksichtigung der aktuellen Fahraufgabe oder gegebenenfalls mehrerer miteinander vereinbarer zu berücksichtigender Fahraufgaben.
Ausführungsbeispiele der Erfindung werden im Folgenden anhand von Zeichnungen näher erläutert.
Dabei zeigen:
Fig. 1 schematisch einen ersten Schritt zur Bestimmung einer Trajektorienschar,
Fig. 2 schematisch einen zweiten Schritt zur Bestimmung einer Trajektorienschar,
Fig. 3 schematisch einen dritten Schritt zur Bestimmung einer Trajektorienschar,
Fig. 4 schematisch eine Kostenfunktion,
Fig. 5 schematisch eine Modifikation einer Kostenfunktion durch Vorsteuerung und
Fig. 6 schematisch eine Modifikation einer weiteren Kostenfunktion durch
Vorsteuerung.
Einander entsprechende Teile sind in allen Figuren mit den gleichen Bezugszeichen versehen.
In Figur 1 ist ein erster Schritt zur Bestimmung einer Trajektorienschar, aus welcher eine u. a. in Figur 2 gezeigte Trajektorie T als Soll-Trajektorie Tsoii, die in Figur 3 näher gezeigt ist, ausgewählt wird, dargestellt.
Ein Fahrzeug 1 verfügt über ein Assistenzsystem zum automatisierten Fährbetrieb, wobei mittels einer entsprechenden Sensorik im automatisierten Fährbetrieb fortlaufend Signale erfasst werden.
Im automatisierten Fährbetrieb eines Fahrzeuges 1 ist es erforderlich, dass das Fahrzeug 1 sich in vielfältigen Fahrsituationen adäquat verhält und unterschiedliche Fahraufgaben erfüllt. Diese Fahraufgaben umfassen in einem normalen automatisierten Fährbetrieb, z. B. ein Abstandhalten in einer Fahrspurmitte, ein Einhalten einer Geschwindigkeitsvorgabe, wobei unter speziellen Fahraufgaben, beispielsweise ein Bilden einer in Figur 5 gezeigten Rettungsgasse R oder einen Fahrspurwechsel, bis hin zur Vermeidung einer Kollision, beispielsweise aufgrund eines plötzlich erfassten Hindernisses auf einer Fahrspur F des Fahrzeuges 1, zu verstehen sind.
Zur Erfassung der vielfältigen Fahrsituationen umfasst die Sensorik eine Vielzahl von im und/oder am Fahrzeug 1 angeordneten Sensoren, die gegebenenfalls fusioniert werden, um beispielsweise erfasste Signale zu plausibilisieren und/oder um einen Erfassungsbereich zu erweitern oder zu optimieren.
Um eine Vielzahl an möglichen Fahrsituationen im automatisierten Fährbetrieb des Fahrzeuges 1 beherrschen zu können, wird üblicherweise ein Trajektorienplanungs- Modell verwendet, das entweder aus einer Vielzahl an möglichen Trajektorien T eine beste Trajektorie T auswählt oder anhand eines Optimierungsverfahrens eine optimale T rajektorie T berechnet.
Diesen beiden Ansätzen liegt eine Bewertung von Trajektorien T mittels, insbesondere in den Figuren 4 bis 6 gezeigten, Kostenfunktionen K zugrunde, wobei sich die Kosten aus unterschiedlichen Teilkosten mit unterschiedlicher Gewichtung zusammensetzen.
Beispiele für einzelne Kostenfunktionen K sind:
- Abweichungen von einem gewünschten Pfad,
- eine hohe Fahrzeugdynamik in Längs- und Querrichtung des Fahrzeuges 1,
- Unterschreiten eines Sicherheitsabstandes,
- eine Kollision mit einem in Figur 3 näher gezeigten Hindernis 2,
- ein Nichteinhalten von Geschwindigkeitsvorgaben, etc.
Um einen vergleichsweise sicheren automatisierten Fährbetrieb des Fahrzeuges 1 sicherzustellen, werden die sicherheitskritischen Kosten höher gewichtet als Kosten, die durch ein unkomfortables Fahren entstehen.
Bei einer Auswahl von Trajektorien T ist es erforderlich, zusätzlich sogenannte harte Randbedingungen einzuhalten, wobei die Trajektorie T eine Fahrbahn nicht verlassen darf und die Trajektorie T physikalisch realisierbar sein muss.
Um die Vielzahl an unterschiedlichen speziellen Fahraufgaben zu beherrschen, ist ein im Folgenden beschriebenes Verfahren vorgesehen, wobei eine Trajektorienplanung gesteuert wird, indem Sollzustände und Stellgrößenbereiche des Fahrzeuges 1 verändert werden.
Vorgaben einer Vorsteuerung werden dann durch die Trajektorienplanung geplant, wenn keine höheren, grundlegenden Ziele, wie z. B. ein Unterschreiten eines Sicherheitsabstandes bis zur Kollision, ein unbeabsichtigtes Verlassen der Fahrspur F, eine zu hohe Fahrzeugreaktion bis hin zu einer Nichtfahrbarkeit, dabei verletzt werden.
Steht beispielsweise durch ein plötzlich vor dem Fahrzeug 1 auftauchendes Hindernis 2 eine Kollision des Fahrzeuges 1 mit dem Hindernis 2 bevor, hat eine Vermeidung der Kollision gegenüber einer Fahraufgabe Vorrang. Wenn eine solche kritische Situation nicht mehr besteht, wird die gewünschte Fahraufgabe erneut bevorzugt.
Das Verfahren sieht eine kontinuierliche Vorgabe an die Trajektorienplanung durch eine maximal zulässige Fahrgeschwindigkeit VEGO, auch in Zusammenhang mit einer vorgegebenen Entfernung und/oder Zeit, einen gewünschten Versatz des Fahrzeuges 1 zur Spurmitte seiner Fahrspur F, ebenfalls in Zusammenhang mit der vorgegebenen Entfernung und/oder Zeit, eine absetzbare Verzögerung, eine zulässige Beschleunigung und eine absetzbare Lenkdynamik vor.
Zudem umfasst das Verfahren eine Priorisierung von möglicherweise unvereinbaren Fahraufgaben. Beispielsweise kann ein System zum automatisierten Fährbetrieb des Fahrzeuges 1 ein sicheres Abstellen des Fahrzeuges 1 anfordern, wobei gleichzeitig eine sogenannte Move-Over-Law-Situation vorliegt, die eine unterschiedliche Geschwindigkeitsverringerung erfordern.
Auch kann ein Verhalten in einer Move-Over-Law-Situation, d. h. bei Gelten einer Ausweichvorschrift, z. B. bei Annäherung eines Einsatzfahrzeuges, beispielsweise der Polizei, einen anderen Versatz innerhalb der Fahrspur F des Fahrzeuges 1 erfordern als zur Bildung einer Rettungsgasse R erforderlich ist.
In beiden Fällen erfolgt durch eine Wahl einer Fahrgeschwindigkeits- und/oder Versatzvorgabe in Bezug auf eine Positionierung des Fahrzeuges 1 innerhalb seiner Fahrspur F eine Priorisierung der Fahraufgabe.
Darüber hinaus sieht das Verfahren eine Anforderung spezieller Fahraufgaben vor, die Vorgaben der Kostenfunktionen K bei der Trajektorienplanung zu verändern, wie z. B.:
- ein sicheres Abstellen des Fahrzeuges 1 auf einem beispielhaft in Figur 4 gezeigten Standstreifen S,
- ein Fahrspurwechsel,
- eine präventive Reduktion einer Fahrgeschwindigkeit VEGO in einer unklaren Fahrsituation, z. B. bei einem Geisterfahrer auf einer Nebenspur oder einem Fußgänger auf der Fahrbahn,
- Reduktion der momentanen Fahrgeschwindigkeit VEGO und gleichzeitiges Fahren an einen Rand der Fahrspur F, wenn eine Move-Over-Law- Situation vorliegt, etc.
Komplexere Fahraufgaben, wie das sichere Abstellen des Fahrzeuges 1 auf dem Standstreifen S oder ein mehrfacher Fahrspurwechsel, wird an die Trajektorienplanung durch eine zeitliche Abfolge von Fahrgeschwindigkeits- und Versatzvorgaben, sogenannten Spuroffsetvorgaben, übermittelt.
Wird ein degradierter Zustand eines Brems- oder Lenksystems gemeldet, wird eine Anpassung einer absetzbaren Brems- oder Lenkdynamik an die Trajektorienplanung vorgenommen.
Zudem wird eine absetzbare Verzögerung des Fahrzeuges 1, d. h. eine Verringerung der momentanen Fahrgeschwindigkeit VEGO an vorherrschende Wetterverhältnisse angepasst.
Insbesondere sieht das Verfahren vor, dass eine in Figur 3 beispielhaft dargestellte Soll- Trajektorie Tsoii, die von dem Fahrzeug 1 im automatisierten Fährbetrieb, insbesondere fahrerlos, abgefahren werden soll.
Unter einer solchen Soll-Trajektorie Tsoii ist ein Datensatz zu verstehen, der sowohl Informationen über einen Ortsverlauf, also Ortskoordinaten, enthält, dem das Fahrzeug 1 beim Abfahren der Soll-Trajektorie Tsoii folgen soll, als auch Informationen über eine Beschleunigung oder die Fahrgeschwindigkeit VEGO umfasst, mit der sich das Fahrzeug 1 beim Abfahren der Soll-Trajektorie Tsoii bewegen soll. Durch die Soll-Trajektorie Tsoii wird somit nicht nur vorgegeben, welche Ortskoordinaten das Fahrzeug 1 abfahren soll, sondern auch, zu welchen Zeitpunkten sich das Fahrzeug 1 an den jeweiligen Ortskoordinaten befindet.
Die Planung beruht dabei auf einer Bestimmung einer diskreten Menge an Kandidaten für die Soll-Trajektorie Tsoii, wobei die Auswahl wie oben beschrieben und aus dem Stand der Technik bekannt ist, auf Kostenfunktionen K basiert.
Im Detail zeigt die Figur 1 ein Koordinatensystem, wobei auf einer Abszisse x- Koordinaten x, bis X4 in Fahrzeuglängsrichtung, d. h. in Fahrtrichtung des automatisiert fahrenden Fahrzeuges 1 , und auf der Ordinate y y-Koordinaten y.i bis yi abgetragen sind und eine Fahrzeugquerrichtung bezeichnen. Ein Dc, also ein Abstand zwischen zwei x- Koordinaten beschreibt eine Funktion einer momentanen Fahrgeschwindigkeit VEGO des Fahrzeuges 1.
Zudem ist eine Mehrzahl von Trajektorienstützpunkten Po,o bis P4,2 dargestellt, wobei der Trajektorienstützpunkt Po,o einen Startpunkt des Fahrzeuges 1 und die Trajektorienstützpunkte P4,o bis P4,2 die Zielkoordinaten des Fahrzeuges 1 darstellen. Insbesondere sind die x-Koordinaten xo bis X4 und die y-Koordinaten y.i bis yi die x-y- Koordinaten der Trajektorienstützpunkte P = (x. , yj)
Die Trajektorienstützpunkte Po,o bis P4,2 sind in Richtung der Abszisse x, also in Fahrtrichtung des Fahrzeuges 1 über einen Vorausschauhorizont V verteilt. Dieser Vorausschauhorizont V definiert eine Strecke, die das Fahrzeug 1 mit seiner momentanen Fahrgeschwindigkeit VEGO innerhalb eines vorgegebenen Zeitintervalls, von beispielsweise
30 Sekunden, passieren wird, d. h. abfahren wird. Innerhalb des Vorausschauhorizontes V sind theoretisch unendlich viele Trajektorien möglich, aus denen eine Soll-Trajektorie Tsoii ausgewählt werden kann. Um einen Rechenaufwand bei der Auswahl der für das Fahrzeug 1 und eine Fahrsituation am besten geeignetsten Soll- Trajektorie Tsoii zu minimieren, wird die Menge der Trajektorien, aus denen die Soll- Trajektorie Tsoii ausgewählt werden soll, diskretisiert. Dazu werden in dem Vorausschauhorizont V, wie in Figur 1 gezeigt ist, eine vorbestimmte Menge von Trajektorienstützpunkte Po,o bis P4,2 bestimmt und eine Menge von Trajektorien T bestimmt, welche in Fahrtrichtung jeweils durch eine Reihe von Trajektorienstützpunkte Po,o bis P4,2 verlaufenden, wie in einem in Figur 2 gezeigten zweiten Schritt zur Bestimmung einer Trajektorienschar in einem weiteren Koordinatensystem dargestellt ist.
Diese Menge der Trajektorien T bildet eine Trajektorienschar, wobei die Trajektorien T die Kandidaten zur Auswahl der Soll-Trajektorie Tsoii bilden, d. h. nur diese begrenzte Anzahl von Trajektorien T werden für die Auswahl der Soll-Trajektorie Tsoii berücksichtigt.
Insbesondere werden einzelne Trajektorienstützpunkte Po,o bis P4,2, wie in Figur 2 gezeigt ist, in x-Richtung, also in Richtung der Fahrzeuglängsachse, gemäß ihrer Ordnung in Fahrtrichtung paarweise miteinander verbunden. Dadurch werden einzelne Trajektorien- abschnitte TR, TR(o,o)(i ,i) der Trajektorien T gebildet, die einer Menge zugeordnet sind, aus welcher eine in Figur 3 dargestellte Soll-Trajektorie Tsoii ausgewählt wird.
Jeder Trajektorienabschnitt TR, TR(o,o)(i ,i) umfasst selbst eine Schar nicht näher gezeigter Teiltrajektorien mit jeweils gleichen x-y-Verläufen aber unterschiedlichen Beschleunigungen und/oder Geschwindigkeiten. Den Teiltrajektorien werden Kosten zugeordnet, wobei die Kostenzuordnung mit für die verschiedenen Randbedingungen vorgegebenen Kostenfunktionen K erfolgt.
Bei einer Feststellung einer Änderung von einzuhaltenden Randbedingungen oder von auszuführenden Fahraufgaben wird die Vorsteuerung der Auswahl vorgenommen, indem die Kostenfunktionen K für einzelne Teiltrajektorien der einzelnen Trajektorienabschnitte TR, TR(o,o)(i ,i) an die geänderten Randbedingungen angepasst werden. Diese Anpassung erfolgt, um Teiltrajektorien und Trajektorienabschnitte TR, TR(o,o)(i ,i), die sich zur Einhaltung geänderter Randbedingungen und/oder zur Ausführung geänderter Fahraufgaben besser eignen als andere geringere Kosten zuzuweisen.
Für jede Trajektorie T aus der Trajektorienschar werden mittels vorgegebener Kostenfunktionen K Kosten ermittelt, wobei die Kostenfunktionen für die einzelnen Teiltrajektorien der einzelnen Trajektorienabschnitte TR(o,oxi,i) einer Trajektorie T und für vorgegebene Randbedingungen definiert werden und angeben, wie gut die jeweiligen Randbedingungen auf dem jeweiligen Trajektorienabschnitt TR(o,oxi,i) mit der jeweiligen Teiltrajektorie erfüllt sind.
Eine vergleichsweise gute Erfüllung wird mit geringen Kosten belohnt, wohingegen eine schlechte eine vergleichsweise schlechte Erfüllung mit hohen Kosten bestraft wird. Anhand einer, insbesondere gewichteten, Summation der für die verschiedenen Randbedingungen bestimmten Kosten einer Teiltrajektorie eines Trajektorienabschnitts TR(o,oxi,i) werden die Gesamtkosten eines Trajektorienabschnitts TR(o,oxi,i) ermittelt.
Die Kosten einer Trajektorie T der Trajektorienschar werden durch Summation der Gesamtkosten der Trajektorienabschnitte TR, TR(o,oxi,i) der jeweiligen Trajektorie T gebildet. Aus der Trajektorienschar wird dann die Trajektorie T als Soll-Trajektorie Tsoii ausgewählt, die die geringsten Kosten aufweist, wie in Figur 3 gezeigt ist.
Die aus der Trajektorienschar ausgewählte Soll-Trajektorie Tsoii zeigt einen Fahrweg des Fahrzeuges 1 aufgrund des Hindernisses 2 auf seiner Fahrspur F, welches im Vorausschauhorizont V erfasst wird.
Die Trajektorienabschnitte TR, die zu einer Kollision des Fahrzeuges 1 mit dem Hindernis 2 führen, werden durch Erhöhung ihrer Kosten sanktioniert. Dadurch ergibt sich für diese Trajektorienabschnitte TR zur Auswahl der Soll-Trajektorie Tsoii eine geringere Priorität als die übrigen Trajektorienabschnitte TR.
In den Figuren 4 bis 6 ist jeweils ein Beispiel für eine Kostenfunktion dargestellt.
Figur 4 zeigt eine Kostenfunktion K(y) für eine Querposition des Fahrzeuges 1 , wobei eine Fahrspur F des Fahrzeuges 1, eine linke Fahrspur F1, eine rechte Fahrspur F2, Spurmarkierungen M, ein jeweiliger Standstreifen S oder Seitenstreifen und Trajektorienstützpunkte Pj = (Xi,yj) dargestellt sind.
Ein Fahren des Fahrzeuges 1 in der Mitte seiner Fahrspur F ist mit geringeren Kosten verbunden, als wenn das Fahrzeug 1 nicht in der Mitte fährt. D. h., dass das Fahren in der Mitte seiner Fahrspur F mit geringeren Kosten belohnt wird.
Das Fahren des Fahrzeuges 1 auf den Spurmarkierungen M wird durch hohe Kosten sanktioniert und das Fahren in der Mitte der linken Fahrspur F1 oder der rechten Fahrspur F2 wird höher sanktioniert als das Fahren in der Mitte der Fahrspur F des Fahrzeuges 1 und weniger sanktioniert als das Fahren auf den Spurmarkierungen M. Das Befahren des Standstreifens S oder Seitenstreifens wird vergleichsweise stark sanktioniert und mit entsprechend hohen Kosten bestraft.
In Figur 5 sind zum einen ein Verlauf der in Figur 4 dargestellten Kostenfunktion K(y) und eine mittels der Vorsteuerung modifizierte Kostenfunktion K1(y) und deren Verlauf gezeigt.
Die Vorsteuerung verfolgt das Ziel, die Trajektorienplanung zur Auswahl der Soll- Trajektorie Tsoii an eine geforderte Fahraufgabe und geforderter Randbedingungen anzupassen und bei gegebenenfalls mehreren auszuführenden Fahraufgaben eine Priorisierung vorzunehmen. Die Auswahl der Soll-Trajektorie Tsoii erfolgt somit unter Berücksichtigung der momentanen Fahraufgabe oder gegebenenfalls mehrerer momentan zu berücksichtigender Fahraufgaben.
Gemäß dem in Figur 5 gezeigten Ausführungsbeispiel wird zur Bildung einer Rettungsgasse R das zur Spurmitte versetzte Fahren des Fahrzeuges 1 innerhalb seiner Fahrspur F durch geringere Kosten stärker belohnt als das Fahren in der Spurmitte der jeweiligen Fahrspur F, F1, F2.
Das Fahren auf der rechten Fahrspur F2 wird durch geringere Kosten stärker belohnt und das Fahren in der Rettungsgasse R wird durch höhere Kosten sanktioniert.
Ein weiteres Ausführungsbeispiel für eine Vorsteuerung ist in Figur 6 gezeigt, wobei die Kostenfunktion K(y) und eine modifizierte weitere Kostenfunktion K2(y) dargestellt sind.
Das Fahren auf der rechten Fahrspur F2 wird, beispielsweise aufgrund eines Unfalles auf der rechten Fahrspur F2, vergleichsweise hoch sanktioniert, wobei das Fahren auf der Fahrspur F des Fahrzeuges 1 ebenfalls sanktioniert wird, um im Einsatz befindliche Rettungskräfte am Unfallort nicht zu gefährden.
In analoger Weise können auch Kostenfunktionen K modifiziert werden, die für andere Fahraufgaben und/oder andere einzuhaltende Randbedingungen vorgegeben sind. Durch die Modifikation wird eine Vorsteuerung für die Trajektorienauswahl erzielt.
Eine Fahraufgabe für das Fahrzeug 1 kann beispielsweise fordern, dass ein zusätzlicher Versatz zur Spurmitte der entsprechenden Fahrspur F, F1, F2 eingehalten wird, z. B. zur Bildung einer Rettungsgasse R oder zur Erhöhung eines seitlichen Abstandes zu Objekten bestimmter Klassen, wie Lastkraftwagen, Tunnelwände, Brückenpfeiler, Leitmauern.
Weiterhin kann eine Fahraufgabe fordern, dass wie weiter oben beschrieben, eine bestimmte maximal zulässige Geschwindigkeit eingehalten wird, dass eine Längsdynamik, insbesondere eine absetzbare Verzögerung oder eine zulässige Beschleunigung, oder eine Querdynamik, insbesondere eine Lenkdynamik in Form einer Gierrate, einer Lenkwinkelgeschwindigkeit und/oder einer Querbeschleunigung, auf bestimmte Werte begrenzt wird, die situationsabhängig vorgegebenen werden kann, beispielsweise in Abhängigkeit von Witterungsverhältnissen, einer Fahrgeschwindigkeit VEGO, einer Straßenkrümmung und/oder einer Degradation eines Lenk- oder Bremssystems des Fahrzeuges 1.
Zudem kann als Fahraufgabe gefordert sein, das Fahrzeug 1 auf einem Standstreifen S abzustellen, z. B. bei Degradation des Lenk- oder Bremssystems, dass die Fahrgeschwindigkeit VEGO präventiv verringert wird, z. B. bei einem Unfall, bei einem Einsatz von Polizei, Rettungsdiensten, Fußgängern auf der Fahrbahn, Geisterfahrern auf entsprechend benachbarten Fahrspuren F, F1, F2.
Darüber hinaus kann als Fahraufgabe vorgegeben sein, dass bestimmte Fahrspuren F, F1, F2 gemieden werden, z. B. die Fahrspur F, F1, F2 neben an einem Einsatzort abgestellten Fahrzeugen der Polizei und Rettungsdienste, d. h. auch bei Vorliegen der sogenannten Move-Over-Law-Situation, und bei Fußgängern oder Geisterfahrern.
Ferner kann eine zu beachtende Fahraufgabe für das Fahrzeug 1 das Vornehmen eines Fahrspurwechsels sein, z. B. zur Meldung von Fahrspuren F, F1, F2, zum Umfahren von Hindernissen 2, zum Überholen vergleichsweise langsamer Verkehrsteilnehmer, zur Steuerung des Fahrzeuges 1 auf eine Abbiege- oder Ausfädelspur.