Kalibracija inercijskih merilnih enot brez dodatne merilne
opreme
Jernej Puc, Janez Podobnik, Marko Munih
Laboratorij za robotiko Fakulteta za elektrotehniko, Univerza v Ljubljani Tržaška 25, 1000 Ljubljana E-posta: jp4745@student.uni-lj.si, {janezp, marko} @robo.fe.uni-lj.si
Abstract
This paper covers a full calibration process of all sensors incorporated in an inertial measurement unit (IMU): the gyroscope, accelerometer and magnetometer. Sensor characteristics are discussed and different methods presented with respect to the specifics of each sensor. The presented methods assume no aid of additional measuring equipment and inevitably differ in complexity; particular focus is given to the scalar calibration of the magnetometer, as it is the most complex. Method effectiveness is compared to two approaches from literature.
1 Uvod
Inercijske merilne enote (ang. inertial measurement unit - IMU) predstavljajo pomemben merilni sistem na področju sledenja gibanja, kjer se na podlagi meritev različnih količin, kot so kotna hitrost, pospešek in gostota magnetnega polja, ter metodami za senzorno fuzijo določa orientacijo objekta glede na globalni koordinatni sistem. Navadno vsebujejo triosni ziroskop in pospeškometer ter pogosto magnetometer. Poleg širše uporabnosti je razvoj majhnih in lahkih naprav (ang. microelectromechanical systems - MEMS) omogočil različne aplikacije v povezavi z določanjem gibanja pri človeku in stroju [1, 2].
Tovrstni problemi, kot je npr. določanje orientačije z integriranjem kotne hitrosti, so občutljivi na nekompen-zirane napake, zato je kalibračija senzorjev ključnega pomena; kot bo vidno pri rezultatih, imajo lahko meritve z nekalibriranimi senzorji velika odstopanja, ki lahko vodijo do napačnega končnega rezultata.
Kalibračija IMU naprav pojmuje določitev parametrov, s katerimi poskušamo zmanjšati vpliv sistemskih pogreškov vseh senzorjev, ki napravo sestavljajo (ziroskop, pospeškometer, magnetometer). Želimo jo izvesti v do-glednem čšasu na načšin, ki je preprost in v praktičšnem smislu ponovljiv. Potreba je torej po kalibračiji brez dodatne merilne opreme, kjer se zahteva zadostno zanesljivost uporabljenega analitičšnega pristopa. Žaradi pomembnih razlik med značšilnostmi senzorjev se do kalibračije posameznega senzorja v splošnem pristopa različno.
Cilj prispevka je podati praktično osnovo za samostojno kalibračijo IMU naprav v čeloti. Predstavljenim modelom senzornih napak sledijo opisi pristopov kali-bračije in ovrednotenje učinkovitosti glede na primerljive metode iz literature.
2 Metodologija
Slika 1 prikazuje primer IMU naprave uporabljene v tem prispevku. Osi lokalnega koordinatnega sistema so skladne z osmi posameznega senzorja in prikazane glede na zaščitno ohišje.
Slika 1: Realen IMU s pripadajočimi osmi.
V idealnem primeru so izmerjeni odzivi ziroskopa (wm), pospeškometra (am) in magnetometra (hm) odvisni le od merjenih količin (kotna hitrost pospešek a in gostota magnetnega polja, v literaturi pojmovana kot magnetno polje h), kjer vsi simboli predstavljajo tridimenzionalne vektorje. Zaradi raznih lastnosti uporabljenih materialov in neizogibnih napak pri izdelavi navedena razmerja odstopajo, kar se lahko glede na vsak senzor ponazori z ustreznim matematičšnim modelom.
2.1 Viri senzornih napak
Poleg visokofrekvenčnega šuma, ki se pojavlja pri vseh senzorjih, so glavni viri napak še skalirni faktor (2.1.1), neortogonalnost (2.1.2) in ničelni odmik (2.1.3).
2.1.1 Skalirni faktor
V splošnem občutljivosti senzorjev niso enake za vse osi. Za skaliranje po oseh zato namesto navadne sorazmerne konstante uvedemo diagonalno matriko S s skalirnimi ko-efičienti K:
S = diag(Kx, Ky, Kz)
(1)
ERK'2018, Portorož, 163-166
163
V primeru magnetometra vpliv prisotnih feromagne-tnih materialov popaci zunanje magnetno polje, kar se odraža v odzivu senzorja (ang. soft iron error) [3]. Sh je tako polna kvadratna matrika velikosti 3 x 3.
2.1.2 Neortogonalnost
Odstopanja v poravnavi osi senzorjev z lokalnimi osmi enote privedejo do neortogonalnosti; prava vrednost za eno os tako ni odvisna le od odziva tiste osi, temvec vsebuje tudi manjšinska prispevka preostalih. Vsak stolpec v matriki neortogonalnosti N je vektor treh vrednosti, ki za pripadajočo os določa opisano razmerje glede na lokalni koordinatni sistem:
N = [nx
z ]
(2)
h„
Sw Nw u + bw + eu
S0N0 a + b0 + ea
ShNh h + bh + eh
(3)
(4)
Matriki S in N zdruzimo v transformacijsko matriko A-1 in zanemarimo šum e. S preoblikovanjem enacb (3), (4) in (5) izpostavimo priblizke pravih vrednosti, ki predstavljajo kalibrirani senzorni odziv:
U3 ■
Aw (um - bw)
Aa (am - ba)
(6)
(7)
h = Ah (hm - bh)	(8)
Problem kalibracije tako privede do iskanja ustreznih transformacijskih matrik in nicelnih odmikov za posamezni senzor na enoti.
2.3 Kalibracija Ziroskopa
Za zanesljivo dolocitev iskane transformacijske matrike Aw bi potrebovali stabilen vir referencne kotne hitrosti, kar pomeni uporabo drage merilne opreme in daljših me-ritvenih postopkov. Zavoljo preprostosti smo primorani privzeti idealno sorazmerje, ponazorjeno s konstanto K. Preostane nam povprecenje meritev v mirovanju za kompenzacijo nicšelnega odmika:
2.4 Kalibracija pospeškometra
Pri dolocšanju odziva na pospesšek imamo zanesljivo referenco vedno na voljo; gravitacijsko polje Zemlje je stabilno in za omejeno geografsko območje prakticno nespremenljivo. Navadno je vektor polja g normaliziran in definiran z enoto g = 9.80665m:
g = [0 0 1]T g (10)
Glede na definicijo (10) so prave izhodne vrednosti v lokalnem sistemu pri vsaki orientaciji enote dolocljive s preprostimi trigonometricnimi zvezami. Za kalibracijo obicajno zadostujejo meritve v vseh 6 medsebojno pravokotnih polozajih; tako je pricakovani odziv ene osi vedno ±1, preostalih dveh pa 0. Pri tem je kljucno mirovanje za vsako os, sicer se v meritve vnašajo motnje translacij-skega pospesška.
V nadaljevanju izhajamo iz enacbe (7) in jo s substitucijo ba = Aa ba preoblikujemo v linearno vsoto, da za pricakovano vrednost n-tega vzorca an velja:
2.1.3 Ničelni odmik
Nicšelni odziv zaradi specificšne narave senzorja izkazuje odmik od nicšelne vrednosti, npr. zaradi vpliva magneti-ziranih feromagnetnih materialov (ang. hard iron error). Nicelni odmik predstavimo z vektorjem b, ki naceloma ni povsem konstanten; poleg nakljucšnega nihanja ima zlasti odmik zširoskopa znatno temperaturno odvisnost, ki ni predstavljena v opisanem modelu, zaradi katere prihaja do casovnega lezenja.
2.2 Modeli senzornih odzivov
Glede na opisano so odzivi za posamezni senzor predstavljeni sledece:
Aa am,n ba
(11)
To nam omogoča, da sestavimo matrični sistem oblike X/ = y:
-1 -1
-1
Aa
b*T
(5) ^
(12)
Sistem je predolocen in zato nima enolicne rešitve. Rešimo ga po linearni metodi najmanjših kvadratov, tako da ga preoblikujemo na minimizacijski problem, kjer za
A T1
išcemo priblizek /3:
b
/3 = argmm||X0 - y^2
(13)
Numerično stabilne rešitve problema najmanjših kvadratov vključujejo ortogonalne razcepe. Rešitev poiščemo npr. s singularnim razčepom (ang. singular value decomposition - SVD) [4]. Iz enačb (11), (12) in rešitve /, podane po vrstičah in stolpčih, sledi:
Aa — Pl:3,1:3
ba
A- /3T3!4
(14)
(15)
b,
(9)
2.5 Kalibracija magnetometra
Z nadzorovano referenco homogenega magnetnega polja bi zveze med primerjanimi vektorji lahko zopet iskali neposredno po metodi najmanjših kvadratov, a se brez merilne opreme zanasšamo na Zemljino magnetno polje. Slednje je zaradi relativne šibkosti (red 10-5 T) zlahka preglasšeno z elektromagnetnimi motnjami. Poleg tega ima horizontalno in vertikalno komponento, kar dodatno otezuje pristop. Pri kalibraciji tako izhajamo iz privzete konstantne magnitude Zemljinega magnetnega polja Hmag in stremimo k njeni ohranitvi, zato gre v tem smislu za skalarno kalibracijo.
n
n
y
n

