Mozgásmodellezés. Lukovszki Csaba. Navigációs és helyalapú szolgáltatások és alkalmazások (VITMMA07)

Átírás

1 TÁVKÖZLÉSI ÉS MÉDIAINFORMATIKAI TANSZÉK () BUDAPESTI MŰSZAKI ÉS GAZDASÁGTUDOMÁNYI EGYETEM (BME) Mozgásmodellezés Lukovszki Csaba

2 Áttekintés» Probléma felvázolása» Szabadsági fokok» Diszkretizált» Hibát tartalmaz» Késleltetés» Interpoláció» Lineáris» Polinomiális» Spline» Extrapoláció»...» Konstans sebesség» Konstans késleltetés» Inerciális mérésekkel segített» IMU» EKF 2

3 Szabadság fokok» Milyen szolgáltatást szeretnénk megvalósítani» A vizsgált eszköz/személy mozgása» A technológiai lehetőségek» Az egyes helymeghatározási technológiák, módszerek milyen lehetőségeket hordoznak» Rádiós helymeghatározás» 3DoF» Autó a földön» 1DoF» Szabad mozgás kézben» 6DoF» Ember mozgás» 3DoF 3

4 Probléma felvázolása» A feladat» A megfigyelt objektum helyének és helyzetének pontos megállapítása bármikor» A helymeghatározás» Diszkrét időpontokban» Hibát hordoznak» Késleltetve jelennek meg p " = p $%&' t " + ε " helymeghatározás eredménye valós trajektória 4

5 Mozgás becslés» Interpoláció» A minták alapján összefüggő útvonalakat (helyeket és/vagy helyzeteket) keresünk» Lineáris» Polinomiális» Spline» Extrapoláció» Az eddigi minták alapján kitaláljuk milyen a várható hely és/vagy orientáció»...» Konstans sebesség/gyorsulás» Szenzor fúzió» Lehetséges bejárható útvonalak figyelembe vétele» Útvonalra illesztés» Előző helyek figyelembe vétele» A hibák figyelembe vétele szerint lehet» Determinisztikus» Valószínűségi alapú 5

6 Interpoláció» Adott értékpárok» t " p t ", k = [0,1 K]» Keressük a nem meghatározott p(t) hely értékeket tetszőleges t t " időpontokhoz p t <, p(t < ) t p(t) t 9, p(t 9 ) t ;, p(t ; ) t <=9, p(t <=9 ) t :, p(t : ) t 8, p(t 8 ) t 6

7 Lineáris interpoláció» Interpoláció szakaszonként» p t = p 8 t 8 + p >? > t t 8, t 8 t t 9» p t = p 9 t 9 + C D? D =C >? >? D =? > t t 9, t 9 t t :»... Slope, meredekség p t <, p(t < ) t 9, p(t 9 ) t ;, p(t ; ) t <=9, p(t <=9 ) t :, p(t : ) t 8, p(t 8 ) t 7

8 Polinomiális (másodfokú) interpoláció» Interpoláció szakaszonként» Másodfokú közelítés» p t = a 9 t : + b 9 t + c 9, t 8 t t 9»...» p t = a < t : + b < t + c <, t <=9 t t <» Peremfeltételek» a 9 t 8 : + b 9 t 8 + c 9 = p t 8, a 9 t 9 : + b 9 t 9 + c 9 = p t 9» :» a < t <=9 + b < t <=9 + c < = p t <=9, a < t : < + b < t < + c < = p t <» 2K egyenlet à aluldefiniált» Szomszédos minták figyelembe vétele» Folytonosság garantálása 8

9 Spline interpoláció» A szomszédos szakaszok első deriváltjának egyezése garantálja a folytonosságot» 1. szakasz: I I? a 9t : + b 9 t : + c 9 = 2a 9 t + b 9, t 8 t t 9» 2. szakasz: I I? a :t : + b : t : + c : = 2a : t + b :, t 9 t t :» A szakaszhatárokon t = t 9» 2a 9 t 9 + b 9 = 2a : t 9 + b :» 2 a 9 a : t 9 + (b 9 b : ) = 0» További K-1 egyenlet» Pl. az első szakasz lineáris 9

