FAS Folien VL13 PDF

Fahrerassistenzsysteme
13. Trajektorienplanung
Jona Ruof
Institut für Mess-, Regel- und Mikrotechnik
FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 1

Motivation
Für eine komplexe Situation mit Fahrzeugen, Ampeln, Fußgängern, etc.,

wurde in den vergangenen Vorlesungen Ansätze zur Detektion der Objekten
und zur anschließenden Handlungsplanung besprochen.
Die Trajektorienplanung beschäftigt sich nun mit der Umsetzung

der ausgewählten Handlungsoption in konkrete Aktionen.
Übersicht
 Grundlagen
●
Systemarchitektur und Einordnung
●
Pfadplanung vs.Trajektorienplanung
●
Anforderungen
●
Formalismus
 Diskrete Verfahren
●
Rapid Random Trees
●
Graphensuche
 Kontinuierliche Verfahren
●
Statische Optimierung
●
Dynamische Optimierung
●
Polynombasierte Planung
 Zusammenfassung

Übersicht
 Grundlagen
●
●
●
Anforderungen
●
Formalismus
●
Rapid Random Trees
●
Graphensuche
●
●
●
 Zusammenfassung

Systemarchitektur für die Fahrzeugautomation
Sensorik inkl. digitale Karte und GPS
Maschinelle Eigenbewegungs-
Lokalisierung
Wahrnehmung schätzung
Umgebungsmodell
Situationsinterpretation
Handlungsplanung
Trajektorienplanung und Fahrzeugregelung
Die Trajektorienplanung bildet, zusammen mit der Fahrzeugregelung, das letzte

Modul in der Automatisierungs-Software.
Pfadplanung vs. Trajektorienplanung
Pfadplanung:
 Planung eines realisierbaren Pfades B
(rein geometrisch)
 statische Umgebung mit Beschränkungen A ?
Trajektorienplanung:
 Planung zeitveränderlicher Sollzustände
 explizite Berücksichtigung der Zeit
 Pfadplanung Teil der Trajektorienplanung ?
Die Pfadplanung bezeichnet allgemein die Planung eines realisierbaren Pfads.

Die Umgebung wird dabei als statisch angenommen und stellt Beschränkungen für die
Pfadplanung dar.
Im Unterschied zur Pfadplanung plant die Trajektorienplanung explizit mit zeitlichen

Komponenten.
Die Pfadplanung ist eine Unterkategorie der Trajektorienplanung da auch bei der
Planung zeitveränderlicher Zustände der Pfad zwangsläufig mitberücksichtigt werden
muss.
Hierarchieebenen in der Planung
Ebene Frequenz Horizont
Routenplanung wenn nötig Start-Ziel

B
Pfadplanung
A ?
(z.B. im Freiraum auf Parkplatz) 2 Hz
Verhaltensplanung (→ VL12)
10 Hz
3-12 s
Trajektorienplanung
? 20 Hz
30 Hz
Regelung (→ VL14)
1-3 s
1 kHz
Bi-Level-Stabilisierung
 Low-Level Stabilisierung (LLS):

geeignet bei kontinuierliche Störung
 High-Level Stabilisierung (HLS):

geeignet bei impulsförmiger Störung
 Kombination beider Varianten:

LLS z.B. für Modellfehler
HLS z.B. bei plötzlicher Änderung der Situation
 Trajektorienplanung → HLS
Regler → LLS
Vergleich zwischen High- und Low-level-Stabilisierung1
Moritz Werling: Ein neues Konzept für die Trajektoriengenerierung und -stabilisierung in zeitkritischen Verkehrsszenarien. Automatisierungstechnik 60(1): 53-54 (2012)
Bei einem idealen System wird die geplante Trajektorie exakt realisiert. In
der Realität kommt es aber zu Störungen welche zu einem Abweichen
von der geplanten Trajektorien führen.
Hierbei wird im Folgenden zwischen kontinuierlicher Störung und

impulsförmiger Störung unterscheiden.
Bei impulsartigen Störungen, welche zu einer großen Regelabweichung

führen ist die Rückführung auf Planungsebene von Vorteil, da somit der
neue Zustand direkt bei der Optimierung berücksichtigt wird (wenn nicht
sogar notwendig für die Gewährleistung von Kollisionsfreiheit). Man
spricht in diesem Fall auch von High-Level-Stabilisierung.
Kontinuierliche Störungen hingegen werden auf Ebene der

