Parametrierung eines Mixture Density Sensor Models

by Paul Balzer on 17. Februar 2016

No Comments

Im ersten Teil der Robotik-Sensor Reihe, wurde ein Look-Up Table für den Tinkerforge Ultraschall Sensor parametriert. Nun ist es möglich reale Entfernungen von diesem Sensor zu erhalten. Der Beitrag schloss mit den Worten, dass eine Normalverteilung für Sensorwerte nur im Bereich 1…2m anzunehmen ist. Etwas fortgeschrittene Filterverfahren, wie das Kalman Filter, arbeiten unter der Annahme, dass die Sensoren ein AWGN (Additive White Gaussian Noise) Signal ausgeben. Dies bedeutet, dass der Sensor den wahren Wert zwar messen kann, aber durch zahlreiche Fehlereinflüsse zusätzlich weißes Rauschen aufaddiert mit ausgegeben wird.

Wie nun also den ‚Wahren Wert‘ erhalten?

Einfaches Modell: Normalverteilt um den Wahren Wert

Die ernüchterndste Aussage eines jeden Messtechnikers ist und bleibt, egal wie genau sein Messverfahren ist, dass der Wahre Wert™ unbekannt ist. Der Sensor versucht lediglich den Wahren Wert geeignet ist Spannung oder Strom umzusetzen. Je nachdem wie geeignet das Messverfahren ist, kommt dies dem Wahren Wert sehr nahe.

Am Beispiel des Tinkerforge Ultraschallsensors zeigt sich das ganz deutlich. Beispielsweise wird dieser in ca. 1 Meter Entfernung vor einer weißen Wand aufgestellt und misst 5min lang die Entfernung. Man erwartet, dass der Sensor 5min lang 1m als Messwert ausgibt. Denn das ist ja der Wahre Wert. Dem ist nicht so. Hier zeigt sich sehr schön die AWGN Annahme. Ein Histogramm für alle Messwerte über 5min sieht so aus:

log100cm_Histogram

Zieht man den Mittelwert ab und legt eine Gauss-Normalverteilung über dieses Histogramm, ergibt sich folgende Abbildung:

log100cm_HistogramDas Sensormodell für diese Entfernung lässt sich sehr gut modellieren mit der mathematischen Formulierung

\[Y_i = X_i + Z_i\sim \mathcal{N}(X_i, N)\]

was nichts anderes bedeutet, dass der Sensorwert \(Y\) zum Zeitpunkt \(i\) zusammengesetzt ist aus dem Wahren Wert \(X\) + einem normalverteilten Sensorrauschen mit Mittelwert um den Messwert und Varianz \(N\). In Python würde man das in etwa so berechnen.

import numpy as np
import matplotlib.pyplot as plt
mu, sigma = 0., np.std(data['ultraschall_distance_cm']) # in data sind die Messwerte
x = np.arange(-5., 5., .01)
std = (1/(sigma * np.sqrt(2 * np.pi)) * np.exp(-(x - mu)**2 / (2 * sigma**2)))
plt.plot(x, std, label=r'Normalverteilung $\mu=%.0f$, $\sigma^2=%.3f$' % (mu, sigma**2.0))

Es zeigt sich, dass diese einfache Annahme für den Tinkerforge Ultraschall Sensor (und alle anderen Ultraschallsensoren) bei Entfernungen von über 2m nicht mehr ausreichend ist. Das Histogramm für eine Entfernung von ca. 3,55m (bei 4m maximalen Messbereich) ist nachfolgend dargestellt.

Histogram-InverseSensorModel

 

Hier kann man offensichtlich keine Normalverteilung um den Mittelwert mehr fitten. Es muss eine gemischte Dichteverteilung modelliert werden.

Vollständige Modellierung: Gemische Dichteverteilung