10 Extrapoláció konstans sebesség alapján» A legutóbbi állapot után nyugalomban marad:» Konstans sebesség» Következő állapot: p KL9 = p K + v K Δt K + ε KL9 v KL9 = v K + κ KL9» Állapotvektorral (x K ) felírva p = I Δt p + ε v KL9 0 I v K κ KL9» A sebesség számítása» Egy lépéses: v K = Sp T S? T = p? T =p(? TU> )? T =? TU>» Átlag: v K = 9 K Sp X V YZK=VL9 = 9 K p? X =p(? XU[ ) V YZK=VL9 S? X? X =? XU[ 10

11 Extrapoláció konstans gyorsulás alapján» A legutóbbi állapot után dinamikai állapot állandó:» Konstans gyorsulás» Következő állapot: p KL9 = p K + v K Δt + ε KL9 v KL9 = v K + a K Δt + κ KL9 a KL9 = a K + ξ KL9» Állapotvektorral (x K ) felírva p v a KL9 = I Δt 0 0 I Δt 0 0 I p v a K + ε κ ξ KL9 11

12 SZENZOR FÚZIÓ 12

13 Inerciális Szenzorok» Inerciális szenzorok» Mobil eszközökben széles körben elterjedt (MEMs)» Gyorsulásmérő» Giroszkóp» Mágneses szenzor» Modellezés» A matematikai modellek csak közelítik a valós működést» A pontos modell a működés megértésén alapul 13

14 Inertial Measurement Unit (IMU) Inertial Measurement Unit» Integrált megoldás I»Integrált megoldás (gyorsulás érzékelő, giroszkóp) Gyorsulás érzékelő, giroszkóp Pontos, jól» IPontos, jól modellezhető modellezhető Elterjedt megoldás» IElterjedt megoldás 14

15 Mobil vonatkoztatási rendszerek z y y Kamera frame IMU (Body) frame z y x I x G x Globális (Word) frame z 15

16 Vonatkoztatási rendszerek, jelölések» Pozíció» p ] : pozíció a globális koordináta rendszerben» p^,] : az I koordináta rendszer a globális koordináta rendszerben» Orientáció» q^, R^: az IMU koordináta rendszerében értelmezett orientáció, rotáció» q ]^, R ]^: passzív kvaternió, rotáció az IMU koordináta rendszerből a globális koordináta rendszerbe» Koordináta rendszerek közötti transzformáció» p ] = R ]^p c + p^,] : az IMU koordináta rendszerben értelmezett helyvektor értelmezése a globális térben» Az egyértelmű indexeket elhagyjuk 16

17 A gyorsulásmérő modellezése» A gyorsulásmérő az IMU-hoz rögzített koordináta rendszerben az eszköz aktuális gyorsulását méri 3D-ben (a R 3, g h )» Az eszköz relatív helyváltoztatásának mérésében van szerepe» A mért érték hibákkal terhelt» Zaj (Noise): modellezése fehér zajjal a K, a K ~N(0, σ &T )» Eltolódás (Bias): normális eloszlás szerinti véletlen bolyongás a m, a n ~N(0, σ &o ), ahol a ṁ = a n» Rossz helyzet és skála hiba (Missalignment and scale errors): modellezése átskálázással T &, T & = I + ε». a g,^ = T & R^] a ] g ] + a m,^ + a K,^ 17

18 A giroszkóp modellezése» A giroszkóp aktuális szögsebességet mér (ω R ;, $&I h ) az eszközhöz rendelt saját koordináta rendszerben» Az orientáció meghatározásában van szerepe» A mért érték hibákkal terhelt» Zaj (Noise): modellezése fehér zajjal ω K, ω K ~N(0, σ vt )» Eltolódás (Bias): normális eloszlás szerinti véletlen bolyongás ω m, ω n ~N(0, σ vo ), ahol ω m = ω n» Rossz helyzet és skála hiba (Missalignment and scale errors): modellezése átskálázással T v, T v = I + ε» Gyorsulás érzékenység (g-sensitivity): transzformáció T h, T h = ε». ω g,^ = T v ω^ + T h a^ + ω K,^ + ω m,^ 18