Fahrzeugregelung rückgeführt. Dies sorgt dafür, dass sich der
geschlossene Regelkreis sehr ähnlich zu der störungsfreien
Modellreferenz verhält.
Anforderungen an die Planung
 Einhalten dynamischer Beschränkungen (kammscher Kreis, nicht holonome Dynamik)

 Einhalten von geltenden Gesetzen (StvO)
 Sicherheit
 Komfort der Passagiere
 Fortschritt zum Ziel
 Kooperativität mit anderen Teilnehmern
 …
 Anforderungen z.T. widersprüchlich und subjektiv

Holonom vs. Nichtholonom
Holonom:
 System mit N Koordinaten beschreibbar über …
 ... ausschließlich Beschränkungen
 z.B. Pendel:
Nichtholonom:
 System mit N Koordinaten beschreibbar über …
 … Beschränkungen
 Beschränkungen abhängig von Geschw. der Koordinaten
(und Geschw. nicht eliminierbar)
 z.B. Radbewegung
Die Einschränkung der Bewegungsfreiheit eines Roboters wird als Zwangsbedingung

bezeichnet durch welche ggf. die Zahl der Freiheitsgrade des Systems reduziert wird.
Dabei wird zwischen holonomen und nichtholonomen Zwangsbedingungen
unterschieden:
Eine holonome Zwangsbedingung enthält hierbei keine Geschwindigkeitsabhängigkeit.

Beispiel: Ein Pendel mit fester Länge l kann sich nur auf einer Kreisbahn bewegen.
Die Anzahl generalisierten Koordinaten lässt sich durch Aufstellen der Zwangsbedingung
durch die Winkelbeziehung auf eins reduzieren. Die Reduktion der Freiheitsgrade
entspricht einer Verringerung der Dimension des Konfigurationsraums.
Fahrzeuge unterliegen i.A. nichtholonomen Beschränkungen: Hierbei enthält die

entsprechende Zwangsbedingung eine Geschwindigkeitsabhängigkeit. Bei der Planung
muss hierbei berücksichtigt werden, dass nicht jede Richtung direkt steuerbar ist.
Beispiel hierfür ist das seitliche Einparken für welches eine Vor- und
Rückwärtsbewegung erforderlich ist.
Formalismus
 Zustände:
 Aktionen:
 Trajektorie:
 Dynamikfunktion:
 Kostenfunktion:
Beispieltrajektorie:

Übersicht
 Grundlagen
●
●
●
Anforderungen
●
Formalismus
●
Rapid Random Trees
●
Graphensuche
●
●
●
 Zusammenfassung

Random Trees
 Zustände:
 Aktion:
 Dynamikfunktion: Ziel
 Kostenfunktion: 0 wenn Ziel erreicht, 1 Ziel nicht erreicht
Solange nicht am Ziel wiederhole:

1. wähle zufälligen Trajektorienpunkt in Graph
2. wähle zufällige Richtung
3. erzeuge neuen Zustand über Dynamik
4. füge neuen Zustand in den Graph ein
Start

Random Trees
 Zustände:
 Aktion: N = 100


Random Trees
 Zustände:
 Aktion: N = 1000


Random Trees
 Zustände:
 Aktion: N = 10000

→ Geringe Exploration

Rapidly Exploring Random Trees (RRT)
 Zustände:
 Aktion:

1. wähle zufälligen Trajektorienpunkt R in Zustandsraum
2. suche Punkt P im Graph mit geringster Distanz zu R
3. wähle Richtung des Vektors R - P
4. erzeuge neuen Zustand über Dynamik R-P R
P
5. füge neuen Zustand in den Graph an Knoten P ein

 Zustände:
 Aktion: N = 100


 Zustände:
 Aktion: N = 1000


 Zustände:
 Aktion: N = 1100


RRT in der Anwendung
 Adaption für Fahren in strukturierte Umgebungen

 Exploration mit RRT dann Bewertung und Selektion
über komplexe Kostenfunktion
 Hinderniskarte für schnelle Kollisionschecks
(ähnlich wie bei vorherigem Beispiel)
 Eingesetzt bei der DARPA Urban Challenge
2007 (Team MIT)
 Planungsfrequenz 10 Hz
 Verwendung von Dubinskurven in Dynamik

um nicht-holonome Dynamik zu beachten
RRT Adaption1
1
Y. Kuwata, G. A. Fiore, J. Teo, E. Frazzoli, J. P. How: Motion Planning for Urban Driving using RRT, In Proceedings of the IEEE International Conference on Intelligent Robots and Systems, 2008.
Zufallsbasiertes Sampling wurde in Form der sogenannten Rapidly Exploring Random

