SenKam - Stopplinienerkennung

→ zurück zur Bahnplanung und Spurführung: BSF - Bahn- und Spurführung
→ zurück zur Übersicht: SDE-Team_2019/20

Bearbeiter: Marcel Bröckelmann
Betreuer: Prof. Schneider

Einleitung

Während der Fahrt auf dem Rundkurs trifft das Fahrzeug auf mehrere Stopplinien. Diese sind zum bei einer Kreuzung vorzufinden. Das Fahrzeug muss diese erkennen, die Geschwindigkeit drosseln und wie in REQ 10.2340 im Abstand von maximal 15cm vor der Stopplinie zum stehen kommen. Anschließend muss das Fahrzeug zwei Sekunden die Weiterfahrt verhindern und abschließend diese regulär fortsetzen.

Abbildung 1: Requirement Stopplinienverhalten

Anforderungen

Damit das Requirement erfüllt werden kann, müssen Anforderungen an die entstehende Anwendung gestellt werden. Diese Anforderungen sind in der Folgenden Tabelle aufgelistet:

ID	Inhalt	Ersteller	Datum	Prüfer
1	Die Fahrzeugposition und Winkel müssen eingelesen werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
2	Der Schalter zur Aktivierung der Funktion muss eingelesen werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
3	Die Parameter des Bahnspurpolynoms müssen eingelesen werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
4	Die Koordinaten der Stopplinien müssen eingelesen werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
5	Die Stopplinienkoordinaten müssen in das Fahrzeugkoordinatensystem umgewandelt werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
6	Nur Stopplinien vor dem Fahrzeug dürfen berücksichtigt werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
7	Die verbleibenen Stopplinien müssen überprüft werden, ob die Stopplinien in der eigenen Fahrspur liegen.	Marcel Bröckelmann	25.10.2019	Marius Köhler
8	Das Fahrzeug muss die Distanz zu den Stopplinien ermittelt werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler
9	Das Fahrzeug muss ein Stopplinien-Flagbit und eine Distanz ausgeben.	Marcel Bröckelmann	25.10.2019	Marius Köhler
10	Das Flagbit muss gesetzt werden und der kürzeste Abstand zu den Stopplinien ausgegeben werden.	Marcel Bröckelmann	25.10.2019	Marius Köhler

Funktionaler Systementwurf / Technischer Systementwurf

Ablaufplan

Um diese Anforderungen zu erfüllen, wurde ein Ablaufplan erstellt. Dieser ist in Abbildung 2 zu sehen.

Zu Beginn der Stopplinienerkennung in der Offline-Simulation muss dem Programm die Position des Fahrzeug, dessen Winkel im Weltkoordinatensystem, die Start- und Endpunkte der Stopplinien und das Bahnspurpolynom übergeben werden.
Da die Funktion sowohl einen Abstand zur Stopplinie als auch ein Flagbit übermitteln soll, müssen diese zu Beginn des Programms initialisiert werden.Sollten Stopplinien übergeben worden sein, so muss zuerst eine Transformation der Stopplinienpunkte in das Fahrzeugkoordinatensystem stattfinden.

Anschließend müssen alle Stopplinien-Koordinaten-Paare extrahiert werden, welche sich innerhalb eines Radius' um das Fahrzeug befinden. Sofern nach dieser Extraktion Stopplinien verbleiben, muss anschließend ermittelt werden, welche Stopplinien sich vor dem Fahrzeug befinden.
Da auch Stopplinien auf der Gegenfahrbahn erkannt werden können, diese jedoch nicht beachtet werden müssen, wird im darauf folgenden Schritt ermittelt, welche Stopplinien einen Schnittpunkt mit dem Bahnspurpolynom besitzen. Hierzu wird eine Gerade durch die Punkte der jeweiligen Stopplinien gezogen und mittels der PQ-Formel die Schnittpunkte ermittelt. Die errechneten Schnittpunkte werden anschließend daraufhin geprüft, ob sie sich zwischen den beiden Stopplinienpunkten befinden. Ist dies der Fall, so wird diese Stopplinie gespeichert.

Da mehrere legitime Schnittpunkte möglich sind, muss anschließend der Abstand zwischen den Schnittpunkten zu dem Fahrzeug ermittelt werden. Jeder ermittelte Wert muss mit dem vorigen Abstand verglichen werden und der kleinere Abstand gespeichert werden.Zusätzlich muss nach dem ersten setzen des Minimalabstands das Flagbit auf 1 gesetzt werden.

Nachdem alle Abstände verglichen wurden oder in einer der zuvor verwendeten if-Verzweigungen der Workflow verlassen wurde, wird abschließend Flagbit und der Abstand übermittelt.