19 Kálmán szűrő (KF) Diszkrét, lineáris modell, normális eloszlás Vezérlés (Control) Aktuális állapottól független u " u "L9 Állapot (State) x " x "L9 x "L: Megfigyelés (Observation) Aktuális állapottól függő z " z "L9 z "L: 19

20 KF, vezérlés» Állapot átmenet x "L9 = A " x " + B " u "=9 + q ", q~n(0, Q " )» Jelölések» Diszkrét idejű állapot átmeneti mátrix: A " = e A(? )S?» Diszkrét idejű vezérlési mátrix: B "» Az x "L9 eloszlásának meghatározása» x " ~N xƒ, P "» p x ", x "L9 közös eloszlás, majd marginalizáció p x ", x "L9» Predikciós lépés: = N( xƒ " A " xƒ " + B " u "=9, x "L9 ~N(A " xƒ " + B " u "=9, A " P " A "ˆ + Q " )» xƒ "L9 A " xƒ " + B " u "=9» P "L9 A " P " A "ˆ + Q " P " P " A?ˆ A " P " A " P " A "ˆ + Q " ) 20

21 KF, megfigyelés» Megfigyelés z " = H " x " + r ", r " ~N(0, R " )» Jelölések» Megfigyelési mátrix: H "» Az x " eloszlásának meghatározása» p x ", z " közös eloszlás, majd feltétel bevezetés» p x ", z " = N( xƒ " H " xƒ ", P " P " H?ˆ H " P " H " P " H "ˆ + R " )» x " ~N(xƒ " + K " z 8 H " xƒ ", P " K " H " P "» K " = P " H "ˆ H " P " H "ˆ + R " =9» Frissítési lépés» xƒ " xƒ " + K (z H xƒ " )» P " P " K " H " P " residual Kalman nyereség 21

22 EKF» Nemlineáris modell, normális eloszlás» Állapotátmenet x "L9 = f x ", u " + q ", q~n(0, Q " )» Megfigyelés z " = h x " + r ", r " ~N(0, R " )» Linearizálás a munkapont körül» A " = x xƒ,u» H " = š x xƒ 22

23 Inerciális mérések integrálása» Vezérlő jelek» IMU mérésekből» Gyorsulásmérő» Giroszkóp» Mintha zajos vezérlés lenne» Mérés» GPS adat» Képi adatfeldolgozás» Távolság alapú helymeghatározás» WiFi lateráció» UWB lateráció 23

24 Belső állapotok (x " )» Azon paraméterek, melyek lehetővé teszik a rendszer pontos állapotának nyomonkövetését» Inerciális mérésekhez tartozó» Kinematika» q " : az eszköz orientációja (kvaternió)» p " : az eszköz becsült helyzete (helyvektor)» v " : az eszköz aktuálisan becsült sebessége» IMU modell» a m : gyorulásmérő nullponti eltolása» ω m : a giroszkóp nullponti eltolása»... Egyéb, a modell szerinti (torzítás, zaj)» Megfigyelésekhez tartozó» Milyen technológiát használunk a megfigyelések során 24

25 Propagáció IMU mérések alapján» A propagálás során a belső állapotok megváltoznak Δt idő alatt» Kinematikai egyenletek» q = 9 : q (ω g ω m )» p = v» v = R ]^ a g a m + g» A nominális állapotok megváltozása» q q q ω g ω m Δt» p p + vδt + 9 : R a g a m + g Δt :» v v + R a g a m + g Δt» a m a m» ω m ω m 25

26 IMU és Kamera» Valós képi elemek nyomonkövetése alapján» A projekció paramétere a kamera» Valós helye és» Helyzete» Megvalósítások» EKF» Az állapotvektor» A nyomon követett jellemző térbeli pontok» MSCKF» Az állapotvektor» A kamera megelőző m darab helye és helyzete 26