SenKam - Stopplinienerkennung

Der Rundkurs kann mehrere Kreuzungen beinhalten, an denen Stopplinien markiert werden und dort das Fahrzeug die Geschwindigkeit reduzieren und vor der Stopplinie halten muss. Dies wird in REQ 10.2340 beschrieben.

Abbildung 17: Requirement Stopplinienverhalten

Folgende Aufgaben werden an die Anwendung gefordert:

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

Ablaufplan

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

Abbildung 18: Programmablaufplan Stopplinienerkennung

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: $y_P = ax^2_P + bx_P +c$
Die Stopplinie liegt zu Beginn der Schnittpunktberechnung lediglich als zwei Koordinatenpunkte vor: $P_L = \binom{x_L}{y_L} 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: $g_s = (P_L-P_R)*s+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.

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.