Trees (RRT) bereits im Rahmen der DARPA Urban Challenge zur Trajektorienplanung
eingesetzt.
Dabei kann das Sampling entweder rein zufällig, oder unter Verwendung eines
Fahrzeugmodells realisiert werden.
Für die Trajektorienplanung kommen aufgrund der nichtholonomen Beschränkungen

Verfahren mit Fahrzeugsmodellen (z.B. Dubinskurven) zum Einsatz.
Prinzipiell lässt sich zufallsbasiertes Sampling sowohl in strukturierter als auch

unstrukturierter Umgebung einsetzten.
Dubins Kurven
 Für ein nicht-holonomes Fahrzeugmodell
kann jeder Zielzustand des Fahrzeugs mit einer Kombination

aus 3 Primitiven auf dem kürzesten Weg erreicht werden [1].
 Die Primitive sind
1 On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents, Dubins 1957

Dubins Kurven
 es gibt nur 6 kurze, verschiedene Pfade:

 Pfade können geometrisch konstruiert werden
 Durchprobieren aller 6 Pfade um den kürzesten Pfad zu finden

 Keine Berücksichtigung von Hindernissen:
→ Kombination mit RRT zur Pfadplanung von nicht-holonomen Fahrzeugen

RRT mit Dubins Kurven

RRT mit Dubins Kurven (2. Versuch)

RRT Verbesserungen
 RRT*: kann gefundenen Pfad verbessern (konvergiert zum kürzesten Pfad)

 bidirektionale RRT: Exploration ausgehend von Start und Ziel
 Informed RRT*
 … bidirektionale RRT
 zufällige Exploration oft unvorteilhaft

𝒙 𝑖𝑛𝑖𝑡 𝒙 𝑡𝑎𝑟𝑔𝑒𝑡
 bei Großen Suchräumen langsam
 → Strukturierung des Suchraums

Graph-basierte Planung
 Aufbau/Verwendung eines (regelmäßigen) Graphen

 Anwendung von Algorithmen der Graphensuche
 z.B. Breitensuche, Tiefensuche
breadth-first depth-first
1. 1.
2. 3. 2. 5.
4. 3.
5. 6. 7. 4. 6. 7.

 Graphen mit heterogenen Kosten
Gesamtkosten:
A
5 3
cost-to-go:
B C  Kosten ab einem gewissen Knoten
4 1 1 2  zu Beginn der Suche unbekannt

 oft mit sog. Heuristikfunktion geschätzt
D E F G

 Dijkstra-Algorithmus:
In jedem Schritt wird der Knoten mit den geringsten Kosten exploriert.
●
betrachtet evtl. sehr viele Knoten um Ziel zu finden
●
konvergiert zum optimalen Pfad
 Greedy Best-First-Search:
In jedem Schritt wird der Knoten mit den geringsten Heuristikkosten exploriert.
●
findet durch die Heuristik oft schnell einen Pfad zum Ziel
●
findet den nicht immer den optimalen Pfad
 A* Algorithmus:
In jedem Schritt wird der Knoten mit den geringsten Kosten exploriert.
●
findet durch Heuristik oft schnell einen „guten“ Pfad
●
konvergiert zum optimalen Pfad

Hybrid-A*
 nicht-holonome Dynamik muss beachtet werden

 Ansatz: verwende nicht-holonomes Dynamikmodell
mit diskretisiertem Aktionsraum z.B.
...

Hybrid-A*
 Beispiel:
Pfadsuche auf Parkplatz Suchgraph
Pfad
Fahrzeug

Hybrid-A*
 Beispiel: Einparken mit Pfadsuche

Hybrid-A*
 Beispiel: Einparken mit Pfadsuche

Übersicht
 Grundlagen
●
●
●
Anforderungen
●
Formalismus
●
Rapid Random Trees
●
Graphensuche
●
●
●
 Zusammenfassung

 keine Modellierung von Steuergrößen und Systemdynamik

 Methode nach Ziegler et al.1 (Bertha-Benz Fahrt)
 Trajektorie als diskretisierte Kurve mit Zuständen
 Kostenfunktion
 Ableitungen werden durch finite Differenzen bestimmt:
 statisches Optimierungsproblem
1
J. Ziegler, P. Bender, T. Dang and C. Stiller, "Trajectory planning for Bertha — A local, continuous method," 2014 IEEE Intelligent Vehicles Symposium Proceedings, Dearborn, MI, 2014, pp. 450-457

 Kostenfunktion
laterale Distanz zur Spurmitte

