Aktuální vydání

celé číslo

08

2022

MSV 2022

Projektování, konstruování a programování automatizačních a řídicích systémů

celé číslo

Histogramové navigačné algoritmy – vývoj a princíp

Andrej Babinec, Anton Vitko
 
Článok informuje o vzniku a vývoji reaktívnych navigačných metód založených na tvorbe polárneho histogramu. Je určený najmä pre tých čitateľov, ktorí hľadajú spoľahlivý spôsob bezkolíznej navigácie pre mobilnú robotickú platformu, no môže poslúžiť aj ako inšpirácia pri vývoji nových techník. Diskutované metódy môžu byť implementované v autonómnych alebo semi-autonómnych servisných mobilných robotoch, popr. v iných špeciálnych aplikáciách vyžadujúcich bezkolízne riadenie. Východiskové algoritmy sú rozpísané do väčšej hĺbky tak, aby bolo možné pochopiť ich princíp.
 
The article informs about beginnings and developments of reactive navigation methods based on creating a polar histogram. It is intended mainly for those readers who are looking for reliable approach to anti-collision navigation of a mobile robotic platforms, but also it can serve as an inspiration for developing new techniques. The discussed methods can be implemented in autonomous or semi-autonomous service mobile robots or in other special applications requiring anti-collision control. The original algorithms are described in more detail in order to make their principles easy understandable.
 

1. Úvod

 
Histogramové metódy pre navigáciu v neznámom prostredí sa označujú skratkou VFH, čo v angličtine znamená Vector Field Histogram. Prvýkrát ho prezentovali J. Borenstein a Y. Koren v roku 1991 [1] ako snahu eliminovať nedostatky potenciálových metód. Jedna z takýchto metód je označovaná VFF (Virtual Force Field). Táto potenciálová metóda využíva mriežku lokálnej mapy, ktorej bunky pôsobia na robot virtuálnymi silami. Veľkosť týchto síl závisí od hodnôt v jednotlivých bunkách, ktoré si robot aktualizuje na základe detekcie vzdialeností k prekážkam. Nie je to však pravdepodobnostná mriežka, ale aditívna (histogramová mriežka). Podstatou je, že jedno meranie senzorom vzdialenosti spôsobí zmenu len jednej bunky, a to inkrement o jednotku. Ak sa používajú ultrazvukové senzory, ide o bunku, ktorá sa nachádza na osi vyžarovacieho uhla senzora vo vzdialenosti nameranej týmto senzorom od pozície robota. Bunky obsahujúce nenulovú hodnotu sa nazývajú aktívne. Možno povedať, že metóda VFH vychádza priamo z metódy VFF, pretože ako počiatočná vstupná informácia do nej vstupuje aktuálna histogramová mriežka. Pôvodná metóda VFH časom prechádzala úpravami. Vznikli tak postupne metódy VFH+ (v roku 1998), VFH* (v roku 2000) a Scaled VFH (v roku 2005).
 

2. VFH

 

2.1 Prístup VFH

Príčinou problémov VFF prístupu, na ktorý metóda VFH nadväzuje, je drastická redukcia dát, keď sa z čiastkových odpudivých síl od jednotlivých aktívnych buniek vytvorí v jednom kroku jedna výsledná sila. Tým sa stratí informácia o lokálnom rozložení prekážok. Aby sa tomuto predišlo, metóda VFH uvažuje s dvojstupňovou redukciou, a preto vzniknú tri úrovne reprezentácie dát:
  1. Na prvej úrovni sú uchovávané detailné informácie o blízkom okolí robota v histogramovej mriežke, ktoré sú neustále aktualizované s periódou vzorkovania snímačov. Tento proces je identický s tým, aký sa používa pri metóde VFF.
  2. Ďalšia úroveň vznikne po prvej redukcii z dvojrozmernej histogramovej mriežky na jednorozmerný polárny histogram H vytvorený okolo aktuálnej pozície robota. Pozostáva z n uhlových sektorov veľkosti a a hodnota každého stĺpca tohto histogramu predstavuje akúsi intenzitu prekážok hk v k-tom sektore.
  3. Poslednou úrovňou je už samotný výstup algoritmu, ktorým je smer pohybu robota, určený z polárneho histogramu.
Popíšme si bližšie jednotlivé redukcie dát.
 

