• Nem Talált Eredményt

DepartmentofControlEngineeringandInformationTechnologyBudapestUniversityofTechnologyandEconomics Prof.Dr.B´elaLantos Supervisor: L´aszl´oKis Ph.D.´ertekez´es Auton´omj´arm˝uveknavig´aci´oja´esir´any´ıt´asakorszer˝u´erz´ekel˝okf´uzi´oj´aval Ph.D.Thesis Nav

N/A
N/A
Protected

Academic year: 2023

Ossza meg "DepartmentofControlEngineeringandInformationTechnologyBudapestUniversityofTechnologyandEconomics Prof.Dr.B´elaLantos Supervisor: L´aszl´oKis Ph.D.´ertekez´es Auton´omj´arm˝uveknavig´aci´oja´esir´any´ıt´asakorszer˝u´erz´ekel˝okf´uzi´oj´aval Ph.D.Thesis Nav"

Copied!
146
0
0

Teljes szövegt

(1)

Budapest University of Technology and Economics Department of Control Engineering and Information Technology

Budapest, Hungary

Navigation and control of autonomous vehicles using

fusion of modern sensors

Ph.D. Thesis

Auton´ om j´ arm˝ uvek navig´ aci´ oja

´ es ir´ any´ ıt´ asa

korszer˝ u ´ erz´ ekel˝ ok f´ uzi´ oj´ aval

Ph. D. ´ ertekez´ es

L´ aszl´ o Kis

Supervisor:

Prof. Dr. B´ ela Lantos

Department of Control Engineering and Information Technology Budapest University of Technology and Economics

May 7, 2013

(2)

ii

(3)

iii

Nyilatkozat ¨ on´ all´ o munk´ ar´ ol, hivatkoz´ asok ´ atv´ etel´ er˝ ol

Alul´ırott Kis L´aszl´o kijelentem, hogy doktori ´ertekez´esem magam k´esz´ıtettem ´es abban csak a megadott forr´asokat haszn´altam fel. Minden olyan r´eszt, amelyet sz´o szerint, vagy azonos tartalomban, de ´atfogalmazva m´as forr´asb´ol ´atvettem, egy´ertelm˝uen, a forr´as megad´as´aval megjel¨oltem.

Budapest, 2013. m´ajus 7.

Kis L´aszl´o al´a´ır´as

Nyilatkozat nyilv´ anoss´ agra hozatalr´ ol

Alul´ırott Kis L´aszl´o hozz´aj´arulok a doktori ´ertekez´esem interneten t¨ort´en˝o nyilv´anoss´agra hozatal´ahoz az al´abbi form´aban:

ˆ korl´atoz´as n´elk¨ul

ˆ el´erhet˝os´eg csak magyarorsz´agi c´ımr˝ol

ˆ el´erhet˝os´eg a fokozat oda´ıt´el´es´et k¨ovet˝oen 2 ´ev m´ulva, korl´atoz´as n´elk¨ul

ˆ el´erhet˝os´eg a fokozat oda´ıt´el´es´et k¨ovet˝oen 2 ´ev m´ulva, csak magyarorsz´agi c´ımr˝ol Budapest, 2013. m´ajus 7.

Kis L´aszl´o al´a´ır´as

Nyilatkozat az ´ ertekez´ es beny´ ujt´ as´ ar´ ol

Alul´ırott Kis L´aszl´o kijelentem, hogy a doktori ´ertekez´esem kor´abban m´as int´ezm´enybe nem ny´ujtottam be ´es azt nem utas´ıtott´ak el.

Budapest, 2013. m´ajus 7.

Kis L´aszl´o al´a´ır´as

(4)

iv

K¨ osz¨ onetnyilv´ an´ıt´ as

Ez´uton szeretn´em megk¨osz¨onni konzulensem Dr. Lantos B´ela t´amogat´as´at, aki a doktori tanulm´anyaim ¨ot

´eve alatt mindig rendelkez´esemre ´allt, lelkiismeretesen seg´ıtette a munk´amat ´es ir´anymutat´ast ny´ujtott a kritikus helyzetekben.

K¨osz¨onettel tartozom a BME-IIT intelligens robotok laborat´orium´aban dolgoz´o koll´eg´aimnak. Nagy- ban seg´ıtette a munk´amat, hogy ha elakadtam egy probl´em´aval ˝oket b´armikor felkereshettem ´es sz´ıvesen seg´ıtettek a megold´as megtal´al´as´aban. Ebben a legt¨obb seg´ıts´eget Dr. Proh´aszka Zolt´ant´ol, Drexler D´anielt˝ol, Kov´acs G´abort´ol, Luk´acs Lor´andt´ol, Dr. Kiss B´alintt´ol ´es Dr. Harmati Istv´ant´ol kaptam.

K¨ul¨on k¨osz¨onet azoknak a hallgat´oknak, akik a doktori tanulm´anyaim ideje alatt engem v´alasztottak kon- zulens¨uknek az ¨on´all´o laborat´orium, szakdolgozat illetve diplomatervez´es t´argyak keret´eben. Ezzel motiv´alva engem, hogy a munk´amat min´el magasabb szinten v´egezzem.

A n´egyrotoros helikopter projekt kapcs´an szeretn´ek k¨osz¨onetet mondani Regula Gergelynek a sok seg´ıts´eg´ert, illetve Dr. Soumelidis Alexandrosnak, aki sz´amos ¨otletet adott a helikopter gyakorlati meg- val´os´ıt´as´anak k´erd´eseiben. A helikopter projekt nem j¨ohetett volna l´etre Dr. Proh´aszka Zolt´an n´elk¨ul, akit˝ol a helikopter megval´os´ıt´asa sor´an a legt¨obb seg´ıts´eget kaptam.

A kutat´as szorosan kapcsol´odik az OTKA K71762 (2008-2012) ”Auton´om f¨oldi, l´egi ´es v´ızi robotok korszer˝u ir´any´ıt´aselm´elete ´es mesters´eges intelligencia eszk¨ozei” projekt keret´eben v´egzett kutat´asokhoz, az OTKA K71762 t´amogat´as´ert ez´uton is szeretn´em kifejezni k¨osz¨onetemet.

A munka szakmai tartalma kapcsol´odik a ”Min˝os´egorient´alt, ¨osszehangolt oktat´asi ´es K+F+I strat´egia, valamint m˝uk¨od´esi modell kidolgoz´asa a M˝uegyetemen” c. projekt szakmai c´elkit˝uz´eseinek megval´os´ıt´as´ahoz.

A projekt megval´os´ıt´as´at az ´UMFT T ´AMOP-4.2.1/B-09/1/KMR-2010-0002 programja t´amogatja.

A kutat´as az Eur´opai Uni´os ´es Magyarorsz´ag t´amogat´as´aval a T ´AMOP 4.2.4.A/1-11-1-2012-0001 azo- nos´ıt´o sz´am´u ”Nemzeti Kiv´al´os´ag Program - Hazai hallgat´oi, illetve kutat´oi szem´elyi t´amogat´ast biztos´ıt´o rendszer kidolgoz´asa ´es m˝uk¨odtet´ese orsz´agos program” c´ım˝u kiemelt projekt keretei k¨oz¨ott val´osult meg.

A disszert´aci´ot nem tudtam volna elk´esz´ıteni a sz¨uleim ´es testv´erem kitart´o t¨urelme ´es t´amogat´asa n´elk¨ul.

Szeretn´em megk¨osz¨onni nekik, hogy megteremtett´ek sz´amomra azokat a k¨or¨ulm´enyeket, melyben a kutat´asi munk´amat zavartalanul v´egezhettem. Tov´abbi k¨osz¨onet bar´ataimnak, akik ak´ar csak egy kedves mondattal vagy egy-egy apr´o mosollyal is hozz´aj´arultak a kutat´as sikeres elv´egz´es´ehez.

