AEP - Einparkalgorithmus

Autoren: Martin Theine und Patrick Schumann

Einleitung

Das Team „AEP - Einparkalgorithmus“ beschäftigt sich mit dem Programmablauf und der Implementierung des autonomen Einparkvorganges unter Verwendung der vorhandenen Einparksensorik. Dazu zählt zum einen das Detektieren einer Parklücke, welche eine außreichende Länge für das vorhandene Carolo-Cup Fahrzeug aufweist und das Einparkmanöver selbst. Dazu wurde auf Basis vorliegender Fachliteratur ein Konzept erarbeitet, welches nachfolgend erläutert wird.

Anforderungen

Durch das Lastenheft des Projekts "Autonomes Fahrzeug" werden nachfolgende Anforderungen an das Autonome Einparken und somit an den umgesetzten Einparkalgorithmus gestellt:

Abb. 1: REQ - Paralleles Einparken	Abb. 2: REQ - Start des Einparkmanövers
Abb. 3: REQ - Durchführen des Einparkmanövers	Abb. 4: REQ - Erfolgreiches Einparkmanöver

Pflichten / Ziele

Aus den genannten Anforderungen (Abb. 1 - Abb. 4) lassen sich nachfolgende Pflichten ableiten.

Abb. 5: Pflichten Team AEP - Einparkalgorithmus SoSe2020

Berechnungen

Die für die Berechnungen erforderlichen Parameter und Variablen werden in den nachfolgenden Parameterdateien deklariert.

• param_AEP.m
• param_CAR.m

Abmaße Carolo-Cup Fahrzeug

Beschreibung:	Bezeichnung in Matlab / Simulink:	Variablen:	Wert:
Fahrzeugbreite	PAR_CAR_Fahrzeugbreite_f64	$w$	0,200m
Fahrzeuglänge	PAR_CAR_Fahrzeuglaenge_f64	$l$	0,430m
Radstand	PAR_CAR_Radstand_f64	$l_R$	0,265m
Max. Lenkwinkel	PAR_CAR_max_Lenkeinschlag_alpha_deg_f64	$\alpha$	23°
Abstand Hinterachse - Front	PAR_CAR_Radstand_f64 + PAR_CAR_Mitte_Vorderachse_Vorne_f64	$l_F$	0,330m
Abstand Hinterachse - Heck	PAR_CAR_Mitte_Hinterachse_Ende_f64	$l_H$	0,100m
Wendekreisradius	PAR_CAR_Wendekreisradius_f64	$R$	siehe Berechnung
Benötigte Parklückenlänge	PAR_AEP_Parkluecke_Soll_f64	$l_P$	siehe Berechnung
Gemessene Parklückenlänge	AEP_parkluecke_ist_f64	$l_g$	siehe Berechnung
Umlenkwinkel	PAR_AEP_Umschlagwinkel_f64	$\beta$	siehe Berechnung
Endabstand Heck - Hindernis	AEP_Abstand_Hinten	$x_H$	siehe Berechnung

Wendekreisradius

$R = \frac{l_r}{\tan(\alpha)} = \frac{0,265m}{\tan(23^\circ)} = 0,624m$

Benötigte Parklücke

$l_P = \sqrt[]{2\cdot R\cdot w + (l_F)^2} + l_H = \sqrt[]{2\cdot 0,624m\cdot 0,200m + (0,330m)^2} + 0,100m = 0,695m$

Umlenkwinkel

$\beta = \arccos(\frac{2\cdot R - w}{2\cdot R}) = \arccos(\frac{2\cdot 0,624m - 0,200m}{2\cdot 0,624m}) = 32,88^\circ$

Endabstand Heck - Hindernis

$x_H = \frac{l_g - l}{2}$

Grundkonzept

Abb. 6: Konzept AEP - Einparkalgorithmus

Dieses Kapitel befasst sich mit dem Konzept des AEP - Eonparkalgorithmus. Das Konzept beruht auf zwei verschiedenen Ansätzen zum autonomen Einparken aus der Literatur und stellt eine kombination dieser dar.

