Die EduMove-Plattform ist kompatibel mit einem Slamtec RPLidar A2M12. Nachdem der Sensor auf der Plattform montiert und per USB an die EmbeddedCtrl angeschlossen wurde, muss die Funktionalität noch aktiviert werden. Dafür muss man zunächst den EduMove mit der iRC-Software verbinden. Unter Datei -> Schnittstellenkonfiguration -> Sensoren, kann der Lidar Sensor aktiviert werden. Nach dem Anwenden fängt der Lidar Sensor an, die Umgebung zu scannen.

Der EduMove sendet die Sensordaten über den CRI, sodass Sie auch für externe Anwendungen zur verfügung stehen, die sich über den CRI verbinden können. Die Sensornachrichten sind wie folgt formatiert:

 CRISTART SENSOR CONTOUR nPoints nThresh x1 y1 x2 y2 … xn yn CRIEND

nPoints: die Anzahl der Punkte, die mit der Nachricht übertragen werden
nThresh: die Anzahl der Punkte, die innerhalb des Sicherheitsradius‘ liegen
x_i: die x Koordinate des iten Punkts
y_i: die y Koordinate des iten Punkts

Ist ein LidarSensor angeschlossen, verhindert die RobotControl Bewegungen in Richtung eines Hindernisses, wenn es sich zu nah an der Plattform befindet. Dieser Sicherheitsradius kann zusätzlichin der iRC visualisiert werden.

SLAM-Prototyp

Um die SLAM-Funktion zu nutzen, muss sie im Sensorkonfigurationsbereich aktiviert werden. Für gute Ergebnisse müssen die Parameter noch angepasst werden. Um die Parameter effektiv verändern zu können, ist ein grobes Verständnis der Funktionsweise des Algorithmus nötig. Im Abschnitt Algorithmus wird der Ablauf des Algorithmus und die einzelnen Schritte näher beschrieben. Im Abschnitt Parameter, ist eine Übersicht über die einstellbaren Parameter, sowie ihre Bedeutung für den Algorithmus zu finden.

Achtung! Bei der hier beschriebenen SLAM Funktion, handelt es sich um einen Prototypen. Lassen Sie die Plattform niemals unbeaufsichtigt fahren.

Algorithmus

Der Algorithmus zum autonomen Kartieren und Lokalisieren basiert auf dem Paper (Kuo, Bor-Woei, et al. "A Light‐and‐Fast SLAM Algorithm for Robots in Indoor Environments Using Line Segment Map." Journal of Robotics 2011.1 (2011): 257852). Das Vorgehen kann grob in drei Teile aufgeteilt werden:

Erkennung der Merkmale
Schätzung der Bewegung basierend auf den Odometriedaten
Abstimmung der Merkmale und der Karte
Einfügen der Merkmale in die bestehende Karte
Vereinfachung der Karte

Erkennung der Merkmale

Der Lidar Scanner liefert in jedem Zeitschritt in etwa 1000 Messpunkte. Jeden Punkt einzeln in die Karte einzufügen, würde einen sehr großen Rechenaufwand bedeuten. Die Grundidee des Algorithmus, ist es die Anzahl der Merkmale, die in die Karte eingefügt und dafür miteinander verglichen werden müssen, zu verringern. Das mit Abstand häufigste Merkmal, dem ein mobiler Roboter in Innenräumen begegnet sind, gerade Wände, die oft auch rechtwinklig zueinander sind. Wände sind in einem Lidarscan klar als gerade Linien zu erkennen. Ziel der Merkmalerkennung ist es also, gerade Linien im Scan zu erkennen und Punkte, die auf dieser Linie liegen zu einem Merkmal zusammenzufassen. Um eine Linie zu definieren, reichen prinzipiell der Start- und der Endpunkt. Um effektiver überprüfen zu können, ob ein Messpunkt teil einer Linie ist, werden zusätzlich die Hough-Parameter verwendet. Ein zusätzlicher Parameter ist die Anzahl der Messpunkte, die verwendet wurden, um die Linie zu berechnen. Die Hough-Parameter definieren eine Linie mit ihrem Abstand zum Ursprung und dem Winkel, mit dem sie die X-Achse schneiden. Das Vorgehen zur Extraktion der Linien läuft dann so ab: Die

Messpunkte des Lidarscanners werden der Reihe nach verarbeitet. Wenn drei aufeinanderfolgende Punkte unter einem Mindestabstand zueinander liegen, werden sie zu einer Linie zusammengefasst. Die Hough-Parameter werden berechnet und der Start und Endpunkt der Linie, werden auf den ersten & letzten Messpunkt dieser drei gesetzt. Der nächste Messpunkt wird dann mit dieser Linie verglichen. Wenn dieser Punkt zwei Kriterien erfüllt, wird er der Linie hinzugefügt und die Parameter der Linie aktualisiert.

