Lokalisierung mit Webots: Wahrscheinlichkeitsbasierte Roboternavigation im 5×5-Gitter

Einführung in die probabilistische Lokalisierung

Die Lokalisierung ist eine der grundlegenden Herausforderungen in der Robotik. In diesem Tutorial lernst du, wie ein Roboter mithilfe von Wahrscheinlichkeiten seine Position in einem bekannten Gitter bestimmt – ähnlich wie ein Smartphone seinen Standort über WLAN-Signale schätzt. Stell dir vor, du spielst eine Runde Minecraft und musst anhand von markanten Türmen deine Position auf der Karte bestimmen: Genau das macht dein Roboter mit Sensoren.

Das 5×5-Gitter und das globale Referenzsystem

Die Arena ist in 25 quadratische Zellen von 1 m × 1 m unterteilt. Der Ursprung (0,0) liegt in der Mitte. Die y-Achse zeigt nach Norden, die x-Achse nach Osten. Jede Ecke des Gitters ist mit einem farbigen Zylinder markiert: gelb, rot, blau und grün. Diese Landmarken dienen als Fixpunkte – vergleichbar mit den Eckfahnen auf einem Fußballfeld.

Die Zellen sind von 1 bis 25 nummeriert, beginnend in der nordwestlichen Ecke. Für jede Zelle berechnest du vorab die Entfernung zu jeder Landmarke (Mittelpunkt der Zelle zum Mittelpunkt des Zylinders). Diese Referenzdistanzen speicherst du in einer Matrix.

Wandkonfiguration als Datenstruktur

Um Hindernisse zu repräsentieren, verwendest du eine 25×4-Matrix, die für jede Zelle die Wände in der Reihenfolge Westen–Norden–Osten–Süden (WNES) angibt. Ein Eintrag "W" bedeutet Wand, "O" bedeutet offen. Diese Struktur hilft dir später bei der Pfadplanung.

# Beispiel für Zelle 1: Wände im Westen und Norden
wall_config = {
    1: ['W', 'W', 'O', 'O'],  # West, Nord, Ost, Süd
    2: ['O', 'W', 'O', 'O'],
    ...
}

Sensoren und ihre Rollen

Dein Roboter nutzt mehrere Sensoren, um seine Umgebung zu erfassen:

Encoder: Zählen die Radumdrehungen und liefern eine Schätzung der zurückgelegten Strecke und Drehung (Odometrie).
Kompass: Gibt die absolute Ausrichtung relativ zur x-Achse (Osten) in Grad an.
Lidar: Misst Entfernungen zu Objekten in 360° – perfekt, um Wände und Landmarken zu erkennen.
Kamera mit Objekterkennung: Identifiziert farbige Zylinder und liefert deren Position im Bild.

Die Fusion dieser Daten ist entscheidend. Stell dir vor, du bist in einer neuen Stadt und nutzt GPS (Odometrie), einen Kompass und markante Gebäude (Landmarken), um deinen Standort auf der Karte zu schätzen – so arbeitet auch dein Roboter.

Wahrscheinlichkeitsbasierte Lokalisierung mit Landmarken

Die Grundidee: Für jede Zelle CN berechnest du die Wahrscheinlichkeit, dass sich der Roboter dort befindet, basierend auf den gemessenen Entfernungen zu den Landmarken. Du vergleichst die gemessene Distanz DR−L_i mit der vorab berechneten Distanz DC_N−L_i. Je näher die Werte beieinanderliegen, desto höher die Wahrscheinlichkeit.

Normalverteilung in Python

Die Wahrscheinlichkeitsdichte für eine gemessene Distanz berechnest du mit der Gaußschen Normalverteilung:

import math

def normal_prob(s, mu, sigma):
    """
    s: vorab berechnete Distanz (Zelle zu Landmarke)
    mu: gemessene Distanz (Roboter zu Landmarke)
    sigma: Standardabweichung (Rauschen)
    """
    coeff = 1.0 / (math.sqrt(2 * math.pi) * sigma)
    exponent = -((s - mu) ** 2) / (2 * sigma ** 2)
    return coeff * math.exp(exponent)