2.2 Vytvorenie polárneho histogramu H

Do úvahy sa berú len bunky ci,j* štvorcového aktívneho okna C* s veľkosťou strany wsbuniek. Obsah každej bunky je interpretovaný ako vektor smerujúci z pozície danej bunky do ťažiska robota pod uhlom βi,js veľkosťou mi,j, pričom platí
 
rovnice (1)
 
rovnice (2)
 
kde
a, b sú kladné konštanty slúžiace na nastavenie nulového vplyvu okrajových buniek aktívneho okna na robot,
c*i, jhodnoty jednotlivých aktívnych buniek v histogramovej mriežke,
di, jjednotlivé vzdialenosti buniek od ťažiska robota,
xia yjsúradnice aktívnych buniek, a x0 a y0 sú súradnice ťažiska robota.
 
Ďalej je treba zvoliť rozlišovaciu schopnosť histogramu a, aby platilo
rovnice (3)
 
kde n je prirodzené celé číslo.
 
Získa sa tak n uhlových sektorov zobrazených na obr. 1. Každému sektoru prislúcha diskrétny uhol r kvantovaný násobkami a, teda r = kα, kde k = 0, 1, ..., n – 1.
 
Príslušnosť bunky ci,j* do k-teho sektora je daná vzťahom
 
rovnice (4)
 
Každému sektoru možno teraz prisúdiť intenzitu prekážok hk v danom smere podľa vzťahu
 
rovnice (5)
 
Ďalej sa môže použiť vyhladzovacia funkcia na zjemnenie nerovností histogramu napr. v tvare dolno-priepustného filtra
 
rovnice (6)
 
kde l je počet susedných hodnôt, ktoré sa pri vyhladzovaní uvažujú. Výsledkom je polárny histogram, ktorého príklad je na obr. 2.
 

2.3 Stanovenie následujúceho smeru pohubu θ

Na histograme možno pozorovať vrcholy a údolia. Stanovením prahovej hodnoty intenzity prekážok sa definujú sektory s nižšou hodnotou h'k, než je daný prah, ktoré sa stanú kandidátmi na možný priechod medzi prekážkami. Algoritmus zmeria veľkosti jednotlivých možných priechodov, ako počet sektorov s, ktoré majú podprahovú hodnotu, a podľa zvolenej hranice smax ich rozdelí na široké (s smax) a úzke (s < smax). Ďalej sa nájde taký okraj možného priechodu, ktorý je najbližšie k smeru, pod ktorým sa nachádza cieľ voči robotu. Tento okraj (sektor) sa označí ako kn (near border). Na stanovenie nového smeru θ je však potrebné určiť aj sektor nazvaný kf (far border), ktorý leží v tom istom priechode, ale je o niečo vzdialený od smeru k cieľu. Jeho umiestnenie závisí od toho, či je kn okrajom širokého alebo úzkeho priechodu:
  • pre s smax platí kf = kn + smax,
  • pre s < smax platí, že kf je opačným okrajovým sektorom zvoleného priechodu.
Nasledujúci smer robota je potom daný vzťahom
 
rovnice (7)
 
Výhodou takéhoto určovania smeru je, že pri úzkych priechodoch sa robot drží v strede medzi prekážkami a pri širokých priechodoch sa presúva popri tej prekážke, ktorá je blízko cieľa, a obíde ju rovnomerným pohybom bez oscilácií.
 

3. VFH+

 

3.1 Prístup VFH+

Keďže pôvodná metóda VFH nezohľadňovala dynamiku ani rozmery robota, ľahko sa mohlo stať, že algoritmus zadal taký nový smer, na ktorý sa robot nedokáže natočiť okamžite, a z dôvodu, že sa pohybuje určitou rýchlosťou, narazí do obchádzanej prekážky. Preto VFH prešla viacerými vylepšeniami a vznikla nová verzia – VFH+ [2], ktorá okrem toho, že zohľadňuje dynamiku a rozmery robota, rieši aj problém vyhladzovania histogramu bez nutnosti použitia dolno-priepustného filtra a navyše pridáva prepracovanejší spôsob určovania nového smeru, ktorý umožňuje prispôsobiť správanie robota podľa jeho účelu. Z pôvodne dvojstupňovej redukcie dát vznikla štvorstupňová, ktorá bude v ďalšom texte popísaná.
 

