Aufgabe 4.1 Robotermodell (1)

Transkript

1 Aufgabe 4.1 Robotermodell (1)! Erstellen Sie ein Simulationsmodell für einen mobilen Roboter (z.b. Pioneer 3DX), der sich über Geschwindigkeit v und Rotationsgeschwindigkeit ω steuern lässt. v ω Pioneer 3DX! Schreiben Sie dazu eine Klasse Robot, die die tatsächliche Pose (x,y,θ) des Roboters kapselt. Die initiale Pose kann mit einer set-methode eingestellt werden. Die aktuelle Pose liefert eine get-methode.! Mit der Methode move(motion) mit motion = [v, ω] kann der Roboter mit der konstanten Sollgeschwindigkeit v und ω über eine Zeitspanne von T bewegt werden. Gehen Sie von einer kleinen Zeitspanne T (z.b. T = 0.02s) aus, so dass die nächste Position durch eine einfache Approximation berechnet werden kann. T soll eingestellt werden können. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-1

2 Aufgabe 4.1 Robotermodell (2)! Die Sollgeschwindigkeit lässt sich nicht exakt umsetzen. Die tatsächliche Geschwindigkeit ist gemäß einer Normalverteilung verrauscht: σ v 2 = k d T * v σ ω 2 = k θ T *ω! Die Parameter k d (z.b. k d = (0.05m) 2 /1m) und k θ (z.b. k θ = (5 Grad) 2 /360 Grad) sollen eingestellt werden können.! Der Roboter setzt auch eine Geradeausfahrt (ω = 0) nicht exakt um und fährt stattdessen eine leichte Kurve. Modellieren Sie dazu einen Driftfehler, der über eine Konstante k Drift parameterisiert werden kann (z.b. k Drift = (2 Grad) 2 /1m). Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-2

3 Aufgabe 4.1 Robotermodell (3)! Realisieren Sie verschiedene Strecken, die Ihr Roboter mit konstanter Geschwindigkeit fahren soll: - Kreisfahrt - Rechteckfahrt - Slalomfahrt! Plotten Sie die Positionen des Roboters. Experimentieren Sie mit unterschiedlichen Rauschwerten für die Geschwindigkeiten. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-3

4 Aufgabe 4.2 Koppelnavigation (1)! Schreiben Sie eine Klasse OdometryPoseEstimator, die die Pose des Roboters (x,y,θ) mittels Odometrie schätzt. Außerdem soll die Kovarianz Σ pose der geschätzten Pose berechnet werden.! Die initiale Schätzung der Pose bzw. ihre Kovarianz kann mit einer set-methode gesetzt werden.! Die aktuelle Schätzung der Pose bzw. ihre Kovarianz liefert eine get-methode.! Mit der Methode integratemovement(motion, sigma_motion) mit motion = [v, ω] wird die neue Posenschätzung und Kovarianz berechnet, nachdem der Roboter in der Zeitspanne T die Geschwindigkeit motion durchgeführt hat. Die Kovarianz für die Geschwindigkeit Σ motion = sigma_motion soll von der Robot-Klasse zur Verfügung gestellt werden. Gehen Sie von einer kleinen Zeitspanne T (z.b. T = 0.02s) aus, so dass die nächste Position durch eine einfache Approximation berechnet werden kann. T soll eingestellt werden können. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-4

5 Aufgabe 4.2 Koppelnavigation! Plotten Sie die geschätzten und die tatsächlichen Positionen des Roboters für verschiedene Fahrten. Visualisieren Sie die Kovarianz der geschätzten Endpose als Fehlerellipse. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-5

6 Aufgabe 4.3 Kalmanfilter (1)! Erweitern Sie Ihre Klasse Robot um einen Abstandssensor, der in der Lage ist, Abstände zu mehreren Landmarken zu messen. Landmarke l 2 d 2 x' d 1 Landmarke l 1 θ y y' x Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-6

