SEBELOKALIZACE

MOBILNÍCH ROBOTŮ

Tomáš Jílek

Sebelokalizace

Autonomní určení pozice a orientace robotu ve

zvoleném souřadnicovém systému

Souřadnicové systémy Globální / lokální

WGS-84, ETRS-89 – globální

Pravoúhlé souřadnice [x, y, z]

Eliptické (zeměpisné) souřadnice [λ, φ, H]

S-JTSK, S-42 – lokální

Prostorové / rovinné

Typické případy lokalizace V prostoru (6DOF) [x, y, z, pitch, roll, yaw]

V rovině (3DOF) [x, y, φ]

Popis orientace v prostoru

Využití lokalizace

Vizualizace polohy mobilního robotu Informace o aktuální pozici a orientaci mobilního

robotu pro operátora

Autonomní chování mobilního robotu Navigace mobilního robotu

Lokalizace měřených dat mobilním robotem Tvorba map prostředí a vizualizace polí (teplota /

ionizující záření / atd.)

Využití – vizualizace polohy

Využití – autonomní chování

?

Využití – tvorba map

Získání lokalizačních údajů

Možnosti přímého měření pozice a orientace

mobilního robotu jsou omezené

Ve většině případů je nutné použít nepřímou

metodu výpočtu lokalizačních údajů

Snímače vhodné pro účel lokalizace jsou

nepřesné a málo robustní

Nutnost kombinovat více různých přístupů

získání lokalizačních údajů pro zvýšení

přesnosti a robustnosti (datová fúze)

Základní dělení lokalizace (1)

Podle stupňů volnosti 3DOF, 6DOF, …

Podle prostředí vnější (mimo budovy)/vnitřní (v budově)

Podle znalosti vztažného bodu absolutní (např. GNSS)/relativní (např. odometrie)

Podle plošné působnosti lokální/globální (např. GNSS)

Podle znalosti mapy lokalizace v mapě/bez mapy

Základní dělení lokalizace (2)

Podle okamžiku vyhodnocení online/offline

Podle nutnosti úpravy prostředí vyžadující/nevyžadující úpravu prostředí

Podle dynamiky prostředí statické/dynamické (detekce, filtrace změny)

Podle způsobu realizace pasivní/aktivní (řízení za účelem zlepšení lokalizace)

Základní dělení lokalizace (3)

Podle počtu robotů jeden/více

Podle místa vyhodnocení interně (v robotu)/externě (mimo robot)

Podle typu algoritmu jednokrokový/iterační

Absolutní lokalizační metody

Absolutní lokalizační metody

Primárně měřenou veličinou je vzdálenost

(pro určení pozice), nebo úhlové natočení

(orientace)

Klíčovou vlastností těchto metod je, že

nekumulují chyby (výpočet lokalizačních dat

nemá integrační/sumační charakter)

Satelitní navigace

Dodává přímo souřadnice robotu (x, y, z) ve

zvoleném souřadném systému, případně i dvě jeho statické rotace (α, β) (DGNSS)

Absolutní metoda nevyžadující úpravu

prostředí, vhodná jen pro vnější prostředí

Nevyžaduje výpočetně náročné zpracování

mimo GNSS přijímač

Vhodné pro určení globální polohy

Po startu je nutná inicializace

Problém ztráty signálu

Trimble BD982

GPS + GLONASS + GALILEO přijímač

Přesnost: 8/15 mm (horiz./vert.), 0,1° (azimut)

Latence: < 20 ms

Frekvence měření: 50 Hz

Hmotnost: 92 g

Spotřeba: 2,3 W

Rozměry: 100 x 85 x 12 mm

Rozhraní: Ethernet, CAN, RS-232

Magnetometr

Pomocí tříosého magnetometru lze získat

průmět vektoru intenzity magnetického pole

Země do jednotlivých na sebe kolmých os

Problém závislosti inklinace (-65° v ČR) a

deklinace (+5° v ČR) na zeměpisné poloze

Měření ovlivňováno magneticky měkkými i

tvrdými materiály a elmg. rušením

Mapa magnetické deklinace

Akcelerometr

Pomocí tříosého akcelerometru schopného

