Billenéskompenzált e-iránytű megvalósítása

Talat Ozyagcilar alkalmazástech. mérnök – Freescale Semiconductor

Hiába szolgálja az iránytű 2000 éve az emberiséget, mégis lehet e téren újat mondani. A Freescale mérnöke a mikro-elektromechanikai szenzorokon, 3·tengelyű gyorsulás- és mágnestér-érzékelőkön alapuló elektronikus navigációs segédeszköz felépítésének matematikai részleteit ismerteti cikkében, amelynek közlését a Freescale termékeit is forgalmazó Farnell tette lehetővé számunkra.

Bevezetés

Ez a cikk azoknak a mérnököknek kíván matematikai eljárások bemutatásával útmutatást adni, akik a térbeli helyzetétől független, „billenéskompenzált”, elektronikus iránytű (eCompass) tervezésével foglalkoznak. Az eCompass – egyaránt háromtengelyű – gyorsulásmérő és mágnestér-érzékelő segítségével oldja meg a feladatot. A gyorsulásmérő a föld gravitációs terének irányát (a nehézségi gyorsulásvektor irányát), a mágnestér-érzékelő pedig a Föld mágneses terének irányát méri a készülékhez rögzített, háromtengelyű koordináta-rendszerben. Ennek következtében mindkét fizikai mennyiség vektorának komponensei változnak attól függően, hogy a készülék milyen térbeli helyzetet foglal el. Ha a hordozólemez vízszintesen fekszik, az északi irányt[1] a mágneses térvektor két vízszintes komponense által bezárt szög arcus tangenséből lehet kiszámítani. Mivel azonban a hétköznapi használat során a két érzékelőt hordozó NyÁK-lap a térben tetszőleges helyzetet foglalhat el, az irány meghatározásához a mágneses és a gravitációs érzékelő három-három vektorkomponensének értékét kell felhasználni. A vízszintes síktól való eltérésre érzéketlen, „billenéskompenzált” elektronikus iránytű algoritmusával valójában nemcsak az iránytű irányszöge, hanem a térbeli helyzetét meghatározó három szögadat (a készülék önkényesen kiválasztott fő irányának az x, y és z tengelyével bezárt szöge) is meghatározható.

Az eCompass pontossága nagymértékben függ attól, hogy képesek vagyunk-e szoftveres úton számításba venni a zavaró szórt mágneses terek hatását a NyÁK-lapra épített mágneses szenzor belsejében és környékén. Szokás ezeket két részre osztani: egyikük a szenzorhoz képest állandó elhelyezkedésű, külső mágneses tér – amit gyakran „keményvas” (hard iron) effektusnak neveznek, megkülönböztetésül a másik összetevőtől, a geomágneses tér által keltett „lágyvas” (soft iron) hatástól, ami a geomágneses hatáson kívüli, lokális, nem az érzékelőhöz kötött mágneses hatás. A geo­mág­neses iránymeghatározás ofszet­hibájának „nullázása” a kemény­vashatások figyelembevétele a kalibráció során.

E cikkben egy egyszerű, háromelemű modellt mutatunk be a keményvaseffektusok kiküszö­bölésére. Ez sokféle szituációban általában elegendő. Csak hogy legyen mihez kötni elvont modellünket, a cikk hátralevő részében tételezzük fel, hogy az eCompass-funkciót egy mobiltelefonba építve valósítjuk meg.

A koordináta-rendszer és a készülék tokozatának illesztése

Ebben a cikkben a szakmában legelterjedtebben használt NED-koordináta-rendszert (North, East, Down – Észak, Kelet, Lefelé) használjuk, amelyben a készülék három tengelye úgy van tájolva, hogy az x tengely a készülék leghosszabb tengelyvonalával párhuzamos, és a pozitív irány a „szöveges képernyő” legfelső, látható sora felé mutat. Az y tengely a készülék síkjában erre merőlegesen, jobb felé, végül a z-tengely mindkét előbbi koordinátatengelyre merőlegesen, a készülék hátlapja felőli oldal felé („lefelé”) mutat. Ez az 1. ábrán is látható. A készülék térbeli helyzetét jellemző szögeket úgy értelmezzük, hogy a ψ elfordulási szög (yaw) a pozitív z-tengely körül, az óramutató járásával megegyező irányban növekszik. A pozitív φ „billenési” szög (roll) az x, a pozitív „lehajlási” ∂ szög (pitch) pedig az y-tengely körüli, óramutató irányú elfordulást jelenti[2].