Abweichung zur Referenzgeschwindigkeit
Beschleunigung
Ruck
Quadratische Kostenfunktion …
 ermöglicht Einsatz von optimierten Verfahren
 Lösung z.B. über SQP Verfahren
 Beschränkungen um Fahrzeugdynamik
zu beachten erhöhen Rechenzeit wieder
1
J. Ziegler, P. Bender, T. Dang and C. Stiller, "Trajectory planning for Bertha — A local, continuous method," 2014 IEEE Intelligent Vehicles Symposium Proceedings, Dearborn, MI, 2014, pp. 450-457

 statische Optimierung: Minimierung von Kosten über Zustände
 dynamische Optimierung: Minimierung von Kosten über Aktionen und Zustände
 Probleme mit dynamischer Optimierung oftmals komplizierter aber
ermöglichen direkte Berücksichtigung der Dynamikfunktion

 Formulierung ohne Beschränkung des Endzustands effizient in einem
Optimierungsproblem lösbar

 Beispiel: Parken mit einem nicht-holonomen Dynamikmodell

 Beschränkung des Lenkwinkels und des Abstands zum Ursprung
 Lösung mit num. Verfahren liefert Lösung mit glatten Verlauf der Stellgrößen
[1] [1]
1 Control-Limited Differential Dynamic Programming, Tassa et al. IEEE International Conference on Robotics & Automation (ICRA), 2014

 populäres Verfahren nach Werling et al. 1
Komponenten:
1. Lösung eines unbeschränkten Optimierungsproblems
2. Sampling der Zielzuständen
3. Bewertung und Selektion um Beschränkungen zu beachten
 Reaktives Verfahren
 Betrachtet kurzen Zeithorizont (ca. 3s)
Moritz Werling: Ein neues Konzept für die Trajektoriengenerierung und -stabilisierung in zeitkritischen Verkehrsszenarien. Automatisierungstechnik 60(1): 53-54 (2012)
Die grundlegende Idee des Algorithmus besteht in der Lösung eines unbeschränkten
Optimierungsproblems.
Die Vernachlässigung von Beschränkungen wie z.B. von Hindernissen oder

kinematischen Beschränkungen ermöglicht die geschlossene Lösung des
Optimierungsproblems.
Da nun aber eine ermittelte Lösung nicht mehr zwingend gültig ist, wird durch
Variation der Zielzustände eine ganze Schar optimaler Trajektorien berechnet.
Diese werden im darauffolgenden Schritt auf die Einhaltung der Beschränkungen

geprüft und es werden alle ungültigen Trajektorien verworfen.
1. Lösung eines unbeschränkten Optimierungsproblems:

 Startzustand zu Zeitpunkt
und Zielzustand zu Zeitpunkt
 Was ist eine „gute“ Verbindung zwischen Start- und Zielzustand?

 → für Fahrkomfort ist eine ruckoptimale Trajektorie wünschenswert
 analytische Lösung: quintische Polynome (5. Grad)

 Koeffizienten können in geschlossener Form über Start-/Zielzustand bestimmt werden
Ein häufig verwendetes Kriterium zur Bewertung des Fahrkomforts geplanter

Trajektorien ist der Ruck d.h. die zeitliche Ableitung der Beschleunigung. Aus diesem
Grund enthält das Gesamtkostenfunktional das Integral über den quadratischen Ruck.
Es kann gezeigt werden, dass die Funktionsklasse der quintischen Polynome das
gegebene Kostenfunktional minimieren.
Sowohl die relative Querbewegung zur Straße als auch die Längsbewegung werden
daher in Form von quintischen Polynomen dargestellt.
 Repräsentation der Trajektorie über jeweils ein

quintisches Polynom für lateral und longitudinale Bewegung
 Planung in Frenét-Koordinaten → relativ zu einer Referenzlinie
Trajektorie
Referenzlinie
quintische Polynome
 Definition:

2. Sampling der Zielzustände:
 je nach Situation werden verschiedene Zielzustände generiert

 je nach Situation Bewertung mit verschiedenen Kostenfunktionen
 z.B. lateral Planung mit hoher Geschwindigkeit:

 Startzustand Zielzustand
 Annahme: am Zielzustand parallele Bewegung zur Referenzlinie
Daher:
 Samplingparameter sind daher
 Kostenfunktion:

2. Sampling der Zielzustände:
 Ergebnis für laterales Sampling, optimale Trajektorie in grün
[1]
 ähnliches Vorgehen für longitudinales Sampling

1 Werling et al. Optimal Trajectory Generation for Dynamic Street Scenarios in a Fren ́ et Frame, ICRA 2010

