# VFH+

Het Vector Field Histogram Plus (VFH+) algoritme is een real time obstacle avoidance algoritme, en is de opvolger van het VFH (Vector Field Histogram). algoritme. VFH+ is ontwikkeld voor een speciaal type mobiele robot, genaamd de GuideCane. De GuideCane is een nieuwe geleidingsinrichting voor blinden waarbij de gebruiker de GuideCane voor zich uit duwt. Wanneer de GuideCane een obstakel opmerkt, stuurt het eromheen. De gebruiker merkt de verandering van de GuideCane en kan het apparaat blindelings volgen zonder enige bewuste inspanning. Vanwege de overeenkomst in functie tussen de GuideCane en andere conventionele mobiele robots, wordt obstacle ovoidance met de VFH+ methode evengoed voor andere mobiele robots gebruikt.

## Het VFH+ algoritme

Het concept van het VFH + obstacle avoidance algoritme is vergelijkbaar met het oorspronkelijke VFH algoritme. De output van dit algoritme is een map grid van de lokale omgeving, het zogenaamde histogram grid, dat is gebaseerd op de eerdere certainty grid -en de occupancy grid methoden. De VFH+ -methode maakt gebruik van een vier-fase datareductieproces om de nieuwe richting te berekenen. In de eerste drie fasen, wordt de tweedimensionale map grid gereduceerd tot eendimensionale polaire histogrammen die zijn opgebouwd rond de robot zijn locatie. In de vierde fase selecteert het algoritme de meest geschikte richting gebaseerd op het masked polar histogram en een kostenfunctie.

## Werking
### Het primaire Polaire Histogram