Alapvető fontosságú, hogy a gyorsulásmérő és a magnetométer kimeneteit a telefon koordináta-rendszerével összhangban azonosítsuk. A két szenzor különböző NyÁK-terveken különbözőképpen lehet elrendezve, sőt, elméletileg akár még az is előfordulhat, hogy egyforma, szerelt NyÁK-lapokat különböző termékekbe eltérő orientációval szerelnek bele. Mielőtt a készülék tokjának elfordulásait és ezeknek a jelekkel való azonosítását véglegesítenénk a szoftverben, egy végső ellenőrzésre van szükség oly módon, hogy ellenőrizzük a NyÁK-ra szerelt gyorsulásmérő és magnetométer „nyers” kimenő adatait. Ez a következőképpen történhet:

1. Fektessük a NyÁK-lapot az asztalra. A gyorsulásmérő z-tengely kimenetének +1g gyorsulást kell mutatnia, miközben az x- és y-tengelyek kimenete elhanyagolhatóan alacsony érték. Fordítsuk meg a NyÁK-lapot „arccal lefelé”, és ellenőrizzük, hogy a z-tengely kimenet most -1g értéket mutat-e. Ismételjük meg ugyanezt a kísérletet „élére állított” NyÁK-lappal úgy, hogy először az x-, majd az y-tengely mutasson lefelé, és ellenőrizzük, hogy az épp lefelé mutató gyorsulásmérő-tengely kimenete ad +1g, majd megfordítva -1g értéket.

2. A geomágneses tér vízszintes komponense mindig[3] az északi mágneses pólus felé mutat. A tér vektora  a Föld északi félgömbjén a földrajzi helytől függő pontos szögértékkel lefelé hajlik el. Ha az x-tengely észak felé és lefelé mutat, meg kell tudni találni a mágneses tér x-kom­ponensének maximális értékét. A NyÁK-lapot ellenkező irányba fordítva az x-komponensnek minimumot kell mutatnia. Ismételjük meg ugyanezt a kísérletet úgy, hogy az y- és z-tengellyel is megkeressük a földi mágneses tér maximumához, majd – megfordítva – a minimumához tartozó irányt.

A gyorsulásmérő és a magnetométer kimenete mint a telefon irányának függvénye

A telefon bármely irányú térbeli elhelyezkedését leírhatjuk a yaw-, a pitch- és a roll-tengelyek körüli elfordulással, amelynek kiinduló helyzete az, hogy a telefon vízszintes síkon fekszik, és fő tengelyvonala a mágneses észak felé mutat. A gyorsulásmérő G_r és a magnetométer B_r kimeneti adatait ebben a kiinduló pozícióban a következő háromdimenziós térbeli vektorokkal lehet megadni (lásd a 2. ábrát is):