7 Aufgabe 4.3 Kalmanfilter (2)! Die Abstandsmessungen sind rauschbehaftet gemäß einer Normalverteilung mit Standardabweichung σ (z.b. σ = 0.1m). σ kann eingestellt werden.! Definieren Sie eine Methode setlandmarks(lm), um Landmarken in der Umgebung zu positionieren. Für jede Landmarke genügt es, ihre (x,y)-position festzulegen.! Definieren Sie eine Methode sense(), die einen Vektor z mit den Abstandsmessungen für jede Landmarke und der entsprechenden Kovarianzmatrix Σ z zurückliefert. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-7

8 Aufgabe 4.3 Kalmanfilter (3)! Definieren Sie nun eine Klasse KalmanFilterPoseEstimator, der die Pose mit einem Kalmanfilter schätzt.! Gehen Sie von der Klasse OdometryPoseEstimator aus. Die Methode integratemovement(motion, sigma_motion) kann unverändert übernommen werden und entspricht dem Vorhersageschritt des Kalmanfilters.! Für den Korrekturschritt wird eine Methode integratemeasurement(z, sigma_z) definiert, die die Abstandsmessungen z zu den Landmarken integriert.! Für das Messmodell in der Methode integratemeasurement benötigt der Kalmanfilter die Landmarkenpositionen. Definieren Sie dazu eine Methode setlandmarks(lm).! Plotten Sie die geschätzten und die tatsächlichen Positionen des Roboters für verschiedene Fahrten. Visualisieren Sie die Kovarianz der geschätzten Endpose als Fehlerellipse.! Starten Sie Ihren Kalman-Filter mit verschiedenen Unsicherheiten in der Initialpose.! Simulieren Sie nun mit Sensormessraten, die bei etwa 1/(10T) liegen. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-8

9 Aufgabe 4.4 Partikelfilter (1)! Die Posenschätzung für das vorhandene Robotermodell soll nun mit einem Partikelfilter erfolgen. Definieren Sie dazu eine Klasse ParticleFilterPoseEstimator.! Die Klasse kapselt eine Partikelwolke (Menge von Posenschätzungen). Definieren Sie eine Methode initialize(posefrom, poseto, n) zur Erzeugung einer Partikelwolke mit n Partikeln, die gleichverteilt im Intervall [posefrom, poseto] liegen.! Sehen Sie Methoden getpose bzw. getcovariance vor, die aus der Partikelwolke eine Schätzung für die Pose bzw. ihre Kovarianz berechnet. Für die Position kann ein einfacher Mittelwert über die Partikeln gebildet werden. Bei der Orientierung genügt keine konventionelle Mittelwertsberechnung. Was ist beispielsweise der Mittelwert von 350 Grad und 30 Grad? Die Kovarianz der Pose wird einfachheitshalber aus der Kovarianz der Positionen und der Varianz der Orientierungen gebildet. Bei der Orientierungsvarianz ist zu beachten, dass Winkeldifferenzen im Bereich [-π,+π) liegen. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/17 4-9

10 Aufgabe 4.4 Partikelfilter (2)! Die Methode integratemovement(motion, sigma_motion) integriert zur Partikelwolke einen Geschwindigkeitsbefehl motion mit Kovarianz sigma_motion.! Die Methode integratemeasurement(z, sigma_z) integriert Abstandsmessungen z mit der Kovarianz sigma_z.! Die Abstandsmessungen erfolgen zu Landmarken, die mit der Methode setlandmarks(lm) dem Partikelfilter bekannt gegeben werden.! Plotten Sie die geschätzten und die tatsächlichen Positionen des Roboters für verschiedene Fahrten.! Plotten Sie für für die Anfangs- und die Endposition des Roboters die Partikelwolken mit ihren Kovarianzen. Prof. Dr. O. Bittel, HTWG Konstanz Mobile Roboter - Lokalisierung WS 16/