De eerste data-reductiefase mapped het actieve gebied van het map grid op het primaire polaire histogram. De actieve regio is een rond venster met een diameter $w_{s}$ dat met de robot meebeweegt. De inhoud van elke actieve cel in het map grid wordt behandeld als een obstacle vector. De vector richting $b_{i,j}$ wordt bepaald door de richting van de actieve cel naar het robot centrum punt (RCP):
$b_{i,j}=tan^{-1}{\frac {y_{0}-y_{j}}{x_{i}-x_{0}}}$
Waar :
* $x_{0},y_{0}$ : Present coördinaten van het RCP.
* $x_{i},y_{j}$ : Coördinaten van de actieve cel $C_{i,j}$.
De vector magnitude van een actieve cel $C_{i,j}$ is gegeven door:
$m_{i,j}=(c_{i,j})^{2}(a-bd_{i,j}^{2})$
Waar :
* $c_{i,j}$ : Zekerheid waarde van de actieve cel $C_{i,j}$.
* $d_{i,j}$ : Afstand van actieve cel $C_{i,j}$ om de RCP.
En de parameters a en b worden gekozen op basis van :
$a-b{\frac {w_{s}-1}{2}}=1$
Merk op dat $c_{i,j}$ exponentieel zal stijgen of dalen. Dit geeft een betrouwbaardere reeks lezingen. Hoge $c_{i,j}$ waarden staan voor daadwerkelijke obstakels, in tegenstelling tot lage $c_{i,j}$ waarden, die veroorzaakt kunnen worden door noise. De vector magnitude is ook een functie van de afstand $(d_{i,j})^{2}$. Indien de bezette cellen dicht bij de robot zijn, zullen ze grotere vector grootheden aannemen. Een handige eigenschap van deze functie is dat deze rotatiesymmetrisch is ten opzichte van de RCP. Hierdoor is het gedrag van de robot onafhankelijk van de richting waarin obstakels worden ontmoet. Gebaseerd op de obstacle vectoren wordt het primaire polaire histogram $H_{p}$ opgebouwd.
De oorspronkelijke VFH methode hield niet expliciet rekening met de breedte van de robot. In plaats daarvan werd een empirisch bepaalde laagdoorlaatfilter gebruikt om de breedte van de robot te compenseren. Maar zelfs met een goede afgestemde filter had de robot toch de neiging om hoeken af te snijden. De VFH+ methode daarentegen gebruikt een theoretisch bepaalde laagdoorlaatfilter ter compensatie. Obstacle-cellen worden vergroot op de map met de robot radius $r_{r}$. Deze radius wordt gedefinieerd als de afstand van het centrum van de robot naar haar verste punt. Voor verdere veiligheid zijn de obstakel-cellen daadwerkelijk vergroot door een straal $r_{r+s}=r_{r}+d_{s}$ waarbij $d_{s}$ de minimum afstand tussen de robot en een obstakel is. Met de obstakels vergroot door $r_{r}+s$, kan de robot worden behandeld als een point-achtige auto. Deze methode werkt goed voor mobiele robots waarvan de vorm kan worden benaderd door een disk. Als vorm van de robot zeer asymmetrisch is, zullen de obstacle-cellen moeten worden uitgebreid naar de afmetingen en oriëntatie van de robot. Deze breedte compensatie methode wordt zeer efficiënt geïmplementeerd door het vergroten van de obstakels.
Voor elke cel wordt de uitbreidingshoek $g_{i}$ gedefinieerd door :
$Y_{i,j}=arcsin{\frac {r_{r+s}}{d_{i,j}}}$
Voor elke sector k wordt de polaire obstacle density berekend door:
$H_{k}^{p}=\sum _{i,j\in Ca}{m_{i,j}.h'_{i,j}}$
Met:
$h'_{i,j}=1$ if $k.\alpha \in [b_{i,j}-Y_{i,j},b_{i,j}+Y_{i,j}]$
$h'_{i,j}=0$ otherwise
Het resultaat van dit proces is een polair histogram dat rekening houdt met de breedte van de robot. De h' functie dient ook als een low-pass filter en "smooth" het polaire histogram. Een andere belangrijke verbetering is het proces dat de moeilijke afstemming van de VFH lowpass Filter elimineert.

### Het Binaire Polaire Histogram

Voor de meeste toepassingen is een vlot traject gewenst en moeten oscillaties in het stuurcommando worden vermeden. De oorspronkelijke VFH methode geeft meestal een zeer smooth traject aan. De vaste drempelwaarde in de oorspronkelijke VFH methode kan echter een probleem veroorzaken bij omgevingen met meerdere smalle openingen. Zo kan bijvoorbeeld een overeenkomstige opening in het histogram meerdere malen afwisselen tussen een open en een geblokkeerde toestand tijdens een paar bemonsteringstijden. In een dergelijke situatie, kan de robot zijn keuze meermaals wisselen tussen deze smalle opening en een opening. Het resultaat is een besluiteloos gedrag, waarbij de mobiele robot problemen kan krijgen dicht bij een obstakel. Dit probleem kan eenvoudig worden verminderd door een hysteresis toe te voegen op basis van twee drempels, namelijk $t_{laag}$ en $t_{hoog}$. Op basis van het primaire polaire histogram $H_{p}$ en de twee drempels, wordt een binair polair histogram $H_{b}$ opgebouwd. In plaats van polaire densiteitswaarden, zijn de sectoren van $H_{b}$ ofwel vrij (0) ofwel geblokkeerd (1). Het binaire polaire histogram wordt bijgewerkt door de volgende regels:
$H_{k,j}^{b}=1$ if $H_{k,j}^{p}>T_{high}$
$H_{k,j}^{b}=0$ if $H_{k,j}^{p}<T_{low}$
$H_{k,j}^{b}=H_{k,j-1}^{b}$ otherwise

### Het masked Polar Histogram

De oorspronkelijke VFH methode verwaarloost de dynamiek en de kinematica van de robot. Het veronderstelt impliciet dat de robot in staat is om de rijrichting direct te veranderen. Tenzij de robot stopt bij elke bemonstering tijd, wordt deze veronderstelling duidelijk geschonden. De VFH+ methode maakt gebruik van een eenvoudige, maar betere benadering van het traject van de meeste mobiele robots. Het gaat ervan uit dat de baan van de robot is gebaseerd op cirkelvormige bogen (constante kromming curves) en rechte lijnen. De kromming van de curve wordt gedefinieerd met k = 1 / r. De maximale traject kromming van een mobiele robot is vaak afhankelijk van de robot snelheid. Hoe sneller de robot verplaatst, hoe kleiner de maximale kromming. In bijzondere gevallen, bijvoorbeeld bij de GuideCane, kunnen de meest gebogen waarden verschillen voor bochten naar rechts en bochten naar links. De waarden voor de minimale draaicirkel als functie van de robot snelheid kunnen gemakkelijk worden gemeten. Deze radius wordt voor beide partijen gedefinieerd als $r_{r}=1/k_{r}$ en $r_{l}=1/k_{l}$. Met deze parameters en het map grid, kunnen sectoren bereikt worden die geblokkeerd zijn door obstakels.
Als een traject cirkel en een vergrote obstakel cel overlappen, zullen alle richtingen van het obstakel tot de contraire richting van de beweging, worden geblokkeerd. In een gegeven opstelling zal bijvoorbeeld obstakel A alle richtingen aan de linkerkant blokkeren vanwege de robot dynamiek. Anderzijds, zal hindernis B niet de rechterkant blokkeren. Met de originele VFH methode zou de routebeschrijving naar links van obstakel A worden beschouwd als geschikte aanwijzingen van de beweging. Met de VFH+ methode zou de robot tussen obstakels A en B gaan en zou hij een bocht naar links maken nadat hindernis A werd ontruimd.
De posities van het rechter -en linker trajectcentra ten opzichte van de huidige robot positie worden bepaald door :
$\Delta x_{r}=r_{r}.sin\phi$
$\Delta y_{r}=r_{r}.cos\phi$
$\Delta x_{l}=-r_{l}.sin\phi$
$\Delta y_{l}=-r_{l}.cos\phi$
De afstanden van een actieve cel $C_{i,j}$ tot de twee traject centra worden gegeven door:
$d_{r}^{2}-(\Delta x_{r}-\Delta x(j))^{2}+(\Delta y_{r}-\Delta y(i))^{2}$
$d_{l}^{2}-(\Delta x_{l}-\Delta x(j))^{2}+(\Delta y_{l}-\Delta y(i))^{2}$
Een obstakel blokkeert de routebeschrijving naar rechts indien:
$d_{r}^{2}<(r_{r}+r_{r+s})$
En een obstakel blokkeert de routebeschrijving naar links indien:
$d_{l}^{2}<(r_{l}+r_{r+s})$
Door elke actieve cel te controleren met deze twee voorwaarden, krijgen we twee maximale hoeken, $\phi _{r}$ voor rechtse hoeken en $\phi _{l}$ voor de linker hoeken. We definiëren ook $\phi _{b}=\theta +\pi$ als de omgekeerde bewegingsrichting. Deze methode kan zeer efficiënt worden doorgevoerd met een algoritme dat alleen rekening houdt met de cellen die een invloed hebben op beide $\phi _{r}$ of $\phi _{l}$:
* 1)Bepaal $\phi _{b}$. Stel $\phi _{r}$ en $\phi _{l}$ gelijk aan $\phi _{b}$.
* 2)Voor elke cel $C_{i,j}$ in het actieve venster $C_{a}$ met $c_{i,j}>t$:
      * a) Indien $b_{i,j}$ rechts is van $\theta$ en links van $\phi _{r}$, controleer voorwaarde 1. Als voorwaarde wordt voldaan, stel $\phi _{r}$ gelijk aan $b_{i,j}$.
      * b) Indien $b_{i,j}$ links is van $\theta$ en rechts van $\phi _{l}$, controleer voorwaarde 2. Als voorwaarde wordt voldaan, stel $\phi _{l}$ gelijk aan $b_{i,j}$.