a
a
a
xi
a
a
a
x
y
z
a
a
a
x
z


U
m
a
m
69
Idealno je norma oz. skupna velikost senzornega odziva h ne glede na orientacijo magnetometra konstantna:
hh = -M-1 n
(22)
= konst. = Hm
mag
(16)
Posledično bi morali vzorci magnetometra, vzeti v različnih orientacijah, opisovati sfero s polmerom, ki je enak podani magnitudi (cca. 47, 95 p,T), in središčem v lokalnem koordinatnem izhodišču.
ax + by + cz + 2fyz + 2gxz + 2hxy + 2px + 2qy + 2rz + d = 0
(17)
Koeficiente poiščemo po splošnem algoritmu za pri-leganje vzorcev na elipsoid (Li [5]). Ta se prevede na resševanje matricšnih sistemov po metodi najmanjsših kvadratov; rezultat končnega sistema sta lastna vektorja vi in v2, ki skupaj tvorita vektor koeficientov kvadrične enačbe v, na podlagi katerih je elipsoid v celoti določen:
v = [a b c f g h p q r d]T
(18)
Preoblikovanje izraza (17) v matrični zapis s substitucijami hx = x, hy = y in hz = z privede do oblike:
hT Mh + hT n + d
kjer so h, M in n definirani kot:
0,
	hx		a	h	g		p
