mobilisrobotoknavigacioja.pdf

139

MOBILIS ROBOTOK NAVIGÁCIÓJA

11. KITERJESZTETT KALMAN-SZŰRŐ Ebben a fejezetben bemutatjuk, hogyan lehet állapotot becsülni Kalman-szűrő al-kalmazásával diszkrétidejű rendszer és sztochasztikus jelek esetén. Először bemutat-juk időben változó lineáris rendszer esetén az ún. aktuális Kalman-szűrő algorit-must, majd a módszert általánosítjuk diszkrétidejű időben változó nemlineáris rendszer esetére a kiterjesztett Kaman-szűrő (EKF, extended Kalman filter) alak-jában. Ez a megoldás lesz az alapja a GPS és IMU szenzorokon, valamint beltéri esetben a GPS-t kiváltó marker-bázisú képfeldolgozáson alapuló állapotbecslésnek.

11.1 Aktuális Kalman-szűrő időben változó diszkrétidejű lineáris rendszer esetén Az "aktuális" Kalman-szűrő az kx állapot becslésénél már az "aktuális" ky kimene-tet veszi figyelembe (és az előző 1−ku bemenetet). Ezáltal az kkk xKu ˆ−= állapot-visszacsatolásban számíthatóvá válik (mert csak az előző 1−ku kell hozzá), de már figyelembe lesz véve az aktuális ky mérési eredmény, ami nyilván jobb irányítási tulajdonságok elérését teszi lehetővé.