(5)

v

Osszefoglal´ ¨ o

A j´arm˝unavig´aci´o napjainkban er˝osen kutatott ter¨ulet. Sz´amos olyan k´erd´es mer¨ul fel, melyre m´eg nincs kiforrott megold´as. ´Altal´anoss´agban az is elmondhat´o, hogy az alkalmazott technik´ak nagy t¨obbs´ege csak speci´alis k¨or¨ulm´enyek k¨oz¨ott haszn´alhat´o. Ezek mellett a kih´ıv´asok mellett fontos figyelembe venni, hogy milyen ´erz´ekel˝ok ´allnak rendelkez´esre, mint a navig´aci´os algoritmusok bemenetei. Valamint fontos, hogy a navig´aci´o eredm´enyeit haszn´al´o szab´alyz´asi algoritmusoknak milyen pontoss´agi ig´enyeik vannak.

A kutat´as m¨og¨otti gyakorlati motiv´aci´ot a Budapesti M˝uszaki ´es Gazdas´agtudom´anyi Egyetem Ir´any´ıt´as- technika ´es Informatika Tansz´ek´enek (BME-IIT) belt´eri n´egyrotoros helikopter projektje adta, ahol sz´amos olyan kih´ıv´as akadt, melyre a klasszikus megold´asok nem voltak kiel´eg´ıt˝oek. Ilyen k´erd´esek p´eld´aul, hogy hogyan lehet kis m´eretben, kis s´uly mellett az alacsony k¨olts´eg˝u elv´ar´asokat betartva megb´ızhat´o ´es pontos

´erz´ekel˝o ´es ´allapotbecsl˝o rendszert ki´ep´ıteni, illetve, hogy ezeknek az eredm´enyeit mik´ent tudj´ak az ir´any´ıt´asi algoritmusok felhaszn´alni.

A kutat´as a szenzorrendszerek vizsg´alat´aval kezd˝od¨ott. Legfontosabb szenzork´ent a fed´elzeten elhelye- zett gyorsul´asm´er˝oket ´es sz¨ogsebess´egm´er˝oket lehetett alkalmazni. Ezek olyan eszk¨oz¨ok, amelyek szinte minden auton´om m˝uk¨od´es˝u ir´any´ıt´asi rendszerben megtal´alhat´oak. A kezdeti vizsg´alatok sor´an kider¨ult, hogy sz¨uks´eg van pontos, nagy megb´ızhat´os´ag´u kalibr´aci´os algoritmusokra, melyek lehet˝os´eget teremtenek a k´es˝obbi ´allapotbecsl´esi algoritmusok fel´ep´ıt´es´ere. A disszert´aci´o els˝o t´eziscsoportja a kalibr´aci´os algo- ritmusokkal kapcsolatos kutat´ast mutatja be. K¨ul¨on m´odszer kapcsol´odik a gyorsul´asm´er˝oh¨oz ´es a sz¨ogse- bess´egm´er˝oh¨oz. A kalibr´aci´os m´odszerek kiterjednek a m´agneses t´erer˝oss´egm´er˝ok kalibr´aci´oj´ara is, melyeknek a k¨ult´eri navig´aci´oban van nagy szerep¨uk.

A k¨ovetkez˝o ´allom´as a kutat´asban a kalibr´alt szenzorokat haszn´al´o ´allapotbecsl´esi algoritmusok kifej- leszt´ese. K´et fontos c´elkit˝uz´es mer¨ult fel az algoritmusokkal kapcsolatban. Az els˝o, hogy a t¨obb forr´asb´ol sz´armaz´o m´er´esi adatokat ´ugy kezelje, hogy azok a m´asik esetleges hib´aj´at jav´ıts´ak. Erre a megold´as a szenzorf´uzi´os m´odszerek haszn´alata. M´asr´eszt olyan megold´asokat c´elszer˝u alkalmazni, melyek nem korl´atoz´odnak egy adott t´ıpus´u j´arm˝ure, ez´ert ´altal´anosan haszn´alhat´ok j´arm˝unavig´aci´os c´elokra. Itt a megold´as a j´arm˝u kinematikus modellj´enek felhaszn´al´asa. Tov´abbi elv´ar´as, hogy a bemutatott m´odszerek ne rendelkezzenek olyan speci´alis esetekben ´erv´enyre jut´o tulajdons´agokkal, melyek bek¨ovetkezte nem v´art probl´em´akat okozna a k´es˝obbi ir´any´ıt´asban.

A n´egyrotoros helikopter projekt kapcs´an felmer¨ult, hogy milyen ir´any´ıt´asi algoritmusok k´epesek megb´ızhat´oan felhaszn´alni a szenzorf´uzi´o eredm´enyeit. Illetve mit lehet tenni abban az esetben, ha va- lamelyik ´erz´ekel˝o menet k¨ozben nem m˝uk¨odik megfelel˝oen. Az ´allapotbecsl´esi, szenzorf´uzi´os ´es ir´any´ıt´asi k´erd´esekkel kapcsolatos eredm´enyeket a m´asodik t´eziscsoport foglalja ¨ossze.

A belt´eri helikopter fejleszt´es´enek kezdetekor t´avlati c´elk´ent felmer¨ult ´es k´es˝obb ¨on´all´o kutat´asi ir´anny´a v´alt, hogy milyen szenzorokat lehetne k¨ult´eri m˝uk¨od´es eset´en haszn´alni. Ebben a t´emak¨orben a GPS vev˝o a leggyakrabban haszn´alt eszk¨oz. Ennek vizsg´alata megmutatta, hogy a hagyom´anyos megold´asok nem el´eg pontosak, ez´ert az un. viv˝of´azis m´er´esen alapul´o differenci´alis GPS technika vizsg´alata v´alt sz¨uks´egess´e. Ez a megk¨ozel´ıt´es elvezet az un. ”integer ambiguity” probl´em´ahoz, melyre ´altal´anos megold´as nem l´etezik. A j´arm˝unavig´aci´oban azonban rendelkez´esre ´allnak m´as szenzorok is, melyek felhaszn´al´as´aval ez a probl´ema hat´ekonyan megoldhat´o. Ez´altal a j´arm˝u orient´aci´oja megb´ızhat´oan megadhat´o ´es a poz´ıci´oja is decim´eter alatti pontoss´aggal m´erhet˝o. A k¨ult´eri navig´aci´oban is rendelkez´esre ´all t¨obbfajta ´erz´ekel˝o. Ezek szen- zorf´uzi´oja ´es a k¨ult´eri j´arm˝u ´allapot´anak becsl´ese is megval´os´ıt´asra ker¨ult. A k¨ult´eri navig´aci´oval kapcsolatos eredm´enyeket a harmadik t´eziscsoport foglalja ¨ossze.

(6)

vi

Abstract

Vehicle navigation is in the focus of researchers nowadays. Plenty of questions emerge which have no reliable answer for every case. In generality it can be said that most of the applied techniques can be used only in restricted environments. In addition to these challenges it is important to consider what kind of sensory systems are accessible as the input of the navigation algorithms. It is also important what kind of requirements have the control algorithms which are using the results of the navigation.

The practical motivation behind the research is the indoor quadrotor project of the Department of Control Engineering and Information Technology of the Budapest University of Technology and Economics (BME- IIT), where plenty of challenges appeared where the classical solutions were not satisfactory. Such question is, for example, how can a sensory and a state estimation system be built which has low size, low weight and can meet with the low-cost approach of the project. Another question is how the control algorithms can use the result of the state estimation.

The research began with the examination of the sensory system. The most important sensors on the board are the accelerometers and the angular velocity sensors. These devices can be found almost in every autonomously operating vehicle. During the first examinations it turned out that a precise and reliable sensor calibration method is required which can be the basis of the further state estimation algorithms.