Indien de sensoren van de robot niet erg betrouwbaar zijn, kunnen $\phi _{r}$ en $\phi _{l}$ ook bepaald worden op een stochastische manier. In plaats van de cel certainty waarden te vergelijken met een drempel, kan men een polair histogram bouwen wiens sector waarden de zekerheid dat een sector wordt geblokkeerd geven. De waarden voor $\phi _{r}$ en $\phi _{l}$ kunnen vervolgens worden bepaald door een drempel toe te passen op dit histogram. Met $\phi _{r}$, $\phi _{l}$, en het binaire polaire histogram, kunnen we het gemaskerde polaire histogram opbouwen:
$H_{k}^{m}=0$ if $H_{k}^{b}=0$ and $(k.\alpha )\in {[\phi _{r},\theta ],[theta,\phi _{l}]}$
$H_{k}^{m}=0$ otherwise
Het gemaskerde polaire histogram laat zien welke richtingen van beweging mogelijk zijn op de huidige snelheid. Als alle sectoren worden geblokkeerd, kan de robot niet verdergaan tegen de huidige snelheid. De robot zal dan een reeks nieuwe waarden bepalen ($\phi _{l}$, $\phi _{r}$) op basis van een lagere snelheid. Wanneer het gemaskeerde polaire histogram steeds geblokkeerd wordt in alle richtingen, zal de robot onmiddellijk moeten stoppen. Het gemaskerde polaire histogram kan dus ook gebruikt worden voor het detecteren van een dead-end.