Das Carlolo-Cup-Fahrzeug fährt zunächst parallel zu den Parkbuchten. Wurde eine passende Parklücke identifiziert, fährt das CCF so lange weiter, bis sich das Fahrzeugheck auf Höhe Heck des parkenden Autos befindet, hinter welchem eingeparkt werden soll (siehe Abb. 6 - 1). Dadurch wird beim Zurücksetzen so eng wie möglich um dieses vordere Hindernis herumgefahren, ohne es zu berühren. Daraufhin soll das Fahrzeug rückwärts eine Rechtskurve fahren, bis dieses im zuvor berechneten Umschlagwinkel zur Fahrbahn steht (siehe Abb. 6 - 2). Anschließend behält das Fahrzeug diesen Winkel zur Fahrbahn bei und fährt eine definierte Strecke gerade rückwärts, um so “tief” wie möglich in die Parklücke hineinzufahren (siehe Abb. 6 - 3). Nun soll das Fahrzeug rückwärts eine Linkskurve fahren, bis es mit der hinteren linken Ecke auf einen definierten Abstand zum dahinterliegenden Auto herangefahren ist (siehe Abb. 6 - 4). Abschließend werden zwei Korrekturzüge gefahren. Zunächst absolviert das Fahrzeug eine Rechtskurve vorwärts, bis es parallel zur Fahrbahn steht (siehe Abb. 6 - 5). Zuletzt fährt es gerade rückwärts, bis es mittig zwischen beiden parkenden Autos steht (siehe Abb. 6 - 6).

Struktur Matlab / Simulink

Abb. 7: Programmablaufplan - Einparkalgorithmus LueckeSuchen

Abb. 8: Programmablaufplan - Einparkalgorithmus Parken

Die nachfolgende Beschreibung des implementierten Zustandsautomaten soll in Verbindung mit den hinterlegten Programmablaufplänen (Abb. 7 und Abb. 8) eine schnellere Einarbeitung in das Simulink-Modell für den Einparkalgorithmus ermöglichen.

Erläuterungen zu Abb. 7:

Superstate „LueckeSuchen“ (AEP_AKT_Zustand = 1):

Entspricht dem Initialzustand des Zustandsautomaten.

Substate „FahreVorBisParkbucht“ (AEP_AKT_Zustand = 1.1):

Das Fahrzeug fährt nach Programmstart zunächst vorwärts und erfasst dabei die gefahrene Strecke ab dem Startpunkt (X: -9.58). Währenddessen wird laufend überprüft, ob die zurückgelegte Strecke dem Abstand zwischen Fahrzeugstartpunkt und Beginn der Parkbucht plus dem Abstand zwischen Fahrzeugfront und Vorderachse entspricht. Ist dies der Fall, so befindet sich das Fahrzeug mit dem vorderen rechten IR-Sensor am Beginn der Parkbucht und kann mit dem Ausmessen der Parklücken beginnen.

Superstate „ParkbuchtVermessen“ (AEP_AKT_Zustand = 2):

Parallel zur Suche einer passenden Lücke misst das Fahrzeug die gefahrene Strecke ab Beginn der Parkbucht. Entspricht die zurückgelegte Strecke der Länge der Parkbucht, bevor eine passende Lücke gefunden wurde, so wird das Fahrzeug angehalten. Dieser Zustand ist ein Substate des Superstates „LueckeSuchen“ und gleichzeitig Superstate für die nachfolgenden zwei Substates.

Substate „ParklueckeSuchen“ (AEP_AKT_Zustand = 2.1):

Das Fahrzeug fährt mit „Suchgeschwindigkeit“ vorwärts, bis der Beginn einer Parklücke gefunden wurde (IR-Sensor vorne rechts detektiert kein Hindernis mehr). In diesem Fall wird in den Substate „ParklueckeAusmessen“ gewechselt.

Substate „ParklueckeAusmessen“ (AEP_AKT_Zustand = 2.2):

