DE102015221626A1

DE102015221626A1 - Verfahren zur Ermittlung einer Fahrzeug-Trajektorie entlang einer Referenzkurve

Info

Publication number: DE102015221626A1
Application number: DE102015221626.8A
Authority: DE
Inventors: Benjamin Gutjahr; Moritz Werling
Original assignee: Bayerische Motoren Werke AG
Current assignee: Bayerische Motoren Werke AG
Priority date: 2015-11-04
Filing date: 2015-11-04
Publication date: 2017-05-04

Abstract

Es wird ein Verfahren zur Ermittlung einer Trajektorie zur Führung eines Fahrzeugs beschrieben. Das Verfahren umfasst das Ermitteln einer Führungs-Referenzkurve, entlang der das Fahrzeug geführt werden soll. Das Verfahren umfasst weiter das Ermitteln einer Linearisierungs-Referenzkurve, so dass die Linearisierungs-Referenzkurve höhere Stetigkeitsanforderungen erfüllt als die Führungs-Referenzkurve. Außerdem umfasst das Verfahren das Ermitteln von Anfangswerten für eine Mehrzahl von Zustandsgrößen des Fahrzeugs relativ zu der Linearisierungs-Referenzkurve. Desweiteren umfasst das Verfahren das Bestimmen einer Trajektorie auf Basis der Anfangswerte, auf Basis der Führungs-Referenzkurve und auf Basis eines Modells der Dynamik des Fahrzeugs. Dabei umfasst das Bestimmen der Trajektorie das Ermitteln einer Gütefunktion, die eine Abweichung der Trajektorie von der Führungs-Referenzkurve berücksichtigt.

Description