h =	hy	, M =	h	b	f	, n =	q
	hz		g	f	c		r
(19)
(20)
Izraz (19) dopolnimo do popolnega kvadrata, da izrazimo središče (iskani odmik bh):
hT Mh + hT n + d = (h - bh)T M (h - bh) - C ,
(21)
kjer velja:
C
bT M-
bh d
(23)
V nadaljevanju določimo skalirne vrednosti, vsebovane v iskani transformacijski matriki A. Velja, da lastni vektorji matrike M (h>) predstavljajo polosi danega elip-soida (ang. principal axis theorem [6]):
(hVfc,e - bh)T M (hAa t c - bh) = A (hVt,o - bh)T (hAa t o - bh) = A ||(hAa,t,o - bh)||2 = C Polosi so tako določene z izrazom (25):
(24)
sa,b,c
(hK,b,c - bh)
C
A
a,b,c
(25)
Slika 2: Sfera glede na koordinatno izhodišče, kot jo opisujejo vzorci magnetometra pred in po kalibraciji.
Kot je razvidno iz slike 2, je v skrajnem primeru oblika sfere popačena, njeno središče pa od izhodišča znatno odstopa, zato je smiselna umestitev na povrsšino elipsoida s premaknjenim središčem. V primeru znanih polosi se lahko te po velikosti izenači in ustrezno skalira, središče elipsoida pa prestavi v lokalno izhodišče, s čimer dobimo pričakovano sfero.
Pri določanju elipsoida izhajamo iz enačbe drugega reda v prostoru treh spremenljivk, ki v splosšnem opisuje različšne kvadričšne povrsšine:
Dolzine polosi izenačimo glede na poljubno polos, npr. sa. Skalirna matrika S je tako:
'1 0
0 ^ 0
Sb
0 0 ^
1
0
0 v^b 00
'y/ATa 0 0 "
0 vAb 0 0 0 VAC
Aa - ^
(26)
Za pravilno skaliranje polosi poravnamo z osmi koordinatnega sistema; spremenimo orientačijo elipsoida in ga po skaliranju vrnemo v prvotno lego. Orientacijo elipsoida določajo orientacije pripadajočih polosi oz. normalizirani lastni vektorji matrike M; slednji tvorijo stolpce rotacijske matrike V, ki opisuje omenjeno orientacijo. Pravilno skaliranje tako dosezemo z zaporedjem operacij VSVT. Za kvadratno matriko M z ortogonalnimi lastnimi vektorji, ki tvorijo stolpce matrike V velja [7]:
M = VAV-
(27)
Z upoštevanjem izrazov (26) in (27) ter lastnosti rotacijskih matrik V-1 = VT lahko zapišemo:
VSVT
VA 2 V
A
1
— M 2 (28)
V zadnjem koraku uposštevamo zvezo (25) in skali-ramo polmer sfere na zeleno dolzino, npr. Hmag ali 1. Poleg izraza (22) sta tako določena oba kalibracijska parametra:
Ah
Hm
mag
VC
M 2
(29)
3 Rezultati
Sledi ovrednotnje metod za kalibracijo pospeškometra in magnetometra na podlagi primerjave z dvema metodama iz literature (Merayo [8], Frosio [9]). Prva temelji na kalibraciji s prileganjem vzorcev na elipsoid, medtem ko je druga iterativna in sloni na Gauss-Newtonovi optimizaciji.
1
S
1
70
3.1 Ovrednotenje kalibracije pospeškometra
Opaznejša odstopanja se pojavljajo pri RMS vrednostih napake (ang. Root-Mean-Square). Vzrok odstopanj leZi v izračunanih ničelnih odmikih, ki RMS vrednostim dodaja izrazito enosmerno komponento; primerjanim metodam pritiče večje odstopanje kot pri navadnem skaliranju, kalibracija po metodi najmanjših kvadratov pa daje boljše rezultate.
Tabela 1: RMS napake odstopanj kalibriranih meritev od pričakovanih vrednosti; skupno in po oseh. LSTSQ označuje opisani postopek po metodi najmanjših kvadratov, Merayo [8] in Frosio [9] pa sta primerjani metodi. Razdelek brez predstavlja originalne rezultate brez kalibračije.
RMS	brez	LSTSQ	Merayo	Frosio
x-os	0.06343	0.00640	0.08447	0.04743
y-os	0.03357	0.00620	0.03284	0.08435
z-os	0.04977	0.02745	0.03312	0.03339
vse osi	0.08734	0.02886	0.09649	0.10237
3.2 Ovrednotenje kalibracije magnetometra
Pri ovrednotenju smo obravnavali magnetometer z velikim ničelnim odmikom kot najslabši moZni primer. Netipično veliki odmiki so se pri iterativni metodi ([9]) izkazali za problematične; konvergenca ni bila nikoli doseZena, zato je metoda na sliki 3 izpuščena.
Uj 0.0
-0.5
2000_4000_6000_SOOO_WC00