měřit i statická zrychlení (např. kapacitní

princip) lze získat průmět tíhového vektoru G

do jednotlivých na sebe kolmých os v případě,

že na akcelerometr nepůsobí zrychlení

způsobené jeho nerovnoměrným pohybem

Rozklad vektoru do složek

Využití AC magnetického pole

V prostoru kolem vysílače je vytvořeno časově

proměnné magnetické pole a na základě

tohoto pole se vyhodnocuje poloha a orientace

vůči základně

Polhemus G4

Integrační lokalizační metody

Dead reckoning

Matematická metoda založená na postupné

integraci (sumaci) přírůstků pohybu robotu

(známé rychlosti a jeho natočení)

Měří se 1. nebo 2. derivace polohy/orientace

Problém narůstající chyby od počátku

lokalizace s narůstajícím počtem kroků

(integrování/sumace chyb)

Vhodné pouze pro dočasné použití (omezeno

vzdáleností nebo časem)

Odometrie

Lokalizace robotu na základě znalosti změn

natočení jeho kol

Varianty: aktivní/pasivní

Vychází z modelu podvozku robotu

(diferenciální, Ackermanův, …)

Odometrie

∆𝑠 =∆𝑠1+∆𝑠2

2

∆𝜑 =∆𝑠1−∆𝑠2

𝑏

𝜑𝑡 = 𝜑𝑡−1 + ∆𝜑

∆𝑥 = ∆𝑠 ∙ 𝑠𝑖𝑛 𝜑𝑡−1 +∆𝜑

2

∆𝑦 = ∆𝑠 ∙ 𝑐𝑜𝑠 𝜑𝑡−1 +∆𝜑

2

𝑥𝑡 = 𝑥𝑡−1 + ∆𝑥

𝑦𝑡 = 𝑦𝑡−1 + ∆𝑦

Odometrie – zdroje chyb

Nedostatečná adheze kol k podkladu –

prokluz/smyk kol

Přesnost (např. linearita) a rozlišení

samotného snímače polohy hřídele kola nebo

motoru

Omezená rychlost vyhodnocovací elektroniky

(vypadávání impulsů z enkodéru, …)

Algoritmus vyhodnocování polohy, jeho

rychlost a vliv zaokrouhlování

Odometrie – zdroje chyb

Vůle v převodech (při umístění snímače

polohy na hřídeli motoru)

Proměnný průměr kol – při opotřebení, zátěži

Nerovnosti povrchu, překážky (model počítá s

ideálně rovným povrchem)

Modifikace odometrie

Informace o úhlu natočení není počítána z

úhlu natočení kol, ale může být brána z

inerciálních snímačů (gyroskopy), případně z

magnetometru

Využití gyroskopu

𝜙 = 𝜔𝑦𝑠𝑖𝑛𝜙 + 𝜔𝑧𝑐𝑜𝑠𝜙 𝑡𝑎𝑛𝜃 + 𝜔𝑥

𝜃 = 𝜔𝑦𝑐𝑜𝑠𝜙 − 𝜔𝑧𝑠𝑖𝑛𝜙

𝜓 = 𝜔𝑦𝑠𝑖𝑛𝜙 + 𝜔𝑧𝑐𝑜𝑠𝜙 𝑠𝑒𝑐𝜃

𝜙 = 𝜔𝑦𝑠𝑖𝑛𝜙 + 𝜔𝑧𝑐𝑜𝑠𝜙 𝑡𝑎𝑛𝜃 + 𝜔𝑥 𝑑𝑡

𝜃 = 𝜔𝑦𝑐𝑜𝑠𝜙 − 𝜔𝑧𝑠𝑖𝑛𝜙 𝑑𝑡

𝜓 = 𝜔𝑦𝑠𝑖𝑛𝜙 + 𝜔𝑧𝑐𝑜𝑠𝜙 𝑠𝑒𝑐𝜃 𝑑𝑡

Využití inerciálních snímačů

Kardanový inerciální navigační systém Snadné vyhodnocování veličin

Náročná mechanická konstrukce

Využití inerciálních snímačů

Bezkardanový inerciální navigační systém

(strapdown) Nevyužívá mechanicky stabilizovanou základnu