Die Erfindung betrifft ein Verfahren und eine entsprechende Vorrichtung zur Ermittlung einer Trajektorie für die Steuerung und/oder Regelung der Quer- und/oder Längsführung eines Fahrzeugs während eines Fahrmanövers.
Die Realisierung eines teil- oder hochautomatisierten Fahrbetriebs eines Fahrzeugs (insbesondere eines Straßen-Kraftfahrzeugs) wird derzeit verstärkt vorangetrieben. Ein zentraler Aspekt des teil- oder hochautomatisierten Fahrbetriebs ist die Planung einer möglichst optimalen Trajektorie des Fahrzeugs, durch die Kollisionen mit anderen Verkehrsteilnehmern vermieden werden. Die Ermittlung einer derartigen Trajektorie (z. B. einer Ausweichtrajektorie, einer Trajektorie für eine Folgefahrt und/oder einer Trajektorie für einen Fahrspurwechsel) ist mit einem hohen Rechenaufwand verbunden, der von Steuergeräten in einem Fahrzeug typischerweise nicht oder nur begrenzt erbracht werden kann.
Das vorliegende Dokument befasst sich mit der technischen Aufgabe, eine Trajektorie für ein Fahrzeug entlang einer Referenzkurve in stabiler, präziser und Ressourcen-effizienter Weise zu ermitteln.
Die Aufgabe wird durch die unabhängigen Ansprüche gelöst. Vorteilhafte Ausführungsformen werden u. a. in den abhängigen Ansprüchen beschrieben.
Gemäß einem Aspekt wird ein Verfahren zur Ermittlung einer Trajektorie zur Führung eines Fahrzeugs beschrieben. Die Trajektorie zeigt dabei typischerweise zukünftige Werte einer Mehrzahl von Zustandsgrößen des Fahrzeugs an. Beispielhafte Zustandsgrößen sind eine Position (x₁(t)) des Fahrzeugs, eine Geschwindigkeit (ẋ₁(t)) des Fahrzeugs, eine Beschleunigung (ẍ₁(t)) des Fahrzeugs und/oder einen Ruck (x₁ ⁽³⁾(t)) des Fahrzeugs. Auf Basis der zukünftigen Werte der Zustandsgrößen, d. h. auf Basis der Trajektorie, können dann Vorgaben (insbesondere Führungsgrößen) für ein oder mehrere Fahrzeugführungsregler des Fahrzeugs ermittelt werden. Insbesondere können in Abhängigkeit von der Trajektorie, eine Lenkvorgabe für eine Hilfskraftlenkung des Fahrzeugs und/oder eine Verzögerungsvorgabe für eine Bremsanlage des Fahrzeugs und/oder eine Beschleunigungsvorgabe für einen Antrieb des Fahrzeugs ermittelt werden. Somit kann das Fahrzeug auf Basis der Trajektorie in zuverlässiger Weise (zumindest teilweise) autonom geführt werden.
Das Verfahren umfasst das Ermitteln einer Führungs-Referenzkurve, entlang der das Fahrzeug geführt werden soll. Dabei zeigt die Führungs-Referenzkurve eine Vielzahl von vorausliegenden Soll-Positionen für das Fahrzeug an. Insbesondere können durch ein oder mehrere Umfeldsensoren (z. B. durch eine Kamera) des Fahrzeugs Umfelddaten bezüglich eines, in Fahrtrichtung vorausliegenden, Umfelds des Fahrzeugs erfasst werden. Die Führungs-Referenzkurve kann dann auf Basis der Umfelddaten ermittelt werden.
Die Führungs-Referenzkurve kann z. B. auf einer Grobplanung für eine Soll-Trajektorie des Fahrzeugs beruhen. Eine derartige Grobplanung kann in Ressourcen-effizienter Weise erfolgen, z. B. um unterschiedliche Typen von Fahrmanövern (z. B. eine Bremsung auf einer aktuellen Fahrbahn bzw. alternativ einen Fahrspurwechsel) zu prüfen und um ein bestimmtes Fahrmanöver auszuwählen. Die Grobplanung des ausgewählten Fahrmanövers kann dann als Grundlage für die Ermittlung der Trajektorie zur Führung des Fahrzeugs verwendet werden. Dabei können im Rahmen der Grobplanung ggf. fahrdynamische Beschränkungen des Fahrzeugs unberücksichtigt bleiben, um den Aufwand bei der Grobplanung klein zu halten. Dies kann dazu führen, dass die vorgegebene Führungs-Referenzkurve unstetige Stellen bzw. Sprünge aufweist, die typischerweise zu Stabilitätsproblemen bei der Ermittlung einer Trajektorie für die Führung des Fahrzeugs führen.
Das Verfahren umfasst daher weiter das Ermitteln einer Linearisierungs-Referenzkurve, die sich von der Führungs-Referenzkurve unterscheidet. Insbesondere kann die Linearisierungs-Referenzkurve im Vergleich zu der Führungs-Referenzkurve einen höheren Grad an Stetigkeit aufweisen. Mit anderen Worten kann die Linearisierungs-Referenzkurve derart ermittelt werden, dass die Linearisierungs-Referenzkurve höhere Stetigkeitsanforderungen erfüllt als die Führungs-Referenzkurve.
Die Stetigkeitsanforderungen einer Referenzkurve können dabei insbesondere eine Anzahl umfassen, mit der die Referenzkurve stetig ableitbar ist. Eine Referenzkurve kann dabei als stetig ableitbar betrachtet werden, wenn die Ableitung der Referenzkurve keine Sprünge zwischen direkt aufeinander folgenden Abtastwerten aufweist, die größer als ein vordefinierter Sprung-Schwellenwert sind. Die Linearisierungs-Referenzkurve kann somit derart ermittelt werden, dass die Linearisierungs-Referenzkurve häufiger stetig ableitbar ist als die Führungs-Referenzkurve. Beispielsweise kann die Linearisierungs-Referenzkurve 1, 2, 3 oder mehr häufiger stetig ableitbar sein als die Führungs-Referenzkurve.
Desweiteren können ggf. Anforderung in Bezug auf die Krümmung der Linearisierungs-Referenzkurve gestellt werden. Beispielsweise kann die Linearisierungs-Referenzkurve derart ermittelt werden, dass ein Maximalwert des Betrags der Krümmung der Linearisierungs-Referenzkurve kleiner ist als ein Maximalwert des Betrags der Krümmung der Führungs-Referenzkurve.
Es kann somit eine Linearisierungs-Referenzkurve bereitgestellt werden, die für die Linearisierung des Optimierungsproblems zur Ermittlung der Trajektorie verwendet werden kann. Insbesondere durch einen erhöhten Grad der Stetigkeit der Linearisierungs-Referenzkurve kann gewährleistet werden, dass das Optimierungsproblem in stabiler und präziser Weise gelöst werden kann. Beispielsweise kann die Linearisierungs-Referenzkurve auf Basis der aktuellen Fahrbahn, auf Basis einer Fahrspur-Markierung und/oder auf Basis digitaler Karteninformation bzgl. einer aktuell befahrenen Fahrbahn ermittelt werden.
Das Verfahren umfasst außerdem das Ermitteln von Anfangswerten für eine Mehrzahl von Zustandsgrößen des Fahrzeugs relativ zu der Linearisierungs-Referenzkurve. Insbesondere kann die Linearisierungs-Referenzkurve dazu verwendet werden, die Zustandsgrößen des Fahrzeugs in ein Frenet-Koordinatensystem bzw. in ein relatives Koordinatensystem zu transformieren. Die Ermittlung der Trajektorie kann dann in dem relativen Koordinatensystem erfolgen. Aufgrund der Stetigkeit der Linearisierungs-Referenzkurve kann eine stabile und präzise Transformation bzw. Linearisierung gewährleistet werden.
Außerdem umfasst das Verfahren das Bestimmen einer Trajektorie auf Basis der Anfangswerte, auf Basis der Führungs-Referenzkurve und auf Basis eines (linearen) Modells der Dynamik des Fahrzeugs. Dabei können aufgrund der Linearisierung separate Trajektorien für die Längsführung und für die Querführung des Fahrzeugs ermittelt werden, die dann überlagert und rücktransformiert werden können, um eine kombinierte Längs-Quer-Führungs-Trajektorie für das Fahrzeug zu ermitteln.
Das Bestimmen der Trajektorie umfasst dabei das Ermitteln einer Gütefunktion (bzw. das Ermitteln eines Wertes einer Gütefunktion), wobei die Gütefunktion eine Abweichung der Trajektorie von der Führungs-Referenzkurve berücksichtigt. Mit anderen Worten, bei der Ermittlung der Trajektorie wird weiterhin berücksichtigt, dass die ermittelte Trajektorie möglichst gut mit der Führungs-Referenzkurve übereinstimmt. Es kann somit eine präzise Trajektorie ermittelt werden.
Dabei kann die Trajektorie derart bestimmt werden, dass eine Abweichung der Trajektorie von der Führungs-Referenzkurve zumindest im Mittel reduziert, insbesondere minimiert, wird. Insbesondere kann die zu ermittelnde Trajektorie eine Vielzahl von Fahrzeug-Positionen umfassen, die der Vielzahl von Soll-Positionen der Führungs-Referenzkurve zugeordnet ist. Insbesondere kann die Vielzahl von Fahrzeug-Position für jede der Vielzahl von Soll-Positionen eine zugeordnete Fahrzeug-Position aufweisen. Die Gütefunktion kann dann einen Term umfassen, der eine Summe der Abweichungen, insbesondere eine Summe der quadratischen Abweichungen, von Fahrzeug-Positionen der Trajektorie zu zugeordneten Soll-Positionen der Führungs-Referenzkurve anzeigt. Dieser Term kann im Rahmen der Bestimmung einer Trajektorie reduziert, insbesondere minimiert, werden.
Durch die Bereitstellung einer Linearisierungs-Referenzkurve (für die Linearisierung der Trajektorien-Ermittlung) und einer Führungs-Referenzkurve (für die Ermittlung einer optimalen Trajektorie) können Trajektorien für die Fahrzeugführung in stabiler, präziser und Ressourcen-effizienter Weise bestimmt werden.
Die Linearisierungs-Referenzkurve kann auf Basis der Führungs-Referenzkurve ermittelt werden. Beispielsweise kann die Linearisierungs-Referenzkurve durch Filtern der Führungs-Referenzkurve mit einem Tiefpass-Filter ermittelt werden. Durch die Bereitstellung einer Linearisierungs-Referenzkurve, die möglichst wenig von der Führungs-Referenzkurve abweicht, kann die Genauigkeit einer ermittelten Trajektorie weiter erhöht werden.
Insbesondere kann das Ermitteln der Linearisierungs-Referenzkurve umfassen, das Ermitteln eines kombinierten Maßes aus einem ersten Maß für eine mittlere (quadratische) Abweichung der Linearisierungs-Referenzkurve von der Führungs-Referenzkurve, und aus einem zweiten Maß für eine Krümmung und/oder Stetigkeit der Linearisierungs-Referenzkurve. Insbesondere kann das zweite Maß anzeigen bzw. davon abhängen, wie häufig die Linearisierungs-Referenzkurve stetig ableitbar ist. So kann sowohl die Stabilität als auch als die Genauigkeit des Ermittlungsverfahrens für eine Trajektorie zur Fahrzeugführung gewährleistet werden.
Die Zustandsgrößen des Fahrzeugs weisen die Anfangswerte zu einem Anfangszeitpunkt auf. Die Trajektorie kann dann (zukünftige) Werte für die Mehrzahl von Zustandsgrößen des Fahrzeugs für eine Vielzahl von Zeitpunkten anzeigen, die auf den Anfangszeitpunkt folgen.
Die ermittelte Trajektorie kann dabei relative Werte für die Mehrzahl von Zustandsgrößen des Fahrzeugs, relativ zu der Linearisierungs-Referenzkurve, umfassen. Das Verfahren kann dann weiter umfassen, das Transformieren, mittels der Linearisierungs-Referenzkurve, der relativen Werte für die Mehrzahl von Zustandsgrößen des Fahrzeugs in absolute Werte, relativ zu einem Koordinatensystem des Fahrzeugs. Diese absoluten Werte können dann direkt zur Führung des Fahrzeugs verwendet werden.
Gemäß einem Aspekt wird eine Vorrichtung (z. B. ein Steuergerät bzw. eine Steuereinheit) beschrieben, die eingerichtet ist, das in diesem Dokument beschriebene Verfahren auszuführen.
Gemäß einem weiteren Aspekt wird ein Fahrzeug (insbesondere ein Straßenkraftfahrzeug wie z. B. ein Personenkraftwagen, ein Lastkraftwagen oder ein Motorrad) beschrieben, das die in diesem Dokument beschriebene Vorrichtung zur Ermittlung einer Trajektorie zur Führung des Fahrzeugs umfasst.
Gemäß einem weiteren Aspekt wird ein Software (SW) Programm beschrieben. Das SW Programm kann eingerichtet werden, um auf einem Prozessor (z. B. auf einem Steuergerät eines Fahrzeugs) ausgeführt zu werden, und um dadurch das in diesem Dokument beschriebene Verfahren auszuführen.
Gemäß einem weiteren Aspekt wird ein Speichermedium beschrieben. Das Speichermedium kann ein SW Programm umfassen, welches eingerichtet ist, um auf einem Prozessor ausgeführt zu werden, und um dadurch das in diesem Dokument beschriebene Verfahren auszuführen.
Es ist zu beachten, dass die in diesem Dokument beschriebenen Verfahren, Vorrichtungen und Systeme sowohl alleine, als auch in Kombination mit anderen in diesem Dokument beschriebenen Verfahren, Vorrichtungen und Systemen verwendet werden können. Desweiteren können jegliche Aspekte der in diesem Dokument beschriebenen Verfahren, Vorrichtungen und Systemen in vielfaltiger Weise miteinander kombiniert werden. Insbesondere können die Merkmale der Ansprüche in vielfältiger Weise miteinander kombiniert werden.
Im Weiteren wird die Erfindung anhand von Ausführungsbeispielen näher beschrieben. Dabei zeigen
1 eine beispielhafte Verkehrssituation, die die Planung einer Trajektorie zur Durchführung eines Fahrmanövers erfordert;
2 ein Ablaufdiagramm eines beispielhaften Verfahrens zur Ermittlung einer Trajektorie;
3 beispielhafte, relativ zu einer Linearisierungs-Referenzkurve linearisierte, Koordinaten für die Trajektorienplanung; und
4 ein Ablaufdiagramm eines beispielhaften Verfahrens zur Ermittlung einer Trajektorie für ein Fahrzeug.
Wie eingangs dargelegt, befasst sich das vorliegende Dokument mit der technischen Aufgabe in Ressourcen-effizienter und stabiler Weise eine präzise Trajektorie für ein Fahrzeug (welches auch als Ego-Fahrzeug bezeichnet wird) zu ermitteln. In diesem Zusammenhang zeigt 1 eine beispielhafte Verkehrssituation. Das Ego-Fahrzeug 100 fährt auf einer mehrspurigen Fahrbahn 101. Ein Fahrzeug 103 auf der gleichen Fahrspur der Fahrbahn 101 vor dem Ego-Fahrzeug 100 (d. h. ein Fahrzeug 103 auf der Ego-Fahrspur) kann eine relativ geringe Fahrgeschwindigkeit aufweisen. Das Ego-Fahrzeug 100 hat dann z. B. die Möglichkeit auf der Ego-Fahrspur zu verbleiben und gemäß einer Trajektorie 112 abzubremsen oder ein Überholmanöver durchzuführen und entlang einer Trajektorie 112 die Fahrspur zu wechseln, um das Fahrzeug 102 zu überholen. Dabei ist eine Kollision mit anderen Fahrzeugen 102, 104 zu vermeiden.
Zur Durchführung der in 1 dargestellten Manöver kann eine Steuereinheit des Ego-Fahrzeugs 100 eine Trajektorie 112 ermitteln, die ein oder mehrere Randbedingungen erfüllt. Bei der Ermittlung einer Trajektorie 112 können u. a. fahrdynamische Aspekte berücksichtigt werden. Insbesondere kann in Abhängigkeit von ein oder mehreren Fahrzeugparametern und/oder in Abhängigkeit von einer aktuellen Fahrsituation eine Trajektorie 112 ermittelt werden, die mit dem Fahrzeug 100 realistisch gefahren werden kann. Dabei kann eine vom Fahrzeug 100 umsetzbare Krümmung berücksichtigt werden. Weitere Beispiele für Fahrzeugparameter, die berücksichtigt werden können, sind eine (bei der aktuellen Fahrsituation umsetzbare) Beschleunigung oder Verzögerung des Fahrzeugs 100.
Desweiteren wird eine Trajektorie 112 typischerweise derart ermittelt, dass mit der Trajektorie 112 eine Kollision mit den detektierten Objekten 102, 103, 104 in der Umgebung des Ego-Fahrzeugs 100 vermieden werden kann. Beispielsweise kann eine Kollisionswahrscheinlichkeit für eine Vielzahl von möglichen Trajektorien 112 ermittelt werden, und es kann die mögliche Trajektorie 112 mit der geringsten Kollisionswahrscheinlichkeit als Trajektorie 112 ausgewählt werden, mit der das Fahrmanöver durchgeführt wird.
Die so ermittelte Trajektorie 112 kann dann an ein oder mehrere Regler für die Querführung/Längsführung des Fahrzeugs 100 übergeben werden. Insbesondere kann ein Bahnführungsregler verwendet werden, um sicherzustellen, dass das Fahrzeug 100 entlang der ermittelten Trajektorie 112 fährt. Desweiteren kann ein Fahrzeugführungsregler dafür sorgen, dass die dafür erforderlichen Brems-/Beschleunigungs-/Lenkmomente erbracht werden.
2 zeigt ein Ablaufdiagramm eines beispielhaften Verfahrens 200 zur Ermittlung einer Trajektorie (z. B. einer Längs-Trajektorie für die Längsführung und/oder einer Quer-Trajektorie für die Querführung) für ein Fahrzeug 100. Die Ermittlung einer Trajektorie erfolgt dabei bevorzugt in einem entkrümmten Koordinatensystem, relativ zu einer Referenzkurve, z. B. relativ zu einem Fahrbahnverlauf. Die Referenzkurve kann dabei von einer Fahrzeugfunktion vorgegeben sein. Beispielsweise kann von einer Fahrzeugfunktion vorgegeben sein, dass das Fahrzeug 100 entlang der Referenzkurve (z. B. entlang einer, typischerweise gekrümmten, Fahrbahn fährt). Das Verfahren 200 kann daher den Schritt umfassen, Zustandsdaten bzw. Werte von Zustandsgrößen des Fahrzeugs 100 (wie z. B. die Position des Fahrzeugs 100, einen Gierwinkel des Fahrzeugs 100 und/oder einen Lenkwinkel des Fahrzeugs 100) aus einem kartesischen Koordinatensystem in ein (entkrümmtes bzw. linearisiertes) Frenet-Koordinatensystem zu transformieren.
Die Entkrümmung bzw. Linearisierung ist beispielhaft in 3 dargestellt. Für die Entkrümmung bzw. Linearisierung werden Messsignale (d. h. Sensordaten von Sensoren des Fahrzeugs 100) bzgl. des Zustands des Fahrzeugs 100 in ein relatives Koordinatensystem transformiert, wobei das relative Koordinatensystem einen Zustand des Fahrzeugs 100 relativ zu einer Linearisierungs-Referenzkurve 300 anzeigt. Dabei entspricht die Linearisierungs-Referenzkurve 300 typischerweise der Referenzkurve, der das Fahrzeug 100 folgen soll. Die Trajektorienplanung selbst findet damit nicht in einem kartesischen Koordinatensystem 301 statt, sondern in einem relativen Koordinatensystem (das auch als Frenet-Koordinatensystem bezeichnet wird). Das relative Koordinatensystem wird somit bezüglich einer Linearisierungs-Referenzkurve 300 (z. B. der Fahrspurmitte eines Fahrbahnverlaufs) beschrieben. Die Fahrzeugposition wird damit durch die Variablen s(t) 303 in Längsrichtung und d(t) 302 in Querrichtung beschrieben. ṡ(t) und d .(t) beschreiben die Längs- und Quergeschwindigkeit und s ..(t) und d ..(t) beschreiben die Längs- und Querbeschleunigungen. Folglich können die verschiedenen Zustandsgrößen des Fahrzeugs 100 relativ zu der Linearisierungs-Referenzkurve 300 beschrieben werden.
Sowohl die Fahrzeugeigenbewegung als auch die zu berücksichtigenden Verkehrsteilnehmer bzw. Objekte können im Frenet-Koordinatensystem berücksichtigt werden. Anschaulich entspricht diese Transformation der Entkrümmung bzw. Linearisierung des kartesischen Koordinatensystems 301 und erlaubt so die getrennte Optimierung der Längs- und Querbewegung des Fahrzeugs 100. Insbesondere wird es durch die Beschreibung des Zustands eines Fahrzeugs 100 relativ zu einer Linearisierungs-Referenzkurve 300 ermöglicht, das dynamische Verhalten des Fahrzeugs 100 in präziser Weise durch ein lineares Zustandsmodell (z. B. durch das unten aufgeführte Integratorsystem) zu beschreiben. Dabei können separate Fahrzeugmodelle für die Längsbewegung und für die Querbewegung des Fahrzeugs 100 verwendet werden.
Die Quer- und Längsbewegung eines Fahrzeugs 100 lässt sich als Optimalsteuerproblem mit Ausgang s(t) = x₁(t) (im Falle der Längsplanung) bzw. d(t) = x₁(t) (im Falle der Querplanung) eines Integratorsystems (d. h. eines Modells der Dynamik eines Fahrzeugs 100) beschreiben. Dabei ist x₁(t) eine erste Zustandsgröße des Fahrzeugs 100, welche die Position des Fahrzeugs 100 (in Längsrichtung bzw. in Querrichtung) beschreibt. Als Eingang des Integratorsystems kann der Ruck x₁ ⁽³⁾(t) (d. h. die 3^te Ableitung der Zustandsgröße x₁(t)) oder die Ableitung des Rucks x₁ ⁽⁴⁾(t) (d. h. die 4^te Ableitung der Zustandsgröße x₁(t)) definiert werden.
Ein beispielhaftes Integratorsystem kann wie folgt definiert werden:
wobei die Eingangsröße u der Ableitung des Rucks x₁ ⁽⁴⁾(t) entspricht. Der Zustand eines Fahrzeugs 100 zu einem bestimmten Zeitpunkt t kann durch den Zustandsvektor x^T = [x₁, x₂, x₃, x₄] beschrieben werden, wobei x₂(t) = ẋ₁(t), x₃(t) = ẋ₂(t) und x₄(t) = ẋ₃(t) ist.
Es kann gezeigt werden, dass sich der Zustandsvektor x(t) ergibt als
Dabei beschreiben die o. g. Gleichungen ein Polynom 7. Ordnung in Bezug auf den räumlichen Verlauf x₁(t), d. h. die sich ergebende Längs-Trajektorie oder Quer-Trajektorie kann durch ein Polynom 7. Ordnung beschrieben werden.
Zur Lösung des Optimierungsproblems basierend auf einem Polynom 7. Ordnung kann folgendes Gütefunktional (welches auch als Auswahlmaß bezeichnet wird) zugrundegelegt werden:
Die Parameter c₀₁₂₃ ^T = [c₀, c₁, c₂, c₃] berechnen sich aus den Anfangsbedingungen x(0) = x₀ einer Trajektorie zum Zeitpunkt t = 0 als C₀₁₂₃ = M₁ ^–1(0)x₀
Die Parameter c₄₅₆₇ ^T = [c₄, c₅, c₆, c₇] berechnen sich aus den Endbedingungen x(t_f) der Trajektorie zum Zeitpunkt t = t_f als c₄₅₆₇ = M₂ ^–1(t_f)(x(t_f) – M₁(t_f)c₀₁₂₃)
Die Endbedingungen können, wie in der obigen Formel, vorgegeben werden. Alternativ kann ein Referenzverlauf beschrieben durch x_ref = [x_1,ref, x_2,ref, x_3,ref, x_4,ref]^T vorgegeben werden. Dabei entspricht der Referenzverlauf x_1,ref der Position des Fahrzeugs 100 typischerweise der Linearisierungs-Referenzkurve 300. Das Optimierungsziel liegt in dem Fall darin, möglichst nah an diesen Referenzverlauf zu kommen. In diesem Fall können die Parameter c₄₅₆₇ ^T wie folgt berechnet werden:
Das Optimierungsproblem besteht nun darin sowohl die Endzeit bzw. den Endzeitpunkt t_f als auch ggf. den Endzustand x(t_f) für eine optimale Trajektorie zu bestimmen. Insbesondere sollen durch das Optimierungsproblem typischerweise der Endzeitpunkt t_f und die Position des Fahrzeugs 100 zum Endzeitpunkt t_f, d. h. x₁(t_f), bzw. die Geschwindigkeit des Fahrzeugs 100 zum Endzeitpunkt t_f, d. h. ẋ₁(t_f), ermittelt werden.
Als Quer-Auswahlmaß oder als Quer-Gütemaß für die Ermittlung einer Trajektorie für die Querführung des Fahrzeugs 100 kann dann folgende Funktion verwendet werden:
Dabei bewertet der erste Ausdruck, die Entwicklung der Ableitung des Rucks entlang der Trajektorie 112. Der zweite Ausdruck bewertet die Abweichung der Position d(t) von der Referenzposition d_ref(t) (insbesondere von der Referenzkurve 300). Desweiteren bewertet der dritte Ausdruck die zeitliche Länge der Trajektorie 112. Über die Gewichtungsfaktoren k_q1 und k_q2 kann die Ausprägung der Trajektorie gewichtet werden.
Die Längsplanung kann in ähnlicher Weise erfolgen. Für die Längsplanung kann z. B. folgendes Längs-Auswahlmaß bzw. Längs-Gütemaß verwendet werden
insbesondere wenn eine bestimmte Zielposition s_ziel erreicht werden soll. Alternativ kann das folgende Längs-Auswahlmaß bzw. Längs-Gütemaß verwendet werden
insbesondere wenn eine bestimmte Zielgeschwindigkeit ṡ_ziel erreicht werden soll.
Zur Ermittlung einer optimalen Längs-Trajektorie oder Quer-Trajektorie kann das jeweilige Auswahlmaß J für unterschiedliche Werte von t_f und/oder für untersehiedliche Endzustände x(t_f) berechnet werden. Dazu können die o. g. Formeln verwendet werden. Es ergibt sich somit ein Verlauf des Auswahlmaßes J, wobei ein Minimalwert des Auswahlmaßes J den optimalen Wert für die Zeit t_f und/oder für den Endzustand x(t_f) angibt. In einem weiteren Schritt können dann ein oder mehrere Nebenbedingungen berücksichtigt werden.
Die Nebenbedingungen können somit nachgelagert an die Optimierung berücksichtigt werden. Dazu kann die im Sinne des Kostenfunktionals J (d. h. des Auswahlmaßes) beste Trajektorie ausgewählt und auf Einhaltung der Nebenbedingungen geprüft werden. Werden die Nebenbedingungen eingehalten wird die Trajektorie umgesetzt. Andernfalls wird die nächstbeste Trajektorie ausgewählt und auf die Nebenbedingungen überprüft. Dieses Vorgehen wird solange angewandt bis eine optimale Lösung gefunden wird, die die Nebenbedingungen erfüllt.
Als Nebenbedingungen können Aktuator- und Fahrdynamikbegrenzungen berücksichtigt werden. Desweiteren kann die Kollisionsfreiheit mit prädizierten Objekttrajektorien von anderen Objekten/Fahrzeugen 102, 103, 104 berücksichtigt werden.
Das in 2 veranschaulichte Verfahren 200 zur Ermittlung einer Trajektorie für die Längs- und/oder Querführung eines Fahrzeugs 100 umfasst somit das Ermitteln 201 von Anfangswerten bzw. Anfangsbedingungen x(0) = x₀ für eine Mehrzahl von Zustandsgrößen x des Fahrzeugs 100. Die Mehrzahl von Zustandsgrößen kann dabei eine Position x₁(t) des Fahrzeugs 100, eine Geschwindigkeit ẋ₁(t) des Fahrzeugs 100, eine Beschleunigung ẍ₁(t) des Fahrzeugs 100 und/oder einen Ruck x₁ ⁽³⁾(t) des Fahrzeugs 100 umfassen. Dabei gibt x₁ ⁽³⁾(t) die dritte Ableitung der Position x₁(t) des Fahrzeugs 100 an. Die Anfangswerte für den Zustand des Fahrzeugs 100 können auf Basis von Sensordaten von Sensoren des Fahrzeugs 100 ermittelt werden.
Das Verfahren 200 umfasst außerdem das Ermitteln 202 von Endwerten x(t_f) zu einem Endzeitpunkt t_f für die Mehrzahl von Zustandsgrößen x des Fahrzeugs 100. Die Endwerte für den Zustand des Fahrzeugs 100 können dabei von einer Fahrzeugfunktion vorgegeben werden (z. B. um einem bestimmten Hindernis auszuweichen). Außerdem umfasst das Verfahren 200 das Bestimmen 203 einer Längs-Trajektorie oder einer Quer-Trajektorie auf Basis der Anfangswerte x(0) = x₀, der Endwerte x(t_f), dem Endzeitpunkt t_f und auf Basis von einem Modell der Dynamik des Fahrzeugs 100.
Zur Ermittlung einer optimalen Längs-Trajektorie oder Quer-Trajektorie (im Sinne eines Längs- oder Quer-Auswahlmaßes J) können die Werte des Auswahlmaßes J für Trajektorien mit unterschiedlichen Endzeitpunkten t_f und/oder mit unterschiedlichen Endwerten x(t) der Mehrzahl von Zustandsgrößen ermittelt werden. Es kann dann die (Zustands-)Trajektorie ausgewählt werden, die das Auswahlmaß J optimiert. Desweiteren kann überprüft werden, ob ein oder mehrere Nebenbedingungen erfüllt werden (wie oben dargelegt).
Die ermittelte Längs-Trajektorie oder Quer-Trajektorie kann abschließend von dem Frenet-Koordinatensystem zurück in ein kartesisches Koordinatensystem transformiert werden. Desweiteren kann die ermittelte Längs- oder Quer-Trajektorie dazu verwendet werden, das Fahrzeug 100 zu führen (z. B. für ein Ausweichmanöver oder für ein Einparkmanöver).
Zur Ermittlung einer optimalen Quer-Längs-Trajektorie 112, welche sowohl für die Querführung als auch für die Längsführung des Fahrzeugs 100 verwendet werden kann, kann zunächst eine Vielzahl von Längs-Trajektorien (für die Längsführung) und eine entsprechende Vielzahl von Werten eines Längs-Auswahlmaßes J_längs ermittelt werden. Anschließend kann für jede Längs-Trajektorie eine Vielzahl von Quer-Trajektorien und eine entsprechende Vielzahl von Werten eines Quer-Auswahlmaßes J_quer ermittelt werden. Durch eine Kombination der Werte der Auswahlmaße J_längs und J_quer können Werte eines kombinierten Quer-Längs-Auswahlmaße für die kombinierte Quer-Längs-Trajektorienplanung ermittelt werden. Dazu kann z. B. ein gewichteter Mittelwert der Werte der Auswahlmaße J_längs und J_quer ermittelt werden. Es kann dann die Kombination aus Längs-Trajektorie und Quer-Trajektorie mit dem geringsten Wert des kombinierten Quer-Längs-Auswahlmaßes als optimale kombinierte Quer-Längs-Trajektorie 112 ausgewählt werden.
Wie oben dargelegt, kann durch die Linearisierung relativ zu einer Linearisierungs-Referenzkurve 300 der Rechenaufwand für die Berechnung einer Trajektorie 112 substantiell reduziert werden. Es kann dann eine Trajektorie 112 ermittelt werden, die eine Abweichung zu dieser Linearisierungs-Referenzkurve 300 gemäß einem Kostenfunktional J_längs und/oder quer reduziert (insbesondere minimiert).
Die Verwendung einer Linearisierungs-Referenzkurve 300 zur Linearisierung der Zustandsgrößen eines Fahrzeugs 100 setzt jedoch die Stetigkeit der Linearisierungs-Referenzkurve 300 voraus. Insbesondere können Diskontinuitäten in der Linearisierungs-Referenzkurve 300 zu Problemen bei der Berechnung einer Trajektorie 112 (insbesondere bei der Transformation zu dem relativen bzw. dem Frenet-Koordinatensystem) führen. Andererseits kann es vorkommen, dass die durch eine Fahrzeugfunktion vorgegebene Referenzkurve 300 für die Führung eines Fahrzeugs 100 Diskontinuitäten aufweist. Derartige Diskontinuitäten können insbesondere durch Rauschen bei der sensorischen Erfassung des Umfelds des Fahrzeugs 100 und/oder aufgrund einer Grobplanung einer Referenzkurve hervorgerufen werden. Es wird daher durch eine Fahrzeugfunktion typischerweise eine Führungs-Referenzkurve 305 vorgegeben, die lokal relativ hohe Krümmungen aufweist (siehe 3). Insbesondere kann die Führungs-Referenzkurve 305 Stellen aufweisen, die einen relativ geringen Grad an Stetigkeit aufweisen (d. h. die z. B. nicht oder nur wenige Male stetig ableitbar sind). Die Verwendung einer solchen Kurve 305 zur Linearisierung der Zustandsgrößen kann zu Instabilitäten bei der Ermittlung einer Trajektorie 112 führen.
Es ist daher vorteilhaft, insbesondere auf Basis der Führungs-Referenzkurve 305, eine Linearisierungs-Referenzkurve 300 zu ermitteln, die gegenüber der Führungs-Referenzkurve 305 einen erhöhten Grad an Stetigkeit aufweist (d. h. die erhöhte Stetigkeitsanforderungen erfüllt). Insbesondere kann eine Linearisierungs-Referenzkurve 300 ermittelt werden, die im Mittel über eine bestimmte Streckenlänge höhere Stetigkeitsanforderungen erfüllt als die Führungs-Referenzkurve 305. Alternativ oder ergänzend kann eine Linearisierungs-Referenzkurve 300 ermittelt werden, die in jedem Punkt zumindest die gleichen und in zumindest einem Punkt höhere Stetigkeitsanforderungen erfüllt als die Führungs-Referenzkurve 305.
Die Erhöhung der Stetigkeitsanforderungen kann z. B. durch eine Tiefpass-Filterung der Führungs-Referenzkurve 305 erreicht werden. Es ergeben sich dadurch lokale Abweichungen bzw. Offsets 306 zwischen der Führungs-Referenzkurve 305 und der Linearisierungs-Referenzkurve 300. Diese Abweichungen bzw. Offsets 306 können dann bei der Bestimmung der Trajektorie 112 berücksichtigt werden. Insbesondere kann anstelle der Linearisierungs-Referenzkurve 300 die Führungs-Referenzkurve 305 als Referenzverlauf x_1,ref in der Gütefunktion J berücksichtigt werden. So kann eine stabile und präzise Ermittlung einer Trajektorie 112 gemäß einer (nicht-stetigen) Führungs-Referenzkurve 305 erreicht werden.
4 zeigt ein Ablaufdiagramm eines beispielhaften Verfahrens 400 zur Ermittlung einer Trajektorie 112 zur Führung eines Fahrzeugs 100. Dabei zeigt die Trajektorie 112 typischerweise zukünftige Werte von Zustandsgrößen des Fahrzeugs 100 an, die als Führungsgrößen für eine Lenkung, für eine Bremsfunktion und/oder für einen Antrieb des Fahrzeugs verwendet werden können, um das Fahrzeug zu führen.
Das Verfahren 400 umfasst das Ermitteln 401 einer Führungs-Referenzkurve 305, entlang der das Fahrzeug 100 geführt werden soll. Dabei zeigt die Führungs-Referenzkurve 305 eine Vielzahl von vorausliegenden Soll-Positionen für das Fahrzeug 100 an. Das Fahrzeug 100 soll demnach bevorzugt derart geführt werden, dass das Fahrzeug 100 entlang der Soll-Positionen der Führungs-Referenzkurve 305 fährt. Die Führungs-Referenzkurve 305 kann z. B. durch eine Fahrzeugfunktion (z. B. durch ein ACC-System oder durch einen Stauassistenten) auf Basis von Umfelddaten des Fahrzeugs 100 ermittelt worden sein. Die Führungs-Referenzkurve 305 kann dabei (z. B. aufgrund von Sensorrauschen und/oder aufgrund von Hindernissen) unstetige Stellen bzw. Sprünge aufweisen. Derartige Verläufe können zu Instabilitäten und/oder Fehlern bei der Ermittlung einer Trajektorie 112 führen.
Das Verfahren 400 umfasst weiter das Ermitteln 402 einer Linearisierungs-Referenzkurve 300, die sich von der Führungs-Referenzkurve 305 unterscheidet. Insbesondere weist die ermittelte Linearisierungs-Referenzkurve 300 typischerweise keine unstetigen Stellen auf. Die Linearisierungs-Referenzkurve 300 kann derart ermittelt werden, dass die Linearisierungs-Referenzkurve 300 höhere Stetigkeitsanforderungen erfüllt als die Führungs-Referenzkurve 305. Beispielsweise kann die Linearisierungs-Referenzkurve 300 derart ermittelt werden, dass ein Frequenzspektrum der Linearisierungs-Referenzkurve 300 eine geringere Bandbreite aufweist als ein Frequenzspektrum der Führungs-Referenzkurve 305. Zu diesem Zweck können die Referenzkurven 300, 305 als Funktion der Zeit (ausgehend von einem Anfangszeitpunkt) betrachtet werden (relativ zu einer geraden Zeitachse). Es kann dann ein Frequenzspektrum dieser Funktionen ermittelt werden (z. B. mittels einer Fast Fourier Transformation, FFT). Die Energie des Frequenzspektrums der Linearisierungs-Referenzkurve 300 kann dann im Mittel bei niedrigeren Frequenzen liegen als die Energie des Frequenzspektrums der Führungs-Referenzkurve 305.
Desweiteren kann die Linearisierungs-Referenzkurve 300 derart ermittelt werden, dass eine Abweichung zwischen Linearisierungs-Referenzkurve 300 und Führungs-Referenzkurve 305 reduziert, insbesondere minimiert, wird. Dabei kann ein Kompromiss zwischen der Erhöhung der Stetigkeitsanforderungen einerseits und der Reduzierung der Abweichung andererseits angestrebt werden. In einem Beispiel wird die Linearisierungs-Referenzkurve 300 aus der Führungs-Referenzkurve 305 durch Filterung mit einem Tiefpass-Filter ermittelt.
Das Verfahren 400 umfasst weiter das Ermitteln 403 von Anfangswerten für eine Mehrzahl von Zustandsgrößen des Fahrzeugs 100 relativ zu der Linearisierungs-Referenzkurve 300. Insbesondere können anhand von Sensordaten des Fahrzeugs 100 aktuelle Werte für Zustandsgrößen des Fahrzeugs 100 (insbesondere Position, Geschwindigkeit, Beschleunigung, etc.) ermittelt werden. Diese Werte können dann mittels der Linearisierungs-Referenzkurve 300 in ein Frenet-Koordinatensystem überführt werden (wie in Zusammenhang mit 3 dargelegt), um die Anfangswerte der Zustandsgrößen relativ zu der Linearisierungs-Referenzkurve 300 zu erhalten. Aufgrund der, gegenüber der Führungs-Referenzkurve 305 verbesserten, Stetigkeit der Linearisierungs-Referenzkurve 300, ist eine solche Linearisierung typischerweise in stabiler Weise möglich.
Das Verfahren 400 umfasst weiter das Bestimmen 404 einer Trajektorie 112 auf Basis der Anfangswerte und auf Basis eines (linearen) Modells der Dynamik des Fahrzeugs 100. Dabei wird die Trajektorie 112 im linearisierten Raum (d. h. relativ zu der Linearisierungs-Referenzkurve 300) ermittelt. Andererseits umfasst das Bestimmen 404 der Trajektorie 112 das Ermitteln einer Gütefunktion, die eine Abweichung der Trajektorie 112 von der Führungs-Referenzkurve 305 berücksichtigt. Insbesondere kann dabei eine Trajektorie 112 bestimmt werden, die in Bezug auf die ursprüngliche Führungs-Referenzkurve 305 optimal ist (die z. B. Abweichungen zu der Führungs-Referenzkurve 305 im Mittel minimiert).
Durch die Verwendung einer Linearisierungs-Referenzkurve 300 und einer Führungs-Referenzkurve 305 kann die Linearisierung z. B. entlang einer (ggf. geglätteten) Fahrbahn erfolgen. Diese Linearisierungs-Referenzkurve 300 kann z. B. mittels einer Fahrzeug-Kamera einer digitalen Karte und/oder auf Basis einer geglätteten Spur eines Vorderfahrzeugs ermittelt werden. Andererseits kann eine zweite Referenz (z. B. ein grob optimierter Spurwechsel) für die Kostendefinition (d. h. für die Gütefunktion) verwendet werden. Die zweite Referenz (d. h. die Führungs-Referenzkurve 305) unterliegt dabei keinen fahrphysikalischen Beschränkungen. Somit kann die zweite Referenz einer Groboptimierung entstammen, die z. B. auf einem vereinfachten Fahrzeugmodell beruht. Die unterlagerte Trajektorien-Optimierung basiert dann auf der Linearisierung entlang der Fahrbahn und setzt die groboptimierte Trajektorie effizient und (nahezu) optimal um. Es kann somit eine effiziente und stabile Trajektorienplanung für die automatische Führung eines Fahrzeugs 100 bereitgestellt werden.
Die vorliegende Erfindung ist nicht auf die gezeigten Ausführungsbeispiele beschränkt. Insbesondere ist zu beachten, dass die Beschreibung und die Figuren nur das Prinzip der vorgeschlagenen Verfahren, Vorrichtungen und Systeme veranschaulichen sollen.