A nehézségi gyorsulásvektor abszolút értéke g = 9,81 ms^-2. B a geomágneses indukció abszolút értéke, amely a földrajzi helyzettől függően változik (a minimum, 22 μT Dél-Amerikában, a maximum, 67 μT pedig Ausztrália déli részén mérhető). δ az a szög, amivel a geomágneses tér vektora elhajlik a vízszintestől. Ez a szögérték -90°a déli mágneses póluson, 0° az Egyenlítőn és +90° az északi mágneses póluson. A mágneses tér irányának és nagyságának a Föld felszínén mérhető eloszlásáról a World Data Center for Geomagnetism honlapján (http://wdc.kugi.kyoto-u.ac.jp/igrf/) részletes geomágneses térképek találhatók. A geo­mágneses tér abszolút értékének és lehajlási szögének részletes ismeretére azonban nincs szükség ahhoz, hogy az eCompass szoftverét működőképessé tehessük. A 20., 21. és 22. egyenletekből látjuk majd, hogy a B mágneses indukció és a δ lehajlási szög kiesik a számításokból.

A telefon gyorsulásmérőjének G_p és magnetométerének B_p mért értékei háromirányú – ψ, ∂ és φ – elforgatás után a 3. és 4. egyenlet szerint számíthatók:

A 3. és 4. egyenletben található R elforgatási mátrixok rendre a következők (5., 6., 7. egyenlet):

A 3. egyenlet feltételezi, hogy a telefon nincs kitéve semmiféle egyenesvonalú gyorsulás hatásának, azaz a gyorsulásmérő G_p kimeneti jele kizárólag a nehézségi gyorsulástól és a telefon térbeli elhelyezkedésétől függ. Fontos hangsúlyozni, hogy a billenés­kompenzált eCompass lineáris gyorsulás esetén téves mérési eredményt szolgáltat.

A 4. egyenlet a keményvas- és lágyvashatások kivételével minden szórt mágneses teret figyelmen kívül hagy. A keményvas­hatások figyelembe vételének szokásos módja egy additív V mágneses vektor, amely együtt forog a telefonnal, következésképpen független a telefon térbeli elhelyezkedésétől. Mivel a magnetometer bármilyen eredetű ofszetje független a telefon térbeli helyzetétől, ezt egyszerűen hozzá kell adni a NyÁK-lap keményvas­kom­ponenséhez, vele együtt kalibrálható és szüntethető meg a hatása. A 4. egyenlet módosított változata eszerint a 8. egyenletben található:

amelyben V_x, V_y és V_z a keményvasvektor komponenseit jelenti. Fontos megjegyezni, hogy a 8. egyenlet a lágyvashatásokat nem modellezi.

A billenéskompenzáló algoritmus

A billenéskompenzált eCompass-algoritmus először kiszámítja a φ (roll) és a ∂·(pitch) szögeket a gyorsulásmérő kimeneti adataiból. Ez úgy történik, hogy a 3. egyenletet megszorozzuk az inverz roll és pitch forgatási mátrixokkal, amely a 9. egyenletet adja:

ahol a vektor a gyorsulásmérő által mért nehézségi gyorsulás három vektorkomponensét tartalmazza. A 9. egyenlet kifejtése után a 10., illetve a mátrixszorzás elvégzését követően a 11. egyen­let adódik:

A 11. egyenlet y-komponenséből a φ (roll) szög a 12. és a 13. egyenletek szerint számítható:

A 11. egyenlet x-komponenséből pedig a ∂ (pitch) számítható (14. és 15. egyenlet):

A gyorsulásmérő adataiból számított φ és ∂ szögek ismeretében a magnetometer kimeneti adatait a 4., 7. és 8. egyenlet felhasználásával vissza lehet forgatni (16. egyenlet):

Részletezve:

Az egyenlet jobb oldala a mátrixszorzás után

alakú, amelyből végül a 19. egyenletrendszert kapjuk:

A B_fx, B_fy, B_fz vektor a magnetométer szenzorkomponenseit mutatja a keményvasofszet kompenzációja és a vízszintes síkba való beforgatás után. A 17. és a 19. egyenletek a B_fx és a B_fy komponensek a következők :

illetve

amiből behelyettesítés után a ψ szöre a

kifejezést kapjuk.

A 22. egyenlet megoldást ad a ψ (yaw) szögre, amely a mágneses északi iránnyal bezárt szöget jelenti. A ψ (yaw) szög tehát az eCompass billenéskompenzált irányszöge, amit kerestünk. Mivel a 13., 15. és 22. egyenlet végtelen számú megoldást ad egymástól 180° egész számú többszörösei által meghatározott helyeken, szokásos megállapodás a roll-, pitch- és yaw-szögeket a -180°…+180°-os tartományra korlátozni. További korlátozást jelent, hogy a pitch-szöget a -90°…+90°tartományban értelmezzük. Ezzel elérhető, hogy a telefon bármilyen térbeli elhelyezkedését egyértelműen meg lehessen adni a három szögadattal. Ebből (a C# nyelvi konvenciója szerint) az következik, hogy a 13. és 22. egyenlet kiszámításánál az ATAN2-függvényt kell használnunk, amelynek kimeneti értéktartománya -180°…+180°. A 15. egyenletet viszont a -90°…+90°közötti értékeket szolgáltató ATAN-függvénnyel kell kiszámítanunk.

A V keményvas-ofszetvektor becslése

A 22. egyenlet feltételezi a V keményvas-ofszetvektor ismeretét, egy állandó mágneses ofszetet, amelyet a szenzor valódi mágnestér-érzékelőjének kimenetéhez kell hozzáadni. A keményvasofszet a magnetométer nulla mágneses térhez tartozó, nullától különböző, állandó kimeneti jele, amihez hozzá kell adni azt a mágneses eredetű ofszetet, amelyet a NyÁK-laphoz rögzítve elhelyezett, „felmágnesezett” ferromágneses tulajdonságú anyag összhatása okoz. Teljesen megszokott, hogy a keményvasofszet jelentősen meghaladja a „hasznos jel”, a geomágneses tér hatását. Emiatt tehát a keményvasofszet becslését és annak a hasznos jelből történő kivonását nagyon pontosan kell elvégeznünk, hogy elkerüljük a 22. egyenlet eredményének durva torzítását, amely az iránytű szögeinek nagyon szűk (és a geomágneses északi irány­nyal kevéssé összefüggő – a szerk. megj.) mozgástartományba való rögzülését eredményezi. Általános gyakorlat, hogy a magnetométer szenzorokat a nulla mágneses térhez tartozó ofszet kalibrációja nélkül hozzák forgalomba, mivel a szabványos keményvasbecslési algoritmus a nulla mágneses térhez tartozó ofszet és a NyÁK-lapra szerelt alkatrészek eredő keményvaseffektusának összességét számítja ki.

Bármiféle keményvashatás hiányában a magnetométer kimeneteiből számítható vektor csúcsa a telefon bármilyen orientációjánál egy gömb felületén mozog a B_px, B_py és B_pz komponensek háromdimenziós terében. A gömb sugara egyenlő a geomágneses indukció B abszolút értékével. Keményvashatás jelenlétében a mágneses mérések vektora által meghatározott gömbfelület egyszerűen eltolódik a V keményvasvektor értékével úgy, hogy a gömb középpontja a V_x, V_y és V_z keményvasofszet által meghatározott térbeli pontba helyeződik át. A keményvasofszetet eszerint úgy lehet egyszerűen kiszámítani, hogy figyeljük a magnetométer által mért indukcióérték x-, y- és z-irányú komponensének maximum- és minimumértékeit, és a keményvasofszet vektor komponenseit a 23. egyenlettel határozzuk meg:

A mágnestér-érzékelő minimum- és maximum-eredményeit két módon lehet meghatározni:

1. a keményvasofszet meghatározásához szükséges méréseknek, számításoknak és adattárolásnak a gyári kalibráció során történő elvégzésével, valamint

2. a telefon használata közben a véletlenszerű térbeli elhelyezkedés különféle irányaiban végzett mérések kiértékelésével és ennek alapján a telefon folyamatos önkalibrációjával.

Szoftvermegvalósítás

A referencia C#-nyelvű program kizárólag egész típusú ope­randusokat használ, és nem hív meg semmiféle külső könyvtárban megvalósított matematikai algoritmust. Az összes trigonometriai és numerikus számítás elvégzésére saját tervezésű programok szolgálnak. A teljes dokumentáció a Freescale Semiconductor AN4248 jelű alkalmazástechnikai jegyzetében található, amely a Farnell element14 mérnöki közösségi portálján is fellelhető az alábbi címen: 

www.element14.com/freescale