The first thesis group shows the result of the research according to the sensor calibration methods. There are separate methods for the accelerometer and the angular velocity sensor. The calibration algorithms are extended to the calibration of the magnetometer which plays major rule in the outdoor navigation.

The next step in the research is the development of state estimation algorithms which use the results of the calibration. Two important goals emerged according to the methods. The first is that the measurement data from separate sources have to be handled in such a way that they decrease the others weaknesses. The solution is the usage of sensor fusion algorithms. The second goal is that it is practical to use methods which are not restricted for a given vehicle, therefore they can be used as general vehicle navigation methods.

In this case the solution is the usage of the kinematic model of the vehicle. Another requirement is that the presented methods do not have such properties appearing in special cases which can cause unwanted problems in the further control algorithms.

According to the quadrotor project, it is emerged what kind of control algorithms can reliably use the results of the sensor fusion. Moreover what can be done if one of the sensors does not operate properly during flight? The results in the field of state estimation, sensor fusion and control are summarized in the second thesis group.

The question appeared at the beginning of the development of the indoor helicopter as a perspective objective and later it becomes an individual research direction to answer what kind of sensors can be used for outdoor navigation. In this field the common sensor is the GPS receiver. The first examinations show that the common GPS is not precise enough. Therefore the carrier phase measurement based differential GPS technique was investigated. This approach leads to the integer ambiguity problem, which has no general solution. In vehicle navigation there are additional sensors which can be used to efficiently solve the problem. By this way the orientation of the vehicle can be reliably determined and the position can also be measured with a sub-decimeter precision. There are different types of sensors in outdoor navigation as well. Hence sensor fusion algorithms and the state estimation of the outdoor vehicle is also developed. The results according to the outdoor navigation are summarized by the third thesis group.

(7)

Contents

List of Acronyms xiii

1 Introduction 1

1.1 Overview of this thesis and motivation . . . 1

1.2 Notations . . . 3

1.3 Typical solutions and unsolved problems of inertial navigation . . . 3

1.3.1 Structure of MEMS sensors . . . 4

1.3.2 Typical models and methods . . . 8

1.3.3 Calibration of independent frames . . . 9

1.4 State estimation and sensor fusion of a moving body . . . 10

1.4.1 Necessity of sensor fusion . . . 10

1.4.2 Possible solutions for state estimation . . . 11

1.4.3 The Kalman filter . . . 13

1.4.4 Quadrotor model . . . 16

1.5 Outdoor navigation . . . 17

1.5.1 Introduction to GPS navigation. . . 18

1.5.2 The carrier phase differential GPS concept . . . 20

1.5.3 Known solutions and unresolved problems . . . 24

2 Inertial and magnetic sensor calibration 27 2.1 Calibration concept . . . 27

2.2 Accelerometer calibration. . . 28

2.2.1 Off-line calibration . . . 28

2.2.2 Calibration before startup . . . 35

2.2.3 Covariance matrix determination . . . 37

2.3 Angular velocity sensor calibration . . . 38

2.3.1 Introduction of error models. . . 38

2.3.2 Bias and acceleration compensation . . . 39

2.3.3 Solution for scaling and misalignment error handling . . . 39

2.3.4 Temperature compensation . . . 43

2.3.5 Variance analysis . . . 43

2.4 Magnetic sensor calibration. . . 44

2.4.1 Magnetometer calibration . . . 44

2.4.2 On-the-fly calibration. . . 46

2.4.3 Measurement variances . . . 49

2.5 Cross calibration between sensor frames . . . 49 vii

(8)

viii CONTENTS

2.5.1 Rotation between accelerometer and angular velocity sensor . . . 50

2.5.2 Rotation between magnetometer and angular velocity sensor . . . 51

2.6 Conclusion. . . 51

3 Quadrotor state estimation and control 53 3.1 Quaternion based extended Kalman filter . . . 53

3.1.1 Sensor fusion concept based on image processing and IMU . . . 54

3.1.2 Rotation representation problems with RPY angles . . . 57

3.1.3 Quaternion based orientation model of a moving body . . . 60

3.1.4 Position model and estimation involving image processing . . . 63

3.1.5 Estimation and tuning the covariance matrices . . . 65

3.1.6 Finalizing the state estimation . . . 66

3.2 Time-delay extension of the Kalman filters . . . 67

3.2.1 Evaluation of the time-delay model . . . 68

3.2.2 Orientation handling . . . 68

3.2.3 Position handling . . . 69

3.2.4 Validation of the method. . . 70

3.3 Control algorithms for quadrotor control. . . 70

3.3.1 Linear control methods . . . 70

3.3.2 LQ feedback with integrators . . . 73

3.3.3 Extended dynamic model of a quadrotor helicopter . . . 73

3.3.4 Control for normal operation . . . 74

3.3.5 Horizontal orientation subsystem . . . 75

3.3.6 Horizontal position subsystem. . . 76

3.3.7 Altitude control . . . 77

3.3.8 Yaw control. . . 77

3.3.9 Flight results . . . 77

3.4 H control for emergency situations . . . 78

3.5 Conclusion. . . 81

4 Carrier phase based GPS navigation 83 4.1 Fast baseline measurement with two CPGPS receivers . . . 83

4.1.1 Search space based solution . . . 84

4.1.2 Integer ambiguity bounds based on INS sensors . . . 86

4.1.3 Cycle slip correction . . . 88

4.1.4 Measurement results . . . 89

4.2 Determination of special kinematic quantities. . . 90

4.2.1 3D orientation determination . . . 90

4.2.2 Sideslip angle and angle of attack determination. . . 92

4.3 Position measurement with a multiple antennas CPGPS receiver system . . . 94

4.3.1 Usability of differential sensors . . . 95

4.3.2 Phase slip effect in position determination . . . 96

4.3.3 Multiple antennas based integer ambiguity resolution. . . 96

4.3.4 Initialization of the LAMBDA method . . . 97

4.4 Sensor fusion with multiple antennas CPDGPS and INS sensors . . . 98

4.4.1 Orientation determination using extended Kalman filter . . . 98

4.4.2 Precise position estimation with extended Kalman filter . . . 100

(9)

CONTENTS ix

4.4.3 Experimental results . . . 103

4.5 Conclusion. . . 106

Appendices 111 A Measurement of the sensor calibration 111 A.1 Accelerometer. . . 111

A.2 Angular velocity sensor . . . 112

A.3 Magnetometer. . . 113

B Realization of a quadrotor helicopter 117 B.1 Mechanical and electronic design . . . 117

B.1.1 Motor control . . . 117

B.1.2 Image processing . . . 118

B.1.3 IMU . . . 119

B.1.4 Radio communication. . . 119

B.1.5 Data Logging. . . 119

B.1.6 Onboard communication . . . 119

B.1.7 Main processing unit . . . 120

B.2 Determination of the helicopter’s parameters . . . 120

B.2.1 Propeller parameter measuring . . . 120

B.2.2 Inertia matrix measuring. . . 121

C Realization of a multiple antennas CPGPS receiver system 123 C.1 Architecture. . . 123

C.1.1 Two receivers architecture . . . 123

C.1.2 Three receivers architecture . . . 124

C.1.3 Base station . . . 125

C.2 Software design . . . 125

C.2.1 Timing questions . . . 126

C.2.2 Data logging and satellite position determination . . . 126

(10)

x CONTENTS

(11)

List of Figures

1.1 Structure of the accelerometer . . . 5

1.2 The electrical structure of a sensor . . . 6

1.3 Structure of the angular velocity sensor . . . 7

1.4 Electrical structure of a one dimensional magnetometer. . . 7

1.5 Frames of a typical inertial sensor. . . 9

1.6 Structure of the sensor fusion and state estimation . . . 11

