Objekttracking mit LiDAR

Autoren: Ahmad Hassan, Lihui Liu

Einleitung

Die Gruppe Hassan/Liu beschäftigt sich im Wintersemester 2020/2021 mit dem Thema Objekterkennung und Objekttracking mit dem Hokuyo LiDAR. Im Sommersemester wurde schon ein Arbeitskonzept dazu entworfen inkl. einem Signalflussplan sowie einem morphologischen Kasten. In dem WS20/21 soll eine Umsetzung dieses Arbeitskonzepts in C erfolgen. Zuerst kommt die Einbindung bzw. Einrichtung und Ansteuerung des LiDARs in C. Darauffolgend wird die Koordinatentransformation implementiert. Zum Testen des Schnittstellenkommunikationsprinzips wird auch eine Funktion programmiert, die innerhalb des ersten Monats des Semesters eine Dummy Objektliste erstellt, die an die DS1104 verschickt werden kann anhand von dem vom Team Heuer/Kruse entwickelten Kommunikationsframework. Zukünftig dient diese Funktion dem Versand der tatsächlichen, erfassten Objekte und ihrer Attribute. Es ist die Implementierung eines Connected Component Clusterung Algorithmus zur Segmentierung und eines L-Shape Fitting Algorithmus zur Objektbildung vorgesehen. An letzter Stelle findet die Programmierung des Kalman-Filters zur Objektverfolgung und Schätzung Attribute wie Geschwindigkeit und Beschleunigung der verfolgten Objekte. Eine detaillierte Beschreibung der erforderlichen, fachlichen Grundlagen und des entwickelten Konzepts ist im Objekterkennung mit Hokuyo LiDAR zu finden.

Eine Auflistung der Aufgaben zu den entsprechenden Meilensteinen ist unten ersichtlich.

1. Meilenstein 3:
   1. Implementierung der Einbindung und Ansteuerung des Hokuyo LiDAR
   2. Implementierung der Koordinatentransformation
   3. Testdokumentation der Koordinatentransformation
   4. Versand einer Dummy-Objektliste an DS1104
   5. Implementierung der Segmentierung (Connected Component Clustering)
   6. Erstellen eines PAPs zum L-Shape Fitting Algorithmus
2. Meilenstein 4:
   1. Implementierung des L-Shape Fitting Algorithmus
   2. Implementierung des Kalman Filters
   3. Attribute schätzen
   4. Versand der echten Objektlisten an die dSpace-Karte
   5. Dokumentation im Wiki

Anforderungen

Abbildung: 1 der Anforderung

Abbildung: 2 der Anforderung

Abbildung: 3 der Anforderung

Pflichten

Die vorgenommenen Pflichten zeigen die folgenden Abbildungen:

Abbildung 4: Pflichtheft Teil1

Abbildung 5: Pflichtheft Teil2

Abbildung 6: Pflichtheft Teil3

Komponentenspezifikation (Hier pro Modul ein PAP)

Erstellung einer Dummy Objektliste zum Testen des Schnittstellenkommunikationsprinzips für LiDAR Daten

Für den Fall, dass kein Lidar genutzt wird werden Dummyparameter für die Objektliste des Lidar generiert. Die versandete Objektliste ist wie folgende Tabelle dargestellt.

Signal	Bedeutung	Datentype
nObjekte	Gesamtanzahl der relevanten Objekte (max. 5)	u8
Objekt[n].Nummer	Objektzähler	u8
Objekt[n].x0	x-Koordinate des Objektmittelpunktes (mitte, vorn)	f32
Objekt[n].y0	y-Koordinate des Objektmittelpunktes (mitte, vorn)	f32
Objekt[n].b	Objektbreite	f32
Objekt[n].t	Objekttiefe	f32
Objekt[n].	Objektausrichtung	f32
Objekt[n].v	Betrag des Geschwindigkeitsvektors	f32
Objekt[n].Plausibel	Vertrauenswert für das Objekt (100: 100% vertrauenswürdig)	u8

Objektbildung: L-Shape Fitting

L-Shape Fitting ist für die Schätzung der Fahrzeugorientierung verantwortlich. Nach der Segmentierung werden Cluster in zwei Gruppe unterteilt. Nehmen Sie an, dass die Scannerpunkte Gruppe 1 sich in der Nahe an der Horziontalen(wird als x-Achse kenngezeichnet) befinden. Hingegen befinden die Scannerpunkte Gruppe 2 sich in der Nahe an der Vertikalen(wird als y-Achse kenngezeichnet). Und die Scanner Punkte in Gruppe 1 sind möglichst senkrecht zur Gruppe 2. Dann kann man ein Koordinatensystem anhand der 2 Gruppen zeichnen. Um eine bessere Visualisierung zu bekommen, können Sie die Linie entlang der Scannerpunkte zeichnen. Die Linien auf dem Koordinatensystem projektiven Winkel wird als θ kenngezeichnet. Die mögliche Richtung θ des Rechtecks reicht von 0◦ bis 90◦, da die beiden Seiten des Rechtecks orthogonal sind und wir uns nur um die einzelne Kante kümmern, die zwischen 0◦ und 90◦ liegt. Die andere Richtung ist einfach θ + π / 2. Die zwei senkrechten Linien der L-Form können in folgende Form definiert werden:

  xcosθ + ysinθ = c1 
 −xsinθ + ycosθ = c2 