Tabela 2: RMS odstopanja norme od pričakovanega radija konstantne dolžine 1 (eh = 1 — ||h||) za vse pristope pred in po predhodnem odštevanju povprečja. Li [5] se nanaša na opisano metodo, Merayo [8] in Frosio [9] sta primerjani metodi, razdelek brez pa se navezuje na originalne rezultate brez kali-bračije.
RMS	brez	Li	Merayo	Frosio
Pred Po	0.62576 0.24736	0.05010 0.04122	0.04220 0.03997	0.04372
0_2000_4000_6000_SOOO_10000
0	2000	4000	6000	8000	10000
n
Slika 3: Napaka norme vzorčev eh = 1 — ||h|| v odvisnosti od n-tega vzorča hn. eLi [5] označuje opisani postopek, eMerayo
[8] primerjano metodo in ebrez originalni rezultat na podlagi nekalibriranih vzorčev.
Pomanjkljivost iterativne primerjane metode seje odpravilo tako, da se je skupini vzorčev predhodno odsštela njihova povprečna vrednost; končni odmik je tako določala vsota povprečja in novo izračunani odmik. Za ustrezno primerjavo je bil enak pristop uporabljen tudi pri drugih metodah.
Iz tabele 2 je razvidno, da je predhodno odštevanje povprečšja povzročšilo splosšno izboljsšanje; napake so pri vseh metodah manjše oz. vsaj manj razpršene. Po izboljšanju opisana metoda le še minimalno odstopa od primerjanih.
4 Zaključek
V prispevku je bil predstavljen hiter in praktičen postopek za čelotno kalibračijo inerčijskih merilnih enot. Podani so bili modeli senzornih napak in glede na spečifične lastnosti vsakega senzorja opisani pristopi za njihovo kalibračijo. Obe metodi, ki sta bili primerjani s pristopoma iz literature, sta dosegli zadovoljive rezultate in predstavljata smiselno alternativo na področju enostavne kalibračije. Od teh se je dokaj neposredna uporaba metode najmanjših kvadratov pri kalibračiji pospeškometra kljub očšitni preprostosti izkazala za posebej natančšno in robustno. Na podlagi rezultatov je bil utemeljen tudi sklep, da z majhnim posegom pred kalibračijo v obliki odštevanja povprečne vrednosti od skupine vzorčev dosezemo vidne izboljsšave.
Literatura
[1]	O. J. Woodman. An introdučtion to inertial navigation. University of Cambridge, Computer Laboratory, Tečhničal Report UCAM-CL-TR-696. Avgust, 2007.
[2]	J. Podobnik, M. Munih. Določanje transformačije med triado optičnih markerjev in inerčijsko merilno enoto. ERK'2014, Portoroz, A:103-106.
[3]	V. Renaudin, M. H. Afzal, G. Lačhapelle. Complete Tri-axis Magnetometer Calibration in the Magnetič Domain. Hindawi Publishing Corporation, Journal of Sensors. Vol. 2010.
[4]	B. Plestenjak. Razširjen uvod v numerične metode. DMFA - zalozništvo (2015), ISBN 978-961-212-264-5.
[5]	Q. Li, J. G. Griffiths. Least Squares Ellipsoid Spečifič Fitting. IEEE Computer Sočiety, Pročeedings of the Geome-trič Modeling and Pročessing 2004.
[6]	J. N. Franklin. Matrix theory. Dover Publičations (2012), seč. 4.6, 95. ISBN 978-048-641-179-8.
[7]	H. Abdi. Eigen-dečomposition: eigenvalues and eigenveč-tors. SAGE Publičations, Enčyčlopedia of Measurement and Statističs (2007), 304-308.
[8]	J. M. G. Merayo, P. Brauer, F. Primdahl, J. R. Petersen, O. V. Nielsen. Sčalar čalibration of večtor magnetometers. IOP Publishing, Meas. Sči. Tečhnol. 11 (2000), 120-132.
[9]	I. Frosio, F. Pedersini, N. A. Borghese. Autočalibration of MEMS Aččelerometers. IEEE Transačtions on Instrumentation and Measurement, Vol. 58, No. 6, June 2009.
71