Belegtheitskarten (Occupancy Grids) sind eine sehr weit verbreitete Variante, die Umgebung für den Computer zu beschreiben. Dabei wird die Welt in kleine Abschnitte eingeteilt und für jede X,Y,Z-Position wird sich die Wahrscheinlichkeit gemerkt, dass dort „etwas“ oder „nichts“ ist. Natürlich kann nur ein Sensor solche Informationen liefern, weshalb eine Beschreibung dieses Sensors notwendig ist. In [Elfes, A. (1989). Using occupancy grids for mobile robot perception and navigation. Computer, 22(6), 46–57. doi:10.1109/2.30720] ist das Vorgehen gut beschrieben:
Eine Messung des Sensors wird über das Sensor Modell als Schätzung der Messung p(r|z) weiterverwendet und mit Hilfe des BAYES-Theorems auf schon bestehende Informationen angewendet. Anschließend wird über Entscheidungsschwellen eine Zelle als „frei“, „belegt“ oder „unbekannt“ eingestuft.
Forward Sensor Model
Wichtig ist die korrekte charakterisierung des so genannten „Forward Sensor Models“. Hierbei wird „nur“ die statistische Verteilung der Messergebnisse beschrieben. Kurzum: Eigentlich misst der Sensor einen Wert normalverteilt um den wahren Wert, allerdings werden ein paar Messwerte auf dem Weg zum eigentlichen Objekt vorher reflektiert (z.B. durch Fensterscheiben oder Blätter oder Staub) oder verlieren sich ganz (werden gar nicht reflektiert). Eine beispielhafte, schematische Wahrscheinlichkeitsverteilung für das Forward Sensor Model eines Laserscanners ist in nachfolgender Abbildung dargestellt:
Die Wahrscheinlichkeitsdichtefunktion setzt sich zusammen aus:
[math]p(r|z)=\begin{cases}
\textrm{Gleichverteilt} & \text{if } r < r_{z}-3\sigma \\ \textrm{Normalverteilt} & \text{if } r = r_{z}\pm 3\sigma \\ \textrm{0} & \text{if } r > r_{z}+3\sigma \\
\textrm{Dirac} & \text{if } r = r_{max}
\end{cases}
[/math]
wobei r die Entfernung zum Scanner, rZ die tatsächliche Entfernung zum Ziel (also das wahrscheinlichste Sensor Messergebnis) und rmax die maximale Entfernung die der Sensor ermitteln kann, bezeichnet.
Zu Beginn herrscht absolute Unsicherheit
Zu Beginn einer Messung herrscht über jede Zelle des Raums absolute Unsicherheit, ob diese belegt oder frei ist.
[math]p(m|z_{t0})=0{,}5[/math]
Ein Sensor misst eine Distanz in eine bestimmte Richtung. Die Wahrscheinlichkeit, dass an genau diesem Ort „etwas“ ist, kann nun mit der Wahrscheinlichkeit dieser Zelle (zu Beginn p=0.5) und der Wahrscheinlichkeit des Sensor Forward Modells berechnet werden.
Berechnung der Wahrscheinlichkeit einer Zelle belegt oder frei zu sein
Hier hilft das BAYES Theorem weiter, welches das Zusammenrechnen von Wahrscheinlichkeiten ermöglicht. Zuerst wird der Faktor S berechnet:
[math]S=\cfrac{p(m|z_t)}{1-p(m|z_t)} \cdot \cfrac{p(m|z_1,…,z_{t-1})}{1-p(m|z_1,…,z_{t-1})}[/math]
Anschließend liefert dieser die Wahrscheinlichkeit p, dass die Zelle m nach z Messungen belegt ist.
[math]p(m|z_1,…,z_t)=\cfrac{S}{1+S}[/math]
Nutzt man ein sehr einfaches Sensor Forward Model, nämlich eine konstante Wahrscheinlichkeit p(m|zt), so kann man sich die Entwicklung der Belegtheitswahrscheinlichkeiten über 10 Zeitschritte sehr einfach berechnen.
httpvh://www.youtube.com/watch?v=Iuj2i2DXvH4
Natürlich gibt es andere, aufwendigere Sensor Forward Modelle, welche das Verhalten des Sensors besser widerspiegelt, doch das ist für’s Verstehen nicht wesentlich.
Schön zu erkennen an oben stehender Grafik: Für eine Sensor-Wahrscheinlichkeit kleiner 0.5 sinkt die Wahrscheinlichkeit, die Zelle wird also zunehmen als „frei“ angesehen, für eine Wahrscheinlichkeit größer 0.5 steigt die „occupied“ Wahrscheinlichkeit an. Beide Informationen sind gleich wichtig, denn für autonom fahrende Fahrzeuge oder Roboter im Haushalt ist es wichtig, wo freier Raum ist, nicht unbedingt wo etwas im Weg liegt.
Man möchte oft wissen, wo Freiraum ist
Die Berechnung der „free“ cells ist mindestens genauso wichtig wie die „occupied“ cells. Beide Ergebnisse werden berechnet.
Beispiel: Entstehung eines 2D Occupancy Grid
Die Entstehung eines 2-Dimensionalen Occupancy Grids mit der Belegtheitswahrscheinlichkeit als dritter Dimension kann man sich in nachfolgender Animation ansehen:
Dabei ist der Sensor an der Position x=10m, y=0m und misst eine Entfernung von 27m entlang der Y-Achse. Von der anfänglichen Gleichverteilung p=0.5 (alles grün) sinkt entlang des Messstrahls die Wahrscheinlichkeit, dass dort ein Hindernis ist. Da der Sensor 10x die gleiche Zelle als belegt misst, wird diese mit einer Wahrscheinlichkeit von fast p(m|zt)=1 als belegt berechnet.
Das hier genutzte Sensor Forward Model zur Berechnung von p(m|zt) ist folgendes:
[matlab]
%% Forward Sensor Model
%
function [pzt] = forwardsensormodel(r,rmax)
if r==rmax
pzt = 0.6;
elseif r<rmax
pzt = 0.4;
elseif r>rmax
pzt = 0.5;
else
return
end
end
[/matlab]
Es gibt aufwendigere Sensor Forward Modelle, beispielsweise vorgestellt in [Pathak, K., Birk, A., & Poppinga, J. (2007). 3d forward sensor modeling and application to occupancy grid based sensor fusion. Intelligent Robots and, 2. Retrieved from http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=4399406].
Die Berechnung der Wahrscheinlichkeiten jeder Zelle geschieht in einer Doppelschleife:
[matlab]
%% Calculate new Probability
%
for i=1:x
for j=1:y
S(j,i)=pzt(j,i)/(1-pzt(j,i)) * p(j,i)/(1-p(j,i));
p(j,i)=S(j,i)/(1+S(j,i));
end
end
[/matlab]
3D Occupancy Grids mit OctoMap
Möchte man nun noch eine Dimension mehr erfassen, nämlich die Welt in 3D mit Belegtheitswahrscheinlichkeiten versehen, so muss diese in so genannte Voxels (Pixel=2D, Voxel=3D) zerlegt werden. Jedes Voxel wird dann mit einer Wahrscheinlichkeit berechnet, belegt oder frei zu sein.
Eine sehr effektive und schöne Implementierung (gleich mit Darstellungsmöglichkeit) eines solchen 3D Occupancy Grids wurde von [Hornung, A., Wurm, K. M., Bennewitz, M., Stachniss, C., & Burgard, W. (2013). OctoMap: an efficient probabilistic 3D mapping framework based on octrees. Autonomous Robots, 34(3), 189–206. doi:10.1007/s10514-012-9321-0] vorgestellt und unter http://octomap.github.io/ zur Verfügung gestellt: OctoMap.
httpvh://www.youtube.com/watch?v=Wu94Q0Py7w4