Abbildung x stellt ein Skizze für L-Shape dar.

Abb x: L-Shape

Indem die zwei senkrechten Linien in mathematische Form gezeigt wird. Mit der ermittelten Winkel ergibt die vier Kanten des angepassten Rechtecks. Um die Richtung zu bestimmen, die das Fahrzeug sich befindet, werden alle möglichen Richtungen des Rechtecks mittels der Varianz durchlaufen. Danach muss eine optimale Richtung gesucht wird, die Varianz erzielt. Die Idee zur Berechnung der Varianz ist, dass die beiden orthogonalen Kanten, die durch die zwei disjunkten Punktgruppen angepasst werden.

Um die Programmierung zu vereinfachen, wurden ein Programmablaufplan anhand oben erläuterten Schritten entwickelt. Der entsprechenden Programmablauf ist wie "L-Shape-Fitting"" dargestellt.

Programmierung (Hier pro Modul Code-Snippets und Erklärung)

Koordinatentransformation

Das Modul zur Umsetzung der Koordinatentransformation besteht aus der Schnittstellendatei coordinate_transformation.h und ihrer Implementierung coordinate_transformation.c. Der Funktionsprototyp sieht folgendermaßen aus:

void coord_transform(const urg_t* lidar_handle, long* distance_array, int numMeasurements, long* x_coord, long* y_coord, int trans_x, int trans_y);

Die Eingangsparameter sind:

1. lidar_handle: Lidarobjekt dient dem Zugriff auf die erstellte LiDAR Instanz und ihre Funktionalitäten
2. distance_array: dynamisch erstellte Array der empfangenen Schrägentfernungswerte
3. numMeasurements: Anzahl der durchgeführten Messungen
4. x_coord: Dieser leere, dynamische Array wird über Call-by-Reference übergeben. Die transformierten x-Koordinaten werden in diesem Array geschrieben
5. y_coord: Dieser leere, dynamische Array wird über Call-by-Reference übergeben. Die transformierten y-Koordinaten werden in diesem Array geschrieben
6. trans_x: x-Komponente des Translationsvektors, falls nötig, zwischen Ursprung dem KoS des Fahrzeugs und dem des LiDARs
7. trans_y: y-Komponente des Translationsvektors, falls nötig, zwischen Ursprung dem KoS des Fahrzeugs und dem des LiDARs

Die Funktion ist void. Es wird kein Rückgabewert zurückgeliefert. Die Ausgangsparameter der Funktion bzw. ihre Resultate werden in den übergebenen, dynamischen, leeren Arrays gespeichert. Im Folgenden wird die Definition der Koordinatentransformationsfunktion in detaillierten Schritten erklärt.

Zuerst findet die Deklaration der lokalen Variablen statt

	long distance; // einen Schrägentfernungswert von dem distance_array ablesen und speichern pro Durchlauf
	double angle_rad; // den entsprechenden Winkel ermitteln und in dieser Variable speichern pro Durchlauf
        long x; // die daraus berechnete x-Koordinate hier speichern pro Durchlauf
	long y; // die daraus berechnete y-Koordinate hier speichern pro Durchlauf

Hiernach kommt die Hauptschleife. Innerhalb dieser Schleife wird ein Entfernungswert von dem übergebenen Array distance_array abgelesen und in eine x- und y- Koordinate überführt. Diese bilden das x-y Koordinatenpaar dieses bestimmten Messpunktes. Dies passiert, indem die folgenden mathematischen Gleichungen angewendet werden:

$x = radius * cos(\vartheta )$

$y = radius * sin(\vartheta )$

Gegebenenfalls erfolgt eine Translation anhand von dem übergebenen Translationsvektor-Argument. Am Ende vom Durchlauf wird jede Koordinate an der richtigen Stelle in dem entsprechenden, leeren Array, der über Call-by-Reference übergeben wurde, gespeichert.

	
        /*Main loop*/
	for (int i = 0 ; i < numMeasurements ; i++) {
		distance = distance_array[i]; // Entfernungswert ablesen und speichern
		angle_rad = urg_index2rad(lidar_handle, i); // Entsprechenden Winkel ermitteln mit der verfügbaren Member-Method                   
		x = (long)(distance * cos(angle_rad)); // x-Koordinate aus Polarkoordinaten ermitteln in mm
		y = (long)(distance * sin(angle_rad)); // y-Koordinate aus Polarkoordinaten ermitteln in mm

		/*Je nachdem ist die Translation erforderlich*/
		//x = (long)(trans_x + x); // Translate x
		//y = (long)(trans_y + y); // Translate y

		/*Speichern in Arrays*/
		x_coord[i] = x;
		y_coord[i] = y;
	}

Literaturverzeichnis

---

→ zurück zum Gruppenartikel: SDE-Team_2020/21
→ zurück zum Hauptartikel: SDE Praktikum Autonomes Fahren