1.7 Structure of the general continuous time state estimation problem . . . 12

1.8 The developed quadrotor helicopter. . . 16

1.9 Forces of the quadrotor helicopter. . . 17

1.10 The geometry of the differential GPS arrangement. . . 21

1.11 Geometry of the two receivers . . . 23

2.1 Variation of the bias . . . 37

2.2 Hand movement for angular velocity scaling and misalignment calibration . . . 40

2.3 An approximation measurement for scaling error . . . 42

2.4 Magnetic measurement with a sail-plane . . . 47

2.5 Magnetic measurement with a car. . . 47

2.6 Recommended rotation for all cross sensor frame calibration. . . 50

3.1 Scheme of the state estimation and control . . . 54

3.2 Scheme of the quadrotor helicopter demonstration platform . . . 55

3.3 Graph of the helicopter system’ coordinate frames . . . 55

3.4 Graphs of the vision system’s coordinate frames . . . 56

3.5 The turnaround problem of the RPY representation. . . 59

3.6 Correction of the RPY turnaround problem . . . 60

3.7 Structure of the state estimation (first option) . . . 67

3.8 Structure of the state estimation (second option). . . 67

3.9 Time delay of the vision system . . . 68

3.10 The structure of the state estimation with time delay extension . . . 69

3.11 Time delay compensation test . . . 70

3.12 Orientation stabilization with LQ control (simulation) . . . 72

3.13 Oreintation control with LQ (real test). . . 72

3.14 Orientation control with integration (real flight) . . . 73

3.15 Structure of the control . . . 74

3.16 Position hold test. . . 77

3.17 Test of the altitude control . . . 78 xi

(12)

xii LIST OF FIGURES

3.18 Yaw control test . . . 78

3.19 Structure of theH synthesis . . . 80

3.20 H stabilization (simulation) . . . 80

3.21 H stabilization (real flight) . . . 81

4.1 Two GPS antennas arrangement on a car . . . 84

4.2 Component Z of the possible baseline extended to the full measurement period . . . 86

4.3 Possible length and Z component of the baseline . . . 86

4.4 North -East directions of the possible baselines (the deviation of the result is also shown) . . 87

4.5 Direction of a car in steady state . . . 90

4.6 Baseline length and components of a steady state measurement . . . 90

4.7 Arrangement of the three GPS antennas solution . . . 91

4.8 Representation of the slip angle and the angle of attack. . . 93

4.9 The path of the plane during the measurement . . . 94

4.10 Orientation of the plane during the flight . . . 94

4.11 Angle of attack and slip angle of the plane . . . 95

4.12 Structure of the base station, moving receiver approach. . . 95

4.13 Flowchart of the baseline determination . . . 98

4.14 Flow diagram of the baseline estimation . . . 100

4.15 Path of the car . . . 103

4.16 Components of the calculated baseline . . . 104

4.17 Orientation relative to the North direction . . . 104

4.18 Number of satellites in use during orientation estimation . . . 104

4.19 The estimated position of car movement . . . 105

A.1 Raw measurement from the accelerometer in 18 different orientation . . . 111

A.2 Calibrated measurement of the accelerometer in 18 different orientation . . . 112

A.3 Raw angular velocity in 18 different orientation . . . 113

A.4 Calibrated measurement of the angular velocity sensor in stationary state. . . 113

A.5 Collected measurement for error handling of an angular velocity sensor . . . 114

A.6 2D and 3D plot of the raw magnetic measurement . . . 114

A.7 Length of the calibrated magnetic vector . . . 115

A.8 Length of the calibrated magnetic measurement in the case of the sail-plane . . . 115

A.9 Length of the calibrated magnetic measurement in the case of the car . . . 115

B.1 Structure of the motor controller subsystem. . . 118

B.2 Block diagram of the main processor program . . . 120

B.3 Measured and estimated characteristics of a propeller . . . 121

C.1 Structure of the first prototype of the data logging system . . . 124

C.2 Structure of the second prototype of the data logging system . . . 125

C.3 Structure of the base station . . . 125

(13)

List of Acronyms

2D two-dimensional

3D three-dimensional

AD Analog to digital

ADC Analog to digital converter

BME Budapesti M˝uszaki ´es Gazdas´agtudom´anyi Egyetem Budapest University of Technology and Economics CEP Circular error probable

CoG Center of Gravity CPGPS Carrier phase GPS CPU Central Processing Unit

DDIA Double differentiated integer ambiguity

DoF Degrees of Freedom

ECEF Earth centered Earth fixed coordinate frame EKF Extended Kalman Filter

ENU East-North-Up coordinate frame GPU Graphical Processing Unit ICP Integrated carrier phase

IIT Ir´any´ıt´astechnika ´es Informatika Tansz´ek

Department of Control Engineering and Information Technology IMU Inertial Measurement Unit

INS Inertial Navigation Sensors

KF Kalman filter

LAMBDA Least square ambiguity decorrelation adjustment LED Light Emitting Diode

LS Least Squares

MIMO Multiple Input Multiple Output

MLAMBDA Modified least square ambiguity decorrelation adjustment NED North-East-Down coordinate frame

PWM Pulse Width Modulation QR Orthogonal-right triangular

RGB Red-Green-Blue

xiii

(14)

xiv LIST OF ACRONYMS

RPY Roll-Pitch-Yaw

RT real-time

SDIA Single differentiated integer ambiguity SISO Single Input Single Output

SNR Signal to Noise Ratio

SVD Singular Value Decomposition UAV Unmanned Aerial Vehicle

(15)

Chapter 1

Introduction

1.1 Overview of this thesis and motivation

In the field of control theory it is a well-known fact that the control system is only as good as its sensory system. Beside the technical development of nowadays, newer and newer sensor concept appears. Most of these sensors can be used only in specific areas, but there are some trends which seem to be useful in wide spectrum of applications. Each device has its own advantages and disadvantages, which should be considered when they are chosen for a given task.

When a sensor is used in an exercise, it is not enough to choose the sensor properly. It is also important to use the device in a proper way, to reach the highest quality possible. Plenty of recommendations and algorithms exist how to use a sensor [16,19]. Usually the best result is given by the combination of these solutions.

It was during my last two years of university studies (equivalent to MSc) when I first started to investi- gate an inertial measurement unit (IMU). It was a Crossbow mNAV100CA unit, which was a modern and expensive unit those days. Just after the first measurements I realized that the factory calibrated sensor produces measurements, which seem to be incoherent to classical physics. I continued this research and it turned out that there are lots of phenomena (like temperature dependency, non-perpendicular axes) which were left out of consideration during the calibration method, but their effect cannot be neglected. This was the first motivation to develop complex calibration algorithms for sensors.

Another important aspect in the field of sensory system is that what kind of advantages and disadvantages a sensor has. It is usually necessary to combine sensors, which can eliminate the unfavorable property of the others and back and forth. For example a micro-electro-mechanical system (MEMS) based accelerometer is able to measure the acceleration of a moving body directly but the velocity and the position can be derived through integration. The integrated result suffers from the drifting problem. On the other hand an image processing system can measure the position of the moving body and in some special case its velocity. But the acceleration can be calculated through derivation, which amplifies the noise of the measurement. For a complete and usable acceleration-velocity-position state vector both sensors should be used. Moreover, the advantages of each sensor can be raised. This approach leads to the field of sensor fusion.

The connection between the different types of sensors are described with models. The celebrated Kalman filter[29] gives an optimal solution for a given model and known covariance matrices. Therefore, the result of the sensor fusion is only as good as the used model. Hence it is a key issue to model the physical world which describes this connection as precisely as necessary.

During my PhD studies I took part in a research where the main goal was to develop an unmanned indoor quadrotor helicopter. This was a project of the Budapest University of Technology (BME) Department of