Die Sensormodellieren, die im folgenden Vorgestellt wird, geht zurück auf den Großmeister der Robotik, Prof. Sebastian Thrun [Thrun, S. (2000). Probabilistic algorithms in robotics. Ai Magazine, 21(4), 93–109. Retrieved from http://www.aaai.org/ojs/index.php/aimagazine/article/viewArticle/1534].

Ziel der Sensormodellierung

Ziel der Sensormodellierung ist es, ein möglichst korrektes Map eines Raumes oder einer Umgebung zu erfassen, obwohl der Sensor nicht perfekt misst. Es gibt zahlreiche Möglichkeiten, welche den Sensorwert eben nicht korrekt von Sensor bis zur Wand laufen lassen. Diese sollten für eine Kartierung des Raumes probalistisch mit berücksichtigt werden.

Ein schöner Vortrag zur Probalistischen Sensormodellierung kann bei der Uni Freiburg angesehen werden:

Die Wahrscheinlichkeitsdichte des Sensormesswertes wird zusammengesetzt aus 4 verschiedenen Einzelverteilungen.

\[P(z\mid x,m) = \left(\begin{matrix}P_{hit}(z\mid x,m) \\ P_{unexp}(z \mid x,m) \\ P_{max}(z \mid x,m) \\ P_{rand}(z \mid x,m) \end{matrix}\right)\]

Diese gibt die Wahrscheinlichkeit wieder, dass ein Sensormesswert \(z\) ermittelt wird, bei einer bestimmten Position \(x\) des Sensors in einer bestimmten Welt \(m\). Letzteres bedeutet schlichtweg, dass es eine Wand oder Hindernis in der Welt gibt und der Sensor an einer Position \(x\) vor der Wand steht.

Normalverteilung

Es bleibt die Normalverteilung \(P_{hit}(z\mid x,m)\) um den Wahren Wert mit Normalisierungsfaktor \(\eta\).

\[P_{hit}(z\mid x,m) = \eta \cdot \frac{1}{\sqrt{2 \pi \sigma_z^2}} \cdot \exp{\left(-\frac{1}{2} \cdot \frac{(z-z_{exp})^2}{\sigma_z^2}\right)}\]

Python Code:

varz = 5.0 # Variance
zexp = 357.0  # here is the obstacle

def Pzx_hit(z):
    Pzx =  1.0/np.sqrt(2*np.pi*varz) * np.exp(-1/2.0*(z-zexp)**2/varz)
    eta =  np.max(np.sum(Pzx))
    return Pzx/eta

Unerwartete Hindernisse

Der Sensor gibt eventuell ein Echo auf ein Hindernis zurück, was sich zwischen dem Sensor und der Wand befindet. Das könnte bei Laserscannern ein Staubkorn oder eine Scheibe sein. Bei Ultraschallsensoren durch die breite Keule etwas, was sich am Rande der Keule befindet und ein Echo erzeugt. Sollte die Umgebung \(m\) in der sich der Roboter befindet nicht statisch sein, so kann mit dieser Komponente auch modelliert werden, dass z.B. ein Mensch o.ä. in den Messbereich zwischen Roboter und Wand läuft. Stehen zwei Hindernisse hintereinander, wird natürlich nur das vordere gemessen. Daher ist es wahrscheinlicher, dass im Nahbereich häufiger Hindernisse auftreten, als im Fernbereich.

\[P_{unexp}(z \mid x,m) =  \eta \cdot \lambda \cdot \exp{\left(-\lambda \cdot z\right)}\]

Der Faktor \(\eta\) ist ein Normalisierungsfaktor, welcher das Integral über die entstehende Wahrscheinlichkeitsdichtefunktion auf 1 normiert.

Python Code:

lamb = 0.5
eta = 1.0/(1.0-np.exp(-lamb*zexp))
Pzx = eta * lamb * np.exp(-lamb * z)
Pzx[z>zexp] = 0.0

Zufälliger Messwert

Zusätlich wird noch modelliert, dass der Sensor auch einfach mal etwas falsches zurück gibt, was überhaupt nichts mit der realen Welt zu tun hat. Das kann z.B. durch Quantisierungsfehler o.ä. passieren oder wenn mehrere Ultraschallsensoren an einem Roboter angebaut sind, die dann vom jeweils anderen das Echo empfangen.

\[P_{rand}(z \mid x,m) = \frac{1}{z_{max}}\]

Python Code:

Pzx = np.ones(len(z)) / maxz

Maximum Messwert Rückgabe

Das Phänomen ist im oben dargestellten Histogramm zu sehen: Es kommt gar kein Echo beim Sensor an, dadurch denkt er, der Messbereich ist völlig ausgeschöpft und gibt den maximalen Messwert zurück. In diesem Fall (Tinkerforge Ultraschall Sensor) 4m.

\[P_{max}(z \mid x,m) = \delta(z_{max})\]

Python Code:

Pzx_maxrange = np.zeros(len(zs))
Pzx_maxrange[-1:] = np.max(zs) # 400cm

Ergebnis: Zusammengesetzte Verteilung

Diese Verteilungen werden nun einfach addiert (und normalisiert).

Pzx = Pzx_hit + Pzx_unexp + Pzx_rand + Pzx_maxrange
InverseSensorModel-MixtureDensity

Obere Zeile: Alle 4 einzelnen Wahrscheinlichkeitsdichteverteilungen (v.l.n.r. Normalverteilung, Nicht vorhersehbare Hindernisse, Zufälliger Rückgabewert, Maximum Messwert)
Untere Zeile: Summe der 4 Verteilungen, normiert

Parametrierung

Nun ist der Anteil jeder Verteilung natürlich abhängig vom jeweiligen Sensor und Einsatzbedingung. Außerdem sind die Parameter \(\sigma_z^2\) (Varianz der Normalverteilung) und \(\lambda\) (Abflachen der Exponentialverteilung für unvorhersehbare Hindernisse) zu bestimmen. Die Wichtung der einzelnen Verteilungen wird durch die Parameter \(z\) bestimmt.

\[P(z\mid x,m) = \left(\begin{matrix}z_{hit} \\ z_{unexp} \\ z_{max} \\ z_{rand} \end{matrix}\right)^T \cdot  \left(\begin{matrix}P_{hit}(z\mid x,m) \\ P_{unexp}(z \mid x,m) \\ P_{max}(z \mid x,m) \\ P_{rand}(z \mid x,m) \end{matrix}\right)\]

Die Parameter \(z_{hit}, z_{unexp}, z_{max}, z_{rand}, \sigma_z, \lambda\) sind nun geeignet zu ermitteln, damit das mathematsiche Sensormodell ideal zur Einsatzbedingung passt.

Daten, Daten, Daten

Hat man nur lange genug Realdaten gesammelt, so kann man die gemessenen Daten mit der Verteilung übereinander legen und die Parameter ‚per Hand‘ anpassen, bis es augenscheinlich stimmig ist. In nachfolgender Abbildung ist zu erkennen, dass der Sensor für großen Distanzen neben dem Wahren Wert (hier: ca. 3,55m) auch maximale Messwerte und Werte vor dem Hindernis zurück gibt. Dies entspricht dem Mixture Density Model.

knapp 2500 Messwerte mit Tinkerforge Ultraschall-Bricklet gegen eine weiße Wand in ca. 3.55m Entfernung

knapp 2500 Messwerte mit Tinkerforge Ultraschall-Bricklet gegen eine weiße Wand in ca. 3.55m Entfernung

Eine entsprechende ‚per Hand‘ Parametrierung könnte so aussehen. Das reicht für die meisten Anwendungsfälle aus. Die benötigten Parameter sind beispielsweise:

z_hit = 7.0
z_unexp = 0.05
z_rand = 0.05
z_maxrange = 0.03

# Normalize
normz = 1.0/(z_hit+z_unexp+z_rand+z_maxrange)
z_hit=z_hit/normz
z_unexp=z_unexp/normz
z_rand=z_rand/normz
z_maxrange=z_maxrange/normz

def Pzx(z):
    # Sum of all with weighted average
    return z_hit*Pzx_hit(z) + \
            z_unexp*Pzx_unexp(z) + \
            z_rand*Pzx_rand(z) + \
            z_maxrange*Pzx_maxrange(z)

Die Funktion Pzx gibt nun für jede Entfernung z die Wahrscheinlichkeit zurück, dass von dort ein Messwert zurück kommt.

Normiert ergeben sich folgende Zahlen:

Histogram einer realen Messung (blau) und parametriertes Mixture Density Model (grün)

Histogram einer realen Messung (blau) und parametriertes Mixture Density Model (grün)

Das reicht für die meisten Anwendungsfälle aus. Mit dieser probalistischen Funktion kann nun von einem umherfahrenden Roboter trotz Sensorstörungen eine Karte des Raumes erstellt werden. Dieses Kapitel soll hier nicht weiter vertieft werden.

Möchte man die Parametrierung nicht per Hand machen, schlägt Thrun die Expectation Maximization vor.

Expectation Maximization

Ist das ‚per Hand‘ parametrieren zu aufwendig, kann auch ein Maximum Likelihood Estimator eingesetzt werden, welcher die Parameter bestmöglich schätzt. In [Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic robotics. Retrieved from http://www.lavoisier.fr/livre/notice.asp?ouvrage=1218883] kann dieser nachgeschlagen werden.

ExpectationMaximization_IntrinsicParameter

Fazit

Ein Sensor gibt nie den Wahren Wert aus und auch nicht immer normalverteilte Messwerte um den wahren Wert. In einigen Fällen müssen noch andere Einflussparameter probalistisch berücksichtit werden, um bessere Ergebnisse zu erhalten.

In diesem Beitrag wurde gezeigt, wie die Tinkerforge Ultraschallsensoren an die gängige Literatur zur Robotik angepasst werden können. Damit sollten Robotikprojekte starten können…

Der Code

Leave a Reply

Deine E-Mail-Adresse wird nicht veröffentlicht. Erforderliche Felder sind mit * markiert.