3.2 Vytvorenie primárneho polárneho histogramu Hp

Postup je rovnaký ako pri pôvodnej metóde VFH, avšak aby sa nemusel použiť spomínaný filter, jednotlivé aktívne bunky sa pomyselne rozšíria na kruh s polomerom rr + s daným vzťahom
 
rorvice          (8)
 
kde
rr je polomer robota,
ds bezpečná vzdialenosť, ktorú má robot dodržiavať od prekážky.
 
Ako vidno na obr. 3, vznikne tým pre každú bunku zväčšovací uhol gi,j
 
rovnice (9)
 
Preto sa hodnoty v jednotlivých sektoroch počítajú podľa vzťahu
 
rovnice (10)
 
kde
 
rovnice (11)
 
Primárny polárny histogram je naznačený na obr. 6a.
 

3.3 Vytvorenie binárneho polárneho histogramu Hb

K tejto úprave sa dospelo z dôvodu, že pôvodný algoritmus počas pohybu robota niekedy osciloval medzi viacerými možnými úzkymi priechodmi blízko smeru k cieľu. A kvôli takémuto nerozhodnému správaniu sa mohol dostať veľmi blízko k prekážke. Táto redukcia dát je založená na použití nie jedného, ale dvoch prahov τlow, τhigh, pre rozhodovanie o vhodnosti priechodu, ktoré tvoria hysterézu. Binárny polárny histogram, zobrazený na obr. 6b, je vytvorený z primárneho polárneho histogramu na základe nasledujúcich pravidiel
 
rovnice (12)
 

3.4 Vytvorenie polárneho histogramu s maskou Hm

Tretí stupeň redukcie dát umožňuje do algoritmu zakomponovať spomínané dynamické obmedzenia. Zatiaľ čo pôvodná verzia počítala s tým, že robot je schopný sa presunúť zo svojej aktuálnej pozície do novej po priamke, tu sa pri určovaní zablokovaných sektorov uvažuje s aproximáciou trajektórie kruhovými oblúkmi s krivosťou k = 1/r, ktorá závisí od rýchlosti robota. Znázorňuje to obr. 4.
 
Po ľavej (left – L) a pravej (right – R) strane robota sa vytvoria kružnice s polomermi rL, rR tak ako v príklade na obr. 5. Stredy kružníc sú dané relatívnym posunom od ťažiska vzťahmi
 
rovnice (13)
 
Pre vytvorenie histogramu s maskou sa kontroluje, či sa niektorá z kružníc trajektórie pretína s kružnicou okolo rozšírenej aktívnej bunky ci,j*. Ak je táto podmienka splnená, v histograme sa označia ako blokované všetky sektory, ktoré sa nachádzajú od smeru k tejto bunke bi,jnaľavo (pre ľavú kružnicu), resp. napravo (pre pravú kružnicu), až po smer, ktorý predstavuje spätný chod robota vzhľadom na aktuálny smer.
 
Vzdialenosti dR, dL od aktívnych buniek ku stredom kružnicových trajektórií sú dané vzťahmi
 
rovnice (14)
 
Prekážka blokuje smery napravo od nej, ak platí podmienka
 
rovnice (15)
 
Prekážka blokuje smery naľavo od nej, ak platí podmienka
 
rovnice (16)
 
Pre pravú aj ľavú stranu robota treba nájsť limitné uhly φR, φL, ktoré určujú, aký je maximálne možný dovolený smer vpravo a vľavo od robota. Nech φb je smer robota vzad, teda φb = θ +π. Ďalej nech na začiatku platí, že φR = φL = φb. Potom pre každú aktívnu bunku s hodnotou ci,j* > τ sa vykonajú nasledovné úkony:
 
a) ak je βi,jnapravo od θ a naľavo od φR, kontroluje sa podmienka (15); keď je splnená, priradí sa φR = bi,j,
b) ak je bi,jnaľavo od θ a napravo od φL, kontroluje sa podmienka (16);
 
keď je splnená, priradí sa φL = bi,j. Po skontrolovaní každej aktívnej bunky je už možné vytvoriť z binárneho histogramu maskovaný polárny histogram pomocou nasledovných pravidiel
 
rovnice (17)
 