Schnittpunktberechnung
Um den Schnittpunkt zwischen Bahnspurpolynom und Stopplinie zu ermitteln, liegt das Bahnspurpolynom in folgender Form vor: ${\displaystyle y_P = ax^2_P + bx_P +c }$
Die Stopplinie liegt zu Beginn der Schnittpunktberechnung lediglich als zwei Koordinatenpunkte vor: $\vec{P_L} = \binom{x_L}{y_L}; \vec{P_R} = \binom{x_R}{y_R}$
Um die Berechnung des Schnittpunktes durchführen zu können, muss zunächst die Gerade in Vektorform erstellt werden. Die Form ist hierbei wie folgt: $\vec{g_s} = (\vec{P_L}-\vec{P_R})*s+\vec{P_R}=\binom{x_L-x_R}{y_L-y_R}*s+\binom{x_R}{y_R}$

Da diese Formeln nicht in der derzeitigen Form gleichgesetzt werden könnne, muss die Vektorform in die Berechnung des X- und Y-Wertes erfolgen, sodass das Ergebnis der nachfolgenden Formel entspricht. $x_g = (x_L-x_R)*s+x_R; y_g=(y_L-y_R)*s+y_R$

Anschließend können das Bahnspurpolynom und die Vektorschreibweise der Stopplinie gleichgesetzt werden: $(x_L-x_R)*s+x_R=x_P; (y_L-y_R)*s+y_R=ax^2_P+bx_P +c$ ;
Da Xp durch das zusammenführen der Formeln einen Ersatzwert hat, wird dieser anschließend ersetzt: $(y_L-y_R)*s+y_R=a*((x_L-x_R)*s+x_R)^2+b*((x_L-x_R)*s+x_R)+c$
Um den Schnittpunkt zu ermitteln, muss durch die PQ-Formel der Wert von S ermittelt werden, wofür die entstandene Gleichung in entsprechende Form umgewandelt werden muss. $x^2+x*p+q=0; x_{1,2}=-\frac{p}{2}\pm \sqrt{\frac{p}{2}-q}$
Hierzu muss die Gleichung zuerst umgeformt werden: $\frac{s^2+s(2ax_l x_r-a^2 x_r +bx_l -bx_R -y_L +y_R)}{ax^2_L-2ax_Lx_R+ax^2_R}+\frac{ax^2_r+bx_r+c}{ax^2_L-2ax_Lx_R+ax^2_R}=0$

Abschließend kann die Gleichung in die PQ-Formel eingesetzt werden, sodass folgende Formel entsteht: $s_{1,2}=-(\frac{\frac{2ax_Lx_r-ax^2_R+bx_L-bx_R-y_L+y_R}{ax^2_L-2ax_Lx_R+ax^2_R}}{2})\pm \sqrt{((\frac{\frac{2ax_Lx_r-ax^2_R+bx_L-bx_R-y_L+y_R}{ax^2_L-2ax_Lx_R+ax^2_R}}{2})^2-\frac{ax^2_R+bx_R+c}{ax^2_L-2ax_Lx_R+ax^2_R})}$
Hierdurch kann das S ermittelt werden, welches in die Vektorform der Geraden eingefügt werden kann, um den Schnittpunkt zu ermitteln. Zusätzlich gibt der Wert Aufschluss darüber, ob der Schnittpunkt innerhalb der Stopplinie oder außerhalb der Begrenzungen sich befindet. Besitzt S eine Wert zwischen 0 und 1, so liegt der Schnittpunkt in den Begrenzungen. Sollte dies nicht der Fall sein, so liegt der Schnittpunkt außerhalb der Begrenzungen und die Stopplinie kann ignoriert werden.

Komponentenspezifikation

Um das Programm vollständig zu programmieren, muss genau beschrieben werden, welche Eingaben und Ausgaben von der Komponente erwartet werden. Als Eingaben sind die Position des Fahrzeugs und dessen Postion im Weltkoordinatensystem zu zählen. Außerdem müssen die Parameter des Bahnspurpolynoms und die Koordinaten der Stopplinien eingelesen werden.

Sollten keine Stopplinien vorhanden sein oder im Zuge der Berechnung keine gültigen Schnittpunkte ermittelt werden, so soll die Komponente ein Flagbit mit Wert "0" und einen Abstand von Wert "0" ausgeben.
Wird ein gültiger Schnittpunkt ermittelt, so soll das Flagbit mit Wert "1" und der ermittelte Abstand ausgegeben werden.

Programmierung