Das Fahrzeug fährt mit „Vermessgeschwindigkeit“ vorwärts und misst die gefahrene Strecke ab Beginn der Parklücke. Wird das Ende einer Parklücke gefunden (IR-Sensor vorne rechts detektiert ein Hindernis), so wird geprüft, ob die zurückgelegte Strecke größer oder gleich der im Vorfeld berechneten benötigten Parklückengröße ist. Wenn die Lücke kleiner ist, wird zurück in den Substate „ParklueckeSuchen“ gewechselt und erneut nach dem Beginn einer Lücke gesucht. Ist die Lücke groß genug, so bleibt das Fahrzeug stehen (Wechsel in den State „Ende“) oder beginnt durch Wechsel in den Superstate „Parken“ mit dem Einparkvorgang (je nach Konfiguration in der start.m-Datei: Parameter „Measure_or_Park“). Alternativ kann es vorkommen, dass kein Hindernis in der Parkbucht steht. In diesem Fall soll das Fahrzeug trotzdem einparken, sobald die zurückgelegte Strecke mindestens der Summe aus benötigter Parklückengröße und der doppelten Fahrzeuglänge entspricht. Trifft diese Bedingung zu, so wird das Bit „kein_Hindernis“ gesetzt und dadurch direkt in den Substate „FahreRechtskurve“ gewechselt, da nun kein vorderes Hindernis existiert, an dem sich das Fahrzeug weiterhin orientieren kann. Zuvor wird ebenfalls geprüft, ob das Fahrzeug stehenbleiben oder einparken soll (Parameter „Measure_or_Park“).

Erläuterungen zu Abb. 8:

Superstate „Parken“ (AEP_AKT_Zustand = 3):

Entspricht dem Initialzustand für den Einparkvorgang (wenn Bit „kein_Hindernis“ nicht gesetzt).

Substate „FahreVorBisHinterachseAufHoeheHeckHindernis“ (AEP_AKT_Zustand = 3.1):

Da eine passende Lücke gefunden wurde, wird zunächst das „Blinker_Rechts_Manual_Enable_Bit“ gesetzt. Das Fahrzeug fährt weiter mit „Vermessgeschwindigkeit“ vorwärts, bis sich die Hinterachse auf Höhe Heck des vorderen Hindernisses befindet (IR-Sensor hinten rechts detektiert ein Hindernis). Daraufhin wird in den Substate „FahreVorBisStartposition“ gewechselt.

Substate „FahreVorBisStartposition“ (AEP_AKT_Zustand = 3.2):

Das Fahrzeug fährt weiterhin mit „Vermessgeschwindigkeit“ vorwärts und erfasst dabei die gefahrene Strecke. Ist die zurückgelegte Strecke größer oder gleich dem Abstand der Hinterachse bis zum Fahrzeugende plus der definierten „Korrekturstrecke_Vor“, so ist das Fahrzeug maximal nach vorne gefahren und es wird in den Substate „FahreRechtskurve“ gewechselt.

Substate „FahreRechtskurve“ (AEP_AKT_Zustand = 3.3):

Entspricht dem Initialzustand für den Einparkvorgang (wenn Bit „kein_Hindernis“ gesetzt).

Zunächst wird die geregelte Geradeausfahrt ausgeschaltet (Famo_Modi_Schalter_Lw_int = 2). Die Lenkung des Fahrzeugs wird maximal nach rechts eingeschlagen. Das Fahrzeug fährt nun so lange mit „Einparkgeschwindigkeit“ rückwärts eine Rechtskurve, bis der gemessene Gierwinkel dem zuvor berechneten Umschlagwinkel entspricht.

Substate „BerechnungenMitParklueckeIst“ (AEP_AKT_Zustand = 3.4):

An dieser Stelle werden nachfolgend benötigte Größen in Abhängigkeit von der gemessenen Parklückenlänge berechnet (siehe Abschnitt „Berechnungen“). Wenn die berechnete „Korrekturstrecke_Rueck“ größer als der festgelegte Maximalwert ist, so wird diese auf den Maximalwert gesetzt. Anschließend wird in den Substate „FahreKorrekturstreckeRueck“ gewechselt.

Substate „FahreKorrekturstreckeRueck“ (AEP_AKT_Zustand = 3.5):

Die Lenkung des Fahrzeugs wird in Mittelstellung gebracht und erneut die zurückgelegte Strecke erfasst. Das Fahrzeug fährt im zuvor erreichten Winkel zur Fahrbahn gerade rückwärts, bis die berechnete „Korrekturstrecke_Rueck“ zurückgelegt wurde, um maximal tief in die Parklücke hineinzufahren. Daraufhin wird in den Substate „FahreLinkskurve“ gewechselt.