Polárny histogram s maskou na obr. 6c vyjadruje, ktoré smery sú pri aktuálnej rýchlosti povolené. Ak by sa stalo, že by v ňom boli blokované všetky sektory, je potrebné znížiť rýchlosť, pre ktorú sa znova vytvorí nový maskovaný histogram. Ak sú rovnako blokované pre všetky možné rýchlosti, je potrebné, aby robot okamžite zastavil, pretože sa s najväčšou pravdepodobnosťou nachádza v priestore, odkiaľ nevie sám vyjsť.
 

3.5 Výber nasledujúceho smeru pohybu θ

V poslednej redukcii dát je potrebné vybrať optimálny smer, ktorým sa v nasledovnom kroku robot pohne. Nie vždy však musí byť výhodný ten, ktorý je najmenej odchýlený od smeru k cieľu. Preto sa v prvom kroku pri určovaní kandidátov na nový smer neuvažuje smer k cieľu. V maskovanom histograme sa teda rozpoznajú všetky priechody. V tých, ktoré sú nazvané úzke, sa označia ako kandidátske smery stredné sektory a vo všetkých širokých sa nájdu dva potenciálne sektory podľa pravidiel podobných ako v pôvodnej verzii VFH, avšak pre obe hranice tohto priechodu. Ak sa smer k cieľu nachádza vo voľnom priestore medzi ľavým a pravým kandidátskym sektorom širokého otvoru, jeho sektor sa takisto zahrnie medzi kandidátov. Tieto vybrané smery potom vstupujú do procesu výberu pomocou hodnotovej funkcie
 
rovnice (18)
 
kde funkcia D(c1,c2) vráti najmenší rozdiel uhlov medzi sektormi c1 a c2. Ďalej c je kandidátsky sektor, kt je sektor, v ktorom leží cieľ, θi/a predstavuje sektor, do ktorého sú natočené kolesá vozidla v aktuálnom kroku, a kn,i-1 je sektor, ktorý bol zvolený ako najvýhodnejší v predošlom kroku. To znamená, že prvý sčítanec je zodpovedný za ohodnotenie funkcie na základe odchýlky k cieľu, druhý na základe potrebného natočenia kolies do nového smeru a tretí na základe rozdielu medzi predošlým a novým smerom. Konštanty m nastavujú charakter výberu podľa toho, na aký typ správania robota sa treba orientovať. Podľa tohto prístupu je najlepší ten smer, ktorého hodnota funkcie g(c) je najnižšia.
 

4. VFH*

 
Mnohé lokálne metódy trpia jedným spoločným nedostatkom. Keď sa pred robotom objaví prekážka v smere jeho priameho pohybu, musí sa rozhodnúť medzi dvoma, v tej chvíli pre robot rovnako výhodnými možnosťami. Obísť ju sprava alebo zľava. Keďže robot pri svojom rozhodovaní využíva len bunky v aktívnom okne, nemusí byť jeho správanie vždy optimálne, lebo nevie, aká situácia nastane, keď sa pohne vybraným smerom. Metóda VFH* [3] je úpravou metódy VFH+, pri ktorej je možné analyzovať aj dôsledky pohybu všetkými prípustnými smermi.
 
V aktuálnej pozícii sa teda vytvorí polárny histogram, na základe ktorého sa určia primárne možné smery. Pre každý smer sa určí nová orientácia a poloha robota, ktorú by mal po prejdení plánovanej dráhy ds. Pre tieto plánované pozície sa na základe informácií z lokálnej mapy stanovia nové polárne histogramy, ktorých analýzou vzniknú nové plánované smery. Opakovaním tohto postupu ng-krát sa získa strom trajektórií, ako napr. na obr. 7, s celkovou plánovanou dráhou dt = ngds, ktorú môže prejsť z aktuálnej pozície. Výber správneho primárneho smeru je podmienený najnižšou hodnotou cesty vypočítanej na základe istej heuristickej funkcie h(c), ktorá je podobná ako funkcia g(c) použitá v algoritme VFH+.
 
Vďaka tejto metóde sa robot vie rozhodnúť pre správny smer tým spoľahlivejšie, čím hlbší strom dokáže vytvoriť. Avšak počet vetvení ng je závislý od toho, do akej miery má robot preskúmanú oblasť mimo aktívne okna. To znamená, že algoritmus zužitkuje pri určovaní optimálneho smeru celú lokálnu mapu, ktorej tvorba závisí od kvality senzorickej informácie. Nie je preto vhodné používať príliš veľké hodnoty ng, pri ktorých by plánovaná trasa presiahla možnosti senzorov preskúmať daný vzdialený priestor.
 

