Für unsere Experimente mit autonomem Fahren haben wir einen Raspberry Pi, zusammen mit Motoren, Akku, Kamera, Lidar und Fernbedienung auf Räder gesetzt und auf Reisen durch verschiedene Parcours geschickt – ein spannendes Experimentierfeld für Algorithmen zum maschinellem Lernen. Das kleine Fahrzeug bekam den Namen „RaspiCar“.
Der Lidar spielt dabei eine wesentliche Rolle. Er liefert einen Rundumblick auf mögliche Hindernisse auf der Strecke, die es zu vermeiden gilt. Wir haben uns für den YDLidar X2 entschieden. Es ist ein einfacher Laser-Scanner, stammt aus China und wird z.B. von Elektor (YDLIDAR X2 – 360-degree laser range scanner | Elektor) für einen akzeptablen Preis angeboten. Typischerweise sitzen solche Gerät in Roboter-Staubsaugern und ähnlichen Geräten. Der X2 ist das kleinste Gerät aus einer Reihe. Hier möchte ich von unseren Erfahrungen berichten und geeignete Treibersoftware vorstellen, die die Anwendung des Lidar einfach macht.
YDLidar X2
Der Lidar arbeitete mit einem Infrarot Laser, der durch Reflektionen den Abstand ermittelt. Laser, Detektor, Optik und Elektronik sitzen auf einer Drehscheibe, die mit einem kleinen Elektromotor über einen Riemen gedreht wird.
So werden Messungen über den ganzen Bereich von 360 Grad produziert. Die Geschwindigkeit der Drehung lässt sich innerhalb Grenzen einstellen. Die maximale Reichweite beträgt 8 m. Für unsere Zwecke ist die minimale Reichweite fast wichtiger. Wenn die Abstände kleiner als etwa 10 cm werden, bekommt man keine brauchbaren Ergebnisse.
Der Lidar benötigt eine Spannungsversorgung von 5V und zieht zwischen 300 und 500 mA. Für batteriebetrieben Geräte ist das eine nicht zu vernachlässigende Anforderung.
Anschlüsse
Der Betrieb des Lidar ist einfach. Es gibt vier Anschlüsse: GND, +5V, Rx und M_CTR. Sobald die Betriebsspannung an GND und +5V anliegt, beginnt er zu drehen und liefert serielle Daten am Tx-Anschluss. Ein Empfang von seriellen Daten ist nicht vorgesehen. Die Baudrate ist fest eingestellt auf 115200. Im Paket liegt ein USB-Wandler dabei, so dass der Lidar direkt an einen der USB-Anschlüsse am PC oder Raspberry Pi angeschlossen werden kann. Für das RaspiCar verwenden wir aber den seriellen Port (Pin 10, RXD) am 40-poligen Pin Header.
Die vierte Leitung M_CTR dient dazu, die Drehgeschwindigkeit zwischen etwa 5 und 8 U/min einzustellen. Dazu wird eine Spannung zwischen 0 und 3.3V benötigt. Beim RaspiCar haben wir einen 10 kOhm Trimmer verbaut, der diesen Spannungsbereich liefert. So können wir mit verschiedenen Drehzahlen experimentieren.
Mitteilsamer Sensor
Sobald der Lidar läuft, liefert er beständig Daten an der seriellen Schnittstelle. Der Datenstrom kann nicht abgestellt oder unterbrochen werden. Wenn keine Interesse an den Daten besteht, kann man sie natürlich ignorieren.
Das Hersteller bietet ein Development Manual zum Download an, in dem das Datenformat genau beschrieben wird. Zusätzlich gibt es ein SDK, das in C++ geschrieben ist, und zur Auswertung der Daten herangezogen werden kann. Um es kurz zu machen: Unsere Experimente mit dem SDK waren nicht wirklich zielführend. Entweder bekamen wir die Software auf dem jeweiligen Host-System nicht recht zum Laufen, oder das Ergebnis entsprach nicht unseren Anforderungen. Deshalb entschieden wir uns für den Eigenbau von Software, die den Datenstrom auswertet. Um die Programmierung zu vereinfachen und die Transparenz zu erhöhen, haben wir uns für Python entschieden. Tatsächlich gibt es im Netz bereits ein paar Beispiele für die Datenauswertung mit Python, die einen guten Startpunkt bereit stellten (z.B. YDLidarX2_python/LidarX2.py at master · nesnes/YDLidarX2_python · GitHub). Auch der Blog von msadowski mit einem C-Programm ist sehr informativ (YDLIDAR X2 – ROS review and Cartographer setup (msadowski.github.io) ).
Der hier vorgestellte Python-Treiber ist auf Github verfügbar:
smlaage/YDLidarX2: Python driver for the YDLidar X2 (github.com)
Python Berechnungen
Die Entwicklung der Software begann mit einem genaue Studium des Development Manuals. Der Datenstrom enthält Pakete, die durch einen Paket Header (PH) eingeleitet werden. Darauf folgen Angaben zum Typ des Pakets (CT) und die Anzahl der Messpunkte (LSN), den gemessenen Winkelbereich (FSA und LSA) und eine Prüfzahl (die wir aber getrost ignorieren). Schließlich folgen die einzelnen Datenpunkte (Si).
Quelle: https://www.ydlidar.com/dowfile.html?cid=6&type=3
Aus dem Datenpaket werden Datenpaare bestehend aus Winkel und gemessener Entfernung berechnet. Die dazu notwendigen Formeln sind im Development-Manual beschrieben und – naja – einigermaßen nachvollziehbar.
Schließlich wird noch eine Winkel-Korrektur benötigt, die abhängig von der gemessenen Entfernung ist. Jeder Winkel muss entsprechend umgerechnet werden. Freundlicherweise wird im Development-Manual ein Beispiel vorgerechnet, so dass man die eigene Software überprüfen kann. Am Ende des Prozesses stehen dann Datenpaare aus korrigierten Winkeln (Grad) und gemessenen Entfernungen (mm) bereit.
Vereinfachungen
Der Lidar liefert also Winkel und Entfernungen, wobei aufgrund der Winkelkorrektur nicht exakt vorhergesagt werden kann, welche Winkel tatsächlich gemessen wurden. Für unsere Anwendung zur autonomen Fahrzeugsteuerung ist eine exakte Winkelmessung aber gar nicht nötig. Wir möchten wissen, wo ungefähr ein Hindernis auftaucht und frühzeitig mit einem Ausweichkurs reagieren. Hier setzen zwei Vereinfachungen ein:
Ersten wird der gemessene Winkel als Integer interpretiert. Wir nehmen also nur ganzzahlige Winkel. Dazu verwendet das Programm ein Integer-Array mit 360 Werten (Index: 0 bis 359), in dem die gemessenen Entfernungen in Millimeter abgelegt werden. Es zeigt sich, dass oft Winkel übersprungen werden, so dass im Array fehlende Werte auftauchen. Diese werden mit dem Wert out_of_range (hier 32768) markiert.
Zweitens werden die Winkel in Sektoren zusammengefasst. Dabei wird der Rundumblick von 360 Grad reduziert auf 40 Sektoren von jeweils 9 Grad. Der Sektor 0 erfasst also 0 bis 8 Grad, Sektor 1 reicht von 9 bis 17 Grad, usw. Für das Navigieren wichtig sind die Sektoren 19 (171 – 179 Grad) und 20 (180 – 188 Grad), die den direkt vorausliegenden Bereich erfassen. Für jeden Sektor wird der kleinste gemessene Abstand erfasst, weil es für die Navigation letztlich nur darum geht, die am nächsten gelegenen Hindernisse zu „sehen“. Mit Blick auf maschinelles Lernen führen wir hier eine Reduktion der Dimensionen durch, die es einigen der Algorithmen einfacher macht.
Sectors are:
- sectors[ 0] -> 0 - 8 degree,
- sectors[ 1] -> 9 - 17 degree,
- sectors[ 2] -> 18 - 26 degree,
…
- sectors[38] -> 342 - 350 degree,
- sectors[39] -> 351 - 359 degree
Sectors with missing values are reset to the minimum range.
Wer es noch weiter vereinfacht haben möchte, kann auch mit 20 Sektoren mit jeweils 18 Grad arbeiten. Prinzipiell sollte es auch kein Problem sein, die Anzahl der Sektoren größer zu machen.
Das Python-Modul beinhaltet einige Funktionen, um die gemessenen Daten mit Hilfe einer Canvas in TKinter anzuzeigen. Das ist für die Visualisierung in der Testphase hilfreich. Allerdings benötigt das zeitnahe Updaten der TKinter-Grafik auf einem Raspberry Pi relativ viel Rechenpower, so dass man bei der eigentlichen Anwendung auf die Grafik lieber verzichtet.
Ablaufsteuerung
Der Treiber geht folgendermaßen vor: Der Datenstrom vom Lidar wird in Datenblöcke aufgeteilt. Die Blockgröße wird über den Parameter chunk_size festgelegt. Als Default-Wert wird 2000 verwendet, was ungefähr 2 Umdrehungen des Lidars entspricht und einen guten Kompromiss zwischen Geschwindigkeit und Genauigkeit darstellt. Wenn ein Datenblock voll ist, wird er ausgewertet und das Array mit den Entfernungen gefüllt. Wenn es mehrere Messungen (von mehreren Umdrehungen) für denselben Winkel gibt, wird der Mittelwert berechnet. Anschließend wird das Flag avaiable gesetzt, um dem Anwender zu zeigen, dass neue Daten verfügbar sind.
Je größer der Parameter chunk_size ist, desto mehr Daten werden gesammelt. Dadurch gibt es weniger fehlende Werte, und ein größerer Anteil der Winkel bekommt 2, 3 oder mehr Messungen. Auf der anderen Seite dauert es länger, bis neue Daten verfügbar sind. Diese Balance mag für jede Anwendung anders sein.
chunk_size (default: 2000) bestimmt die Balance zwischen der Datenqualität und der Geschiwndigkeit. Awendungen
Wie verwendet man den Treiber? Zuerst wird er als Python-Modul importiert. Die Verbindung zur seriellen Schnittstelle wird mit der Methode .connect() hergestellt. Dann kann die Verarbeitung der Lidar-Daten mit der Methode .start_scan() aktiviert werden. Die Auswertung arbeitet als eigener Thread im Hintergrund. Immer dann, wenn ein Datenblock verarbeitet wurde, wird ein Flag gesetzt, das als Property .available abrufbar ist. Das aufrufende Programm kann also nachschauen, ob es aktuelle Daten gibt. Mit der Methode .get_data() können die Entfernungswerte über 360 Grad abgerufen werden. die Methoden .get_sectors20() und .get_sectors40() liefern die Werte für die Sektoren, wie oben beschrieben. Immer wenn die Daten oder Sektoren ausgelesen wurden, wird das Flag .available zurück gesetzt, bis eine neuer Satz von Daten bereitsteht. So kann vermieden werden, dass ein aufrufendes Programm mehrfach dieselben Daten verarbeitet.
Ein einfaches Beispiel-Programm (hier für den Raspberry Pi) ist unten gezeigt. Die Messdaten des Lidar werden als ein Numpy-Array distances ausgelesen und stehen für die weitere Verarbeitung bereit.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
import ydlidar_x2 import time port = '/dev/serial0' lid = ydlidar_x2.YDLidarX2(port) lid.connect() lid.start_scan() try: while True: if lid.available: distances = lid.get_data() # process the distance measurements time.sleep(0.1) except KeyboardInterrupt: pass lid.stop_scan() lid.disconnect() print("Done") |
Performance
Eine Sorge bei der Entwicklung des Python-Treibers war, ob das Programm mit dem Tempo der Datenlieferung vom Lidar zurecht kommt, zumal Python die Ausführung von Threads nur auf demselben Prozessorkern ermöglicht. Deshalb verwendet der Python-Treiber in weiten Teilen Numpy-Arrays, die eine gute Performance haben. Für die Berechnungen der Winkel-Korrektur wird der Arkustangens benötigt. Damit das schneller geht, werden die Korrekturfaktoren für alle ganzzahlige Winkel-Grade vorbereitet und als Lockup-Array abgelegt.
Insgesamt haben wir eine gute Performance bei geringer Prozessor-Last erzielen können. Unsere Sorge hat sich als unnötig erwiesen. Das System läuft sehr gut auf einem Raspberry Pi 3 oder 4. Es ist erstaunlich, wie der Raspberry Pi den ständigen Datenstrom im Hintergrund abarbeitet. Auf einem Raspberry Pi 3 zeigt sich im Performance-Monitor nur eine geringe Erhöhung der CPU-Aktivität. Wenn parallel noch eine Kamera betrieben wird, dann ist der Raspberry Pi 4 sicherlich im Vorteil.
Erfahrungen
In der Praxis zeigt sich, dass der Lidar gut arbeitet und zuverlässig Hindernisse erkennt. Das Arbeiten mit den Sektoren funktioniert sehr gut. Wir haben erfolgreiche Routen durch anspruchsvolle Parcours mit automatischer Navigation absolviert. Die Frequenz von etwa 3 Datensätzen pro Sekunden (chunksize = 2000) ist für unser maximales Tempo von etwa mäßiger Schrittgeschwindigkeit vollständig ausreichend. Wenn die Abstände zu Hindernissen kleiner werden, regelt der Algorithmus die Fahr-Geschwindigkeit deutlich herunter.
Sehr schmale Objekte, zum Beispiele dünne Stuhlbeine, können aber übersehen werden. Außerdem haben wir den Eindruck, dass runde reflektierende Objekte nicht erkannt werden. Als weitere Entwicklung wird eine Kamera eingesetzt, die zusätzliche Daten für das maschinelle Lernen bereitstellen soll. Das sind zur Zeit laufende Experimente.
Ressourcen
Python-Treiber für den YDLidar X2:
smlaage/YDLidarX2: Python driver for the YDLidar X2 (github.com)