Všechny snímače jsou pevně spojeny s pohybujícím

se objektem

Náročnější zpracování dat (např. nutnost správně

odečíst vektor tíhového zrychlení G podle aktuálního

úhlového natočení akcelerometru)

xSens MTi-G

Kombinace GNSS (GPS) a IMU

Statická přesnost: <0,5° (roll/pitch), <1° (yaw)

Dynamická přesnost: <1° (RMS)

Rozlišení: 0,05°

Max. rychlost poskytování dat: 120/512 Hz

Přesnost polohy: 2,5 m (CEP)

Iterační metody

Iterační metody

Iterative Closest Point (ICP) Nalezení korespondujících bodů (bodů s nejmenší

vzdáleností)

𝐸 𝑇𝑥 , 𝑇𝑦,𝜔 = 𝑅𝜔𝑃 + 𝑇 − 𝑃′𝑛𝑖

Vhodné pro odstranění translační složky

Iterative Matching Range Point (IMRP) Stejný postup jako u ICP, jiné pravidlo korespondence

Vhodné pro odstranění rotační složky

Iterative Dual Correspondence (IDC) Kombinace ICP (pro translační složku) a IMRP (pro

rotační složku)

ICP algoritmus

1. Aplikace

transformace z

předchozí iterace

2. Nalezení

korespondujících

bodů

3. Výpočet

transformace

(minimalizace

čtverce vzdáleností)

4. Aplikace

transformace

Zdroj: R. Bedroš: Modul lokalizace mobilního robotu pro systém Player

Velodyne HDL64E

Počet laserů/detektorů: 64

Počet měřených bodů: 1 300 000/s

Zorné pole: 360/27° (horiz./vert.)

Rozsah měření: 50 až 120 m

Frekvence měření: 5 až 15 Hz

Přesnost měření (1σ): 2 cm, 0,1°

Vlnová délka laserů: 905 nm

Spotřeba: 30 W

Rozhraní: Ethernet 100 Mbit/s, RS-232

Pravděpodobnostní lokalizace

Pravděpodobnostní lokalizace

Vše je náhodnou veličinou – data ze snímačů,

parametry robotu – tedy i pozice a orientace

robotu

Odhad pozice není jeden bod ale funkce

pravděpodobnosti

Pravděpodobnostní lokalizace

Přehled zdrojů dat

Dostupné primární veličiny (1)

Enkodéry – poloha kol

(φ1, φ2) → (x, y, α) → 𝑑

𝑑𝑡 → (vx, vy, ωα) →

𝑑

𝑑𝑡 → (ax, ay, εα)

Tachodynamo – rychlost kol

(x, y, α) ← 𝑑𝑡 ← (vx, vy, ωα) ← (ω1, ω2) →

(vx, vy, ωα) → 𝑑

𝑑𝑡 → (ax, ay, εα)

Magnetometr – vektor intenzity magn. pole

(Hx, Hy, Hz) → (|H|, α, β) → 𝑑

𝑑𝑡 → (ωα, ωβ) →

𝑑

𝑑𝑡 → (εα, εβ)

Gyroskop – úhlová rychlost

(α, β, γ) ← 𝑑𝑡 ← (ωα, ωβ, ωγ) → 𝑑

𝑑𝑡 → (εα, εβ, εγ)

Dostupné primární veličiny (2)

Akcelerometr – vektor celkového zrychlení (x, y, z) ← 𝑑𝑡 ← (vx, vy, vz) ← 𝑑𝑡 ← (ax, ay, az)

(ax, ay, az) → (|A|, α, β)

GNSS – polohový vektor

(x, y, z) → 𝑑

𝑑𝑡 → (vx, vy, vz) →

𝑑

𝑑𝑡 → (ax, ay, az)

Diferenciální GNSS – 2x polohový vektor

(x1, y1, z1), (x2, y2, z2) → (|A|, α, β) → 𝑑

𝑑𝑡 → (ωα, ωβ) →

→ 𝑑

𝑑𝑡 → (εα, εβ)

Laserový skener – mračno bodů (|Mi,1|, λi,1, φi,1), (|Mi,2|, λi,2, φi,2) → (x, y, z, α, β, γ)