Tekintsük a zajokkal terhelt diszkrétidejű lineáris rendszert: )0(,1 xvuBxAx kkkkkk ++=+ (11.1a) kkkk zxCy += (11.1b) Itt ku determinisztikus jel. A kv és kz zajokra és az )0(x kezdeti állapotra a kö-vetkező sztochasztikus hipotézis legyen érvényes ( klδ a Kronecker-delta):

)0(x független kv -tól és kz -tól, (11.2a)

0)0( xEx = , 0]))0()()0([( 000 ≥=−− ΣTxxxxE (pozitív szemidefinit) (11.2b)

0=kEv , 0,][ ,, ≥= kvklkvTlk RRvvE δ (pozitív szemidefinit) (11.2c)

0=kEz , 0,][ ,, >= kzklkzTlk RRzzE δ (pozitív definit) (11.2d)

0][ =Tlk zvE , 0][ =T

kl vzE ( kv és lz korrelálatlanok) (11.2e)

140 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA

Keressük azt a lineáris szűrőt, amely az ku és 1+ky mérési eredményekből az )0(ˆ,ˆˆ 111 xuHyGxFx kkkkkkk ++= +++ (11.3) algoritmus alapján az kx állapot optimális kx becslését adja abban az értelemben, hogy

0]ˆ[ =− kk xxE minden k esetén, (11.4a)

infimum])ˆ)(ˆ[( →=−− kT

kkkk xxxxE Σ . (11.4b)

Az infimum az snnM × szimmetrikus mátrixok (kvadratikus alak révén definiált)

részben rendezése szerint értendő. Az optimális megoldás a kovarianciamátrix ösz-szes sajátértékét egyszerre teszi minimálissá. A feltételek alapján teljesülnie kell, hogy

0000000 ])ˆ)(ˆ[(:ˆ)0(ˆ Σ=−−⇒== TxxxxExxxE . (11.5)

Képezzük a becsült állapotot és a futó hibát 1+k esetén:

kkkkkk

kkkkkkkkkkkkkkkk

kkkkkkkkkkkk

kkkkkkkkk

vuBxAxzGvCGuHBCGxACGxF

uHzvuBxACGxFuHzxCGxFx

++=+++++=

=+++++==+++=

+

++++++++

+++

+++++

1

11111111

111

11111

)(ˆ)(ˆ

)(ˆˆ

.)(ˆ

])[()(ˆ

1111

111111

++++

++++++

−−+−−−−+−=−

kkkkkkk

kkkkkkkkkkk

zGvCGIxFuHBCGIxACGIxx

(11.6)

A hiba várható értékére vonatkozó (11.4a) feltétel szerint

kkkkkkkk BCGIHACGIF )(:,)(: 1111 ++++ −=−= , (11.7) ezért

1111

111111

111

ˆ)()()ˆ()(

ˆ:~

++++

++++++

+++

−−−++−==−−+−−=

=−=

kkkkkkkkkkkkk

kkkkkkkkkk

kkk

zGuBxAvuBxACGIzGvCGIxxACGI

xxx (11.8)

Vezessük be az

11. KITERJESZTETT KALMAN-SZŰRŐ 141

]))([(:

ˆ:

11111

1T

kkkkk

kkkkk

xxxxEM

uBxAx

+++++

+

−−=

+= (11.9)

jelölést, akkor

Tkkzk

Tkkkkkk GRGCGIMCGI 11,1111111 )()( +++++++++ +−−=Σ . (11.10)

Mivel 1+kΣ -et infimummá kell tenni, ezért 1+kΣ -nek a 1+kG szerinti deriváltjá-

nak a )(⋅O nulla transzformációt kell adnia. Képezzük ezért a 1+kG szerinti diffe-renciált:

).()(

)()()(

111,111,1

1111111111

+++++++

++++++++++

=++

+−−−−

kT

kkzkTkkzk

Tk

Tkkkk

Tkkkkk

dGdGRGGRdG

dGCMCGICGIMCdG

O (11.11)

Vezessük be az )(⋅U és )(⋅T transzformációkat:

.:)(

,:)(T

T

XXT

XXXU

=

+= (11.12)

A differenciált átírhatjuk az

)()(])([ 111,11111 ++++++++ =+−− kkkzkTkkkk dGdGTRGCMCGIU O (11.13)

alakba, ahonnan következik az infimum alábbi feltétele operátor alakban:

)()(])([ 1,11111 ⋅=⋅+−− ++++++ OTRGCMCGIU kzkTkkkk . (11.14)

Tetszőleges X esetén teljesül, hogy

0)()()( =⇔⋅=⋅ YXTUXTU O minden Y esetén 0=⇔ X . (11.15)

Az infimum feltétele ezért

),(

)(0

1,111111

1,1111111

1,11111

+++++++

++++++++

++++++

++−=

=++−=

=+−−=

kzTkkkk

Tkk

kzkTkkkk

Tkk

kzkTkkkk

RCMCGCM

RGCMCGCM

RGCMCGI

(11.16)

ahonnan következik, hogy az optimális megoldás


11,111111 )( −+++++++ += kz

Tkkk

Tkkk RCMCCMG . (11.17)

Helyettesítsük vissza az optimális 1+kG megoldást a (11.83) egyenletbe:

1111111

1

0

1,111111111

11,1111111111

)(

)()(

)()(

+++++++

+++++++++++

++++++++++++

−=−=

=++−+−=

=+−−−=

kkkkkkk

Tkkz

Tkkkk

Tkkkkk

Tkkzk

Tk

Tkkkkkkkk

MCGMMCGI

GRCMCGCMMCGI

GRGGCMCGIMCGI

44444444 344444444 21

Σ

A becslési hiba optimális kovariancia mátrixa ezért

111

1,1111111 )( ++−

++++++++ +−= kkkzTkkk

Tkkkk MCRCMCCMMΣ . (11.18)

Bontsuk fel a becsült állapot képzését a mérési időpontok között elvégezhető 1+kx frissítésre és az új mérés 1+ky pillanatában elvégezhető 1ˆ +kx állapotbecslésre,

akkor

,

,)ˆ(,ˆ

,1

11

1

kvTkkkk

kkkkkk

kkkkk

RAAM

vxxAxxuBxAx

+=

+−=−+=

+

++

+

Σ

(11.19)

ahol 1+kM szintén meghatározható a mérési időpontok között. Másrészt a becsült állapot felírható

)()ˆ(ˆ

)(ˆ)(ˆˆ

11111111

1111

1111

kkkkk

kkkkkkkkkkk

kkkkkkkkkk

kkkkkkk

xCyGxuBxACyGuBxA

uBCGIyGxACGIuHyGxFx

−+==+−++=

=−++−==++=

−−−−−−−−

−−−−

−−−−

(11.20)

alakban is. Végül felhasználván a (11.17) egyenletet kG tovább egyszerűsíthatő:

⇒+= −1, )( kz

Tkkk

Tkkk RCMCCMG

kzkTkkkkkz

Tkkkk

Tkk RGCMCGRCMCGCM ,, )( +=+=

⇒=−= Tkk

Tkkkkkzk CCMCGIRG Σ)(,

.1,−= kz

Tkkk RCG Σ (11.21)

11. KITERJESZTETT KALMAN-SZŰRŐ 143 Az "aktuális" Kalman-szűrő algoritmusa:

1. Frissítés mérési időpontok között (time update between measurements):

.)(

,)(

,

,ˆ

1,

1,

1,

1,111

1111

−−

−

−−−−

−−−−

=+=

+−=

+=

+=

kzTkkkz

Tkkk

Tkkk

kkkzTkkk

Tkkkk

kvTkkkk

kkkkk

RCRCMCCMG

MCRCMCCMM

RAAM

uBxAx

Σ

Σ

Σ (11.22)

2. Mérési eredmény frissítése (measurement update): )(ˆ kkkkkk xCyGxx −+= (11.23) Megjegyzés:

1. Különleges eset, ha nincs kimeneti zaj, mert ekkor 0, =kzR és kG számítására (11.21) nem használható. Ekkor (11.17) használandó.

2. Vegyük észre, hogy a (11.22)-(11.23) egyenletek előretartó rekurzív egyenletek, ezért a Kalman szűrési algoritmus online realizálható. A Kalman szűrési algorit-mus időben változó rendszer esetén a következő sémával ábrázolható:

Inicializálás: ]))0()()0([(:),0(:ˆ 00000TxxxxEExxx −−=== Σ

Előretartó rekurzió :,2,1 K=k

ky

kkkkk xxGMx k ˆ,,,ˆ )23.11(,)22.11(1 ⎯⎯⎯ →⎯⎯⎯ →⎯− Σ

Megjegyzés: Ha az állapotegyenletben a rendszerzaj kkv vB , , a mérési zaj pedig

kkz zC , alakban szerepel, akkor elegendő elvégezni az Tkvkvkvkv BRBR ,,., := és

Tkzkzkzkz CRCR ,,,, := helyettesítéseket, mert például

T

kvkvkvT

kvTkkkv

Tkkvkkv BRBBvvEBvBvBE ,,,,,,, ][]))([( ==

A Kalmán-szűrő algoritmusa könnyen általánosítható korrelált rendszerzaj és

mérési zaj esetére is, ebben a tekintetben az irodalomra utalunk (lásd pl. Lantos: Irá-nyítási rendszerek elmélete és tervezése II. Akadémiai Kiadó, 2003).


11.2 Kiterjesztett Kalman-szűrő (EKF) Tekintsük a következő nemlineáris rendszert diszkrét időben:

),,(1 kkkk vuxfx =+ , (11.24a) ),( kkk zxgy = . (11.24b)

Teljesüljenek a lineáris eset sztochasztikus hipotézisei, ezen belül legyen az egysze-rűség kedvéért a rendszerzaj és a mérési zaj korrelálatlan.

Tegyük fel, hogy ismerjük a rendszer állapotának 1ˆ −kx becslését a ],)1[( kTTk − időintervallum bal oldali végpontjában, és keressük az állapot kx becslését a jobb oldali végpontban. Ha a rendszer sima, akkor próbálkozhatunk a nemlineáris modell linearizálásával az ismert 1ˆ −kx és 01 =−kv esetén az állapot, valamint az 1ˆ −kx és

1−ku értékéből kiszámítható kx és 0=kz esetén a kimenet számára:

111

111

11)0,,ˆ(

)ˆ()0,,ˆ(

)0,,ˆ( −−−

−−−

−− ∂∂

+−∂

∂+≈ k

kkkk

kkkkk v

vuxf

xxxuxf

uxfx (11.25a)

kk

kkk

kk zz

xgxx

xxg

xgy∂

∂+−

∂∂

+≈)0,(

)()0,(

)0,( . (11.25b)

Vegyük észre, hogy

111

11 ˆ)0,,ˆ()0,,ˆ( −

−−−− ∂

∂− k

kkkk x

xuxf

uxf (11.26a)

és

kk

k xx

xgxg

∂∂

−)0,(

)0,( (11.26b)

ismert jeleknek tekinthetők.

Vezessük be a következő jelöléseket:

xuxf

A kkk ∂

∂= −−

−)0,,ˆ(

: 111 (11.27a)

vuxf

B kkkv ∂

∂= −−

−)0,,ˆ(

: 111, (11.27b)

xxg

C kk ∂

∂=

)0,(: (11.27c)

zxg

C kkz ∂

∂=

)0,(:, (11.27d)

12. A NAVIGÁCIÓNÁL HASZNÁLT KOORDINÁTA-RENDSZEREK 145 T

kvkvkvkv BRBR 1,1,1,1, : −−−− = (11.27e) T

kzkzkzkz CRCR ,,,, := (11.27f)

Foglaljuk össze az eddigieket. Tekintsük a nemlineáris rendszert a ],)1[( kTTk − intervallumban, amelynek balodali végpontjában már ismert az állapot 1ˆ −kx becslé-se. Linearizáljuk a nemlineáris rendszert az 1ˆ −kx , 01 =−kv esetén az állapot, illetve az kx , 0=kz esetén a kimenet számára. A linearizált rendszerre alkalmazzuk a li-neáris rendszerekre kidolgozott aktuális Kalman szűrő algoritmust.

Az így kapott állapotbecslő az ú.n. kiterjesztett Kalman-szűrő (EKF, extended Kalman filter):

)0,,ˆ( 11 −−= kkk uxfx A nemlineáris rendszer linearizálása (11.27a-f) alapján.

.)(

,)(

,

1,

1,

1,

1,111

−−

−

−−−−

=+=

+−=

+=

kzTkkkz

Tkkk

Tkkk

kkkzTkkk

Tkkkk

kvTkkkk

RCRCMCCMG

MCRCMCCMM

RAAM

Σ

Σ

Σ

(11.28)

))0,((ˆ kkkkk xgyGxx −+= (11.29)

Megjegyzés: A kiterjesztett Kalman-szűrő algoritmusában az )(⋅f és )(⋅g nemlineá-ris függvényekhez úgy jutottunk, hogy a Taylor sorok helyett a nemlineáris függvé-nyek értékét írtuk vissza, és figyelembe vettük 1−kA és kC definícióját:

)0,,ˆ(ˆ)0,,ˆ()0,,ˆ(ˆ 111

111111 −−−

−−−−−− →

∂∂

−+= kkkkk

kkkkk uxfxxuxf

uxfxAx

))0,(()))0,(

)0,((ˆ kkkkkkkk

kkkkk xgyGxxCxx

xgxgyGxx −+→−

∂∂

+−+=


12. A NAVIGÁCIÓNÁL HASZNÁLT KOORDI-NÁTA-RENDSZEREK

Ebben a fejezetben a navigációnál használt fontosabb koordinátarendszerekkel (ECI, ECEF, NED, ABC=BODY) ismerkedünk meg. Ezek a koordináta-rendszerek a 3D navigáció alapját képezik. A koordináta-rendszereket a 12.1. ábrán foglaltuk össze.

12.1. ábra. A navigációnál használt koordináta-rendszerek

12.1 Az ECI inercia rendszer A ECIK (Earth Centered Inertia) koordináta-rendszer a bolygóközi mozgások terve-zésekor fontos. Úgy keletkezik, hogy egy megválasztott fix időpontban tekintjük a nap körül keringő és a tengelye körül forgó Föld aktuális középpontját, amely az ECI origója lesz. A továbbiakban egy távoli fix csillaghoz képest képezzük az orien-tációt. Az ECI rendszer azért fontos, mert a klasszikus mechanika alaptörvényei, így a Newton–Euler-egyenletek is inerciarendszerben érvényesek.

12. A NAVIGÁCIÓNÁL HASZNÁLT KOORDINÁTA-RENDSZEREK 147

12.2 Az ECEF koordináta-rendszer A ECEFK (Earth Centered Earth Fixed) koordináta-rendszer origója a Föld közép-pontja, az ECEF koordináta-rendszer együtt mozog a Földdel (kering a Nap körül és forog a tengelye körül), az ECEF z -tengelye a Föld forgástengelye, amely merőle-ges az egyenlítő síkjára. Az ECEF x -tengelye a Greenwichi Obszervatóriumon ke-resztülhaladó főmeridián (nulladik hosszúsági kör) síkjának és az egyenlítő síkjának metszésvonala, az y -tengely jobb sodrású koordináta-rendszerre egészít ki.

Az ECEF szögsebessége az ECI inerciarendszerhez képest

sradhs

cycleradh

cyclesie /102921151467.7

/3600/2

2425.365)25.3651( 5−⋅=⋅⋅

+=

πω . (12.1)

A Föld forgásellipszoiddal approximálható a GPS alapját képező WGS-84 szab-vány szerint. Az ellipszis féltengelyei:

ma 0.6388137= , mb 3142.6356752= , )( ba > (12.2)

A forgásellipszoid egy ellipszisnek a z -tengely körüli forgatásával keletkezik. Az ellipszis f simasága és e excentricitása a következésképp van definiálva:

0818.0)2(,0034.0 =−==−

= ffea

baf (12.3)

2

22

2

2

2

22

22 11))((2

abe

ab

aba

ababa

aba

abae =−⇔−=

−=

+−=⎟

⎠⎞

⎜⎝⎛ −

−⋅−

= (12.4)

Az ECEF koordináta-rendszerben egy P pont az origóból a pontba mutató

Tzyxr ),,(= vektorral jellemezhető. A pont megadása ECEF-ben azonban a geode-

tikus Th),,( φλ koordinátákkal történik, ahol h,,φλ angol neve rendre latitude,

longitude és altitude, ezekből kell az Tzyx ),,( vektor komponenseit meghatározni. Ez a következőképpen történhet.

A P ponton és a z -tengelyen átfektetünk egy síkot, és meghatározzuk eme síknak a metszetét a forgásellipszoiddal. Ez a forgatott ellipszis egy példánya, a keletkező ellipszis tengelyeit jelölje z és y . A sík kimetsz az egyenlítő síkjából egy 0y hosszúságú szakaszt, amelynek a vetülete az ellipszoid yx, tengelyeire az egyenlítő síkjában az x -tengellyel bezárt φ szög ismeretében számítható, nevezete-sen φCyx 0= és φSyy 0= . A P pont távolsága az ellipszis y tengelyétől köz-vetlenül a P pont z koordinátája. A h magasság a P pont legrövidebb távolsága az ellipszistől, amely az ellipszisen egy ),( 00 yzQ = pontot definiál. A Q pontban

148 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA tekinthetjük az ellipszis érintőjét és az arra merőleges egyenest, amely utóbbi a z -tengelyen egy ),0( cR −= pontot metsz ki. A QR egyenes az érintő normálisa, hossza N . Az érintőnek Q -tól az y tengellyel való metszéspontig terjedő hossza N . Az érintő normálisa és az y tengely által bezárt szüg λ . A viszonyokat a 12.2. ábra mutatja be.

12.2. ábra. A térbeli koordináták számításának elve ECEF keret esetén

Az ábrának megfelelően a következő összefüggések érvényesek:

Ellipszis egyenlete: 2

2

2

2

2

211

aybz

ay

bz

−=⇒=+

Derivált: ⎟⎠⎞

⎜⎝⎛−

−

=′ 2

2

2

2

1

1a

y

ay

bz

Érintő meredeksége:

2

20

02

1ay

yabm

−

−=

Normális egyenlete: )(100 yy

mzz −−=


Normális egyenesének metszete a z -tengellyel (ahol 0=y ):

02

2

2

20

2

000

2

20

2

0 1)(1

zba

ay

baczy

yay

bazc =−=+⇒−

−+=−

Hasonló háromszögekből:

NabN

ba

zcz

NN

2

2

2

2

0

0 =⇒=+

=

Geometriából: λλ ShNabShNz ⎟⎟

⎠

⎞⎜⎜⎝

⎛+=+=

2

2)(

( ) λSheNz +−= )1( 2 (12.5)

Geometriából: λChNy )(0 +=

φλ

φλ

SChNy

CChNx

)(

)(

+=

+= (12.6)

Meg kívánjuk még határozni az )(λN függvényt, amelyhez néhány azonosságot és az eddigi eredményeket fogjuk felhasználni.

Azonosságok: 2202

2202

20

2

20 1 az

bay

ay

bz

=+⇒=+

222222222

22

2

022

04

4

2

2200

2202

220

2204

4

2

220

204

42002

2

0

)1()1(

1,)(,

)()(

)(

aSeNSNeCN

eabNCyacz

ab

bayNScz

aczabyacz

ab

bay

czabzz

bacz

=−=−+

−===++=+

=++⇒=++

+=⇒=+

λλλ

λλ

221

)(λ

λSe

aN−

= (12.7)

150 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA A (12.5-7) eredményeket a következő algoritmusban foglaljuk össze.

Konverzió geodetikus koordinátákról derékszögű koordinátákra:

( ) λ

φλ

φλ

λ

λφλ

SheNzSChNyCChNxSe

aNh T

+−=+=+=

−=→

)1()()(

1)(),,(

2

22

(12.8)

A GPS információból a később megismert módon derékszögű koordinátákat le-het kiszámítani, a konverzió geodetikus koordinátákra egy rekurzív algoritmussal végezhető el a fenti összefüggések felhasználásával.

Konverzió derékszögű koordinátákról geodetikus koordinátákra: TT hzyx ),,(),,( φλ→

Inicializálás:

φφλ ⎯⎯ →⎯=+=↔+=== atan222 /,)(::,:,0: xyTChNpyxpaNh (12.9a) Ciklus:

N

CphSeaN

CS

ChNShN

pNSez

TheN

zS

−=−=

⎯⎯→⎯=++

=+

=+−

=

λλ

λ

λ

λ

λλλλ

λ

λ

,1/:)(

)()(

:,)1(

:

22

atan2

2 (12.9b)

A tapasztalat azt mutatja, hogy gyorsabb a konvergencia, ha λ értékét nem asin, hanem atan függvénnyel számítjuk ),(atan: 2 pNSez λλ += alapján. Az ECEF koordináta-rendszer alapvető fontosságú a föld körül keringő mesterséges holdak (szatellitek) pályájának és a hordozó rakéta irányításának tervezésekor is. A rakéta hajtómű által kifejtett erő/nyomaték mellett a tömegvonzás rörvénye szerint

32 r

rGMmrr

r

MmGFg −=−= (12.10)

erő is hat a rakétára, ahol

2314 /1098599927.3 smGM ⋅= , (12.11)

a nehézségi gyorsulás vektora pedig


rr

GMg 3−= (12.12)

függ a rakéta tömegközéppontjába mutató r vektortól. A konverziót geodetikus koordináták és ECEF, valamint ECEF és geodetikus koor-dináták között Los Angeles példáján az alábbi MATLAB script program mutatja be. %frame_LosAngeles %ECEF position of Los Angeles clear all close all clc %Geodetic data of Los Angeles lambda_deg=34+0/60+0.00174/3600; phi_deg=-(117+20/60+0.84965/3600); h=251.702; lambda=lambda_deg*pi/180; phi=phi_deg*pi/180; a=6378137; b=6356752.3; f=(a-b)/a; e=sqrt(f*(2-f)); %Direct problem format long fprintf('Direct problem: lambda_deg,phi_deg,h -> x,y,z ECEF\n'); lambda_deg phi_deg h N=a/sqrt(1-e^2*sin(lambda)^2) x=(N+h)*cos(lambda)*cos(phi) y=(N+h)*cos(lambda)*sin(phi) z=(N*(1-e^2)+h)*sin(lambda) format short %Inverz problem format long %Init h=0; N=a; p=sqrt(x^2+y^2); phi=atan2(y,x); phi_deg=phi*180/pi; %Iter niter=9; data_iter=NaN*ones(niter,3); for i=1:niter S_lambda=z/(N*(1-e^2)+h); lambda=atan2(z+e^2*N*S_lambda,p);

152 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA lambda_deg=lambda*180/pi; N=a/sqrt(1-e^2*S_lambda^2); h=p/cos(lambda)-N; data_iter(i,:)=[lambda_deg N h]; end; %Geodetic data of Los Angeles fprintf('Iteracios lepesek eredmenyei: lambda_deg,N,h sorrend\n'); data_iter fprintf('Inverz problem: x,y,z ECEF -> lambda_deg,phi_deg,h\n') x y z lambda_deg=data_iter(niter,1); N=data_iter(niter,2); h=data_iter(niter,3); N lambda_deg phi_deg h format short A futási eredmények a direkt feladat esetén:

Direct problem: lambda_deg,phi_deg,h -> x,y,z ECEF lambda_deg = 34.00000048333333 phi_deg = -1.173335693472222e+002 h = 2.517020000000000e+002 N = 6.384823214436118e+006 x = -2.430601829367448e+006 y = -4.702442706380628e+006 z = 3.546587344780241e+006 Futási eredmények az inverz feladat esetén:

Iteracios lepesek eredmenyei: lambda_deg,N,h sorrend data_iter = 1.0e+006 * 0.00003400000754 6.38483779492169 0.00023765195628 0.00003400000088 6.38482321352208 0.00025173252342 0.00003400000048 6.38482321437358 0.00025170199822 0.00003400000048 6.38482321443625 0.00025170199987 0.00003400000048 6.38482321443612 0.00025170200000 0.00003400000048 6.38482321443612 0.00025170200000 0.00003400000048 6.38482321443612 0.00025170200000 0.00003400000048 6.38482321443612 0.00025170200000 0.00003400000048 6.38482321443612 0.00025170200000 Inverz problem: x,y,z ECEF -> lambda_deg,phi_deg,h x = -2.430601829367448e+006 y = -4.702442706380628e+006

13. A Globális Pozícionáló Rendszer (GPS) 153 z = 3.546587344780241e+006 N = 6.384823214436118e+006 lambda_deg = 34.00000048333332 phi_deg = -1.173335693472223e+002 h = 2.517020000005141e+002

12.3 A NED koordináta-rendszer A NEDK (North-East-Down) koordináta-rendszert úgy kapjuk meg, hogy meghatá-rozzuk a P ponttól a forgásellipszoidig a legközelebbi Q pontot, amely a NED ko-ordináta-rendszer origója lesz. A Q pontban meghatározzuk a forgásellipszoid érin-tősíkját, és abban az NEDx tengely észak felé, az NEDy tengely kelet felé és a NEDz tengely az érintősík normálisa irányában lefelé, a forgásellipszoid belseje felé mutat.

Az alkalmazásokban NEDK -et egy adott pillanatban lerögzítjük, és a továbbiak-ban ehhez képest értelmezzük a mozgó járműhez rögzített ABC (Aircraft-Body Coordinate System, vagy általában Body Coordinate System) mozgását. A robot ko-ordináta-rendszere a BABC KK = koordináta-rendszer, amelynek origója tipikusan a robot tömegközéppontjában van, a tengelyek irányítása az alkalmazás függvényében megválasztható.

A NEDK és a BABC KK = közötti kapcsolatot a robot TNEDNED hyx ),,( − pozíci-

ója és az Euler-szögekkel jellemzett T),,( ψϑϕ orientációja azonosítja, ahol ψϑϕ ,, rendre a roll, pitch, yaw szögek és ),(),(),(, ϕϑψ xRotyRotzRotA

ABCNED KK = . Ez a koordinátarendszer a rövid időtartamú irányítások vizsgálatánál alapvető fontossá-gú.


13. A Globális Pozícionáló Rendszer (GPS) A GPS (Global Positioning System) egy a Föld környezetében tetszőleges időjárás-viszonyok között alkalmazható helyzetmeghatározó rendszer, amely megkülönböz-tetett (katonai) felhasználók számára pontosabb, általános polgárai felhasználók számára kevésbé pontos pozició információt képes szolgáltatni. A rendszert az USA hozta létre, teljes szolgáltatása 1993 óta hozzáférhető.

13.1 A GPS rendszer áttekintése A GPS rendszer 3 szegmenstípust tartalmaz: űr-szegmenst (space), felügyelő szegmenst (control)és felhasználói szegmenst (user). Űr-szegmens: Az űr-szegmens 24 szatellit járműből (SV, satellit vehicle) áll. A szatellitek 6 föld-körüli pályán keringenek, egy pályán 4 szatellit. A keringési idő kb. 12 h (11 h 58 min), a pálya közel köralakú, sugara 20200 km.A pályák az egyenlítő síkjával o55 -os szöget zárnak be. A hat pálya az egyenlítő kerülete mentén o60 -ra van egymás-tól, egyenletesen elosztva. Ez biztosítja, hogy a földi felhasználó mindig legalább 4 szatellitre rálásson. Az SV rádiójeleket sugároz, amelyet a felhasználó GPS vevője vesz és dekódol. Felügyelő szegmens: A felügyelő szegmens tartalmaz 4 földi antennát (Cape Canaveral, Ascension Is-land, Diego Garcia, Kwajalein), 1 monitor állomást (Falcon Air Force Base Colo-rado Springs) és 5 távoli monitor állomást (Hawaii, Ascension Island, Diego Garcia, Kwajalein, Cape Canaveral), 1 master felügyelő állomást (Falcon Air Force Base Colorado Springs) és 1 backup master felügyelő állomást (Gaithersburg). A földi monitor állomások mérik az SV-től a master felügyelő állomás számára sugárzott in-formációt és továbbítják a master felügyelő állomásnak, amely meghatározza a ke-ringési modellt és az órajel-korrekciót a szatellit számára, és továbbítja azt a földi antennáknak, amelyek továbbsugározzák azt az SV-knek. Felhasználó szegmens: A felhasználói szegmens a vevők antennáiból és processzoraiból áll, amelyek mérik és dekódolják a szatellitek által sugárzott információt. A felhasználók száma kor-látozatlan, mivel az üzenetforgalom egyirányú. A GPS rendszer ún. line-of-sight (LOS) rendszer, csak azon szatellitek adata vehető, amelyekre a vevő akadálytalanul rálát.

13. A Globális Pozícionáló Rendszer (GPS) 155 Minden GPS szatellit távolságkódot és navigációs adatot sugároz code-division

multiple access (CDMA) technikával 2 hordozófrekvencián: L1 (1575.42 MHz) és L2 (1227.60 MHz). A hordozó frekvenciák szórt-sprektumú jelekkel vannak modulálva, amelyek a vevők számára sugárzott információt tartalmazzák. Minden szatellit 3 pszeudo-random (PRS) távolságadatot küld.

A C/A kód modulálja az L1 hordozó fázisát. Ennek a kódnak a hossza 1024 chips (chip=bit, de nem hordoz információt). Minden szatellit számára van egy ettől különböző és erre kvázi-ortogonális C/A PRN kód. Bár a szatellitek ugyanazon a két frekvencián sugároznak, a vevők képesek rákapcsolódni egy meghatározott sza-tellitre és különbséget tenni a szatellitek között azáltal, hogy korrelláltatják a belül generált verzióját a szatellitre jellemző C/A kódnak a vett jellel. Mivel a szatellitek C/A kódjai kvázi-ortogonálisak, a szatellitek közötti áthallás a vételi oldalon kicsi. A GPS SV azonosítása gyakran az SV PRN kódja alapján történik.

Az ún. precíz (P) kód modulálja mind az L1, mind pedig az L2 hordozó fázisát. A P kód egy nagyon hosszú (7 nap) 10.23 MHz PRN kód. Az ún. antispoofing (AS) módú üzemmódban a P kód át van kódolva Y kóddá, minden vételi csatornára egy számára megkülönböztetett AS modult igényel, kriptográfiai kulccsal védett, és ezért a normál vevő számára nem hasznosítható.

A navigációs üzenet szintén modulálja az L1 C/A kódjelet. A navigációs üzenet egy 50 bit/s jel, amelyet a GPS vevő dekódol pálya, órajel korrekció és más rend-szer-paraméterekké. Ezek az ún. ephemeris parameterek használhatók a szatellit po-zíció, az órajel korrekció és az adott időben érvényes atmoszférikus korrekció ki-számítására a vevőben.

A GPS két szintű szolgáltatást biztosít: standard-pozionálási szolgáltatást (SPS, standard-positioning service) és precíz pozícionális szolgáltatást (PPS, precise-positioning service).

Az SPS pozícionálási és ütemezési szolgáltatás olyan C/A kódon alapul, amely hozzáférhető minden GPS felhasználó számára. Ez a szolgáltatás az L1 frekvenciát használja, amely tartalmaza a C/A kódot és egy navigációs-adat üzenetet, várható pontossága m100~ horizontálisan, m156 %)95( vertikálisan és az idő tekinteté-ben ns340 %)95( . Differenciális GPS (DGPS) technikák jelentősen javíthatnak a pontosságon.

A PPS sokkal pontosabb pozíció, sebesség és idő szolgáltatást biztosít, de csak az US kormány által elfogadott autorizált felhasználók vevői számára hozzáférhető. A hozzáférést AS és szelektív hozzáféréses (SA, selective availability) technikával biztosítja. Az AS implementáció a P kódot helyettesíti a megkülönböztetett Y kód-dal. Az SA implementáció célszerű módon degradálja a szatellit órajelet és az ephemeris adatokat nemautorizált felhasználók számára. Autorizált felhasználó ve-vője rendelkezik az Y kódhoz való hozzáférés képességével, és ezért el tudja távolí-tani az SA effektust. A PPS várható pontossága m22 horizontálisan, m7.27

%)95( vertikálisan és az idő tekintetében ns200 %)95( . A teljes PPS szolgáltatás 1995 tavasza óta létezik.


13.2 A GPS matematikai alapjai A GPS a beérkezési időből számított távolságmérésen (time-of-arrival ranging) ala-pul. A GPS vevő tartalmaz egy belső órát, amelyet a szatellit távolság-kódjának (C/A, P vagy Y az üzemmmódtól függően) beérkezési idejének mérésére használ fel. Ebből az információból a GPS vevő meghatározza a jelterjedési időt a szatellit adójától a felhasználó vevőjéig. Mivel a jel smc /1099792458.2 8⋅= fénysebes-séggel terjed, ezért a távolság ennek az időnek és a fénysebességnek a szorzata. Ez az érték egy offszettel (bias) rendelkezik, amelynek oka a szatellit adójának órajele és a vevő órajele közötti különbség, ezért a szokásos elnevezés pszeudo-távolság (pseudorange). Az ms1 időre vonatkoztatott időzítési pontatlanság távolságra át-számítva km300~ , ezért az idő bias nagy pontossággal kompenzálandó.

Ha a szatellit helyzete ismert és az órajel hiba elhanyagolható lenne, akkor a mért távolság a szatellit és a vevő között egy gömböt definiál, két ilyen gömb egy körben metszi egymást, és egy harmadik gömb két pontot határoz meg, amelyek kö-zül a vételi hely a priori ismeretében vagy egy negyedik szatellit távolság informá-ciójának bevonásával a vevő pozíciója meghatározható. Ez a geometriai elv azzal ekvivalens a matematika nézőpontjából, hogy három független egyenlet kell három ismeretlen meghatározásához. A három ismeretlen a vevő antennájának 3D pozíció-ja. Ha a vevő órajelének hibáját is tekintjük, akkor a szimultán távolságmérési eredmények ugyanazzal a bias értékkel vannak eltolva. Ezért minden szimultán tá-volságmérési eredmény-együttes 4 ismeretlent tartalmaz, ezért legalább 4 távolság-mérési eredmény kell az egyértelmű megoldás meghatározásához.

A felügyelő (control) szegmens monitorozza minden egyes szatellit órajelét és meghatározza az offszetet, a drift sebességét és gyorsulását a GPS időhöz képest. Ezeket az információkat periódikusan áttölti a szatellitekre, amely a navigációs in-formáció része. A GPS vevő felhasználja a szatellit órajel offszet értékét a mért pszeudo-távolságok korrekciójára. Az ezzel kapcsolatos képleteket később az ephemeris adatok feldolgozásánál tárgyaljuk.

Megjegyezzük még, hogy amíg a szatellitek nagy pontosságú atomórákat hasz-nálnak az időmérésre, addig a GPS vevő órajele csak kvarc-kristály pontosságú.

Legyen a vevő és a szatellit idő eltérése rt∆ . Akkor a standard GPS pozícionáló

rendszer négy ismretlen mennyiséget tartalmaz, ezek a vevő Tzyx ),,( koordinátái az ECEF koordináta-rendszerben és rtc∆ . Feltesszük, hogy ezek meghatározásához

rendelkezésre áll a négy szatellit iρ~ pszeudo-távolsága és Tiii ZYX ),,( )()()(

koordinátája az ECEF rendszerben, 4,3,2,1=i . A távolságinformáció hibáját a vevő

rt∆ órajel bias, az SV szatellit jármű )(iSVt∆ órajel bias, a )(i

at∆ atmoszférikus késés,

a szatellit jelek korrupciójának )(iSA hatása a szelektív elérhetőségi filozófia követ-

13. A Globális Pozícionáló Rendszer (GPS) 157

keztében, az ephemeris adatok )(iE hibája, a többirányú jelterjedés )(iMP ún. multipálya (multipath) hibája és a vevő követés )(iη zaja okozza, együttes hatásuk )()()()()()( iiiii

ai

SVr MPESAtctctc ϑ∆∆∆ ++++++ (13.1)

A négy szatellittől származó pszeudo-távolság a következő egyenleteket kell, hogy kielégítse:

4,3,2,1,

)()()(~

)()()()()()(

2)(2)(2)()(

=++++++

+−+−+−=

iMPESAtctc

tczZyYxXiiiii

ai

SV

riiii

η∆∆

∆ρ (13.2)

Az egyenletekben elhanyagolható nagyságrendű mérési hibának tekintjük a

4,3,2,1,: )()()()()()()()()( =+=+++++= iMPESAtctc iiiiiiia

iSV

i ηχη∆∆κ (13.3) mennyiségeket, a megmaradó nemlineáris egyenleteket pedig az ismeretlen változók szerint linearizáljuk a változók előző becslése körül és a kapott egyenleteket LS technikával megoldjuk. Képezzük a deriváltakat az utolsó becslés helyén:

1)(

~)()()(

)(~)()()(

)(~)()()(

)(~

)(

20

)(20

)(20

)(

0)()(

20

)(20

)(20

)(

0)()(

20

)(20

)(20

)(

0)()(

=∂∂

−+−+−

−−=

∂∂

−+−+−

−−=

∂∂

−+−+−

−−=

∂∂

r

i

iii

ii

iii

ii

iii

ii

tc

zZyYxX

zZz

zZyYxX

yYy

zZyYxX

xXx

∆ρ

ρ

ρ

ρ

(13.4)

Jelölje Trtczyx ),,,(:ˆ ∆=x az ismeretleneket, és legyen az ismeretlenek becsült

értéke Trtczyx ),,,(:ˆ 00000 ∆=x az iteráció megelőző lépésében, továbbá vezessük

be a következő jelöléseket:


⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

−−−−

=

⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜

⎝

⎛

−−−−

=

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

∂∂

=

),,,(~~),,,(~~),,,(~~),,,(~~

~

,ˆ

1~~~

1~~~

1~~~

1~~~

0000)4()4(

0000)3()3(

0000)2()2(

0000)1()1(

0

0

0

0

,),,(

)4()4()4(

)3()3()3(

)2()2()2(

)1()1()1(

000

r

r

r

r

rr

zyx

tczyxtczyxtczyxtczyx

tctczzyyxx

zyx

zyx

zyx

zyx

T

∆ρρ∆ρρ∆ρρ∆ρρ

δ

∆∆

δ

ρρρ

ρρρ

ρρρ

ρρρ

ρ

xH

(13.5)

Akkor a megoldandó egyenletrendszer és annak LS (least squares) megoldása: xxxρHHHxxHρ ˆˆˆ~)(ˆˆ~

01 δδδδδ +=⇒=⇒= − TT (13.6)

Az iterációt addig kell folytatni, amíg a becsült értékek nem stabilizálódnak.

Az elhanyagolt )(iκ mennyiségek által okozott hiba csökkenthető i) PPS imple-mentációval, ii) a hiba modellezésével, beleértve az atmoszférikus hatásokat is, iii) differenciális technikával.