Der Abstand zum vorherigen Punkt im Scan liegt unter einem adaptiven Maximalwert
Der Mindestabstand zu der aktuell bearbeiteten Linie liegt unter einem Maximalwert

Sobald ein Punkt im Scan gefunden wird, der nicht in die aktuell bearbeitet Linie passt, gilt die Linie als abgeschlossen. Wenn die Linie aus einer Mindestanzahl von Punkten besteht, gilt sie als groß genug und wird in die Merkmalliste aufgenommen, die in die Karte eingefügt wird. Sind zu wenig Punkte teil der Linie, wird die Linie verworfen.

Merkmale aus dem letzten LiDAR-Scan werden orange dargestellt.

Um die Merkmale, die aus dem letzten Scan extrahiert wurden zu visualisieren, muss im Footer-Bereich unter dem Tab Karte, das Häkchen bei 'Merkmalen anzeigen' gesetzt werden. Die Linien, die im letzten Lidar-Scan erkannt wurden, werden dann orange in die Karte eingezeichnet.

Abstimmen der Merkmale mit der bestehenden Karte

Die Koordinaten der Merkmale sind immer relativ zur aktuellen Position der Plattform. Um sie mit den Linien, die bereits in der Karte liegen vergleichen zu können müssen sie der Schätzung der aktuellen Position entsprechend transformiert werden. Die transformierten Merkmale können dann mit denen in der Karte verglichen werden. Ziel dieses Schrittes ist es zu bestimmen welche Linien in der Karte zu welchen Linien im Scan gehören und diese entsprechend der neuen Information zu aktualisieren. Jedes Merkmal aus dem aktuellen Scan wird dafür mit jedem Merkmal aus der Karte verglichen und ein Ähnlichkeitswert berechnet. Das Merkmal des Scans wird dann dem Kartenmerkmal zugeordnet mit dem besten Wert.

Die Ähnlichkeit wird nur berechnet, wenn die Hough-Parameter der beiden Linien ähnlich genug sind, und die Distanz der Start- und Endpunkte zur jeweils anderen Linie unter einem Maximalwert liegen. Ist beides der Fall wird geprüft ob die Linien überlappen. Die Ähnlichkeit wird dann basierend auf den Hough-Parametern berechnet. Welcher der Parameter stärker gewichtet werden soll, wird durch die zwei Parameter weight_rho und weight_theta bestimmt.

Einfügen der Merkmale in die Karte

Nachdem im vorherigen Schritt die Merkmale des Scans einem Merkmal in der Karte zugeordnet wurden, werden jetzt die Merkmale der Karte entsprechend aktualisiert. Merkmale des Scans, die keiner Linie der Karte ähnlich genug waren, werden einfach als neues Merkmal eingefügt. Für alle anderen Merkmale müssen die Parameter angepasst werden.

Dazu werden zunächst die Hough-Parameter der Linien kombiniert. Dabei werden die Parameter der Linie implizit entsprechend der Anzahl der zur Berechnung dieser Linie verwendeten Messpunkte gewichtet. Die exakten Formeln für die Berechnung kann im Paper nachgelesen werden, ist für die Bestimmung der Parameter nicht notwendig zu verstehen. Anschließend werden der Start- und Endpunkt der zusammengefügten bestimmt und auf die Linie projiziert.

Wenn Darstellung der Karte aktiviert ist, werden Merkmale, die Teil der Karte sind schwarz dargestellt.

Vereinfachung der Karte

Um eine möglichst simple Karte zu erzeugen, wird in regelmäßigen Abständen überprüft ob die Linien in der Karte miteinander verbunden werden können. Das Vorgehen ist exakt das gleiche, wie wenn ein neuer Scan in die Karte eingefügt wird, mit dem einzigen unterschied, dass jede Linie mit jeder anderen Linie in der Karte verglichen wird. Um diesen Schritt präziser kontrollieren zu können gibt es für diesen Schritt einen zweiten unabhängigen Parametersatz.