Für jede Landmarke Li und jede Zelle CN berechnest du diese Wahrscheinlichkeit. Das Gesamtgewicht einer Zelle ergibt sich aus dem Produkt der Einzelwahrscheinlichkeiten über alle Landmarken. Nach der Messung aktualisierst du die Gewichtungen aller Zellen (Bayes-Filter).

Schritt-für-Schritt-Implementierung

Initialisierung: Weise jeder Zelle die gleiche Startwahrscheinlichkeit zu (1/25).
Bewegung: Führe eine Odometrie-Bewegung aus und aktualisiere die Wahrscheinlichkeiten durch Faltung mit einem Bewegungsmodell (z. B. Gauß-Faltung).
Messung: Erfasse Distanzen zu den Landmarken (via Lidar oder Kamera). Berechne für jede Zelle die Wahrscheinlichkeit der Messung und multipliziere sie mit der aktuellen Gewichtung.
Normalisierung: Teile jedes Gewicht durch die Summe aller Gewichte, sodass die Summe = 1 ergibt.
Schätzung: Die Zelle mit der höchsten Wahrscheinlichkeit ist die geschätzte Position.

Dieser Zyklus wiederholt sich in jeder Iteration. Mit der Zeit konvergiert die Verteilung – der Roboter „weiß“, wo er ist. Ein bekanntes Beispiel aus dem Alltag: Wenn du in einer Parkgarage die Lichtverhältnisse und die Nähe zu Säulen nutzt, um deinen Parkplatz wiederzufinden, machst du genau das Gleiche.

Praktische Tipps für Webots

Verwende den Lidar, um die Distanz zur nächsten Wand oder Landmarke zu messen. Filtere Ausreißer durch Medianfilter.
Die Kamera liefert dir die Farbe und ungefähre Richtung einer Landmarke. Kombiniere dies mit der Lidar-Entfernung, um eine genaue Position der Landmarke im Weltkoordinatensystem zu berechnen.
Achte auf die Kalibrierung des Kompasses: In Webots zeigt der Kompass standardmäßig den Winkel relativ zur Welt-x-Achse an.
Nutze die Encoder für kurze Bewegungen, aber korrigiere mit den absoluten Messungen, da Odometrie driftet.

Fehlerbehandlung und Optimierung

Wenn die Wahrscheinlichkeiten über alle Zellen sehr niedrig sind, könnte eine falsche Landmarke erkannt worden sein. Implementiere einen Resampling-Schritt (Partikelfilter), um die Anzahl der Partikel in Bereichen mit hoher Wahrscheinlichkeit zu erhöhen. Das ist besonders nützlich, wenn der Roboter in einer symmetrischen Umgebung steckt – wie ein Auto in einer leeren Parkfläche, das keine eindeutigen Merkmale findet.

Zusammenfassung

In diesem Tutorial hast du gelernt, wie du einen Roboter in Webots mithilfe von Landmarken und Normalverteilung lokalisierst. Die Kombination aus Encodern, Kompass, Lidar und Kamera liefert dir die nötigen Daten, um die Position in einem 5×5-Gitter zu schätzen. Mit der bereitgestellten Python-Funktion kannst du die Wahrscheinlichkeiten berechnen und einen einfachen Bayes-Filter implementieren. Dieses Wissen ist die Grundlage für fortgeschrittene Verfahren wie Monte-Carlo-Lokalisierung (MCL) und SLAM (Simultaneous Localization and Mapping), die in autonomen Fahrzeugen und Lieferrobotern eingesetzt werden – Technologien, die auch bei Tesla oder Amazon Robotics zum Einsatz kommen.

Weiterführende Ressourcen

Webots Dokumentation: Lidar, Kamera, Encoder
Buch: „Probabilistic Robotics“ von Thrun, Burgard, Fox
Online-Kurs: „Robot Localization“ auf Coursera