13.3 Szatellit óra korrekció Legyen ρ~ a pszeudo-távolság és ctt /~~ ρ= a szatellit és a vevő közötti átvitel mért ideje, továbbá mt

~ az az időpillanat, amelyben a pszeudo-távolság meg lett mérve. Akkor a nominális SVt idő és a valódi t GPS idő, amikor a szatellit a jelet elküdte: tmSV ttt ~~ −= nominális idő, SVSV ttt ∆−= valódi GPS idő (13.7)

A rendszer polinomiális korrekciót használ SVt∆ számára: rocfocffSV tttattaat ∆∆ +−+−+= 2

210 )()( , (13.8)


ahol oct és a korrekciós együtthatók a navigációs üzenet részei, rt∆ relativisztikus korrekció: )sin( kr EAFet =∆ (13.9) Itt 510 /10442807633.4 msF −⋅−= konstans, A a szatellit pályájának nagyobbik féltengelye, kE pedig az excentrikus anomália Kepler-egyenletével számítható. A tapasztalat szerint kielégítő pontosságú, ha a korrekciós egyenletben SVtt ≈ közelí-téssel élünk.

A oct paraméter a referencia idő az órajel korrekció alkalmazhatóságához. A rendszer feltételezi, hogy a ]302400,302400[−∈− octt , ami egy GPS hétnek felel meg sec-ben. Előfordulhat, hogy egy hét kezdetén vagy végén t és oct eltérő hetek-re vonatkozik, vagyis 302400>− octt . Ekkor a felhasználó felelőssége a

604800: ±= ococ tt korrekció elvégzése úgy, hogy octt − visszakerüljön az előírt tartományba. A bejövő szatellit üzenet tartalmaz egy IODC integer adatot, amelynek monitorozásával a felhasználó megállapíthatja, ha új clock modell paraméterek van-nak érvényben.

13.4 A szatellit pozíció számítása Az ephemeris paraméterek írják le a szatellit pályáját (orbit) egy legalább 1 h időin-tervallumban. Az ephemeris paraméterek a Kepler-féle becslés pályamodelljének pontosítására szolgálnak. Az ephemeris paramétereket a szatellit sugározza és dekó-dolás után a vevő számára rendelkezésre állnak.

13.4.1 Clock és ephemeris paraméterek definíciója A clock és ephemeris paraméterek értelmezését a 13.1. táblázat tartalmazza.

13.1. táblázat. Clock és Ephemeris paraméterek értelmezése

GDT Group delay ][s IODC Issue of data, clock

oct Clock data reference time ][s

2fa Second-order correction to satellit clock ]/[ 2ss

1fa First-order correction to satellit clock ]/[ ss

0fa Constant correction to satellit clock ][s


0M Mean anomaly at reference time [semicircles]

n∆ Mean motion difference from comuted value [semicircles/s] e eccentricity

2/1A Square root of the semimajor axis ][ 2/1m

0∆Ω Right ascension at reference time [semicircle]

0i Inclination angle at reference time [semicircle] ω Argument of perigee [semicircle] Ω∆ & Rate of right ascension [semicircles/s] i&∆ Rate of inclination angle [semicircles/s]

ueC Amplitude of the cosine harmonic correction term to the argument of latitude [rad]

usC Amplitude of the sine harmonic correction term to the argument of latitude [rad]

rcC Amplitude of the cosine harmonic correction term to the orbit radius ][m

rsC Amplitude of the sine harmonic correction term to the orbit radius ][m

icC Amplitude of the cosine harmonic correction term to the angle of inclination [rad]

isC Amplitude of the sine harmonic correction term to the angle of inclination [rad]

oet Ephemeris reference time [s] IODE Issue of data, ephemeris

13.4.2 A szatellit antenna fáziscentruma ECEF koordinátáinak szá-mítására szolgáló egyenletek A szatellit Tiii ZYX ),,( )()()( ECEF koordinátáinak számítási szabályait a 13.2. táb-

lázat tartalmazza. A rsrc CCA ,, paraméterek befolyásának érzékenysége az ECEF pozíció pontosságára mm /1~ , ezzel szemben a szögparamétereké

810 m/semicircle, a szösebesség paramétereké 1210 m/(semicircle/s), következésképp a szögparaméterek számításának pontossága alapvetően fontos. Az eddig nem sze-replő paraméterek közül 2314 /10986005.3 sm⋅=µ a WGS-84 univerzális gravitá-ciós állandó.

Az IODE integer paraméter monitorozásával megállapítható, módosultak-e az ephemeris paraméterek az előző vételhez képest.


13.2. táblázat. Az ECEF szatellit pzíció számítási egyenletei

)sin( kr EAFet =∆ s Reativistic correction term

r

ocSVocSVSV

tttattaat

∆∆

+−+−+= 2

210 )()(

s Correctionn to satellit clock

SVSV ttt ∆−= s Corrected message transmission time

2)( AA = m Orbit semimajor axis

30 / An µ= rad/s Computed mean motion

oek ttt −= s Time from ephemeris reference epoch

nnn ∆+= 0 rad/s Corrected mean motion

ntMM kk += 0 rad Mean anomaly

)sin( kkk EeME += rad Kepler’s equation for eccen-tric anomaly

⎟⎟

⎠

⎞

⎜⎜

⎝

⎛

−−

−−

=)cos(1

)cos(,

)cos(1)sin(1 2

2arctank

k

k

kk Ee

eEEe

Eev

rad True anomaly

ωφ += kk v rad Argument of latitude

)2cos()2sin( kuckusk CCu φφδ += rad Argument of latitude correction

)2cos()2sin( krckrsk CCr φφδ += m Radius correction

)2cos()2sin( kickisk CCi φφδ += rad Inclination correction

kkk uu δφ += rad Corrected argument latitude

kkk rEeAr δ+−= )]cos(1[ m Corrected radius

itiii kkk&∆δ ++= 0 rad Corrected inclination

)cos( kkk urx =′ m x position in orbital plane

)sin( kkk ury =′ m y position in orbital plane

oeiekiek tt ωωΩ∆∆ΩΩ −−+= )(0& rad Corrected longitude of

ascending node )sin()cos()cos( kkkkkk iyxx ΩΩ ′−′= m Satellit x ECEF coordinate

)cos()cos()sin( kkkkkk iyxy ΩΩ ′+′= m Satellit y ECEF coordinate

)sin( kkk iyz ′= m Satellit z ECEF coordinate


Vegyük észre, hogy a táblázat 1-7 sorában álló egyenletek iterációt igényelnek kE meghatározására, amely ezen kívül a 7. sor egyenlete miatt egy nemlineáris egyenlet megoldását is igényli.

13.5 A számítások szoftver megvalósítása Az alábbiakban bemutatjuk a GPS matematikai alapfeladat megoldását egy teszt-esetben, valamint a szatellit ECEF koordinátáinak számítását az ephemeris adat-szegmens adataiból.

13.5.1 A vevő ECEF GPS koordinátáit meghatározó szoftver megva-lósítása A 11.3. táblázat mutatja a négy szatellit feltételezett ECEF koordinátáit és a vevő ezekkel összetartozó pszudo-távolság értékeit m-ben.

11.3. táblázat. A négy szatellit ECEF koordinátái és a vevő pszeudo-távolságai a szatellitektől

X Y Z ρ~ SV 2 +7766188.44 -21960535.34 +12522838.56 22228206.42 SV 26 -25922679.66 -6629461.28 +31864.37 24096139.11 SV 4 -5743774.02 -25828319.92 +1692757.72 21729070.63 SV 7 -2786005.69 -15900725.80 +21302003.49 21259581.09 Az [xyzc,xyzciter,H]=solveRange(Satsxyz,Satsrho,niter) függvény az iterációt valósítja meg, a keretprogram pedig frame_SolveRange. function [xyzc,xyzciter,H]=solveRange(Satsxyz,Satsrho,niter) %Solve Pseudorange Equations for Satposes in ECEF n=size(Satsxyz,1); xyzc0=zeros(4,1); xyzcact=xyzc0; xyzciter=zeros(niter+1,4); xyzciter(1,:)=xyzc0'; xyzcact=xyzc0; for j=1:niter H=[zeros(4,3) ones(4,1)]; rhoj=zeros(4,1); for i=1:4 Satixyz=Satsxyz(i,:)'; dixyz=Satixyz-xyzcact(1:3); di=norm(dixyz,2); H(i,1:3)=-dixyz'/di; rhoj(i)=di+xyzcact(4);

13. A Globális Pozícionáló Rendszer (GPS) 163 end; drhohat=Satsrho-rhoj; dxyzchat=inv(H'*H)*H'*drhohat; xyzcact=xyzcact+dxyzchat; xyzciter(j+1,:)=xyzcact'; end; xyzc=xyzciter(niter+1,:)'; %frame_SolveRange.m %Compute estimated rover coordinates clear all close all clc format long Satsxyz=[+7766188.44 -21960535.34 +12522838.56; ... -25922679.66 -6629461.28 +31864.37; ... -5743774.02 -25828319.92 +1692757.72; ... -2786005.69 -15900725.80 +21302003.49] Satsrho=[22228206.42 24096139.11 21729070.63 21259581.09]' niter=5 [xyzc,xyzciter,H]=solveRange(Satsxyz,Satsrho,niter); fprintf('Results of iteration in x,y,z,c*Delta_tr order\n'); xyzciter fprintf('Last H matrix\n'); H fprintf('Receiver coordinates in x,y,z,c*Delta_tr order\n'); xyzc format short Futási eredmények: Results of iteration in x,y,z,c*Delta_tr order xyzciter = 1.0e+006 * 0 0 0 0 -2.97757147597244 -5.63527815933950 4.30423450558470 1.62523980181658 -2.45172853419043 -4.73087846098026 3.57399752045577 0.31407073267925 -2.43077221884605 -4.70237580233497 3.54660387208444 0.26474970653356 -2.43074509597674 -4.70234511363092 3.54656870605020 0.26469112951536 -2.43074509593620 -4.70234511359277 3.54656870599656 0.26469112943121

164 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA Last H matrix H = Columns 1 through 3 -0.46426691725303 0.78576630371213 -0.40869003596240 0.98575355484955 0.08086441780387 0.14748177865320 0.15435009076044 0.98423412639754 0.08636685658695 0.01692128868937 0.53338601476057 -0.84570268371747 Column 4 1.00000000000000 1.00000000000000 1.00000000000000 1.00000000000000 Receiver coordinates in x,y,z,c*Delta_tr order xyzc = 1.0e+006 * -2.43074509593620 -4.70234511359277 3.54656870599656 0.26469112943121 A vevő ECEF koordinátáinak értékeit az iteráció egymást követő lépéseiben külön is összefoglaltuk a 13.4. táblázatban.

13.4. táblázat. A vevő ECEF koordinátáinak értékei az iteráció során

x ECEF y ECEF z ECEF rtc∆ 0 0 0 0

-2977571.48 -5635278.16 4304234.51 1625239.81 -2451728.53 -4730878.46 3573997.52 314070.73 -2430772.22 -4702375.80 3546603.87 264749.71 -2430745.10 -4702345.11 3546568.71 264691.13 -2430745.10 -4702345.11 3546568.71 264691.13

13.5.2 Az ephemeris adatszegmens kiértékelésének szoftver megvaló-sítása Az alábbi MATLAB programrészlet bemutatja egy ephemeris adatszegmens feldol-gozásának példáján az kE iteratív számítására szolgáló EE_k=compEk(t,E_k0) függvényt, az általa az fsolve keretében meghívott y=funEk(x) függvényt és az

13. A Globális Pozícionáló Rendszer (GPS) 165 ephemeris adatokból a szatellit ECEF koordinátáit meghatározó frame_Ephemeris MATLAB scriptet és a futási eredményeket. Az ephemeris adatszegmens adatai és a fontos konstansok értékei a keretprogramban könnyen megtalálhatók a %Re-ceived Satellite Information és az %Important Constants com-mentek után. Az iteráció addig folyik, amíg runEk=input('runEk=') kérdésre 1 válasz érkezik.

13.5. táblázat. A szatellit feltételezett ephemeris adatszegmense

SVt 4.03272930e+05

nw %GPS week number 910

owt %Seconds in GPS week 403230

gdt %group delay in s 2.3283e-09

aodc %clock data issue 409 oct 410400

2a 0.00000e+00

1a 1.819e-12

0a 3.2977667e-05 AODE %orbit data issue 153

n∆ 4.3123e-09

0M 2.24295542 e 4.27323824e-03

2/1A 5.15353571e+03

oet 410400

icC 9.8720193e-08

rcC 282.28125

isC -3.9115548e-08

rsC -132.71875

ucC -6.60121440e-06

usC 5.31412661e-06

0∆Ω 2.29116688 ω -0.88396725

0i 0.97477102

Ω∆ & -8.025691e-09

i&∆ -4.23946e-10

166 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA function EE_k=compEk(t,E_k0) %Compute Ek global t_sv w_n t_ow t_gd aodc t_oc a_2 a_1 a_0 AODE D_n M_0 e global sqrtA t_oe C_ic C_rc C_is C_rs C_uc C_us DOmega omega i_0 global DOmegaDot DiDot global omega_ie mu c F f_1 f_2 lambda_1 lambda_2 lambda_w lambda_n global A n_0 Dt_sv t t_k n M_k E_k A=sqrtA^2; n_0=sqrt(mu/A^3); t_k=t-t_oe; if t_k>302400, t_k=t_k-604800; end; if t_k<-302400, t_k=t_k+604800; end; n=n_0+D_n; M_k=M_0+t_k*n; option=optimset('TolFun',1e-10); EE_k=fsolve(@funEk,E_k0,option); function y=funEk(x) %Function solve funEk(x)=0 global t_sv w_n t_ow t_gd aodc t_oc a_2 a_1 a_0 AODE D_n M_0 e global sqrtA t_oe C_ic C_rc C_is C_rs C_uc C_us DOmega omega i_0 global DOmegaDot DiDot global omega_ie mu c F f_1 f_2 lambda_1 lambda_2 lambda_w lambda_n global A n_0 Dt_sv t t_k n M_k E_k y=M_k+e*sin(x)-x; %frame_Ephemeris.m %Frame program for ephemeris computation clear all close all clc global t_sv w_n t_ow t_gd aodc t_oc a_2 a_1 a_0 AODE D_n M_0 e global sqrtA t_oe C_ic C_rc C_is C_rs C_uc C_us DOmega omega i_0 global DOmegaDot DiDot global omega_ie mu c F f_1 f_2 lambda_1 lambda_2 lambda_w lambda_n global A n_0 Dt_sv t t_k n M_k E_k

13. A Globális Pozícionáló Rendszer (GPS) 167 %Received Satellite Information t_sv=4.03272930e+05; w_n=910; %GPS week number t_ow=403230; %Seconds in GPS week t_gd=2.3283e-09; %group delay aodc=409; %clock data issue t_oc=410400; a_2=0.00000e+00; a_1=1.819e-12; a_0=3.2977667e-05; AODE=153; %orbit data issue D_n=4.3123e-09; M_0=2.24295542; e=4.27323824e-03; sqrtA=5.15353571e+03; t_oe=410400; C_ic=9.8720193e-08; C_rc=282.28125; C_is=-3.9115548e-08; C_rs=-132.71875; C_uc=-6.60121440e-06; C_us=5.31412661e-06; DOmega_0=2.29116688; omega=-0.88396725; i_0=0.97477102; DOmegaDot=-8.025691e-09; DiDot=-4.23946e-10; %Important Constants %pi=3.1415926535898 omega_ie=7.2921151467e-05; %WGS-84 earth rotation rate mu=3.986005e14; %WGS-84 gravitational constant c=2.99792458e08; %speed of light F=-4.442807633e-10; f_1=1575.42; %MHz L1 carrier frequency f_2=1227.60; %MHz L2 carrier frequency lambda_1=c/f_1; %19.0 cm L1 wavelength lambda_2=c/f_2; %24.4 cm L2 wavelength lambda_w=c/(f_1-f_2); %86.2 cm wide-lane wavelength lambda_n=c/(f_1+f_2); %10.7 cm narrow-lane wavelength % t=t_sv; runEk=1; E_k0=0; while runEk E_k=compEk(t,E_k0) Dt_r=F*e*sqrtA*sin(E_k) Dt_sv=a_0+a_1*(t_sv-t_oc)+a_2*(t_sv-t_oc)^2+Dt_r t=t_sv-Dt_sv E_k0=E_k; runEk=input('runEk='); end;

168 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA % [E_k,A,n,t_k,n,M_k]=compEk(t,E_k0); E_k A n t_k M_k v_k=atan2(sqrt(1-e^2)*sin(E_k)/(1-e*cos(E_k)),(cos(E_k)-e)/(1-e*cos(E_k))) phi_k=v_k+omega du_k=C_us*sin(2*phi_k)+C_uc*cos(2*phi_k) dr_k=C_rs*sin(2*phi_k)+C_rc*cos(2*phi_k) di_k=C_is*sin(2*phi_k)+C_ic*cos(2*phi_k) u_k=phi_k+du_k r_k=A*(1-e*cos(E_k))+dr_k i_k=i_0+di_k+t_k*DiDot x_kprime=r_k*cos(u_k) y_kprime=r_k*sin(u_k) Omega_k=DOmega_0+(DOmegaDot-omega_ie)*t_k-omega_ie*t_oe format long x_k=x_kprime*cos(Omega_k)-y_kprime*cos(i_k)*sin(Omega_k) y_k=x_kprime*sin(Omega_k)+y_kprime*cos(i_k)*cos(Omega_k) z_k=y_kprime*sin(i_k) format short Futási eredmények frame_Ephemeris esetén: A változók nevei követik az egyenletekben szereplő jelölést. E_k = 1.2073 Dt_r = -9.1449e-009 Dt_sv = 3.2956e-005 t = 4.0327e+005 runEk=1 E_k = 1.2073 Dt_r = -9.1449e-009 Dt_sv = 3.2956e-005 t = 4.0327e+005 runEk=0 E_k = 1.2073 A = 2.6559e+007 n_0 = 1.4587e-004 t_k = -7.1271e+003 n = 1.4587e-004 M_k = 1.2033 v_k = 1.2113 phi_k = 0.3274 du_k = -2.0003e-006 dr_k = 143.0967 di_k = 5.4489e-008 u_k = 0.3274 r_k = 2.6519e+007 i_k = 0.9748 x_kprime = 2.5111e+007

13. A Globális Pozícionáló Rendszer (GPS) 169 y_kprime = 8.5267e+006 Omega_k = -27.1159 x_k = -5.678411013083118e+006 y_k = -2.492396293091095e+007 z_k = 7.056518876151539e+006 A 13.6. táblázat összefoglalja a kiértékelés eredményeit táblázatos alakban is.

13.6. táblázat. Az ECEF szatellit pozíció számítási végeredményei ephemeris adatokból az iteráció végén

kE 1.2073 az iteráció végén

)sin( kr EAFet =∆ s -9.1449e-009

r

ocSVocSVSV

tttattaat

∆∆

+−+−+= 2

210 )()( s 3.2956e-005

SVSV ttt ∆−= s 4.0327e+005 2)( AA = m 2.6559e+007

30 / An µ= rad/s 1.4587e-004

oek ttt −= s -7.1271e+003

nnn ∆+= 0 rad/s 1.4587e-004

ntMM kk += 0 rad 1.2033

)sin( kkk EeME += rad 1.2073

⎟⎟

⎠

⎞

⎜⎜

⎝

⎛

−−

−−

=)cos(1

)cos(,

)cos(1)sin(1 2

2arctank

k

k

kk Ee

eEEe

Eev

rad 1.2113

ωφ += kk v rad 0.3274

)2cos()2sin( kuckusk CCu φφδ += rad -2.0003e-006

)2cos()2sin( krckrsk CCr φφδ += m 143.0967

)2cos()2sin( kickisk CCi φφδ += rad 5.4489e-008

kkk uu δφ += rad 0.3274

kkk rEeAr δ+−= )]cos(1[ m 2.6519e+007

itiii kkk&∆δ ++= 0 rad 0.9748

)cos( kkk urx =′ m 2.5111e+007

)sin( kkk ury =′ m 8.5267e+006

oeiekiek tt ωωΩ∆∆ΩΩ −−+= )(0& rad -27.1159


)sin()cos()cos( kkkkkk iyxx ΩΩ ′−′= m -5678411.01

)cos()cos()sin( kkkkkk iyxy ΩΩ ′+′= m -24923962.93

)sin( kkk iyz ′= m 7056518.87

13.6 Differenciális GPS (DGPS) a pozíció térben A pszeudo-távolságot (13.2-6) szerint a ηχ + zaj is terheli, amelyben χ normája jelentősen nagyobb η normájánál, továbbá χ nem függ a vevőtől, hanem csakis a szatellitektől. Ezért ha rendelkezésre áll egy bázisállomás, amelynek az ECEF koor-dinátái ismertek, és amely ugyanazt a 4 szatellitet látja, mint a saját jármű (rover), valamint képes saját mérési eredményeiből számított korrekciós adatok szétsugárzására a környezetében lévő járműveknek, akkor mint látni fogjuk, χ eliminálható a saját járműben, és ezáltal a rover pozíciómeghatározásának a pon-tossága jelentősen feljavul. Azonos szatellitek vétele azonban feltételezi, hogy a sa-ját jármű távolsága a bázisállomástól nem nagyobb kb. 50 km-nél.

Megjegyzendő, hogy a bázissállomást kiválthatja a saját járműre szerelt második antenna a hozzátartozó második vevővel, ugyanis feltehető, hogy a két antenna elhe-lyezkedése ismert a saját járművön (2-antennás DGPS).

A DGPS működési elvét a (13.6) egyenletből vezetjük le. A bázisállomást és a saját járművet 0 és r indexszel különböztetjük meg.

rrr ηχxHρ

ηχxHρ++=++=

ˆ~ˆ~

000

δδδδ

(13.10)

Mivel 0η , rη normája jelentősen kisebb, mint χ normája, ezért csak χ kompen-zálására koncentrálunk. (13.10)-ből és a bázisállomás helyzetének pontos ismereté-ből következik, hogy

χHHHρHHH

xxxxχHHHxηχHHHxρHHH

TTTT

TTTTTT

10

10000

100

100

1

)(~)(

0ˆˆ)(ˆ)()(ˆ~)(

−−

−−−

≈

=⇒+=+≈++=

δ

δδδδδ

(13.11)

χHHHxηχHHHxρHHH TTrr

TTrr

TT 111 )(ˆ)()(ˆ~)( −−− +≈++= δδδ (13.12)

0

11

11

~)(~)(

)(~)(ˆ

ρHHHρHHH

χHHHρHHHx

δδ

δδTT

rTT

TTr

TTr

−−

−−

−=

−≈ (13.13)

Célszerű tehát bevezetni egy corrx∆ korrekciós tagot, amellyel a vevő ECEF po-

zíciójának a becsléséből eltüntethető a domináns χHHH TT 1)( − tag hatása, miáltal a vevő pozíció-meghatározásának pontossága jelentősen javul:

14. ÁLLAPOTBECSLÉS KÉPFELDOLGOZÁS ÉS IMU BEVONÁSÁVAL 171

01 ~)( ρHHHx δ∆ TT

corr−−= (13.14a)

corrrTT

r xρHHHx ∆δδ += − ~)(:ˆ 1 (13.14b) rrr xxx ˆ: 0 δ+= (13.14c)

Egyúttal az is következik a fentiekből, hogy a bázisállomásnak a járművek szá-mára a corrx∆ üzenetet kell forgalmaznia.


14. ÁLLAPOTBECSLÉS KÉPFELDOLGOZÁS ÉS IMU BEVONÁSÁVAL

14.1 Az érzékelők fizikai alapjai Inerciális mérőegység (IMU, Inertial Measurement Unit) fejlett verziója képes a 3D gyorsulás és a 3D szögsebesség mérésére. Egyik korszerű kivitele a Crossbow MNAV 100CA egység, amely MEMS technológiával készült.

A MEMS szögsebességérzékelő tartalmaz egy kisméretű, rezgést végző rudacs-kát, amelynek oszcillációs frekvenciája nemnulla szögsebesség esetén a Coriolis ha-tás következtében megváltozik. A szögsebesség érzékelő kimenőjele közelíthető az

gyrogyrogyrogyro Tky ηβω ++= )( (14.1)

összefüggéssel, ahol gyroy a V-ban mért kimenet, gyrok az erősítés, )(Tgyroβ a

hőmérsékletfüggő bias és gyroη nulla várhatóértékű Gauss-zaj.

A MEMS gyorsulásérzékelő egy kis lapocskát tartalmaz, amely hozzá van erősítve egy torziós rudacskához. A lapocka forog és gyorsulás hatására megvál-tozik a kapacitása a környezet falához képest, amelyre fel van erősítve. A kapacitás-változás arányos az inerciarendszerbeli gyorsulással. A gyorsulás érzékelő kimenő-jele közelíthető az

accaccaccacc Taky ηβ ++= )( (14.2)

összefüggéssel, ahol accy a V-ban mért kimenet, acck az erősítés, )(Taccβ a hőmérsékletfüggő bias és accη nulla várhatóértékű Gauss-zaj.

A gyorsulásérzékelő egy specifikus erőt mér, mely az inerciarendszerben hat, amelyet azonban a helikopterhez rögzített, azzal együtt mozgó (nem inercia) koor-dináta-rendszerben fejezi ki. Ha a a gyorsulás az inerciarendszerben, v a sebesség és ω a szügsebesség a helikopterhez rögzített koordináta-rendszerben, akkor a mozgó koordináta-rendszerekben érvényes deriválási szabály szerint

gravity

z

y

x

Fm

vvaaa

1−×+=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛ω& . (14.3)

14. ÁLLAPOTBECSLÉS KÉPFELDOLGOZÁS ÉS IMU BEVONÁSÁVAL 173 A továbbiakban ügyelni kell arra, hogy az egyes vektorok milyen koordináta-rendszerben keletkeznek, és a szükséges koordináta transzformációkat ki kell dol-gozni és alkalmazni kell.

A vizsgált probléma egy négyrotoros beltéri helikopter (UAV) érzékelőrendszerét mutatja be, ezért a beltéri jelleg következtében pozíció/orientáció érzékelésre GPS technika nem alkalmazható (beltérben rendszerint nem vehető a szatellitek jele). Ezért pozíció és orientáció érzékelésre marker-bázisú képfeldolgozó rendszer alkalmazható. A helikopterre világító ledek vannak szerelve, amelyek markerként szolgálnak, és amelyek mozgását a szoba mennyezetére szerelt kamera detektálja, és a mozgás sztereo (motion stereo) elméletére alapozva meghatározza a helikopter pozícióját és orientációját.

Az IMU a helikopterre van szerelve, saját processzora van, amely CAN buszon keresztük kommunikál a helikopter beágyazott szabályozójával, amely egy Freescale MPC555 processzorral van megvalósítva. Az IMU kb. 100 Hz mintavételi frekvenciával tudja mérni a 3D gyorsulást és szöggyorsulást. A képfeldolgozó rend-szer egy host számítógépen fut, 10 frame/sec gyakorisággal küld pozíció és orientá-ció információt a beágyazott szabályozó számára. A nem mérhető állapotváltozók becslése a mért jelekre van alapozva, az állapotbecslést kiterjesztett Kalman-szűrők végzik.

14.2 A helikopter kinematikai modellje Multikomponensű robotrendszerek leírásánál fontos szerepet játszanak a koor-dináta-rendszerek. Jelölje 0HW KK = a helikopter koordináta-rendszerét a kezdeti álló helyzetben (world vagy base frame), HK a helikopter tömegközéppontjában a helikopterhez rögzített és azzal együtt mozgó keretet, továbbá SK az IMU szenzor keretét, amely a helikopterre van szerelve. Egyszerűség kedvéért a látórendszer ke-rete legyen azonos WK -vel. A gráf a következő:

SSH

HHW

HW KKKKTT⎯⎯ →⎯⎯⎯ →⎯= ,,

0 (14.4)

A WK keretet jó közelítésel inercia keretnek tekinthetjük a rövididejű szabályo-zások számára. A helikopter pozícióját és orientációját 0HW KK = -hoz képest jel-

lemezhetjük a Tzyx ),,(=ξ pozícióvektorral és a T),,( ΨΘΦη = orientációval, ahol ΨΘΦ ,, a navigációban használt Euler-szögek (a robotikában RPY-szögek) és ),,(, ΨΘΦAA

HW KK = az orientációs mátrix:


⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−++−

=

ΦΘΦΘΘ

ΦΨΦΘΨΦΨΦΘΨΘΨ

ΦΨΦΘΨΦΨΦΘΨΘΨ

CCSCSSCCSSCCSSSCSSSCSCCSSSCCC

A (14.5)

A szögsebesség felírható ηΓω &= alakban, ahol ηΓηΓω &&&&& += és

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−

−=

ΦΘΦ

ΦΘΦ

Θ

ΓCCSSCC

S

00

01 (14.6)

14.3 Állapotbecslés IMU és képfeldolgozás bevonásával Az állapotbecslést kétszintű kiterjesztett Kalman-szűrővel realizáljuk. Az állapot-becslés során az IMU és a marker-bázisú képfeldolgozás által szolgáltatott mérési eredményeket használjuk fel. Az IMU mérési frekvenciája 100 Hz (mintavételi ideje 10 ms), a képfeldolgozás 10 frame/sec gyakorisággal szolgáltat mérési ered-ményeket (mintavételi ideje 100 ms). Az eltérő mintavételi dőkre az állapotbecslés során tekintettel kell lenni.

14.3.1 Az orientáció és szögsebesség becslése (EKF1) Fogjuk össze az IMU által mért szögsebesség komponenseket a SS K∈ρ vek-torban, amely a korrekt 0,Sρ értéken kívül tartalmazza még az ismeretlen bias értékét és a Gauss-zajt, és transzformáljuk ezeket HK -ba SA -sel (a zaj előjele nem lényeges):

nSbSSS ,,0, ρρρρ ++= (14.7a) nsSbSSSSSS AAAA ,,0, ρρρρ ++= (14.7b) nsSbSSSSSS AAAA ,,0,: ρρρρρ +−== (14.7c)

Itt HK∈ρ a szögsebesség vektor, amelynek szokásos alakja a jármű-navigációban TRQP ),,(=ρ , amelyből az Euler-szögek deriváltjai kiszámíthatók, felhasználván hogy

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−==−

ΘΦΘΦ

ΦΦ

ΦΘΦΘ

ΘΦΓCCCS

SCCTST

F//0

01

),(:1 (14.8)


nbSbS

nSSSSbSS FAFAFAF

,,,

,,),(

ρρ

ρρρρΘΦΨΘΦ

=

++−===⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

&

&

&

&

(14.9a)

nm⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛+

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛=

⎟⎟⎟

⎠

⎞

⎜⎜⎜

⎝

⎛

ΨΘΦ

ΨΘΦ

ΨΘΦ

(14.9b)

Itt Sρ mérését az IMU, T

m),,( ΨΘΦ mérését pedig a marker-bázisú képfeldol-gozó rendszer végzi.

Jelölje T az IMU mintavételi idejét, akkor a folytonosidejű nemlineáris állapot-egyenlet Euler-approximációval diszkrétidejű nemlineáris alakra hozható:

),(),,(

),(,,,

,),(,,),,(

1

,1

,2,21,2

,1,2,11,1

21,,2,1

21,21

kkk

kkkk

kkk

kkk

kSkkkSkkkSkkkk

TTTnbSnSS

TTTbS

T

zxgywuxfx

zxyTwxx

wATFuATFxATFxx

wwwwwu

xxxxx

==

+=

+=

++−=

====

===

+

+

+

ρρρ

ρΨΘΦ

(14.10)

Tegyük fel, hogy w és z korrelálatlanok, és vezessük be a következő jelölése-

ket:

Tkzkzkzkz

kkz

kk

Tkwkwkwkw

kkkw

kkk

Tkkkz

Tkkkw

CRCRz

xgC

xxg

C

BRBRwuxf

Bxuxf

A

zzERwwER

,,,,,

1,1,1,1,11

1,11

1

,,

:,)0,(

,)0,(

:,)0,,ˆ(

,)0,,ˆ(

][],[

=∂

∂=

∂∂

=

=∂

∂=

∂∂

=

==

−−−−−−

−−−

−

(14.11)

176 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA A kiterjesztett Kalman-szűrőnek (EKF1) a következő számításokat kell elvégeznie: 1. Prediction:

Tkwkwkw

Tkkkk

kkk

BRBAAM

uxfx

1,1,1,111

11 )0,,ˆ(

−−−−−−

−−

+=

=

Σ

2. Time update:

( )Tkkkkk

kkkkk

kTkkk

Tkzkzkz

Tkkkk

GSGM

xgyGxxSCMG

CRCCMCS

−=

−+=

=

+=−

Σ

)0,(ˆ

1

,,,

Mivel az IMU mintavételi ideje 10 ms, míg a képfeldolgozásé 100 ms, ezért ha nem érkezett orientáció adat, akkor a time update módosul: 0:ˆ += kk xx , miközben a többi számítás elvégrésre kerül.

14.3.2 A sebesség és pozíció becslése (EKF2) Az IMU 3D gyorsulás mérési eredményei a következő alakban modellezhetők:

gAAaaaa SnSbSSS11

,,0, ),,( −−−++= ΨΘΦ (14.12a)

( ))(10, SSSS ppaAa ××+×+= − ωωε (14.12b)

gAppaAa SSSSm1),,()(: −+××−×−= ΨΘΦωωε (14.12c)

ahol ε a szöggyorsulás (nem mért). Jelölje HKv∈ a helikopter sebességét, akkor a differenciálás mozgó koordináta-

rendszerbeli alakjának felhasználásával írható, hogy

np

nbSbS

nSSmbSS

vvApaa

aAaaAvv

,

,,,

,,

),,( +=

=

++−×−=

ΨΘΦ

ω

&

&

&

(14.13a)

nm ppp += (14.13b)

A folytonosidejű nemlineáris modell könnyen átírható diszkrétidejűvé az Euler-approximáció felhasználásával, és a következő alakot veszi fel:


),(),,(

)(

,

),,(,,),,(

1

,,

,,1

,,,,,1,,

,,,,,,,1

,,,,,

kkk

kkkk

knkkm

knpkkkk

knbSkbSkbS

knSkSkmkbSkSkkkk

nm

TTnp

TnbS

TnSm

TTTbS

T

zxgywuxfx

ppp

TvvTAppTaaa

aTAaaAvTvv

pzpy

vaawaupavx

==

+=

++=

+=

++−×−+=

==

===

+

+

+

+ ω

(14.14)

Ezután már a második kiterjesztett Kalman-szűrő (EKF2) egyszerűen képezhető

az EKF1 mintájára. A ma számításához szükséges ε szöggyorsulás numerikus de-riválással képezhető ω érétékéből a bais levonása után, a többi jel közül az IMU méri az Sa gyorsulást és a képfeldolgozó rendszer a mp pozíciót. Az eltérő mérési mintavételi időket a time update során lehet figyelembe venni, hasonlóan EKF1-hez.

14.1. ábra. A kétszintű Kalman-szűrő struktúrája

A kétszintű Kalman-szűrő struktúráját a 14.1. ábra mutatja be. Az ábrán a ξ pozíciót és az η orientácót a marker-bázisú képfeldolgozó rendszer méri, az ω szögsebességet és a ξ&& gyorsulást az IMU méri. Az indexek közül m (measured) a jelek mért értékére, e (estimated) pedig a becsült értékére utal. Megjegyezzük, hogy EKF2 felhasználja az EKF1 eredményeit, ezért EKF2 az EKF1 után indítható.

178 Lantos: MOBILIS ROBOTOK NAVIGÁCIÓJA Ezen kívül fontos rendszertechnikai szempont, hogy az EKF-en alapuló állapot-

becslés stabilitását az elmélet általában nem garantálja. Problémák esetén a T min-tavételi idő csökkentése javasolható, amely az Euler-approximáció pontosságát is javítja. A négyrotoros helikopter (UAV) beágyazott irányítása során a kétszintű ki-terjesztett Kalman-szűrővel stabilitási problémák nem léptek fel.

Az állapotbecslés az irányítási algoritmus része. Az irányítási törvény ún. visszalépéses (backstepping) stabilizálási technikát használ (lásd pl. Lantos: Irányí-tási rendszerek elmélete és tervezése II. Akadémiai Kiadó, 2003).

Az állapotbecslést és az irányítási törvényt Freescale MPC 555 processzor reali-

zálja valós időben. A beágyazott szabályozó MATLAB/Simulink környezetben fej-leszthető és a MATLAB, Simulink, Real Time Workshop, MPC555 Target Compiler segítségével kifordítható a target processzorra (gyors prototípus tervezés).

A beágyazott szabályozó repülés előtti tesztelése hasonló technikával végezhető a dSPACE DS1103 eszköz bevonásával, ahol a DS1103 szimulálja a helikopter di-namikát, a beavatkozó szerveket és a szenzor rendszert (hardware-in-the-loop test).

A két komponens között az adatforgalom a valóságnak megfelelően CAN buszon keresztül történhet a tesztelés során.

mobilisrobotoknavigacioja.pdf

Documents