1

(16)

2 CHAPTER 1. INTRODUCTION Control Engineering and Information Technology (IIT). During this research one of the objectives was to develop every part of the helicopter on our own, including the mechanical, electronic and informatics parts of the helicopter and the ground devices. Usually simple commercial devices were used as sensors, which were developed for different applications. Therefore, the measurement results from these devices suffer from some phenomena (like measurement errors of the low-cost IMUs or slow processing speed and large time delay in the vision system) which could have been neglected if expensive, high precision sensors would have been used. Some of the novel result of this thesis is inspired by this low-cost approach.

As the full system of the quadrotor helicopter is a quite unique one, there are aspects which are not typical in other researches. Therefore, some needs emerged to develop control algorithms which can handle these special circumstances.

The main line of the quadrotor research was the indoor operation. Beside that my task was also to discover the possibilities of outdoor operation. In this manner I became acquainted with GPS navigation. For the unmanned control of an outdoor helicopter high precision position sensing is essential. This effectiveness can be reached by the usage of the carrier phase measurement of the GPS (CPGPS) signal.

There are already a lot of applications which use this technique. The solution is used in surveying [13, 48], agricultural applications [4, 40] or aviation [15, 18]. Every field of usage has the property that the initialization of the application does not require to run in hard real-time. This is because the carrier phase approach leads to the integer ambiguity problem, where the task is to determine the integer number of carrier cycles between the receiver and the satellite. The typical solutions for this problem need a huge computational capacity or need some minutes of measurements to give a reliable result[8].

In the field of mobile robotics these non-real-time solutions cannot be applied. On the other hand the typical solutions use only the GPS measurement data. I realized that in some special cases other sensors of mobile robots (unmanned vehicles) can be used which make the solution of the integer ambiguity problem faster, can even be executed in real-time. I also realized that these special cases are also important in robotic navigation.

This thesis is structured as follows. Chapter1starts with the problem statements and shows the typical solutions for the examined problems. It also demonstrates the disadvantages of the well-known solutions and justifies the existence of the novel solutions of the following chapters.

Chapter 2 is about my work with inertial measurement units. The precise calibration is in the focus of this chapter. The existing error models are extended with some phenomena which seemed to be significant during the examination of the sensors. Robust and low-cost calibration method is presented.

Chapter3describes the state estimation problems of this thesis. The solutions are based on the develop- ment of the indoor quadrotor helicopter. In this case extended Kalman filters are used as state estimators.

The models of the estimators are including some uncommon extensions, like quaternion based orientation representation or time-delay handling. A robust control algorithm of the helicopter is also shown in this chapter.

Results corresponding to my GPS based development are described in Chapter4. This part of the thesis starts with an orientation measurement method with a two antenna CPGPS system. With the extension to three antenna system it is possible to measure the slide slipping and angle of attack of a sail plane. Based on the extension with a base station a solution is presented which is able to measure precise position of a moving vehicle. This chapter is finished with a state estimation algorithm which fusion the measurement of the GPS, magnetometer and IMU.

The appendices describe the test measurements of sensor calibration, the technical background of the quadrotor helicopter development and the developed multi antenna GPS system. Finally there is a compar- ison between the existing and novel CPGPS algorithms.

(17)

1.2. NOTATIONS 3

1.2 Notations

All of the three main chapters of this thesis will have their own variable sets. Each of the chapters composes a separate part, but they can use the results from others. However there will be some generally used variable and notations, which are collected here.

The index will be frequently used, where the same algorithm or equation can be applied for similar cases. For example, if the calculation of the variablessx,sy,sz is the same for all of theX,Y,Zaxes, only the result forswill be given.

NotationRwill be used for both rotation matrix, covariance matrix and the upper triangular matrix of the QR decomposition. Their indices will define the meaning ofR.

The indexes of the transformation will be the following. Equationxb=Tb,axa represents a homogenous transformation, wherexa is represented by the base of frameKa andxb is represented by the base of frame Kb. If it is important to notice a third reference frame of its property, then it will be represented in a superscript.

The notation„refers to the pseudo inverse of a matrix in the least square error in sense: A„= (ATA)1AT Quick reference of mathematical and other symbols

A,B, . . .Z matrices a,b, . . .z vectors Cα, Sα cos(α), sin(α)

[u×] cross product matrix: [u×]v=(u×v)

diag(A) the diagonal elements ofAmatrix formed in a column vector

diag(b) a diagonal matrix, where the diagonal elements are the components of vectorb Frequently used variables

Typical size Description

I{size} 3×3 Unit matrix, the index refers to the matrix’s size

R 3×3 Rotation matrix

ϕ,ϑ,ψ scalar rotation angles

q 4×1 quaternion vector (∥q∥=1)

qi scalar components of a quaternion (i=1...4) T 4×4 Homogenous transformation matrix

Rv,Rz 3×3 Covariance matrices system and measurement noises

R{sensor} 3×3 Covariance matrix of a measured vector, the index defines the sensor type

1.3 Typical solutions and unsolved problems of inertial navigation

When I started to investigate an inertial measurement unit (IMU) during the fourth year of my university studies, I found some interesting phenomena which I did not understand for the first time. The sensor was a Crossbow mNAV100CA unit, which was an expensive and precise sensor those days. It was also mentioned in the documentation that the sensor is pre-calibrated by the factory. For the first measurement I put down the sensor onto the table and I got the measurement from the accelerometer something like (0.1 −0.05 1.05)TG1. The length of the vector differed from 1. Second time I rotated the sensor from

1The mNAV100CA measures inG, whereGis the constant of the gravitational acceleration, 1G9.81m/s2

(18)

4 CHAPTER 1. INTRODUCTION upside to down and the length of the result also differed from 1, but differed from the previous result as well.

I got similar results from the angular velocity sensor and from the magnetometer as well. That meant the sensors should be recalibrated. For calibration method I found the possible algorithms described in this section. The trouble with these methods is that they usually require some additional equipment which can produce a specific movement. Moreover, the used error models usually are so simple that they cannot measure even all the error parameters described in the sensor datasheet2.

My goal during the development of novel calibration methods was that they should measure every error parameter which seemed to be significant during the operation of the quadrotor helicopter. The methods should be usable in the operation of other mobile robots as well. The other goal was that the calibration could be performed with simple and cheap equipment. Usually the methods do not need any additional tool.

The sensors produce measurements which usually are represented by a discrete number. But in the engineering practice it is sometimes forgotten that the result of the measurement should contain two different parts. The first part is a number which is the value of the measurement. The second part describes somehow the uncertainty of the measurement. Usually it is described with the standard deviation or the variance, or in the case of multiple data with the covariance matrix. Therefore, it also should be examined how to get this uncertainty information about the sensor.

1.3.1 Structure of MEMS sensors

The purpose of this section is to present the basics of the typical micro-electro-mechanical system (MEMS) based sensors. The models of these sensors will be deducted from their structure.

In the field of mobile navigation the most common sensors are the accelerometer and the angular velocity sensor. These devices compose together an inertial measurement unit (IMU). This set is very popular because there are no restrictions for their usage.

Also very popular sensors are the magnetometers. They use typically magneto resistive components to sense the level of the magnetic field. Therefore, they are not really MEMS sensors. Although their size and electrical characteristics are very similar to inertial sensors. Moreover, the approach of the calibration will also be similar.

Accelerometer

There are a lot of solutions for the structure of the accelerometer, but each one can trace back to the simplest one, which is a one dimensional spring-mass system[28]. Its schematics can be seen in Fig. 1.1. The classical spring-mass system is extended here with the actual component of the gravitational force.

In this structure there is a central mass whose displacement can be measured3. The name of this mass in the literature is test mass, proof mass or seismic mass. This mass is connected to the case of the sensor with a spring. Let the spring constant be denoted byD. The whole structure of the sensor is like that the Hooke’s law gives a very good description of the spring force for every possible displacement of the test mass:

Fs=D∆x (1.1)

It comes also from the mechanical structure of the sensor that the mass can move only along one direction.

This dedicated direction in Figure1.1is the X direction. Every force and acceleration which is applied for the sensors considered here by its X component. Let’s examine the stable state of the system, where the acceleration of the whole system along the X direction isax.

2like misalignment error or acceleration effect on angular velocity sensors

3usually by a capacitive way

(19)

1.3. TYPICAL SOLUTIONS AND UNSOLVED PROBLEMS OF INERTIAL NAVIGATION 5

Δx

G

x

a

x m

F

s

X

Figure 1.1: Structure of the accelerometer

In this case the forces applied for the test mass are the spring force and the X component of the gravitational force. The Newton’s law can be applied:

max=Fs+Gx (1.2)

max=D∆x+mgx (1.3)

∆x=m

D(ax−gx) (1.4)

wheregxis the X component of the gravitational acceleration.

By this way the displacement of the measurement mass is in linear connection with the acceleration of the sensor including the gravitational acceleration. It means that if the sensor has no acceleration in the direction of the X axis, the measured value is the X component of the gravitational force. On the other hand, in a free fall situation4, where the acceleration is purely the gravitational acceleration (ax=gx), the output of the sensor is zero [46].

To measure the full three dimensional acceleration vector, at least three pieces of one dimensional sensors are needed. Because of the placement of the sensors their axes are usually non-parallel, need not to be per- pendicular. Hence orthogonal component vectors can only be achieved by an appropriate transformation[11].

The measurement noise of each one dimensional sensor can be considered as a zero mean white noise.

It also can be assumed that the noises of different sensors are independent. If the axes of the sensors are perpendicular, the components of the acceleration vector can be directly measured. Hence the components can be considered as independent stochastic variable. This means that the covariance matrix of the mea- surement vector is diagonal. The independent property remains valid if the vector is transformed by an orthogonal transformation to any other Cartesian coordinate system.

xnew=Rx (1.5)

E{xnewxTnew}=E{RxxTRT}=RE{xxT}RT (1.6) whereRis the orthogonal transformation andE{x}is the expected value of the stochastic variablex.

If the axes are not perpendicular, the acceleration vector can be achieved through a non-orthogonal transformation. In this case the components of the calculated vector are already not independent. This means that the covariance matrix is not diagonal.

4like a satellite in the space

(20)

6 CHAPTER 1. INTRODUCTION If a measurement data is collected from a sensor system, its value usually consists a non-zero mean error.

It is worth to spend time to examine the sources of this offset.

The sources of the errors basically come from the mechanical and electrical structure of the measurement system. The mechanics is discussed before. The electrical structure can be seen in Fig. 1.2. The system has basically three components. The sensor has a supply voltage whose nominal value differs from the real value. The output is an analog voltage. The meaning of the output value refers to the nominal value of the supply voltage. Therefore, an offset like error occurs.

The analog output of the sensor is scaled with an amplifier. In the realization of the amplifier an output offset appears. Moreover, the nominal value of the amplification level differs from the real, which introduce a gain error.

The amplified analog voltage is digitalized by an analog to digital converter (ADC). This calculates the digital value of the analog signal. The base of the calculation is comparing the analog signal to a voltage reference. As this reference has also a different nominal and real value, another offset appears.

The AD converter has another type of error, which is called quantization error. It appears like a non-zero mean noise. Its level depends on the resolution of the AD converter. Usually the level of the quantization error in case of a 16 or 24 bits AD converter is small enough to be neglected.

sensor ADC analogdata

digital data

ADC supply voltage

Sensor supply voltage ADC reference

voltage

analog Amp data

Figure 1.2: The electrical structure of a sensor

Angular velocity sensor

Similarly to the accelerometers, the angular velocity sensors have plenty of forms in practice. Usually two types are common. The first one is the MEMS based solution which is low-cost, small but noisy and inaccurate. The other type is the optical angular velocity sensor, which is expensive, larger than the MEMS based solution and very precise.

In the following only the MEMS based solution is discussed. The mechanical structure of the sensors is wide-scale, but the principles have usually the same basis. There is an mtest mass, which can move in two dimensions of a plane [50]. Along the first dimension, the mass is artificially excited with a well-known frequency, while in the second dimension the mass has free movement.

If the sensor has an angular velocity which has non-zero component perpendicular to the plane, the Coriolis force will act on the test mass. Therefore, the mass will oscillate with the excitation frequency in the free dimension. The magnitude of the generated frequency is in linear connection with the amount of the angular velocity. The sign of the angular velocity is given by the phase of the free movement frequency relative to the excitation frequency. The actual value of the displacement is measured by a capacitive way.

The operation principle of the sensor is shown in Fig. 1.3.

The output of the sensor is a DC voltage, which is originated from the generated sine wave. The output value is proportional to the magnitude. The exact method is usually not published by the sensor manufacturers. Based on the experiments with angular velocity sensors, only very simple solutions are used.

(21)

1.3. TYPICAL SOLUTIONS AND UNSOLVED PROBLEMS OF INERTIAL NAVIGATION 7

m f

excitation

f

generated

ω

Figure 1.3: Structure of the angular velocity sensor

It can be noticed that if there is an external force in the direction of the free movement, it will give an offset to the generated waveform. In other words the acceleration of the sensor distorts the measurement of the angular velocity sensor. This offset usually goes through the magnitude to DC voltage conversion therefore this error appears in the measured value.

It also comes from the structure, that the amount of the disturbance of the acceleration is connecting only the mechanical structure of the sensor therefore the resulted error for a given acceleration can be considered as same during the whole lifetime of the sensor.

The typical electrical structure of an angular velocity sensor is the same as that of the accelerometer, shown in Fig. 1.2. Therefore, the other types of errors are basically the same as for the accelerometer.

As the angular velocity sensor has similar structure as the accelerometer, a similar error model could have been developed. The only difference could be the extension of the acceleration based distortion.

Magnetometer

Beside the accelerometer and the angular velocity sensor, magnetometers are commonly used in the same sensor system. For the compact size implementation the magnetometers use a small magneto-resistive component connected into a Wheatstone bridge circuit[22]. The schematic of a typical construction can be seen in Fig. 1.4.

ADC Amp.

Sensingdirection

R3

R2 R1

Vsupply

Digital Output

R4

RSR Roff

iSR

t

Figure 1.4: Electrical structure of a one dimensional magnetometer

The magneto-resistive element has a sensitive direction. If there is a non-zero component of the magnetic field in this direction, the resistance of the element will change proportional5to the value of this component.

5If we examine the change of the resistance of the magneto-resistive materials, it can be seen that its value is proportional

(22)

8 CHAPTER 1. INTRODUCTION That means the voltage of the differential output also changes. This value is scaled with an amplifier and digitalized by an AD converter. Every of these components is usually built in a single chip.

The magneto-resistive components have the disadvantage that a constant magnetic field can fix the magnetic domains of the resistor that makes the sensor insensitive. Therefore, a set-reset circuit is used to avoid this problem[30]. This circuit is usually a simple resistor whose value is only some Ohms. This makes possible that a relatively big current can flow on this element. Usually two narrow spikes are applied on this resistor. The change of the current generates a fast change in the direction of the magnetic field, which forces the magnetic domains to rotate. From the measurement’s point of view this means that the sampling point should avoid the operation of set-reset method. Because of this synchronization and as the usage of this set-reset circuit depends on the characteristics of the magneto-resistive element, the sampling and the set-reset control circuit is also built in the chip.