Claims

Verfahren (400) zur Ermittlung einer Trajektorie (112) zur Führung eines Fahrzeugs (100), wobei das Verfahren (400) umfasst, – Ermitteln (401) einer Führungs-Referenzkurve (305), entlang der das Fahrzeug (100) geführt werden soll; wobei die Führungs-Referenzkurve (305) eine Vielzahl von vorausliegenden Soll-Positionen für das Fahrzeug (100) anzeigt; – Ermitteln (402) einer Linearisierungs-Referenzkurve (300); – Ermitteln (403) von Anfangswerten für eine Mehrzahl von Zustandsgrößen des Fahrzeugs (100) relativ zu der Linearisierungs-Referenzkurve (300); und – Bestimmen (404) einer Trajektorie (112) auf Basis der Anfangswerte, auf Basis der Führungs-Referenzkurve (305) und auf Basis eines Modells der Dynamik des Fahrzeugs (100); wobei das Bestimmen (404) der Trajektorie (112) das Ermitteln einer Gütefunktion umfasst, die eine Abweichung der Trajektorie (112) von der Führungs-Referenzkurve (305) berücksichtigt.
Verfahren (400) gemäß Anspruch 1, wobei die Linearisierungs-Referenzkurve (300) höhere Stetigkeitsanforderungen erfüllt als die Führungs-Referenzkurve (305).
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei das Ermitteln (402) der Linearisierungs-Referenzkurve (300) umfasst, Ermitteln der Linearisierungs-Referenzkurve (300) auf Basis der Führungs-Referenzkurve (305), insbesondere durch Filtern der Führungs-Referenzkurve (305) mit einem Tiefpass-Filter.
Verfahren (400) gemäß Anspruch 3, wobei das Ermitteln (402) der Linearisierungs-Referenzkurve (300) umfasst, Ermitteln eines kombinierten Maßes – aus einem ersten Maß für eine mittlere Abweichung der Linearisierungs-Referenzkurve (300) von der Führungs-Referenzkurve (305); und – aus einem zweiten Maß für eine Stetigkeit der Linearisierungs-Referenzkurve (300).
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei die Linearisierungs-Referenzkurve (300) derart ermittelt wird, dass die Linearisierungs-Referenzkurve (300) häufiger stetig ableitbar ist als die Führungs-Referenzkurve (305).
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei die Trajektorie (112) derart bestimmt wird, dass eine Abweichung der Trajektorie (112) von der Führungs-Referenzkurve (305) zumindest im Mittel reduziert, insbesondere minimiert, wird.
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei – die Trajektorie (112) eine Vielzahl von Fahrzeug-Positionen umfasst; – die Vielzahl von Fahrzeug-Positionen für jede der Vielzahl von Soll-Positionen der Führungs-Referenzkurve (305) eine zugeordnete Fahrzeug-Position aufweist; und – die Gütefunktion einen Term umfasst, der eine Summe der Abweichungen, insbesondere eine Summe der quadratischen Abweichungen, von Fahrzeug-Positionen der Trajektorie (112) zu zugeordneten Soll-Positionen der Führungs-Referenzkurve (305) anzeigt.
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei – die Zustandsgrößen des Fahrzeugs (100) zu einem Anfangszeitpunkt die Anfangswerte aufweisen; und – die Trajektorie (112) Werte für die Mehrzahl von Zustandsgrößen des Fahrzeugs (100) für eine Vielzahl von Zeitpunkten anzeigt, die auf den Anfangszeitpunkt folgen.
Verfahren (400) gemäß Anspruch 8, wobei die – die Trajektorie (112) relative Werte für die Mehrzahl von Zustandsgrößen des Fahrzeugs (100), relativ zu der Linearisierungs-Referenzkurve (300), umfasst; und – das Verfahren (400) weiter umfasst, Transformieren, mittels der Linearisierungs-Referenzkurve (300), der relativen Werte für die Mehrzahl von Zustandsgrößen des Fahrzeugs (100) in absolute Werte, relativ zu einem Koordinatensystem (301) des Fahrzeugs (100).
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei die Mehrzahl von Zustandsgrößen eine Position (x₁(t)) des Fahrzeugs (100), eine Geschwindigkeit (ẋ₁(t)) des Fahrzeugs (100), eine Beschleunigung (ẍ₁(t)) des Fahrzeugs (100) und/oder einen Ruck (x₁ ⁽³⁾(t)) des Fahrzeugs (100) umfasst.
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, weiter umfassend, Bestimmen, in Abhängigkeit von der Trajektorie (112), – einer Lenkvorgabe für eine Hilfskraftlenkung des Fahrzeugs (100); und/oder – einer Verzögerungsvorgabe für eine Bremsanlage des Fahrzeugs (100); und/oder – einer Beschleunigungsvorgabe für einen Antrieb des Fahrzeugs (100).
Verfahren (400) gemäß einem der vorhergehenden Ansprüche, wobei – das Verfahren (400) umfasst, Ermitteln von Umfelddaten bezüglich eines, in Fahrtrichtung vorausliegenden, Umfelds des Fahrzeugs (100); – die Führungs-Referenzkurve (305) auf Basis der Umfelddaten ermittelt wird.