3. Bewertung und Selektion:
 Kombination der Längs- und Quertrajektorien und Berechnung der Gesamtkosten

 Verwerfen von Trajektorien mit zu hohen Beschleunigungswerten
 Verwerfen von Trajektorien mit Kollisionen
 Selektion der Trajektorie mit geringsten Gesamtkosten (grün)

[1]
1
Moritz Werling, Sören Kammel, Julius Ziegler, Lutz Gröll: Optimal trajectories for time-critical street scenarios using discretized terminal manifolds. I. J. Robotic Res. 31(3): 346-359 (2012)
Nach der Erstellung der Trajektoriensätze für Längs- und Querbewegung werden diese
miteinander kombiniert und in Weltkoordinaten transformiert.
Dabei werden die (gewichteten) Kosten zu Gesamtkosten für jede resultierende

Trajektorie kombiniert.
Anschließend werden alle Trajektorien auf die Einhaltung der Beschränkungen geprüft
und abschließend die Trajektorie mit den geringsten Gesamtkosten ausgewählt.
1
Quelle: Moritz Werling, https://www.youtube.com/watch?v=Cj6tAQe7UCY

Übersicht
 Grundlagen
●
●
●
Anforderungen
●
Formalismus
●
Rapid Random Trees
●
Graphensuche
●
●
●
 Zusammenfassung

Lernziele Kap. 13: Trajektorienplanung
Mit dem gelernten Wissen sind Sie in der Lage:

 Den Unterschied zwischen Pfad- und Trajektorienplanung zu erläutern
 Den Unterschied zwischen High- und Low-Level-Stabilisierung zu erklären
 Gängige Ansätze zur Pfad- und Trajektorienplanung zu benennen und ihre prinzipielle
Funktionsweise zu erläutern
 Einen Planungsalgorithmus im Detail zu beschreiben

FAS Folien VL13 PDF

Hochgeladen von

Dokumentinformationen

Originaltitel

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

FAS Folien VL13 PDF

Hochgeladen von

Copyright:

Verfügbare Formate

Fahrerassistenzsysteme

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 1

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 2

Für eine komplexe Situation mit Fahrzeugen, Ampeln, Fußgängern, etc.,

Die Trajektorienplanung beschäftigt sich nun mit der Umsetzung

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 3

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 4

Sensorik inkl. digitale Karte und GPS

Trajektorienplanung und Fahrzeugregelung

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 5

Die Trajektorienplanung bildet, zusammen mit der Fahrzeugregelung, das letzte

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 6

Die Pfadplanung bezeichnet allgemein die Planung eines realisierbaren Pfads.

Im Unterschied zur Pfadplanung plant die Trajektorienplanung explizit mit zeitlichen

Ebene Frequenz Horizont

Routenplanung wenn nötig Start-Ziel

 Low-Level Stabilisierung (LLS):

 High-Level Stabilisierung (HLS):

 Kombination beider Varianten:

Vergleich zwischen High- und Low-level-Stabilisierung1

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 8

Hierbei wird im Folgenden zwischen kontinuierlicher Störung und

Bei impulsartigen Störungen, welche zu einer großen Regelabweichung

Kontinuierliche Störungen hingegen werden auf Ebene der

 Einhalten dynamischer Beschränkungen (kammscher Kreis, nicht holonome Dynamik)

 Anforderungen z.T. widersprüchlich und subjektiv

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 9

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 10

Die Einschränkung der Bewegungsfreiheit eines Roboters wird als Zwangsbedingung

Eine holonome Zwangsbedingung enthält hierbei keine Geschwindigkeitsabhängigkeit.

Fahrzeuge unterliegen i.A. nichtholonomen Beschränkungen: Hierbei enthält die

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 11

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 12

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 13

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 14

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 15

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 16

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 17

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 18

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 19

Solange nicht am Ziel wiederhole:

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 20

 Adaption für Fahren in strukturierte Umgebungen

 Verwendung von Dubinskurven in Dynamik

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 21

Zufallsbasiertes Sampling wurde in Form der sogenannten Rapidly Exploring Random

Für die Trajektorienplanung kommen aufgrund der nichtholonomen Beschränkungen

Prinzipiell lässt sich zufallsbasiertes Sampling sowohl in strukturierter als auch

 Für ein nicht-holonomes Fahrzeugmodell

kann jeder Zielzustand des Fahrzeugs mit einer Kombination

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 22

 es gibt nur 6 kurze, verschiedene Pfade:

 Durchprobieren aller 6 Pfade um den kürzesten Pfad zu finden

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 23

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 24

FAS, VL13 | Trajektorienplanung | WS 2022/2023 6. Februar 2023 25