Based on the structure of the sensor there are two types of error sources. An offset like error appears, originated from the differences between the resistors in the Wheatstone bridge, the disturbances in the supply voltage and the offset error in the amplifier. A gain like error can be derived from the amplifier part. It should be mentioned that the offset error sometimes is treated in the sensor with an offset register. If a current flows through this element, that generates a magnetic field which can compensate the offset of the magneto resistive components.

Similarly to the accelerometers and angular velocity sensors, if a three dimensional sensor is built from one dimensional components the misalignment of the axes can also introduce a source of error.

1.3.2 Typical models and methods

In the literature many different methods exist for accelerometer, angular velocity sensor and magnetometer calibration. It is worth to examine the typical sensor models of the most relevant papers [11,14,17,26,31, 35,43, 45]. Most of the models introduce a scale error, an offset error and perpendicularity compensation or cross sensitivity:

T =

⎡⎢⎢⎢

⎢⎢⎢⎢

1−α1 α2

0 1 −α3

0 0 1

⎤⎥⎥⎥

⎥⎥⎥⎥

(1.7)

S=

⎡⎢⎢⎢

⎢⎢⎢⎢

1−σ1 σ2

0 1 −σ3

0 0 1

⎤⎥⎥⎥

⎥⎥⎥⎥

(1.8)

K=diag([kxky kz]) (1.9)

b=[bxbybz]T (1.10)

smeasured=KT Ssreal+b+µ (1.11)

whereT is the perpendicularity compensation matrix, K is the scaling error compensation matrix, S is a cross sensitivity matrix,bis the offset vector,µis an independent zero mean white noise vector. smeasured

is the measurement of the sensor, sreal is the real value of the measured motion state. The calibrated measurement is formed as:

to the square of the strength of the magnetic field. To handle this problem, low resistance bars (usually aluminum bars) are steamed on the magneto-resistive stripes with an inclination angle of 45. In this case the change of the resistance caused by the magnetic field can be well approximated with a proportional behavior. This arrangement of the element is called the ”Barber pole” circuit[22]

(23)

1.3. TYPICAL SOLUTIONS AND UNSOLVED PROBLEMS OF INERTIAL NAVIGATION 9

scal=(KT S)−1(smeasured−b) (1.12) The basic difference among the common solutions is the way how to determine the value of the pa- rameters and how many measurements are needed. The measurement should be acquired under different circumstances. Some solutions need only different orientations, others specify the rotation between the ori- entations and sometimes a known trajectory of movement is required. Table1.1summarizes some relevant methods from the literature.

A different category of calibration methods is where a complicated mechanical structure is built to produce a specific movement. By this way the unknown parameters in (1.11) can be measured individually step by step. Let stand here an example with an angular velocity sensor:

The first step is to measure in an orientation direction, where there is no angular velocity (stationary state). Then the measured value will be theboffset vector of the sensor. In the following steps this vector should be subtracted from every measurement. The next step is to rotate the sensor around the axes with a known angular velocity. The ratio of the real angular velocity and the measured one gives theKscale error matrix. The next step is to rotate the sensor around three exactly perpendicular axes. From the measured values the T misalignment matrix can be achieved. If the whole procedure can be repeated in different temperatures even the temperature dependency of the parameters can be determined.

There are two problems with the direct measurement. The first is that the result is only as precise as the measurement device. It is important because it is a very difficult mechanical problem to rotate a sensor exactly parallel to its axes, especially if the three rotation axes should be exactly perpendicular. The second disadvantage is the price of the measurement device, which can be very high (e.g. high precision robot).

1.3.3 Calibration of independent frames

In the previous subsections, the different types of sensors were handled individually. Using the output of the calibration methods, the measurements are represented in an orthogonal (typically right-handed) coordinate frame. But the acceleration represented in its own coordinate frame, the angular velocity in a different one and the magnetic measurements in a third one. The robot graph of a typical sensor in shown on Fig. 1.5.

K

S

K

acc

K

ω

K

m

T

S,acc

T

S,ω

T

S,m

Figure 1.5: Frames of a typical inertial sensor

In the literature of the calibration methods this question is usually suppressed. The typical answer is that the mechanical assembly is so precise, that the frames are almost the same. I cannot agree with this approach that disregards the misalignments of the sensor axes. The usual error in the axes’ misalignment is in the order of one degree. The misalignment of the frames is also in this order therefore I think this question cannot be neglected.

Another approach in the literature is that after the assembly of the sensor to the vehicle, the relation between the vehicle’s frame and the sensor frame should be calibrated and this question should be discussed

(24)

10 CHAPTER 1. INTRODUCTION in that latter step. This opinion is already a better one, the only problem is that there could be effects in the sensor behavior which affect the different components of sensors. For example the direction of the acceleration can disturb the offset of the angular velocity sensor. Also it is a usual technique to perform the estimation of the orientation based on the kinematic model of a moving body [16]. These methods use only the measurements of the accelerometer, magnetometer and angular velocity sensor and they are usually processed in the microprocessor of the sensor. In these cases the question of the relationship between the frames cannot be postponed to a latter stage of the calibration.

1.4 State estimation and sensor fusion of a moving body

Let us consider a system, which has different types of sensors. In this case some questions emerge. Is it possible to use the sensors together? If the sensors measure the same property, how should they use to work together? Is it possible to use one of the sensors to decrease one another’s error? If one sensor measures a parameter and another measures its derivate, how can they be used in the same system. The theory of state estimation and sensor fusion gives the answers for these questions.

The state estimation subsystem is part of the control loop. In the case of a moving body the typical components of the vehicles state is its position, velocity, acceleration, orientation and angular velocity. Some of the components can be measured directly by sensors, others should be estimated.

The commonly used sensors are the MEMS based accelerometers and angular velocity sensors. These sensors can be used in any situation. But the other type of sensors have usually restrictions about their usage. They can be categorized as indoor and outdoor sensors.

The typical indoor sensors are the vision based units. The lighting conditions can usually be controlled and in indoor circumstances the variety of the scenes can be handled. The orientation and the position of the vehicle can be determined by the image processing units.

Similar solutions can be the ultrasonic based solutions, where the transmitters are fixed in the room and the receivers are on the board of the vehicle.

In the outdoor case one of the most common sensors is the GPS receiver. It can determine its own spatial position. Using multiple receivers and the most precise carrier phase differential solutions, the orientation of the vehicle can also be calculated.

The magnetometer cannot be used in indoor situation as the iron parts of the building distort the magnetic field. But it is a useful outdoor sensor. It can measure the direction and the strength of the magnetic vector.

Therefore it can be used for orientation determination.

Considering these challenges the definition of state estimation and sensor fusion can be formed.

The state estimation is an algorithm where the inputs are the measurement of one or more sensors. The output is the (observable) state of the system.

The sensor fusion is an algorithm where the inputs are measurements from different types of sensors with different properties (i.e. different time delay, sample rate or noise). The outputs are the approximations of the (observable) states, but each component has the same property (i.e. same sample rate etc.).

1.4.1 Necessity of sensor fusion

If different types of sensors are used in the same system, it is a challenge to handle the different parameters of the sensors. These parameters are for example the sampling time, the time delay or the noise parameter.

It can be an important property of the sensor fusion algorithm that it should be able to decrease the error of one sensor by using another one.

Let us consider the following typical example for the sensor fusion. Let a car be given which is moving on the motorway with 30m/s(108km/h) speed. The task is to control the car to keep in track. The available

(25)

1.4. STATE ESTIMATION AND SENSOR FUSION OF A MOVING BODY 11 sensors are the GPS receiver and the accelerometer. The sampling time of the GPS receiver is 1s. This means that the car moves 30mbetween two consecutive GPS measurements. If only the GPS receiver would be used it would be a hard control task.

Therefore the accelerometer will also be used. Its sampling frequency is 100Hz, that means only 30cm movement between two measurements. The problem with the accelerometer is its offset, which can cause significant drift between two GPS measurements, if it is not handled.

