Mozgó jármű helyzetének és tájolásának meghatározása alacsony árú GNSS és inerciális érzékelők szoros csatolású integrációjával Farkas Márton Rédey István Geodéziai Szeminárium 2019. április 2.
Áttekintés Kutatási terület bemutatása Kiterjesztett Kálmán-szűrőn alapuló becslő algoritmus Ciklustöbbértelműség feloldása Mérési eredmények Összegzés
Koordináta-rendszerek Z Északi-sark Y Egyenlítő Greenwichi meridián X ECEF
Koordináta-rendszerek Z Északi-sark Y Egyenlítő Greenwichi meridián X ECEF
Koordináta-rendszerek Z Északi-sark N D E Y Egyenlítő Greenwichi meridián X ECEF Navigációs
Koordináta-rendszerek Z Északi-sark N z D x y E Y Egyenlítő Greenwichi meridián X ECEF Navigációs Test
Kutatási terület bemutatása S Bázisvonal 2 B y INS P Bázisvonal 1 Bólintási-szög Legyezési-szög x z Orsózási-szög
Kutatási terület bemutatása Szenzorfúziós eljárások t Különböző mérési frekvenciák INS: 50-2000 Hz GNSS: 1-50 Hz Különböző mérési hibaforrások
Kutatási terület bemutatása Laza csatolású GNSS/INS szenzorfúzió INS m a m ω INS szűrő r A r Φ Navigációs P, V, Φ Kálmánszűrő GNSS m ρ m φ m d GNSS szűrő r P r V
Kutatási terület bemutatása Szoros csatolású GNSS/INS szenzorfúzió INS m a m ω Navigációs P, V, Φ Kálmánszűrő GNSS m ρ m φ m d
Kiterjesztett Kálmán-szűrőn alapuló becslő algoritmus Állapotvektor és kovarianciamátrix (x, P) Az elsődleges GNSS antenna pozíciója ( X p ), sebessége ( V p ) és gyorsulása ( A p ) ECEF koordináta-rendszerben Orientációs kavterniók ( q) és deriváltjaik ( q) Gyorsulásmérő bias hibája ( b a ), szögsebességmérő bias hibája ( b ω ) GNSS vevők órahibái (δ GPS, δ GLO, δ GAL ) GNSS vevők óradiftjei ( δ GPS, δ GLO, δ GAL ) Egyszeres különbségben vett GLONASS inter-channel bias (ICB) Egyszeres különbségben vett ciklustöbbértelműségek (N RB )
Kiterjesztett Kálmán-szűrőn alapuló becslő algoritmus Predikció Frissítés ˆx t = F t t 1x t 1 ˆP t = Ft 1P t t 1 Ft 1 t T + Qt K t = ˆP t Ht T (H t ˆP t Ht T + R t ) 1 x t = ˆx t + K t (z t h(ˆx t )) P t = (I K t H t )ˆP t
RTK alapú hely- és helyzetmeghatározás Vevő-Műhold távolság lb = X b X S lp = X p X S ls = ( X p + R ecef body b s p body ) X S Vevő-Műhold sebesség különbségek ν b = 0 V S ν p = V p V S Műholdirányú egységvektor ν s = V p + R ecef body skew(2w T q q) b s p body V S E = l T l
RTK alapú hely- és helyzetmeghatározás Kódmérés Doppler-mérés ρ = E l + cδ r cδ S cδ rel cδ GD + I + T Fázismérés d = 1 λ ( E ν + c δ r c δ S ) λφ = ( E l + cδ r cδ S cδ rel cδ G D I + T + ch no ICB) + λn
RTK alapú hely- és helyzetmeghatározás Egyszeres különbség képzése a bázisvonal két vevője között ρ RB = ρ R ρ B d RB = d R d B φ RB = φ R φ B Kétszeres különbség képzése egyszeres különbség értékek és a pivot műhold között ρ 1...n,i RB d 1...n,i RB φ 1...n,i RB = ρ 1...n RB = d 1...n RB = φ 1...n RB ρi RB di RB φi RB
Inerciális szenzorok Gyorsulásmérő Test koordináta-rendszerben mért gyorsulás adatok a = Rbody ecef T Ap skew(2wq T q)skew(2w q T q) bp INS body + b a R nav body T 0 0 + Rbody ecef T (2skew g 0 0 Ω E V p )
Inerciális szenzorok Szögsebességmérő [ ] ω = 2 q q 0 + + bω R ecef T body 0 0 Ω E 0 + R nav T body ωnav ecef nav
Ciklustöbbértelműség feloldása A centiméteres pontosság elérésének feltétele Kerekítés, sigma-módszer, bootstrapping Legelterjedtebb a LAMBDA eljárás Dekorrelációs eljárás a hatékony keresés érdekében x N = arg min x N Z m x N ˆx N 2ˆP NN
Ciklustöbbértelműség feloldása orientációbecslésnél Az optimalizációba bevisszük az ismert kvaternió normát Nemkonvex optimalizációs probléma x N = arg min x N Z m(c(x N)) C(x N ) = x N ˆx N 2ˆP NN + ˆx q (x N ) ˇx q (x N ) 2ˆP q(n)q(n) Feltételes kvaterniók és kovarianciamátrix ˆx q (x N ) = ˆx q ˆP qn ˆP 1 NN (x N ˆx N ) Második optimalizáció ˆP q(n)q(n) = ˆP qq ˆP qn ˆP 1 NN ˆP Nq ˇx q (x N ) = arg min x q 2 =1 ˆx q (x N ) x q 2ˆP q(n)q(n)
Ciklustöbbértelműség feloldása orientációbecslésnél
Ciklustöbbértelműség feloldása orientációbecslésnél q 1 r = 1 q 0 ˆx N : x N : ˆx q (x N ): ˇx q (x N ):
Ciklustöbbértelműség feloldása orientációbecslésnél q 1 r = 1 q 0 ˆx N : x N : ˆx q (x N ): ˇx q (x N ):
Ciklustöbbértelműség feloldása orientációbecslésnél 3D-s példa 2 Kényszerezett LAMBDA LAMBDA N3 0 2 4 6 4 8 2 0 12 10 N 1 N 2 6
Ciklustöbbértelműség feloldása orientációbecslésnél Eredeti optimalizációs probléma megoldása számításigényes A keresési tér alsó és felső korlátfüggvénye ahol C 1 (x N ) = x N ˆx N 2ˆP NN + ξ min ( ˆx q (x N ) 1) 2 C 2 (x N ) = x N ˆx N 2ˆP NN + ξ max ( ˆx q (x N ) 1) 2, ξ min = min(eig(ˆp 1 q(n)q(n) )) ξ max = max(eig(ˆp 1 q(n)q(n) ))
Ciklustöbbértelműség feloldása orientációbecslésnél LAMBDA n legjobb megoldása C 2 (x N ) C 1 (x N ) min min(c 2 (x N )) > C 1 (x N ) # > 1 C(x N ) #1 min x N
UAV teszt repülés Nyers adatok GNSS: Ublox NEO-M8T vevők INS: PIXHAWK INS Összehasonlítás Pozíció: RTKLIB Orientáció: PIXHAWK megoldás és PIX4D megoldás
UAV teszt repülés
UAV teszt repülés Észak (m) Kelet (m) Fel (m) 10,000 5,000 0 4,000 2,000 0 200 100 0 0 1,000 2,000 3,000 Idő (s) Észak (m) Kelet (m) Fel (m) 0.04 0.02 0 0.02 0.02 0.01 0 0.01 0.1 0.05 0 0 1,000 2,000 3,000 Idő (s) EKF alg. ( ), RTKLIB ( ) és különbségük ( )
UAV teszt repülés Orsózási ( ) 40 20 20 0 40 Orsózási ( ) 2 1 01 2 Bólintási ( ) 20 10 0 Bólintási ( ) 1 01 2 Legyezési ( ) 100 100 0 0 1,000 2,000 3,000 Legyezési ( ) 0 5 0 1,000 2,000 3,000 Idő (s) Idő (s) PIXHAWK ( ), EKF alg. ( ), Pix4D ( ) és a Pix4D megoldástól vett különbségek PIXHAWK ( ), EKF alg.
UAV teszt repülés Észak Kelet Fel Fixed pozíciók mean 0.007 0.002-0.053 különbségei [m] rms 0.010 0.003 0.055 AR aránya Bázisvonal 1 99.12% Orsózási Bólintási Legyezési PIXHAWK - PIX4D mean 0.02-1.63-3.06 Euler-szögek rms 0.10 1.64 3.52 EKF algo. - PIX4D mean -0.21-0.26 0.27 Euler-szögek rms 0.53 0.49 0.62 AR aránya Bázisvonal 2 97.35%
Algoritmus fejlődése Bázistávolság becslése Euler-szögek becslése Inerciális szenzorok Kvaterniók becslése
Összegzés Szoros csatolású GNSS/INS szenzorfúziós eljárás Kvaternió kényszerezett ciklustöbbértelműség-feloldás a mozgó bázison Jövőbeli tervek Több bázisvonal a mozgó bázison Két frekvenciás vevő alkalmazása Több INS szenzor alkalmazása PPP pozicionálás Alapszintű kamera alapú tájolás integrálása Tesztek