Estimation and Detection with Applications to Navigation
Total Page:16
File Type:pdf, Size:1020Kb
Linköping studies in science and technology. Dissertations. No. 1216 Estimation and Detection with Applications to Navigation David Törnqvist Department of Electrical Engineering Linköping University, SE–581 83 Linköping, Sweden Linköping 2008 Linköping studies in science and technology. Dissertations. No. 1216 Estimation and Detection with Applications to Navigation David Törnqvist [email protected] www.control.isy.liu.se Division of Automatic Control Department of Electrical Engineering Linköping University SE–581 83 Linköping Sweden ISBN 978-91-7393-785-6 ISSN 0345-7524 Copyright c 2008 David Törnqvist Parts of this thesis have been published previously. Paper A is published with permission from Springer Science and Business Media c 2008 Papers B and E are published with permission from IFAC c 2008. Paper C is published with permission from IEEE c 2008. Paper D is published with permission from EURASIP c 2007. Printed by LiU-Tryck, Linköping, Sweden 2008 To my family! Abstract The ability to navigate in an unknown environment is an enabler for truly autonomous systems. Such a system must be aware of its relative position to the surroundings using sensor measurements. It is instrumental that these measurements are monitored for distur- bances and faults. Having correct measurements, the challenging problem for a robot is to estimate its own position and simultaneously build a map of the environment. This prob- lem is referred to as the Simultaneous Localization and Mapping (SLAM) problem. This thesis studies several topics related to SLAM, on-board sensor processing, exploration and disturbance detection. The particle filter (PF) solution to the SLAM problem is commonly referred to as FastSLAM and has been used extensively for ground robot applications. Having more complex vehicle models using for example flying robots extends the state dimension of the vehicle model and makes the existing solution computationally infeasible. The factor- ization of the problem made in this thesis allows for a computationally tractable solution. Disturbance detection for magnetometers and detection of spurious features in image sensors must be done before these sensor measurements can be used for estimation. Dis- turbance detection based on comparing a batch of data with a model of the system using the generalized likelihood ratio test is considered. There are two approaches to this prob- lem. One is based on the traditional parity space method, where the influence of the initial state is removed by projection, and the other on combining prior information with data in the batch. An efficient parameterization of incipient faults is given which is shown to improve the results considerably. Another common situation in robotics is to have different sampling rates of the sen- sors. More complex sensors such as cameras often have slower update rate than ac- celerometers and gyroscopes. An algorithm for this situation is derived for a class of models with linear Gaussian dynamic model and sensors with different sampling rates, one slow with a nonlinear and/or non-Gaussian measurement relation and one fast with a linear Gaussian measurement relation. For this case, the Kalman filter is used to process the information from the fast sensor and the information from the slow sensor is processed using the PF. The problem formulation covers the important special case of fast dynamics and one slow sensor, which appears in many navigation and tracking problems. Vision based target tracking is another important estimation problem in robotics. Dis- tributed exploration with multi-aircraft flight experiments has demonstrated localization of a stationary target with estimate covariance on the order of meters. Grid-based estima- tion as well as the PF have been examined. v Populärvetenskaplig sammanfattning Tänk dig att du är en brandman på väg in i ett eldhärjat hus. Du vet inte hur det ser ut där inne eller vilka vägar som är möjliga att ta. Väl inne i huset utforskar du det på jakt efter brandorsaken. Det är samtidigt mycket viktigt att komma ihåg vilken väg du kom och hur långt det är till närmaste utgång om du snabbt behöver lämna huset. Under utforskandet byggs en mental karta över huset upp genom att använda synintryck och balanssinne. Ibland kanske du återkommer till en bekant plats och kan då uppdatera din karta lite extra eftersom du förstår hur olika delar av huset hänger ihop. Eftersom vi vill undvika att människor skadas i samband med att de ska släcka brän- der eller utföra andra farliga uppdrag kommer det i framtiden bli vanligt med robotar som hjälper oss med dessa uppdrag. Scenariot ovan är därför något som framtida robotar kom- mer ställas inför. De här robotarna måste förstås ha sensorer för att känna av omgivningen och sin egen rörelse. Sensorer som används för detta är bland annat kameror, laser, radar, mikrofon, kompass, accelerometer och gyroskop. Från dessa sensorer flödar mängder med information som måste utnyttjas på ett smart sätt för att roboten ska bli så framgångsrik som möjligt. Det är denna hantering av information som den här avhandlingen behandlar. I det inledande scenariot handlade det mycket om att försöka bygga upp en karta över huset. Inom robotiken kallas detta problem för samtidig lokalisering och kartuppbyggnad och förkortas SLAM (eng. Simultaneous Localization And Mapping). SLAM-problemet har studerats flitigt under de senaste tjugo åren, men det kräver mycket beräkningar vilket gör att datorer inte klarar av att bygga särskilt stora kartor. Mycket forskning görs därför på hur man kan förenkla problemet så att mindre beräkningar krävs. Ett bidrag i avhan- dlingen behandlar ett sätt att minska beräkningsbördan för robotar som kräver avancerade modeller. I det här fallet används en helikopter som är utrustad med en kamera, accelerom- eter, gyroskop och barometer. Helikoptern flyger över ett område och tittar ner på marken med kameran. Intressanta objekt i bilden sparas och bygger upp kartan som helikoptern kan använda för att positionera sig med. Processen att bygga upp kartan och positionera sig bygger på så kallade estimeringsmetoder. För att välja ut särskilt intressanta objekt i bilden, som lämpar sig för att bygga en karta med, används olika bildbehandlingsmetoder. De försöker välja ut objekt som ska vara lätta att känna igen från en bild till en annan. Ibland blir det fel då metoderna luras av ett snarlikt objekt i bilden. För att upptäcka den här typen av fel används statistiska detektionsmetoder. Ett annat område där dessa metoder kan användas är för att upptäcka fel i sensormätningar. Ibland används en kompass för att veta i vilken riktning roboten färdas. Den är mycket känslig för störande magnetfält och dessa störningar måste därför upptäckas. Ett annat område som behandlas i avhandlingen är sökning efter t.ex. en försvunnen person med hjälp av flera obemannade flygplan. Flygplanen är utrustade med kameror som spanar ner på marken. Estimeringsmetoder används för att representera information om vad flygplanen har sett och inte har sett så att den kan distribueras mellan flygplanen. Tanken med tekniken är att flygplanen ska få beslutsunderlag för att själva kunna fatta beslut om var det är lämpligt att söka. Sammanfattningsvis kan man säga att den här avhandlingen syftar till att robotar ska kunna använda sina “ögon”, “öron” och andra sensorer genom utnyttjande av estimerings- och detektionsmetoder. vii Acknowledgments When crossing a river, you must make sure to find good rocks to stand on. My rocks when writing this thesis has been Prof Fredrik Gustafsson and lately Dr Thomas Schön. Fredrik has a constant flow of ideas and can find a seed to something positive in everything. Thomas, discussing with you is great and you have provided invaluable feedback during this work. Thank you both! I would also like to thank Prof Lennart Ljung for drafting me to the control group and for creating a good atmosphere in the group. Our secretary Ulla Salaneck also deserves my gratitude for always arranging practical matters in a smooth way. Various parts of this thesis have been proofread by Fredrik, Dr Gustaf Hendeby, Dr Umut Orguner, Thomas, Per Skoglar, Martin Skoglund, and Lic Henrik Tidefelt. Your comments have been invaluable! Thank you! Gustaf also deserves appreciation for an- swering all these LATEX-questions! Henrik helped me with some of the figures, thanks! During 2007, I had the opportunity to visit two different research centers for a couple of months each. First, I visited the Australian Center for Field Robotics. This was really inspiring, seeing all the different airplanes, helicopters and ground robots. The people at the department were great, they learned me how to appreciate good pub food (especially those burgers with beetroot), they made great espresso and we had nice discussions about SLAM. Thank you! After this visit, I was invited to C3UV at University of California, Berkeley. That was a great time flying helicopters and airplanes, experiencing the desert heat and discussing particle filters. I really liked to work with you there, thank you! The four of us who started our PhD studies at the same time, we did this journey together. Thank you Dr Daniel Axehill, Gustaf and Dr Johan Sjöberg for company during many late nights at the office, spare time activities, and for simply being really good friends! I would also like to thank all the other people at the Automatic Control group for the good atmosphere and for all the laughs created by fika-discussions and junk-mail. My co-authors in different papers over the years are Dr Gunnar Bark, Lic Gianpaolo Conte, Dr Eva Englund, Dr Erik Geijer Lundin, Dr Fredrik Gunnarsson, Prof Fredrik Gustafsson, Prof Karl Hedrick, Dr Rickard Karlsson, Dr Zu Kim, Ass Prof Inger Klein, Allison Ryan, Dr Thomas Schön, John Tisdale, and Dr Niclas Wiberg. Thank you very much for inspiring collaborations! This work was partially sponsored by SSF strategic research center Modeling, Visu- alization and Information Integration MOVIII, the Center for Industrial Information Tech- nology, CENIIT, and the Excellence Center in Computer Science and Systems Engineering in Linköping, ECSEL, which are hereby gratefully acknowledged.