In regelmäßigen Abständen wird in diesem Schritt zusätzlich überprüft, ob Merkmale in der Karte gelöscht werden können. Dafür werden die Merkmale in der Karte, die in Reichweite des Sensors liegen (d.h. Merkmale, die nicht außerhalb des gültigen Messbereichs des Lidars liegen, sowie nicht von anderen Objekten verdeckt sind) überprüft ob sie vom aktuellen Scan erfasst werden müssten. Dafür wird aus der aktuellen Positionsschätzung und der Karte ermittelt, welche Merkmale erwartet werden. Wird ein Merkmal erwartet, wurde im letzten Scan aber nicht gematched, wird ein Zähler hochgezählt. Erreicht dieser Zähler einen bestimmten Wert, gilt das Merkmal als fehlerhaft und wird gelöscht. Der Zähler wird zurück auf null gesetzt, sobald das Merkmal in einem Scan wiedererkannt wurde.

Monte-Carlo-Verfahren

Um Fehler, die im Laufe der Kartierung zwangsläufig passieren, ausmerzen zu können, hat der Algorithmus viele Partikel, die nach jeder Kartenaktualisierung ihrer Qualität entsprechend neugezogen werden. Ein Partikel besteht dabei nicht nur aus der geschätzten Position der Plattform, sondern auch aus der geschätzten Karte. Wie gut ein Scan in die aktuelle Karte passt, ist abhängig davon, wie die aktuelle Position der Plattform geschätzt wird. Das bedeutet, dass auch die Aktualisierung der Karte abhängig von der Positionsschätzung ist. Um dem gerecht zu werden, muss der Algorithmus für jede geschätzte Plattformposition eine eigene unabhängige Karte erzeugen. Nach jeder Kartenaktualisierung werden die Partikel neugezogen. Während der Aktualisierung, berechnet jedes Partikel, wie gut der letzte Scan in die aktuelle schätzung der Karte passt. Basierend auf diesem Gütekriterium, werden die Partikel für den nächsten Zeitschritt zufällig gezogen, sodass ein Partikel, dessen Schätzung gut zum letzten Scan passt, eine höhere Wahrscheinlichkeit hat auch im nächsten Zeitschritt verwendet zu werden.

Motion-Model (Schätzung der Bewegung basierend auf der Odometrie)

Damit die Partikel nicht alle die gleiche Plattformposition annehmen, muss die Schätzung der Odometrie künstlich verrauscht werden. Grundsätzlich ist die Odometrie nicht fehlerfrei. Je nach Bodenbeschaffenheit ruthscen die Räder mal mehr und mal weniger oder Schritte der, die Räder antreibenden Schrittmotoren, werden nicht gezählt. Wie genau diese Fehler sich auf die endgültige Schätzung auswirken ist allerdings unmöglich zu berechnen, weshalb in jedem Zeitschritt viele mögliche Positionen berechnet werden und anschließend ihrer Qualität entsprechend weiterverwendet werden. Um eine künstlich verrauschte Position zu erzeugen, wird wie folgt vorgegangen:

Aus den Bewegungskommandos seit der letzten Aktualisierung werden die dafür notwendigen Motorbewegungen berechnet.
Auf diese Motorbewegungen wird ein normalverteilter Fehler addiert. Die Standardabweichung dieses Fehlers kann eingestellt werden.
Mit den künstlich verrauschten Motorbewegungen wird mit der Odometrie eine kartesische Bewegung der mobilen Plattform berechnet

Die aus diesem Verfahren resultierenden Plattform Positionen werden dann für die Positionsschätzung der einzelnen Partikel verwendet. Auf diese Weise bewegt sich jedes Partikel ein klein wenig anders, sodass auch die resultierenden Karten sich leicht unterscheiden. Indem nur die Partikel weiterverwendet werden, die den letzten Scan gut in ihre Karte einfügen können, erzeugt, der Algorithmus eine Schätzung der Umgebung, die ausreichend ist, um sich darin zu orientieren.

Parameter

Im Mobile Plattform Tab, öffnet sich durch Klick auf die Schaltfläche SLAM Parameter anpassen das Parameter Konfigurations Fenster. Um die Parameter zu ändern, müssen zunächst die aktuellen Parameter von der RobotControl heruntergeladen werden. Nachdem die Parameter angepasst wurden, kann die Änderung durch Klicken auf Save & Reset bestätigt und zur Robotersteuerung gesendet werden. Der Algorithmus wird nun mit den neuen Parametern neugestartet.

Allgemeine Parameter

NrOfSLAMParticles: Das verwendete Verfahren ist ein MonteCarlo Verfahren, dieser Parameter steuert die Anzahl der Schätzungen von Karte und Position (!Die Anzahl der Partikel beeinflusst maßgeblich die Laufzeit des Algorithmus, die Schätzung wird zwar tendenziell genauer, braucht aber sowohl mehr Speicher als auch Rechenzeit. Es kann nicht ausgeschlossen werden, dass dadurch die Bewegungssteuerung beeinträchtigt wird.)
NrOfSearchParticles: Die Anzahl von Partikeln, mit denen in einer zuvor erstellten Karte die Position geschätzt wird.
UpdatePeriod: die Zeit zwischen den Aktualisierungen der Karte & Positionsschätzung in ms