Substate „FahreLinkskurve“ (AEP_AKT_Zustand = 3.6):

Die Lenkung wird maximal nach links eingeschlagen. Das Fahrzeug fährt mit aktiver Streckenerfassung und gleichbleibender Geschwindigkeit rückwärts eine Linkskurve, bis …

(1) … der linke hintere Infrarotsensor einen festgelegten Mindestabstand zum hinteren Hindernis misst. Dies ist der Fall, wenn ein hinteres Hindernis vorhanden ist.

(2) … die zurückgelegte Strecke der festgelegten Streckenbegrenzung entspricht. Diese Abbruchbedingung wird nur geprüft, wenn sich hinter dem Fahrzeug kein Hindernis befindet (beide hinteren IR-Sensoren den Maximalabstand messen) und die Parklücke zu klein ist, um bereits beim Rückwärtsfahren parallel zur Fahrbahn zu gelangen. Das ist der Fall, wenn die erstmögliche Parklücke in der Parkbucht nur durch ein vorderes Hindernis begrenzt wird und groß genug zum Einparken jedoch kleiner als 95 cm ist.

(3) … bis der gemessene Gierwinkel dem Schlussparkwinkel entspricht (Fahrzeug parallel zur Fahrbahn). Dies ist der Fall, wenn die Parklücke groß genug (> 95 cm) ist, um bereits beim Rückwärtsfahren parallel zur Fahrbahn zu gelangen.

Im Fall (1) oder (2) steht das Fahrzeug noch nicht parallel zur Fahrbahn und muss daher einen Korrekturzug nach vorne fahren, um dies zu erreichen. Es wird in den Substate „FahreKorrekturzugVorFall1“ gewechselt. Im Fall (3) steht das Fahrzeug bereits parallel zur Fahrbahn. Daher muss kein Korrekturzug nach vorne gefahren werden, wie in den Fällen (1) und (2). Es wird direkt in die „Junction“ gewechselt.

Substate „FahreKorrekturzugVorFall1“ (AEP_AKT_Zustand = 3.7):

Die Lenkung des Fahrzeugs wird maximal nach rechts eingeschlagen. Das Fahrzeug fährt nun so lange mit „Vermessgeschwindigkeit“ vorwärts eine Rechtskurve, bis der gemessene Gierwinkel dem festgelegten Schlussparkwinkel entspricht (Fahrzeug parallel zur Fahrbahn).

Junction:

An dieser Stelle wird mit Hilfe einer Junction (siehe Stateflow-Modell) geprüft, ob …

(1) … das Bit „kein_Hindernis“ zuvor gesetzt worden ist. In diesem Fall wird das Fahrzeug gestoppt, da keine Hindernisse vorhanden sind, an denen sich nachfolgend orientiert werden kann (Wechsel in State „Ende“).

(2) … beide hinteren IR-Sensoren den Maximalabstand messen, also hinten kein Hindernis vorhanden ist. Auch in diesem Fall wird das Fahrzeug gestoppt, indem in den State „Ende“ gewechselt wird.

(3) … einer der beiden hinteren IR-Sensoren einen größeren Abstand misst als den zuvor berechneten Endabstand. Dies ist der Fall, wenn hinten ein Hindernis vorhanden ist und das Fahrzeug zu weit vorne in der Parklücke steht. Es ist also notwendig einen Korrekturzug rückwärts zu fahren, um mittig zwischen vorderem und hinterem Hindernis zu stehen. Dazu wird in den Substate „FahreKorrekturzugRueck“ gewechselt.

(4) … einer der beiden hinteren IR-Sensoren einen kleineren Abstand misst als den zuvor berechneten Endabstand. Dies ist der Fall, wenn hinten ein Hindernis vorhanden ist und das Fahrzeug zu weit hinten in der Parklücke steht. Es ist also notwendig einen Korrekturzug vorwärts zu fahren, um mittig zwischen vorderem und hinterem Hindernis zu stehen. Nun wird geprüft, ob der berechnete Endabstand größer als die maximale Sensorreichweite ist. In diesem Fall wird der Endabstand auf den Maximalwert und das „Weiterfahrt_Enable_bit“ gesetzt. Daraufhin wird in den Substate „FahreKorrekturzugVorFall2“ gewechselt.