Da die Daten über Spur und Stopplinien in Online- und Offline-Simulation unterschiedlich sind, muss die Implementierung der Funktion der Stopplinienerkennung an verschiedener Stelle stattfinden. In der Offline-Simulation wird hierfür der Offline-Sensoren-Block verwendet, da dieser in der Online-Simulation durch die Kamerafunktionen und deren Funktionen ersetzt wird.

Die Stopplinienerkennung wird mittels einer implementierten Matlab-Funktion realisiert, welche die Eingänge A,B,C des Fahrspurpolynoms, X,Y und Ausrichtung des Fahrzeugs und die Liste aller Stopplinien besitzt. Als Ausgänge werden ein double für den Abstand und ein Boolean für das Flagbit definiert.
Zu Beginn wird ermittelt, ob Stopplinien vorhanden sind. Hierzu wird der Abstand der ersten beiden Stopplinienpunkte verglichen und überprüft, ob der Abstand zwischen den beiden Punkten größer 0 ist. Ist dies der Fall, so wird die Funktion regulär fortgesetzt.
Um Stopplinien zu filtrieren, welche sich innerhalb eines Radius' um das Fahrzeug befinden, müssen die Stopplinien-Koordinaten vom Weltkoordinatensystem in das Fahrzeugkoordinatensystem überführt werden. Hierzu wird eine Drehmatrix auf die kordinaten angewendet, welche die Stopplinien-Koordinaten unter Zuhilfenahme der Fahrzeugposition und dem Winkel im Weltkoordinatensystem umwandelt. Die resultierenden Koordinaten beschreiben die relative Postion der Stopplinien zum Fahrzeug.

Da nicht alle Stopplinien zu überprüfen sind und eine realistische Sichtweite in der Offline-Simulation simuliert werden soll, wird der Abstand der Punkte zum Fahrzeug mittels des Satzes von Pythagoras ermittelt. Stopplinien-Koordinaten-Paare mit einem kleineren Abstand als die zuvor festgelegte Sichtweite werden gespeichert und mit ihnen weitergearbeitet.Da Stopplinien hinter dem Fahrzeug nicht berücksichtigt werden müssen, werden diese anschließend durch auf negative X-Werte überprüft. Alle Stopplinienpaare mit mindestens einem positiven X-Wert werden gespeichert und anschließend weiterverwendet.
Die Verbleibenden Stopplinien, bestehend aus jeweils 2 Punkten, können trotz ihrer Distanz zum Fahrzeug dennoch für dieses irrelevant sein, wenn die Stopplinie nicht die prädizierte Fahrspur schneidet.Daher wird im nachfolgenden Schritt eine Schnittpunktberechnung mittels der zuvor errechneten Formel durchgeführt. Der daraus resultierende Faktor s wird dabei ermittelt und anschließend überprüft. Sollte dieser Wert zwischen 0 und 1 liegen, so wird der Schnittpunkt errechnet, indem s in die Geradengleichung eingefügt wird.
Sobald alle gültigen Schnittpunkte errechnet wurden, wird mittels einer Abstandsberechnung der zum Fahrzeug am nächsten befindliche Punkt ermittelt. Der Abstand zu diesem Punkt wird in einer Ausgabevariable gespeichert und das Flagbit auf 1 gesetzt.
Abschließend wird, unabhängig davon ob eine gültige Stopplinie gefunden wurde, der Abstand und das Flagbit ausgegeben.

Die entsprechende Funktion ist unter https://svn.hshl.de/usvn/project/MTR_SDE_Praktikum/show/branches/2019_10_11_BSF_Stopplinienverhalten/Marcel_Broeckelmann_Arbeitsordner/CaroloCupFahrzeug/bibliotheken/bib_Sensoren_Aktoren_offline.mdl zu finden. In dieser Bibliothek befindet sich der Block "Stopplinienerkennung", welches die neu entwickelte Funktion beinhaltet.

Komponententest

Ein Komponententest konnte bis zum derzeitigen Zeitpunkt nicht durchgeführt werden.

Zusammenfassung

Die Stopplinienerkennung ermittelt unter der Verwendung der Fahrzeugparameter, Spurpolynom-Parameter und der Stopplinienparameter Stopplinien, welche die prädizierte Fahrspur des Fahrzeugs schneidet.
Hierbei wird mittels Abstandsberechnung und Gating gültige Stopplinien ermittelt. Anschließend wird durch eine Schnittpunktberechnung der entsprechende Punkt errechnet. Der Abstand zu diesen Schnittpunkten wird ermittelt und der kürzeste Abstand zusammen mit einem gesetzten Flagbit ausgibt.

Link zum Quelltext in SVN

Der Link konnte derzeit nicht bereitgestellt werden.

→ zurück zur Bahnplanung und Spurführung: BSF - Bahn- und Spurführung
→ zurück zur Übersicht: SDE-Team_2019/20