Parameter für die Mermalextraktion

Grundprinzip des Algorithmus, ist es die Anzahl an Merkmalen, die in der Karte eingezeichnet sind zu verringern, sodass bei der Positionsschätzung weniger Merkmale verglichen werden müssen. Anstatt jeden Messpunkt des LiDAR Scans einzeln in die Karte zu zeichnen, werden zunächst Merkmale aus dem Scan extrahiert. Auf diese Weise können die ~1000 2-dimensionalen Punkte aus jedem Scan, zu <20 Linien zusammengefasst werden, die jeweils durch einen Start- und Endpunkt sowie ihrem Abstand zum Ursprung und ihrer Winkel zur X-Achse (Hough-Parameter) definiert sind. Die Parameter beeinflussen, wann benachbarte Punkte als Teil einer Linie betrachtet, werden:

MinPointsPerFeature: Definiert die Mindestanzahl vn Punkten, die eine Linie definieren können
lineMaxNeighbourDist: Definiert die Maximaldistanz eines Punktes zu seinem nächsten Nachbarn um noch als Teil einer Linie betrachtet zu werden
ResidualFactor: die maximale Distanz eines Punktes zu einer Linie um noch als Teil von hier zu gelten
maxAngleDiff: legacy wird nicht verwendet
AngularResolution: Die Winkelauflösung des LidarSensors

Mapfitting

Die aus einem Scan extrahierten Linien, werden anschließend in die Karte eingefügt. Dabei muss für jede Linie entschieden werden, ob Sie bereits in der Karte ist oder ein neues Merkmal ist. Gleichzeitig müssen die Linien, die bereits in der Karte eingezeichnet sind, erweitert werden, falls der Scan bisher unbekannte Merkmale der Umgebung enthält. Dafür müssen die Merkmale des letzten Scans, mit den Merkmalen in der Karte verglichen werden. Die folgenden Parameter steuern, wie ähnlich diese Merkmale sein müssen um als Teil des gleichen Merkmals in der echten Umgebung zu gelten.

maxAverageLineDistance: Die maximale durchschnittliche Distanz, die die insgesamt 4 Endpunkte, von der jeweils anderen Linie haben dürfen
maxAngleDiff: die Linien werden durch ihren Start und End punkt sowie ihre Hough Parameter definiert. Dieser Parameter bestimmt, wie sehr der Winkel theta der beiden Linien abweichen darf um noch als dieselbe Linie zu gelten
weightRho / weightTheta: Die Gewichtung der beiden Hough-Parameter wenn zwei Linien miteinander verglichen werden. Wird intern normalisiert.

Map-Pruning

In regelmäßigen Abständen, wird geprüft, ob Linien innerhalb der Karte zusammengefügt werden können, sodass weniger (aber größere) Merkmale in der Karte sind. Dafür werden alle Merkmale in der Karte miteinander verglichen und, falls sie ähnlich genug sind, miteinander verschmolzen. Die Bedeutung der Parameter ist identisch mit denen aus Mapfitting.

Motion-Model

Das MotionModel schätzt aus den Daten der Odometrie die Bewegung der Plattform seit dem letzten Zeitschritt. Die Schätzung der Position mittels Odometrie ist nicht ausreichend, um die tatsächliche Position zu bestimmen. Verschiedene Bodenbeschaffenheit kann zum Durchdrehen der Räder führen, oder die Schrittmotoren verlieren Schritte. Um solche Ungenauigkeiten auszugleichen, wird in jedem Schritt die Bewegung stochastisch geschätzt. Dafür werden die Motorbewegungen der einzelnen Räder künstlich verrauscht. Diese verrauschten Daten werden in die Odometrie gegeben, um so verrauschte Plattform positionen zu erzeugen. Durch den Parameter StdDev wird die Standardabweichung dieses Rauschens gesteuert. Sodass ein höherer Wert zu mehr Varianz in der Partikelwolke sorgt.

Bei dem hier beschriebenen System handelt es sich um einen Prototyp. Für einen längeren autonomen Betrieb ist das System nicht geeignet. Die SLAM-Funktion eignet sich um einen Eindruck der Probleme, die bei der autonomen Navigation und Wahrnehmung, auftreten zu vermitteln. Das Ausprobieren verschiedener Parameter und den Einfluss auf die Qualität der resultierenden Karte kann ein erster Einstieg in dieses komplexe Thema dienen.