5. Scaled VFH

 
Napriek tomu, že táto úprava nasleduje chronologicky za VFH*, nie je jej vylepšením, ale obmieňa pôvodnú metódu VFH. Zmena spočíva len v spôsobe, akým sa vytvára histogram. Do jedného stĺpca histogramu sa totiž nezarátavajú hodnoty len z príslušného sektora, ale aj zo susedných sektorov, ktoré sa nachádzajú napravo a naľavo od aktuálneho smeru merania v oblúkovej vzdialenosti menšej ako θ [4]
 
rovnice (19)
 
Rozširovací oblúk vplyvu prekážky θ je nepriamo úmerný k nameranej vzdialenosti r v danom smere. Tento vplyv je možné ešte škálovať rozširovacím faktorom k. Robot sa tak dokáže oveľa razantnejšie vyhnúť blízkej prekážke, a pritom ďalekú prekážku obchádza pozvoľna.
 
Scaled VFH zachováva jednoduchosť pôvodnej metódy VFH, pričom do nej vnáša podobný prvok, aký sa nachádza vo VFH+ pri tvorbe histogramu po rozšírení buniek histogramovej mriežky na kruh s polomerom rr + s.
 

6. Záver

 
Postupný vývoj v oblasti histogramových metód naznačuje, že tieto algoritmy sú dostatočne tvarovateľné pri súčasnom zachovaní ich podstaty. Umožňujú vytvoriť prispôsobenie, ktoré metódu nielen vylepšuje, ale aj dopĺňa o kvalitatívne iné možnosti. Najprepracovanejšia úprava VFH* totiž dokáže v obmedzenej forme aplikovať výhody globálneho plánovania na lokálnej úrovni. V článku boli podrobne rozpísané najmä východiskové metódy, na ktoré nadväzujú iné úpravy doplnením, popr. zmenou niektorých postupov.
 
Literatúra:
[1] BORENSTEIN, J. a i.: The vector field histogram – fast obstacle avoidance for mobile robots. IEEE Journal of Robotic and Automation, 1991, Vol. 7, No. 3, 6.
[2] ULRICH, I. a i.: VFH+: Reliable obstacle avoidance for fast mobile robots. In: Proceedings of The IEEE International Conference of Robotics and Automation, 5/1998.
[3] URLICH, I. a i.: VFH*: Local obstacle avoidance with look-ahead verification. In: Proceedings of The IEEE International Conference of Robotics and Automation, 4/2000.
[4] YAMAUCHI, B.: The Wayfarer modular navigation payload for inteligent robot infrastructure. In: Proceedings of SPIE, Vol. 5804, Bellingham, 2005.
 
Ing. Andrej Babinec
doc. Ing. Anton Vitko, PhD.
Ústav riadenia a priemyselnej informatiky,
Fakulta elektrotechniky a informatiky,
Slovenská technická univerzita
 
Obr. 1. Rozdelenie aktívneho okna na sektory (modrou farbou sú vyznačené bunky prislúchajúce jednému sektoru)
Obr. 2. Polárne histogramy H: a) vyhladený polárny histogram, b) ten istý polárny histogram rozložený do kruhovej formy prekrývajúci sa s histogramovou mriežkou aktívneho okna, v ktorej sú hodnoty aktívnych buniek úmerné veľkosti čiernych štvorcov [1]
Obr. 3. Zväčšenie aktívnej bunky o polomer rr + s[2]
Obr. 4. Princíp polárneho histogramu s maskou: a) predpokladané trajektórie pri metóde VFH, b) aproximácia trajektórií pri metóde VFH+ [2]
Obr. 5. Príklad blokovaných smerov [2]
Obr. 6. Vytvorenie polárneho histogramu s maskou: a) primárny polárny histogram, b) binárny polárny histogram, c) polárny histogram s maskou [2]
Obr. 7. Prehľadávacie stromy metódy VFH* pri: a) ng= 2, b) ng= 5, c) ng= 10 (tmavé krivky naznačujú najlepšiu možnú naplánovanú trasu [3])