Substate „FahreKorrekturzugRueck“ (AEP_AKT_Zustand = 3.81):

Die Lenkung des Fahrzeugs wird in Mittelstellung gebracht. Das Fahrzeug fährt mit „Einparkgeschwindigkeit“ gerade rückwärts, bis der zuvor berechnete Endabstand zum hinteren Hindernis erreicht ist. Daraufhin wird das Fahrzeug durch einen Wechsel in den State „Ende“ gestoppt.

Substate „FahreKorrekturzugVorFall2“ (AEP_AKT_Zustand = 3.82):

Die Lenkung des Fahrzeugs wird in Mittelstellung gebracht. Das Fahrzeug fährt mit „Einparkgeschwindigkeit“ gerade vorwärts, bis der zuvor berechnete Endabstand zum hinteren Hindernis erreicht ist. Daraufhin wird geprüft, ob das „Weiterfahrt_Enable_bit“ gesetzt wurde. Falls nicht, wird das Fahrzeug durch einen Wechsel in den State „Ende“ gestoppt. Falls doch wird in den Substate „FahreWeiterBisMitte“ gewechselt.

Substate „FahreWeiterBisMitte“ (AEP_AKT_Zustand = 3.9):

Dieser Substate wird nur aktiv, wenn das Weiterfahrt_Enable_bit gesetzt wurde. Dies ist nur der Fall, wenn der berechnete Endabstand zum hinteren Hindernis die Sensorreichweite übersteigt. Somit kann die hintere IR-Sensorik nicht vollständig genutzt werden, um mittig zwischen beide Hindernisse zu fahren. Daher wird zusätzlich die Streckenerfassung genutzt, um die restliche Strecke vorwärts zu fahren, die über die Reichweite der Sensoren hinausgeht. Diese Differenz wird im Vorfeld im Substate „BerechnungenMitParklueckeIst“ ermittelt. Das Fahrzeug fährt also weiter vorwärts, bis die zurückgelegte Strecke größer oder gleich der berechneten Differenz ist. Daraufhin wird in den State „Ende“ gewechselt und das Fahrzeug gestoppt.

State „Ende“ (AEP_AKT_Zustand = 4):

Das „Warnblinker_Manual_Enable_bit“ wird gesetzt, um anzuzeigen, dass der Parkvorgang beendet wurde und das Fahrzeug wird angehalten.

Unit- und Integrationstest

Abb. 8: Simulinkbibliothek - Autonomes EInparken

Mit Hilfe der Unit- und Integrationstests soll die Funktion des AEP - Einparkalgorithmus getestet und überpfrüft werden. Nach erfolgreichem Bestehen der Tests kann so gewährleistet werden, dass die Anforderungen aus dem Lastenheft erfüllt sind. Nachfolgend wird das Vorgehen beim Unit- und Integrationstest stichpunktartig erläutert. Eine Vorlage und die genaue Beschreibung zur Durchführung der Tests ist im SVN hinterlegt.

Vorlage im SVN: https://svn.hshl.de/svn/MTR_SDE_Praktikum/trunk/Teams/AEP/Dokumentation

Abb. 8 zeigt die Simulink-Bibliothek "AEP-Autonomes Einparken" mit den aus dieser hervorgehenden Variablen. Die rot eingerahmten Variablen werden vorwiegend zur Auswertung der Unittests herangezogen. Zusätzlich müssen während der Simulation weitere Merkmale beobachtet und dokumentiert werden.

Vorgehen Unittest

1. Öffnen der Datei "AEP_Unittest_und_Integrationstest.docx" in Word
2. Vorlage zu den Unittests (siehe „AEP_Vorlage_Unittest_und_Integrationstest.docx“ Kapitel 2) ausfüllen. Wichtig: Anmerkungen zur Simulation beachten!
3. Öffnen der Datei "start.m" in Matlab R2019b
4. Folgende Parameter setzten
   • Auswahl des Simulationstyps: 1 (Simulation offline)
   • Laden verschiedener Fahrbahnformen: 0 (Rundkurs ohne Kreuzung)
   • Auswahl des Simulationsmodus: 2 oder 3 (je nach durchzuführenden Unittest)
   • Im Fall AEP: Positionswahl der Objekte 0…12 (je nach durchzuführenden Unittest)
