Linköping studies in science and technology. Dissertations. No. 1409 Sensor Fusion for Automotive Applications Christian Lundquist Department of Electrical Engineering Linköping University, SE–581 83 Linköping, Sweden Linköping 2011 Cover illustration: The intensity map describes the density of stationary targets along the road edges. A photo of the the driver’s view in the green car is shown in Figure 5a on Page 216. Linköping studies in science and technology. Dissertations. No. 1409 Sensor Fusion for Automotive Applications Christian Lundquist [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-023-9 ISSN 0345-7524 Copyright © 2011 Christian Lundquist Printed by LiU-Tryck, Linköping, Sweden 2011 To Nadia Abstract Mapping stationary objects and tracking moving targets are essential for many autonomous functions in vehicles. In order to compute the map and track esti- mates, sensor measurements from radar, laser and camera are used together with the standard proprioceptive sensors present in a car. By fusing information from different types of sensors, the accuracy and robustness of the estimates can be increased. Different types of maps are discussed and compared in the thesis. In particular, road maps make use of the fact that roads are highly structured, which allows rel- atively simple and powerful models to be employed. It is shown how the informa- tion of the lane markings, obtained by a front looking camera, can be fused with inertial measurement of the vehicle motion and radar measurements of vehicles ahead to compute a more accurate and robust road geometry estimate. Further, it is shown how radar measurements of stationary targets can be used to estimate the road edges, modeled as polynomials and tracked as extended targets. Recent advances in the field of multiple target tracking lead to the use of finite set statistics (fisst) in a set theoretic approach, where the targets and the measure- ments are treated as random finite sets (rfs). The first order moment of a rfs is called probability hypothesis density (phd), and it is propagated in time with a phd filter. In this thesis, the phd filter is applied to radar data for construct- ing a parsimonious representation of the map of the stationary objects around the vehicle. Two original contributions, which exploit the inherent structure in the map, are proposed. A data clustering algorithm is suggested to structure the description of the prior and considerably improving the update in the phd filter. Improvements in the merging step further simplify the map representation. When it comes to tracking moving targets, the focus of this thesis is on extended targets, i.e., targets which potentially may give rise to more than one measure- ment per time step. An implementation of the phd filter, which was proposed to handle data obtained from extended targets, is presented. An approximation is proposed in order to limit the number of hypotheses. Further, a framework to track the size and shape of a target is introduced. The method is based on mea- surement generating points on the surface of the target, which are modeled by an rfs. Finally, an efficient and novel Bayesian method is proposed for approximating the tire radii of a vehicle based on particle filters and the marginalization concept. This is done under the assumption that a change in the tire radius is caused by a change in tire pressure, thus obtaining an indirect tire pressure monitoring system. The approaches presented in this thesis have all been evaluated on real data from both freeways and rural roads in Sweden. v Populärvetenskaplig sammanfattning Dagens bilar blir säkrare för varje år. Tidigare var det den passiva säkerheten med bilbälten, krockkuddar och krockzoner som förbättrades. Nu sker de snabbaste förändringarna inom aktiv säkerhet, med förarstödsystem såsom antisladd och nödbromssystem och förarvarningssystem för farliga filbyten och framförvaran- de hinder. Förarstödsystem kräver omvärldsuppfattning, och de sensorer för detta som finns i bruk idag är kamera och radar. Kameran ser t.ex. filmarkeringar och kan använ- das för att detektera fordon och fotgängare. Radarn är mycket bra på att detekte- ra rörliga objekt och på avståndsbedömning, och används idag för att följa bilen framför. Radarn ger en mängd information som idag är outnyttjad, och ett syfte med avhandlingen är att undersöka hur radarinformationen kan användas fullt ut i framtida säkerhetssystem. Ett bidrag i avhandlingen är att använda radarns mätningar av stillastående ob- jekt på och jämte vägen för att bygga upp en lokal karta. Kartan visar områden som är körbara och hinder som ska undvikas. Ett annat bidrag är att sortera alla de träffar från rörliga objekt som radarn registrerar och ge en noggrann karta över hur många andra fordon det finns, samt deras positioner, hastigheter och storle- kar. Tillsammans bildar dessa två kartor en lägesbild som kan användas i nästa generations kollisionsundvikande system som kan kombinera broms och styring- repp för att på ett intelligent, säkert och kontrollerat sätt väja undan i kritiska nödsituationer. En annan typ av säkerhetssystem är varningssystem till föraren på förändring- ar i fordonsdynamiska parametrar som kan försämra köregenskaperna. Exempel på sådana parametrar är däcktryck, friktion och fellastning. Ett bidrag i avhand- lingen är ett nytt sätt att skatta en sänkning i däcktryck, genom att kombinera sensorer i fordonet med satellitnavigering. De metoder som presenteras i avhandlingen har utvärderats på verkliga data från bland annat motorvägar och landsvägar i Sverige. vii Acknowledgments It all started in the end of the 80s, when my teacher asked me what I wanted to become when I grow up. I had only one wish, to study at Chalmers; followed by my answer my teacher laughed and told me “you will never make it, you are too stupid". This incentivized me, and encourage by other teachers I managed to finish school and be admitted at Chalmers. Several years later I’m now here and I have finished my thesis, something I would never have managed alone, without the support of many wonderful persons. I would like to thank my supervisor professor Fredrik Gustafsson for his positive, inspiring, encouraging and relaxed attitude and my co-supervisor Dr. Thomas B. Schön for giving me a speedy start into academic research. Whenever I run into problems Dr. Umut Orguner, who possesses an enormous amount of knowledge about everything beyond least squares, pushed me in the right direction. Thank you for your help and for always having time. Further, I would like to acknowl- edge professor Svante Gunnarsson, who is a skillful head of the group, and his predecessor professor Lennart Ljung. They have created a wonderful research atmosphere which makes enjoyable going to office. Part of the work has been performed with colleagues. I would like to mention Lic. Karl Granstöm, since one can’t find a better partner to collaborate with. Further I like to mention Dr. Lars Hammarstrand, at Chalmers, with whom I had very useful and interesting discussions. Finally, I would like to thank Dr. Emre Özkan, who opened the Gaussian window and let me see other distributions. Andreas Andersson at Nira Dynamics AB and Dr. Andreas Eidehall at Volvo Per- sonvagnar AB have rescued me from the simulation swamp, by supporting me with measurement data from prototype vehicles. Dr. Gustaf Hendeby and Dr. Henrik Tidefelt helped me with latex issues, without their support this thesis would not be in this professional shape. Ulla Salaneck, Åsa Karmelind and Ninna Stensgård have helped with many practical things during the years in the group. An important part of the PhD studies is the time spent outside the office bunker, i.e., in the fika room and in pubs. I would like to thank Lic. Jonas Callmer, Lic. Karl Granström and Lic. Martin Skoglund for sharing vagnen with me. Further I would like to thank Lic. Christian Lyzell, Dr. Ragnar Wallin, Dr. Henrik Ohls- son Lic. Zoran Sjanic and Sina Khoshfetrat Pakazad for being generous and good friends. Dr. Wolfgang Reinelt became a mentor for me, he supported and encouraged me during the time at ZF Lenksysteme GmbH. With him I wrote my first publica- tions. My former boss Gerd Reimann taught me about vehicle dynamics and the importance of good experiments; I will always remember Sinuslenken. One of the most inspiring persons I met is Dr. Peter Bunus. Together with pro- fessor Fredrik Gustafsson, Dr. David Törnqvist, Lic. Per Skoglar and Lic. Jonas Callmer we started SenionLab AB. I’m looking forward to keeping on working with all of them in the near future. ix x Acknowledgments I would like to acknowledge the supported from the SEnsor fusion for Safety (sefs) project within the Intelligent Vehicle Safety Systems (ivss) program and the support from the Swedish Research Council under the frame project grant Extended Target Tracking. I would never have been able to fulfill my wish to study without the support from my family, for this I am endlessly thankful. Finally, I would like to thank amore della mia vita Nadia, who has brought so much love and joy into my life. Linköping, October 2011 Christian Lundquist Contents Notation xvii I Background 1 Introduction 3 1.1 Sensor Fusion . 3 1.2 Automotive Sensor Fusion . 4 1.3 Sensor Fusion for Safety . 6 1.4 Extended Target Tracking . 7 1.5 Components of the Sensor Fusion Framework . 7 1.6 Publications . 10 1.7 Contributions . 16 1.8 Thesis Outline . 16 1.8.1 Outline of Part I . 17 1.8.2 Outline of Part II . 17 2 Models of Dynamic Systems 21 2.1 Overview of the Models Used in the Thesis . 22 2.2 Discretizing Continuous-Time Models .