Using the precision of the GPS receiver the offset of the accelerometer can be handled. Using the fast sampling frequency of the accelerometer the state of the movement could be available for the control 100 times more frequently than in the only GPS case.

The other challenging problem is that the acceleration measured by the accelerometer is the second derivative of the position measured by the GPS sensor. Therefore they cannot be compared directly. The connection between the two variables is the kinematic model of the moving body. During the sensor fusion this should be taken into consideration.

Let us consider another example which will be presented in details in the further chapters. The task is to determine the state of a moving indoor quadrotor helicopter. One of the sensors is a camera based image processing system. It can calculate the position and orientation of the helicopter. The other sensors are accelerometer and angular velocity sensor.

The sampling rate of the vision system is about 10−15Hz, depending on the lighting condition and the position of the helicopter. This varying sampling time should be handled in the sensor fusion. The accelerometer and the angular velocity sensor have 100Hzsampling frequency, which makes possible a high frequency state estimation.

Other challenging problem is that the image processing has a large computation requirement, which implies a significant (≈300ms) time delay in the measurements. The state estimation algorithm should be prepared to handle this delay.

In the sequel we shall deal with deterministic state observers and stochastic state estimators. However for simplicity we shall call both as estimators.

1.4.2 Possible solutions for state estimation

The state estimation and the sensor fusion are usually merged together and handled in a common method.

The structure of the methods for a linear system is shown in Fig. 1.6.

Based on the previously estimated state of the system and the noise properties of the sensors the state estimation calculates the fusion parameters (matrix Gk in Fig. 1.6). This parameter defines the weights which describes how the measurements are considered in the state estimation. Based on the measured data, the previously estimated state of the system and the fusion parameters, the state estimation calculates the actual state of the system.

Sensor fusion

System state (A ,B ,C ,x )k-1 k-1 k-1^k-1 Sensor properties

(R ,R )v z

State estimation

Fusion parameters

(G )k

estimated state (x^k)

x =F x +G y +H uk+1 k k k k k k

measured data (y , u )k k

Figure 1.6: Structure of the sensor fusion and state estimation

The place of the state estimator in the control loop can be seen in Fig. 1.7. The inputs of the estimation algorithm are the actuator signals (u) and the output (y) of the controlled system. The output of the

(26)

12 CHAPTER 1. INTRODUCTION algorithm is the actual state (x) of the system. The difference between the state estimators is in the algorithm to be used.

Figure 1.7: Structure of the general continuous time state estimation problem

One of the emerging questions is whether the input of the state estimation contains enough information to estimate the full state of the system. If there is not enough information then which part of the system state can be calculated? In linear systems the observability matrix can give the answer. Let us consider a continuous time linear system in the form

x˙ =Ax+Bu (1.13)

y=Cx+Du (1.14)

where A,B, C, D matrices describe the behavior of the linear system. Because of practical assumptions the matrixD is usually chosen to zero. Then the observability matrix is defined as

Mo=

⎡⎢⎢⎢

⎢⎢⎢⎢

⎢⎢⎢⎣

C CA

⋮ CAn−1

⎤⎥⎥⎥

⎥⎥⎥⎥

⎥⎥⎥⎦

(1.15)

wherenis the dimension of the state. IfMohas full rank, then the full state of the system can be observed by a state estimator.

In the case of nonlinear system it is a more difficult problem to decide whether the full state of the system is observable. In this case the observability property depends on the actual state of the system therefore only local observability can be defined. One of the common approaches is the use of the differential geometry methods. In this context the definition and the properties of the local observability are described in detail in the Section 1.9 of [27].

Let us first investigate the linear case. The state estimation is usually given by a dynamic system, which inner state is the estimation of the actual state of the system. Therefore the output of the state estimator is denoted by ˆx. In linear case the dynamic of the observer is searched in the form of

˙ˆ

x=Fxˆ+Gy+Hu (1.16)

whereF,GandH are the matrices of the observer.

In the simplest form the only requirement against the observer that the error of the estimation should converge to zero. This criteria is defined as

(27)

1.4. STATE ESTIMATION AND SENSOR FUSION OF A MOVING BODY 13

e=xˆ−x→0 (1.17)

then

˙

e=x˙ˆ−x˙ =Fxˆ+Gy+Hu−Ax−Bu=

=F(xˆ−x) + (H−B)u+ (F − (A−GC))x (1.18) A reasonable choice can beF ∶=A−GC andH∶=B, that means the error dynamics is independent from the actual state and the input of the system. The choice of the matrixG can define the dynamic of the error.

There are methods where the poles of the dynamics can be given, like the Ackermann method for SISO systems, or the pole placement method for MIMO systems. Other methods introduce criteria, i.e. a cost function is introduced and minimized for an infinite long period of the error where the method uses the solution of the filter algebraic Riccati equation.

In the thesis the most frequently used state estimator will be the Kalman filter. This estimator is defined for a linear system, where the stochastic behavior of the system is also considered. The Kalman filter can also be used as a sensor fusion algorithm, handling the different sampling times of the measurement devices.

1.4.3 The Kalman filter

The Kalman filter will be used in Chapter 3 and Chapter4. One of the goals of these chapters is to give state estimation methods which can be easily implemented on embedded processors. Therefore the state estimation should be considered in discrete time.

The stochastic discrete linear dynamic model which is used in Kalman filter [33] is

xk+1=Akxk+Bkukk (1.19)

yk=Ckxk+zk (1.20)

where matricesAk, Bk, Ck describe the behavior of the system in the kth epoch. Vectorsνk, zk are the actual values of the state and the output noise. The requirements for the noise components are

E{νk}=0; E{νkνlT}=0, k≠l; E{νkνkT}=Rν,k (1.21) E{zk}=0; E{zkzlT}=0, k≠l; E{zkzkT}=Rz,k (1.22)

E{νkzlT}=0 ∀k, l (1.23)

The state estimator is assumed in the form of ˆ

xk+1=Fkk+Gk+1yk+1+Hkuk (1.24) where the advantage is used that the(k+1)th output is already available when the(k+1)th state vector of the system is estimated.

The goal of the design is to reach zero average error and infimum error covariance matrix:

E{xk−xˆk}=0 (1.25)

Σk=E{(xk−xˆk)(xk−xˆk)T}→min (1.26)

Hivatkozások

KAPCSOLÓDÓ DOKUMENTUMOK

The other military ranks held by Menyhért Ráttky was more significant than the office of deputy district-captain-general and the main function of György Ráttky in turn

Acute myeloperoxidase, estradiol and cortisol treatment improves, indomethacin treatment does not affect the bacterial killing capacity of isolated human

of the total current output per the sector of manufacture of chemicals and chemical products and that means regardless overproduction, still import has crucial impact to meet the

The Kalman filter is the optimal solution for state space estimation of linear systems based on measurements corrupted by additive zero mean Gaussian white noise with

Zhang, PhD Thesis, “Phase field model for the nucleation in solid state phase transformations:.. theories, algorithms and application.” (Pennsylvania State University, Ann

The results of the complex analytical procedure for determining the 89/90 Sr content of the liquid radioactive waste of the Paks Nuclear Power Plant (NPP), as well as the results

Nem szeretn´em, ha ez a meg´allap´ıt´as az ´ertekez´es ´ert´ekel´es´et negat´ıvan befoly´asoln´a, ´es ism´etelten hangs´ulyozom, hogy a jel¨olt b˝oven t´ulmegy a

A kor´ abbi konf´ uzi´ os zaj eredm´ enyek illetve a PACS kamer´ ak ´ ep´ıt´ ese ´ es kalibr´ aci´ oja k¨ ozben szerzett tapasztalatok alapj´ an ´ ep´ıtettem fel