### Selectie van de Steering Direction

Het gemaskerde polaire histogram laat zien welke richtingen vrij zijn van obstakels en welke geblokkeerd zijn. Er zijn echter een aantal paden betere kandidaten dan anderen voor de nieuwe richting van de beweging. De oorspronkelijke VFH methode is zeer doelgericht door het selecteren van de “valley” die het meest overeenkomt met de richting van de goal $k_{t}$. Het selecteert vervolgens de nieuwe richting van de beweging afhankelijk van de grootte van de “valley”. De VFH+ methode vindt alle openingen in de gemaskeerd polaire histogram en berekent een reeks mogelijke kandidaat-richtingen. Een kostenfunctie wordt vervolgens toegepast op deze kandidaat-richtingen. De kandidaat richting $k_{n}$ met de laagste kosten wordt vervolgens gekozen als de nieuwe bewegingsrichting $\phi _{n}=\alpha .k_{n}$
Allereerst zijn de rechter en linker randen $k_{r}$ en $k_{l}$ van alle openingen in de gemaskerde polaire histogram bepaald. We kunnen, vergelijkbaar met de oorspronkelijke VFH methode, twee soorten openingen onderscheiden, namelijk breed en smal. Een opening wordt breed beschouwd als het verschil tussen de twee grenzen groter is dan $S_{max}$ sectoren ( zelf te definiëren, is mede afhankelijk van de breedte van de robot). Anders zal de opening smal beschouwd worden. Voor een smalle opening is er slechts één kandidaat-richting. Hier zal de robot door het midden van het verschil tussen de overeenkomstige obstakels rijden:
$c_{n}={\frac {k_{r}+k_{l}}{2}}$
Bij een brede opening zijn er twee kandidaat richting, één rechts en één aan de linkerzijde van de opening. De doelrichting is ook een kandidaat richting als het ligt tussen de twee andere richtingen:
$c_{r}=k_{r}+{\frac {S_{max}}{2}}$ naar de rechterkant
$c_{l}=k_{l}-{\frac {S_{max}}{2}}$ naar de linkerkant
$c_{t}=k_{t}$ als $k_{t}\in [c_{r},c_{l}]$
Bij een robot die willekeurig zijn omgeving moet verkennen, voegt men de kandidaat-richtingen toe aan de huidige bewegingsrichting $\theta _{i}$ of aan de eerder geselecteerde bewegingsrichting $k_{n,i-1}$
In het geval van een doelgerichte robot, krijgen we tussen de een en de drie kandidaat-richtingen voor elke opening in het gemaskerde polaire histogram. Vervolgens moeten we een geschikte kostenfunctie definiëren, zodat de robot de meest geschikte kandidaat-richting selecteert als de nieuwe richting van de beweging $\phi _{n}$. De kostenfunctie is een subtielere uitvoering dan de grove benadering van de oorspronkelijke VFH methode. Een ander voordeel is dat het gedrag van de mobiele robot eenvoudig gewijzigd kan worden door de kostenfunctie aan te passen.