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

Átírás

1 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 április 2.

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

3 Koordináta-rendszerek Z Északi-sark Y Egyenlítő Greenwichi meridián X ECEF

4 Koordináta-rendszerek Z Északi-sark Y Egyenlítő Greenwichi meridián X ECEF

5 Koordináta-rendszerek Z Északi-sark N D E Y Egyenlítő Greenwichi meridián X ECEF Navigációs

6 Koordináta-rendszerek Z Északi-sark N z D x y E Y Egyenlítő Greenwichi meridián X ECEF Navigációs Test

7 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

8 Kutatási terület bemutatása Szenzorfúziós eljárások t Különböző mérési frekvenciák INS: Hz GNSS: 1-50 Hz Különböző mérési hibaforrások

9 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

10 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

11 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 )

12 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

13 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

14 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

15 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

16 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 Rbody ecef T (2skew g 0 0 Ω E V p )

17 Inerciális szenzorok Szögsebességmérő [ ] ω = 2 q q bω R ecef T body 0 0 Ω E 0 + R nav T body ωnav ecef nav

18 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

19 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)

20 Ciklustöbbértelműség feloldása orientációbecslésnél

21 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 ):

22 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 ):

23 Ciklustöbbértelműség feloldása orientációbecslésnél 3D-s példa 2 Kényszerezett LAMBDA LAMBDA N N 1 N 2 6

24 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) ))

25 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

26 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

27 UAV teszt repülés

28 UAV teszt repülés Észak (m) Kelet (m) Fel (m) 10,000 5, ,000 2, ,000 2,000 3,000 Idő (s) Észak (m) Kelet (m) Fel (m) ,000 2,000 3,000 Idő (s) EKF alg. ( ), RTKLIB ( ) és különbségük ( )

29 UAV teszt repülés Orsózási ( ) Orsózási ( ) Bólintási ( ) Bólintási ( ) Legyezési ( ) ,000 2,000 3,000 Legyezési ( ) ,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.

30 UAV teszt repülés Észak Kelet Fel Fixed pozíciók mean különbségei [m] rms AR aránya Bázisvonal % Orsózási Bólintási Legyezési PIXHAWK - PIX4D mean Euler-szögek rms EKF algo. - PIX4D mean Euler-szögek rms AR aránya Bázisvonal %

31 Algoritmus fejlődése Bázistávolság becslése Euler-szögek becslése Inerciális szenzorok Kvaterniók becslése

32 Ö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