5. Unter Registerkarte "Editor" in Matlab auf "RUN" klicken, um Matlab-Skript auszuführen --> Simulink-Modell öffnet sich
6. Erforderliche Variablen (siehe "AEP_Vorlage_Unittest_und_Integrationstest.docx" Kapitel 2) im Watch-Window anzeigen und zur Auswertung der Test nutzen
7. Im Subsystem "AEP-Autonomes Einparken" mit klicken von „Run“ unter der Registerkarte "Simulation" die Simulation starten
8. Testergebnisse in "AEP_Vorlage_Unittest_und_Integrationstest.docx" Kapitel 2 dokumentieren und mit nachfolgender Dateibezeichnung in SVN hinterlegen . Gegebenenfalls zusätzlich Screenshots der Simulation in SVN hinterlegen
   • Ablage in SVN: https://svn.hshl.de/svn/MTR_SDE_Praktikum/trunk/Teams/AEP/Dokumentation/Unittest
   • Dateibezeichnung Testergebnisse (PDF-Datei): „CCF_AEP_Testergebnis_Datum_NameTestperson.pdf“
   • Dateibezeichnung Screenshots (PNG-Datei): „CCF_AEP_Testergebnis_IDxxx_Datum_NameTestperson.png“

Testfälle Unittest

Unittest-ID:	Beschreibung:	Testergebnis:	Stand:
001	Test der Funktionen Streckenerfassung und Parklückenvermessung	i.O	18.06.2020
002	Einzelnes Hindernis am Anfang der Parkbucht	i.O	18.06.2020
003	Kein Hindernis in der Parkbucht	i.O	18.06.2020
004	Keine ausreichend große Parklücke	i.O	18.06.2020
005	Ein einzelnes Hindernis im Abstand von < 0,95m vom Anfang der Parkbucht	i.O	18.06.2020
006	Größte Parklücke an beliebiger Position und prozentuale Erhöhung der Parklückenlänge	i.O.	19.06.2020
007	Größte Parklücke an beliebiger Position, Parklückenlänge minimal und prozentuale Erhöhung der Hindernisbreite	i.O.	19.06.2020

Vorgehen Integrationstest

Der Integrationstets wird im Anschluss an die Unittest durchgeführtt und bildet die abschließende Funktionskontrolle des AEP - Einparkalgorithmus. Hierzu wird der während der Entwicklungs- und Optimierungsphase im SVN angelegte "Branch" mit dem "Trunk" gemerget und anschließend die Unittests "Test der Funktionen Streckenerfassung und Parklückenvermessung" und "Größte Parklücke an beliebiger Position und prozentuale Erhöhung der Parklückenlänge" wiederholt. Die Ergebnisse der Unittets vor und nach dem mergen werden miteinander verglichen. Sind die Ergebnisse der Test identisch, so gilt der Integrationstest als bestanden.

Testfälle Integrationstest

Unittest-ID:	Beschreibung:	Testergebnis:	Stand:
001	Test der Funktionen Streckenerfassung und Parklückenvermessung	...	...
006	Größte Parklücke an beliebiger Position und prozentuale Erhöhung der Parklückenlänge	...	...

Zusammenfassung

Offene Punkte

• Umsetzung des vorhandenen Offline-Konzepts im Online-Modell​ (reales Fahrzeug)
• Anpassen der verwendeten Formeln im Hinblick auf reale Einflüsse
• ...

Ausblick

• Nutzen des Kamera-Moduls und des LIDAR-Sensor zum Erfassen und Ausmessen der Parklücke
• Optimierung der Fahrzeuggeometrie um größeren Lenkwinkel zu relisieren
• ...

Literaturverzeichnis

---

→ zurück zum Hauptartikel: SDE Praktikum Autonomes Fahren
→ zum Artikel Autonomes Einparken: AEP - Autonomes Einparken
→ zum Artikel Einparksensorik: AEP - Einparksensorik