Linköping Studies in Science and Technology. Dissertations No. 950 Collision Avoidance Theory with Application to Automotive Collision Mitigation Jonas Jansson Department of Electrical Engineering Linköping University, SE–581 83 Linköping, Sweden Linköping 2005 Collision Avoidance Theory with Application to Automotive Collision Mitigation c 2005 Jonas Jansson [email protected] http://www.control.isy.liu.se Division of Automatic Control Department of Electrical Engineering Linköping University SE–581 83 Linköping Sweden ISBN 91-85299-45-6 ISSN 0345-7524 Printed by UniTryck, Linköping, Sweden 2005. To Eva, Alexander, and Alice Abstract Avoiding collisions is a crucial issue in most transportation systems as well as in many other applications. The task of a collision avoidance system is to track objects of potential collision risk and determine any action to avoid or mitigate a collision. This thesis presents theory for tracking and decision making in collision avoidance systems. The main focus is how to make decisions based on uncertain estimates and in the presence of multiple obstacles. A general framework for dealing with nonlinear dynamic systems and arbitrary noise distributions in collision avoidance decision making is proposed. Some novel decision functions are also suggested. Furthermore, performance evaluations using simulated and experimental data are presented. Most examples in this thesis are from automotive applications. A driving application for the work presented in this thesis is an automotive emergency braking system. This system is called a collision mitigation by braking (CMbB) system. It aims at mitigating the consequences of an accident by applying the brakes once a collision becomes unavoidable. A CMbB system providing a maximum collision speed reduction of 15 km/h and an average speed reduction of 7.5 km/h is estimated to reduce all injuries, classiﬁed as anything between moderate and fatal, for rear-end collisions by 16 %. Since rear-end collision correspond to approximately 30 % of all accidents this corresponds to a 5 % reduction for all accidents. The evaluation includes results from simulations as well as two demonstrator vehicles, with diﬀerent sensor setups and diﬀerent decision logic, that perform autonomous emergency braking. i Acknowledgments For me the project leading up to this thesis has been a long (roughly 60000 km) and fantastic journey. To help me on this journey I have had two excellent guides Professor Fredrik Gustafsson at Linköping University and Jonas Ekmark Volvo Car Corporation. You have helped me stay on a collision free path throughout the project and I have greatly enjoyed working with both of you. Another guide on my journey has been Robert Hansson. Your support of my project has been invaluable and I feel privileged having worked under your leadership. Several people helped me with proof reading this thesis. I am specially grateful to Dr. Rickard Karlsson, Gustaf Hendeby, and Andreas Eidehall who helped me with the illustrations. You really went out of your way to help me during the ﬁnal stage of my writing. For this I am very grateful. I would also like to thank Ulla Salaneck, Dr. Ragnar Wallin, Michael Shanahan, and Daniel Levin you all contributed signiﬁcantly to improve my work. Other people who deserve to be thanked for helping me during diﬀerent phases of the project are Magdalena Lindman VCSC for providing accident statistics. Lars Nilsson VTEC, Fredrik Lundholm VCC, and Lena Westervall VCC who all have helped performing various tests with the demonstrator vehicles. I also express my gratitude to Professor Lennart Ljung, Dr. Ahmed El-Bahrawy, and Raymond Johansson who together with Robert Hansson recruited me for this project. I would also like to acknowledge all my colleagues at the Chassi & Vehicle Dynamic department at Volvo and at the Control & Communication group at Linköping University, it has been a privilege working with all of you. This project was supported by the Swedish foundation for strategic research through the Excellence Center in Computer Science and Systems Engineering in Linköping (ECSEL) and Volvo Car Corporation’s PhD program, for this I am very grateful. Finally I would like to thank my family, Eva for enduring my endless travels between Linköping and Göteborg, Alexander for helping me design the cover of the thesis, and Alice for keeping me awake with cheerful shouts of support while writing these acknowledgments at 2:00 am in the morning. Jonas Jansson Linköping, April 2005 iii Contents I Introduction 1 Preliminaries 1.1 Background . . 1.2 Thesis Outline 1.3 Contributions . 1.4 Publications . . 1 . . . . . . . . . . . . . . . . 3 3 4 4 5 2 Collision Avoidance Overview 2.1 Collision Avoidance Applications . . . . . . . . . . . . . . . . . 2.1.1 Automotive Collision Avoidance . . . . . . . . . . . . . 2.1.2 Aerospace Applications . . . . . . . . . . . . . . . . . . 2.1.3 Marine Applications . . . . . . . . . . . . . . . . . . . . 2.1.4 Industrial Robot Manipulators . . . . . . . . . . . . . . 2.1.5 Unmanned Autonomous Vehicles . . . . . . . . . . . . . 2.2 Automotive Safety . . . . . . . . . . . . . . . . . . . . . . . . . 2.2.1 Passive Safety . . . . . . . . . . . . . . . . . . . . . . . . 2.2.2 Active Safety . . . . . . . . . . . . . . . . . . . . . . . . 2.2.3 Forward Collision Mitigation . . . . . . . . . . . . . . . 2.2.4 Ideal Collision Mitigation by Braking Performance . . . 2.2.5 Automotive Safety History . . . . . . . . . . . . . . . . 2.3 Accident Statistics . . . . . . . . . . . . . . . . . . . . . . . . . 2.3.1 Accident Types . . . . . . . . . . . . . . . . . . . . . . . 2.3.2 Eﬀectiveness of Collision Mitigation by Braking Systems 2.3.3 Weather Conditions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 7 8 8 8 9 9 9 11 11 14 15 16 17 17 18 22 II . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Tracking 25 3 Automotive Tracking Sensors 3.1 The Radar Sensor . . . . . . . . . . . . 3.2 The Lidar Sensor . . . . . . . . . . . . . 3.3 Vision Systems for Obstacle Recognition 3.4 Infrared Vision Sensors . . . . . . . . . . v . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 27 29 30 32 vi Contents 3.5 3.6 Inter-Vehicle Communication . . . . . . . . . . . . . . . . . . . . . . 32 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 4 Tracking for Collision Avoidance Applications 4.1 The Kalman Filter . . . . . . . . . . . . . . . . . . . . . . 4.1.1 The Stationary Kalman Filter . . . . . . . . . . . . 4.1.2 The Time-Varying Kalman Filter . . . . . . . . . . 4.1.3 The Extended Kalman Filter . . . . . . . . . . . . 4.2 The Point Mass Filter . . . . . . . . . . . . . . . . . . . . 4.3 The Particle Filter . . . . . . . . . . . . . . . . . . . . . . 4.4 Measurement Association . . . . . . . . . . . . . . . . . . 4.4.1 Gating . . . . . . . . . . . . . . . . . . . . . . . . . 4.4.2 Measurement Association Methods and Techniques 4.5 Ranking Measurements . . . . . . . . . . . . . . . . . . . 4.5.1 Ranking Measurements by Range Only . . . . . . 4.5.2 Ranking by Range Rate and Time-to-Collision . . 4.5.3 Ranking Objects by their Azimuth Angle . . . . . 4.6 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . 4.7 Models for Tracking and Navigation . . . . . . . . . . . . 4.7.1 The Constant Velocity Model . . . . . . . . . . . . 4.7.2 The Constant Acceleration Model . . . . . . . . . 4.7.3 The Coordinated Turn Model . . . . . . . . . . . . 4.7.4 The Nearly Coordinated Turn Model . . . . . . . . 4.8 Navigation . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.8.1 Bicycle Model . . . . . . . . . . . . . . . . . . . . . III . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Decision Making 5 Deterministic Decision Making 5.1 Collision Avoidance Decision Strategy . . . . . . . . . . . . . . . . . 5.2 Static Analysis of Collision Mitigation . . . . . . . . . . . . . . . . . 5.3 Collision Avoidance Decision Functions . . . . . . . . . . . . . . . . 5.3.1 Headway and Headway Time . . . . . . . . . . . . . . . . . . 5.3.2 Time to Collision . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.3 Closest Point of Approach . . . . . . . . . . . . . . . . . . . . 5.3.4 Potential Force . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.5 Required Longitudinal Acceleration . . . . . . . . . . . . . . 5.3.6 Required Longitudinal Acceleration for Piecewise Constant Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3.7 Required Longitudinal Acceleration with Actuator Dynamics 5.3.8 Required Lateral Acceleration . . . . . . . . . . . . . . . . . . 5.3.9 Required Centripetal Acceleration . . . . . . . . . . . . . . . 5.3.10 Multi-Dimensional Acceleration to Avoid Collision . . . . . . 35 37 37 38 39 41 42 43 44 45 47 49 49 50 50 53 54 55 55 56 58 59 63 65 65 68 71 72 73 73 74 75 75 76 78 79 80 Contents vii 6 Multiple Obstacles Decision Making 6.1 Constant Control . . . . . . . . . . . . . . . . . . . . 6.1.1 Multiple Objects Constant Acceleration . . . 6.1.2 Summary of the Constant Control Approach 6.2 Heuristic Control . . . . . . . . . . . . . . . . . . . . 6.3 Exhaustive Search . . . . . . . . . . . . . . . . . . . 7 Statistical Decision Making 7.1 Numerical Approximation . . . . . . . . . 7.2 Analytical Approximation . . . . . . . . . 7.2.1 Estimating Statistical Properties of 7.3 Probability of Collision . . . . . . . . . . . IV . . . . . . . . . . . . . . . 83 83 84 85 85 87 . . . . . . . . . . . . . . . . . . . . . . . . . . the Decision Function . . . . . . . . . . . . . . . . . . . . . 91 93 95 95 96 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Evaluation 8 Collision Mitigation by Braking System Testing 8.1 Simulation Environment . . . . . . . . . . . . . . . . . . . . . . 8.2 The Demonstrator Vehicles . . . . . . . . . . . . . . . . . . . . 8.2.1 Demonstrator Vehicle Tracking Sensors . . . . . . . . . 8.2.2 The Braking System of the First Demonstrator Vehicle 8.3 Demonstrator 1 — Probabilistic Decision Making . . . . . . . . 8.3.1 Collision Scenarios . . . . . . . . . . . . . . . . . . . . . 8.3.2 Scenarios to Provoke Faulty Interventions . . . . . . . . 8.3.3 Test Results . . . . . . . . . . . . . . . . . . . . . . . . . 8.4 Demonstrator 2 — Multiple Object Decision Making . . . . . . 8.4.1 Test Results . . . . . . . . . . . . . . . . . . . . . . . . . 101 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103 104 105 107 109 112 112 120 126 128 128 9 Supplementary Simulation Studies 9.1 Comparison of Multiple Object Algorithms . . . . . . . . . . . . 9.1.1 Test Method and Speciﬁcations . . . . . . . . . . . . . . . 9.1.2 Test Results . . . . . . . . . . . . . . . . . . . . . . . . . . 9.2 Statistical Decision Making by Stochastic Numerical Integration 9.2.1 Tracking Model . . . . . . . . . . . . . . . . . . . . . . . . 9.2.2 Estimation Approach . . . . . . . . . . . . . . . . . . . . 9.2.3 State Noise Model . . . . . . . . . . . . . . . . . . . . . . 9.2.4 Measurement Noise Model . . . . . . . . . . . . . . . . . . 9.2.5 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . 9.2.6 Simulations and Tests . . . . . . . . . . . . . . . . . . . . 9.2.7 Simulation Results . . . . . . . . . . . . . . . . . . . . . . 9.2.8 Hardware and Real-Time Issues . . . . . . . . . . . . . . . 9.2.9 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . 9.3 Statistical Decision Making Using a Gaussian Approximation . . 9.3.1 Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9.3.2 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133 133 133 134 136 136 136 136 137 139 140 141 141 142 143 144 147 viii Contents 10 Summary V 149 Appendices 151 A Coordinate Systems 153 B Notation 155 C Safety Systems Introduced in Volvo Cars 159 Bibliography 161 Part I Introduction 1 Chapter 1 Preliminaries Safe and collision-free travel is vital in today’s society. It is also an important issue in many industrial processes. In aerospace and naval applications, radar based support systems to avoid collisions have been used for several decades. Currently, collision avoidance (CA) systems are starting to emerge in automotive applications. The challenge in designing a CA system is in balancing the eﬀectiveness of avoiding collisions versus the risk of false alarms. Automotive applications in particular present several challenges; dense traﬃc causing complex scenarios with many moving objects; low-cost sensors and computational units have to be used. Furthermore, the dynamic capabilities of a vehicle may change rapidly, e.g. tire-toroad friction may change signiﬁcantly from one moment to the next. This thesis discusses general theory for CA decision making and its application in automotive systems. The main focus is dealing with uncertainties in the decision making process and how to handle complex multiple obstacle scenarios. Speciﬁcally, a framework for dealing with uncertainty is introduced. In this framework stochastic numerical integration is used to evaluate the conﬁdence of each decision. Furthermore, algorithms for decision making in multiple obstacle scenarios are presented. The proposed algorithms use diﬀerent strategies to search the set of feasible avoidance maneuvers, to ﬁnd an escape path if it exists. Some novel collision avoidance decision functions are also introduced. These functions address diﬀerent issues such as brake system characteristics, ﬁnding the optimal avoidance maneuver for a constant acceleration motion model, and changing obstacle dynamics when the obstacle comes to a stop. 1.1 Background The work on this thesis has been carried out in cooperation between Volvo Car Corporation and the Control and Communication group at Linköping university. A driving application for the project has been an emergency braking system that tries to reduce the collision speed by autonomous braking. Thus, most of the 3 4 Chapter 1 Preliminaries examples in this thesis come from automotive applications. The initial research question was to ﬁnd methods and algorithms for tracking and decision making for automotive collision mitigation systems that ensure a positive and noticeable impact on accident statistics, while minimizing the risk of false alarms. 1.2 Thesis Outline This thesis presents theory for tracking and decision making in collision avoidance systems. The main application example is from automotive collision avoidance. Part I of the thesis contains an introduction and gives an overview of diﬀerent collision avoidance applications and automotive safety. The remaining thesis is divided into three parts. In Part II existing tracking and positioning theory is reviewed. In particular a Bayesian approach to tracking is emphasized. In the Bayesian approach each parameter of interest is considered to be a stochastic variable. The goal is to ﬁnd the entire probability density function for each variable. Furthermore, an overview of the most common tracking sensors is presented. Typical performance of sensors used in automotive applications is also discussed. In Part III theory for collision avoidance decision making is discussed: most of the research contribution is in this part. Several novel collision risk metrics are introduced and analyzed in Section 5.3. Algorithms for multiple obstacles are discussed in Chapter 6. In Chapter 7 a framework based on statistical decision making is introduced. In Part IV evaluation of several automotive emergency braking systems is performed, using theory from Part I and II. Tests with two diﬀerent demonstrator vehicles and simulation studies are presented. In the ﬁrst demonstrator vehicle, probabilistic decision making and a sensor fusion concept is tested. The second demonstrator vehicle was used to test multiple object decision making. The simulation studies evaluate diﬀerent approaches to calculating the probability of a collision and an extensive comparison of multiple object CA algorithms. 1.3 Contributions The main contributions of this thesis are: • A framework for collision avoidance decision making based on statistical decision making, in Chapter 7. • Three novel algorithms for multiple object collision avoidance decision making, in Chapter 6. • Results of computational complexity bounds of a tree-search algorithm for multiple object collision avoidance decision making, in Chapter 6. Section 1.4 Publications 5 • A comparison of two alternative methods for calculating the acceleration required to avoid a collision in two-dimensional motion, in Section 5.3. • Novel alternative expressions to calculate the longitudinal acceleration required to avoid a collision, in Section 5.3. • Results from ﬁeld tests with two demonstrator vehicles, Chapter 8. • A comparison of three alternative multiple object algorithms in a comprehensive simulation study, in Section 9.1. • A real-time implementation of a probabilistic CA tracking and decision algorithm based on particle ﬁltering methods and stochastic numerical integration, in Section 9.2. 1.4 Publications This sections presents the publications, patents, and patent applications written or co-authored by the author, relating to this thesis. Publications: F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P-J Nordlund. Particle ﬁlters for positioning, navigation and tracking. IEEE Transactions on Signal Processing, 50:425–437, February 2002. J. Jansson, J. Ekmark, and F. Gustafsson. Decision making for collision avoidance systems. In SAE technical paper 2002-01-0403, volume SP1662, Detroit, MI, USA, March 2002. Society of Automotive Engineers 2002 World Congress. J. Jansson and F. Gustafsson. A probabilistic approach to collision risk estimation for passenger vehicles. In Proceedings of the 15th International Federation of Automatic Control World Congress, pages T–Th– M05:Model–based fault diagnosis of automotive systems/Area code 8b : Automotive Control, Barcelona, Spain, July 2002. J. Jansson. Tracking and Decision Making for Automotive Collision Avoidance. Linköping Studies in Science and Technology. Licentiate Thesis No. 965, Linköping University, Linköping, Sweden, September 2002. R. Karlsson, J. Jansson, and F. Gustafsson. Model-based statistical tracking and decision making for collision avoidance application. In Proceedings of the American Control Conference, Boston, MA, USA, July 2004. 6 Chapter 1 Preliminaries J. Jansson. Advanced Microsystems for Automotive Applications 2004, chapter Dealing with Uncertainty in Automotive Collision Avoidance, pages 165–180. Springer, Berlin, Germany, March 2004. ISBN 3-54020586-1. J. Jansson and F. Gustafsson. Multiple object collision avoidance decision making. Submitted to International Federation of Automatic Control Journal: Control engineering practice, 2005. J. Jansson and F. Gustafsson. A framework and overview of collision avoidance decision making. Submitted to International Federation of Automatic Control Journal: Automatica, 2005. Patents: J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. Collision mitigation by braking system. U.S. Patent No. 6607255, 2001. J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. Accelerator actuated emergency braking system. U.S. Patent No. 6655749, 2001. J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. System for exerting an ampliﬁed braking force. U.S. Patent No. 6659572, 2001. J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. Radar-guided collision mitigation system with a passive safety feature. U.S. Patent No. 6617172, 2001. J. Jansson and J. Johansson. A probabilistic framework for decision making in collision avoidance. Swedish/German/UK Patent No. 273930, 2001. Patent applications: J. Jansson and J. Ekmark. A method and a computer readable storage device for estimating tire-to-road friction. U.S. Application No. 10/ 858896, patent pending, 2004. J. Jansson and W. Birk. An alternative brake criterion for collision avoidance. Submitted to the European patent authority, patent pending, 2005. J. Jansson and J. Ekmark. A multiple object collision avoidance algorithm. Submitted to the European patent authority, patent pending, 2005. Chapter 2 Collision Avoidance Overview Avoiding collisions is a crucial issue in most transportation systems as well as many other applications. Detecting and avoiding a possible collision have been studied for several diﬀerent ﬁelds of application such as air traﬃc control (ATC), automotive collision avoidance, robot manipulator control etc. The task of any collision avoidance system is ultimately to avoid two or more objects from colliding. However, in this thesis the notion of a collision avoidance system will be extended to also include systems trying to reduce the consequence of an imminent collision i.e., collision mitigation (CM) systems. CA systems avoid collisions either by performing an autonomous avoidance maneuver or by issuing a warning to an operator. To mitigate collision consequences other actions might be taken, e.g. in an automotive application this might be to pretension the seat-belts and inﬂate the airbags; in a ﬁghter aircraft one could consider ejecting the pilot if a collision is unavoidable. Any action performed by a CA system will be called an intervention. Depending on application and the type of intervention considered, the metric for measuring collision threat and the decision making algorithm might vary signiﬁcantly. This chapter presents an overview of common collision avoidance applications, it also discuses passenger car safety, which is the main motivation for the work in this thesis. The last part of this chapter is devoted to accident statistic and a brief analysis of the potential beneﬁt of a speciﬁc automatic emergency braking system. 2.1 Collision Avoidance Applications CA systems are being used in a wide range of diﬀerent areas and under very diﬀerent circumstances. Typical sensors used to detect obstacles are radar, lidar or vision sensors. This section presents an overview of some of the important areas of application and highlights important issues. 7 8 Chapter 2 Collision Avoidance Overview 2.1.1 Automotive Collision Avoidance Traﬃc accidents are one of the major causes of death and injuries in todays society. Automotive manufactures have started to introduce more and more driver support systems to help prevent accidents. The ﬁrst step in CA systems for automotive applications is adaptive cruise control (ACC), which is currently available as an option for several car models. ACC systems adapt the speed to any in-path vehicle, should it travel slower than the set speed of the host vehicle. The cruise control system is only allowed to exert limited deceleration (typically −3 m/s2 ); some systems also issue a warning to the driver when this acceleration is not suﬃcient to avoid collision. Current ACC systems are sold as a comfort system and can be switched on and oﬀ by the driver; they also disengage at low speeds (below 40 km/h). The next step in automotive CA is to introduce systems that are always active and perform autonomous braking and/or give warning when a collision is imminent. Such systems are starting to emerge on the market, and are described in [10, 20, 61, 77, 85, 119]. A big challenge in automotive collision avoidance is that even in normal driving, the traﬃc situation might be very complex from a sensing view point, with numerous obstacles to be detected and classiﬁed. In addition to this, sensors and computational resources must be low-cost. Another issue is that often the tire-to-road friction is unknown and might change rapidly. A more thorough introduction to automotive CA is given in Section 2.2. 2.1.2 Aerospace Applications Radar-based air traﬃc control (ATC) systems have been in use for several decades. Traﬃc alert and collision avoidance system (TCAS) has been used on board US transport aircraft since the beginning of 1990s. These systems typically aim at helping pilots and air traﬃc controllers in keeping a regulated minimum separation between any two aircraft. A breach of this distance is referred to as a conﬂict, [125]. In [81] an extensive review of methods for collision avoidance in ATC systems is given. This area becomes ever more interesting as the airspace surrounding large airports becomes more crowded. There exist diﬀerent types of systems both in cockpit and on the ground to provide decision-support to pilots and air traﬃc controllers. More references to ATC applications can be found [80, 104, 109, 132]. Other applications of airborne collision avoidance is mid-air collision avoidance for ﬁghter aircrafts. Here, the CA system also attempt to maintain a regulated minimum separation between aircrafts. Apart from issuing a warning the system may also be authorized to take over steering controls to perform an evasive maneuver to avoid collision. Such a system is described in [116]. 2.1.3 Marine Applications In marine applications, radar systems are commonly used to detect other vessels in the vicinity. For larger ships automatic radar plotting aid (ARPA) [17] is widely used. These systems provide valuable help in adverse weather conditions, when other vessels might go unnoticed, and also at large ports with high vessel density. Section 2.2 Automotive Safety 9 Large ships normally have very limited maneuvering capabilities. Therefore it is crucial that a possible collision is detected early so that an avoidance maneuver can be initiated before the collision becomes unavoidable [40]. As with ATC, CA is a growing issue as the waters close to large ports are becoming more crowded. 2.1.4 Industrial Robot Manipulators Industrial robotic control is also a discipline where CA plays an important role. These applications diﬀer from the ones discussed previously in that normally no human operator is involved. Instead, one is interested in ﬁnding a control law which guarantees reaching a goal state without colliding. Thus, the CA system is an integral part of a control system that has other mission objectives. Common approaches to this problem are potential ﬁeld algorithms, which was introduced in [76] and path planning methods [74, 84, 117]. Manipulator arms are often described by a kinematic chain and can be quite complex. This often results in quite complex kinematic models. Hence, one often talks about conﬁguration space or joint space of the robot. Further references that deal with nonholonomic kinematics, uncertainty and multiple moving obstacles are given in [34, 95, 96, 129]. 2.1.5 Unmanned Autonomous Vehicles There are numerous examples of applications with unmanned autonomous vehicles. Similarly to robot manipulator applications, one is here normally not interested in a system that takes over or warns a human operator. Instead, one looks for a controller that tries to reach the goal in an eﬀective manner and at the same time avoid collisions with any obstacle. Often similar CA algorithms that are applied for robot manipulators can also be used for autonomous vehicles. Apart from CA, for autonomous vehicles, simultaneous localization and map-building is often also an issue [21, 29, 30]. For detailed discussions of autonomous vehicles see [18]. 2.2 Automotive Safety Passenger car safety is an issue that has received increasing attention over the past few decades. Over the last decades, automobile crashworthiness has improved drastically. The risk for a fatal injury in a new car has decreased by 90% compared to a car from the early 80’s, according to [32]. Other evidence of improvements in crash performance can be found from the results of crash tests carried out by organizations all over the world (see Section 2.2.1). Despite this progress, many people are still killed or injured in traﬃc accidents. In the USA, there were 37409 accidents reported in FARS (a database recording all fatal car crashes reported in the USA [28]) for year 2000. In the reported accidents, a total of 41821 people were killed. The number of non-fatal crashes and non-fatal injuries the same year is of course much greater. Despite the improved crashworthiness of modern cars it is clear that there is still a need to signiﬁcantly reduce traﬃc related injuries. Having evolved rapidly, existing passive safety technologies have become quite mature. 10 Chapter 2 Collision Avoidance Overview Figure 2.1: Example of a CM system. The system uses radar to measure distance and speed relative to the vehicle ahead. If there is danger of colliding, the system warns the driver by a ﬂashing red light projected on the windshield. Therefore, a signiﬁcant reduction of injuries by passive safety technologies becomes technically infeasible as well as too costly. In recent years, people in the automotive industry have increased their eﬀorts in trying to ﬁnd other ways of reducing the number of people injured in traﬃc. One alternative way to avoid dangers is to try to prevent accidents from happening by means of driver support systems. One of the ﬁrst such systems was the anti blockier system (ABS) braking system (see [3] for a description), which helps the driver maintain steerability during hard braking maneuvers. One active safety system which have proved very eﬀective in reducing the risk for a injury is the yaw control which will be described below. In [32] this system is claimed to reduce the risk for a injury by up to 50% depending on the road condition. In PartIV of this thesis prototype vehicles hosting a collision mitigation systems will be studied. The CM system uses sensors to observe the environment directly in front of the host vehicle. Based on information from the forward-looking sensors, a decision can be made to deploy countermeasures to avoid imminent frontal collisions. Typical interventions are warning signals (audible, visible or haptic/tactile), braking and steering. Figure 2.1 shows an example of a CM system that warns the driver of an imminent threat by a visible display. When vehicle safety is discussed many people normally think of what will be referred to as passive safety. However, in modern cars, several other aspects have to be considered. Systems such as ABS brakes, yaw control etc. are introduced to enhance driver and passenger safety. These systems are sometimes called active safety systems. Conversely, a car with good handling Section 2.2 Automotive Safety 11 capability is sometimes also said to have good active safety. In Sections 2.2.1 and 2.2.2 an account of diﬀerent aspects of vehicle safety is given. In broad terms the safety properties of a car can be divided into three groups: • Security • Passive safety • Active safety Security deals with the car’s capacity to protect against theft and vandalism and possibly also attacks on the owner. This will not be discussed further in this thesis. The deﬁnition of passive and active safety will be discussed in more detail in the following sections. 2.2.1 Passive Safety When a vehicle’s passive safety properties are discussed, this refer to the ability of the car to protect passengers from injuries in a collision. The ability of the car to protect its passengers is sometimes called crashworthiness. In this thesis a car’s crashworthiness or passive safety properties is deﬁned as its ability to protect passengers from injury from the instant that the collision occurs and afterwards. One way to achieve good passive safety is to optimize the structure of the car’s body in such a way that it absorbs crash energy whilst keeping the crash pulse (momentaneous acceleration) experienced by the passengers as low as possible. Some systems that help to protect the passengers in a collision are seat belts, airbags, belt pretensioners, and belt load limiters. The interior of the car should also be designed in such a way that panels, switches, etc. cause minimal injury. To achieve good passive safety properties, quite a few areas of the car’s design must be considered. The crashworthiness of automobiles is tested in collision tests. Organizations that perform such tests are EuroNCAP [101], ANCAP [100], JNCAP [68], and NHTSA [102]). In the tests performed by these organizations, passive safety properties are measured in terms of acceleration and forces experienced by a crash test dummy in speciﬁc collision scenarios. The crashworthiness of cars is also evaluated by insurance institutes (for example, Folksam [31] and IIHS [33]) which rate the performance for diﬀerent vehicles in real-life accidents. 2.2.2 Active Safety Active safety also comprises several diﬀerent aspects of the car. Here, they are divided into three groups: preventive, dynamic and collision mitigation. A brief account of these groups is given below: 1. Preventive — The preventive aspect of active safety is about the driver seeing threats and being seen himself. If threats are detected early the driver can take precautionary action before the situation becomes dangerous. Several factors play a role in this: 12 Chapter 2 Collision Avoidance Overview One safety technology, aimed to improve the drivers perception, presented in the Volvo safety concept car was a see-through A-pillar. Figure 2.2: • Detecting threats: To improve the driver’s perceptive ability the traditional approach is through improving headlight performance, headlight cleaning, windshield cleaning, minimizing glare in the windows etc. Examples of new innovations that improve driver perception are night vision systems and see-through A-pillars. A night vision system uses an IR camera and a driver display unit to present the IR image to the driver. This enables the driver to detect warm objects further away than is possible using conventional headlights. In [82, 90, 118] night vision systems for automotive applications are discussed. The see-through A pillar, which was presented in the Volvo Safety Concept Car (SCC), improves the driver’s perception by allowing him or her to see through the A-pillar, see Figure 2.2. • Visibility: Visibility of the driver’s own vehicle is another important issue in preventive active safety. In short this means that the car should be designed in such a way that it is easily observed by other road-users. Examples of features to improve visibility are daytime running lights and high-positioned brake lights. • Driver information: Driver information also plays an important role in the driver’s ability to detect dangers. Through GPS technology and digital maps, the driver can be provided with geographically-dependent information. Examples of other systems that could provide the driver with essential information to avoid collisions are, road friction monitoring systems [47] and tire pressure monitoring systems [106, 107]. It is important to remember that the driver’s cognitive ability is limited, and Section 2.2 Automotive Safety 13 information overload can cause the driver to fail to detect, or react erroneously to, imminent threats. Today, there are systems that prioritize what information to display to the driver depending upon the situation, e.g. the intelligent driver information system (IDIS) introduced on the Volvo S40 and V50. • The driver: To ensure that the driver’s perception of the traﬃc environment is adequate, it might be sensible to monitor the driver himself. A driver monitoring system can, for instance, be used to warn the driver if he falls asleep behind the steering wheel. 2. Dynamic — From a safety perspective, a car should be designed to have safe handling and ride characteristics. By this we mean that it should be easy for the driver to keep control of the vehicle in all road conditions, in any traﬃc situation and during all types of maneuvers. Examples: • It should be easy to perform avoidance maneuvers without losing control of the vehicle. • The car should be minimally inﬂuenced by side wind. In short, it should be easy for the driver to make the car follow his intended path. To achieve these properties, quite a few areas of the vehicle’s design come into play. Important factors are steering and brake system characteristics. There are already several active systems in existence today that improve a vehicle’s dynamic properties. Here “active systems” refers to the use of sensors and actuators to perform some kind of closed loop control to help the driver in controlling the vehicle. Some common systems are listed below: • ABS brake systems prevent the wheels from locking during hard braking. This enables the driver to achieve fair deceleration whilst maintaining steerability of the vehicle. • Yaw control systems monitor steering angle, yaw rate and lateral acceleration. Brake force is applied to individual wheels to aid the driver in keeping control of the vehicle in situations where the lateral traction limit is reached. • Traction control systems help maintain traction and stability by reducing engine torque and applying brakes if the wheels are spinning. • Roll stability systems apply individual brakes to decrease the risk of rolling over when the car experiences high roll angle rates and roll angles. 3. Collision Mitigation — CM systems are given some perception of the environment surrounding the vehicle. Based on this perception the system takes steps to avoid or mitigate imminent collisions. The main scope of this thesis falls in this ﬁeld. It must be pointed out that these systems are often called Collision Avoidance systems. The term “avoidance” might imply that accidents should be completely avoided by these systems. For several reasons, 14 Chapter 2 Collision Avoidance Overview systems that avoid all collisions are unattainable for automotive applications. This thesis will therefore talk about CM systems. Generally, a CM system will try to reduce the severity of the accident as much as possible under some constraints. In the best case, accidents might be avoided altogether whilst in the worst cases the systems have no positive eﬀect at all. The perception of a CM system can come from several sources. The environment may be perceived with radar sensors, laser radar, vision sensors, ultrasonic sensors, GPS sensors and inter-vehicle communication. Properties of individual sensors will be discussed in Chapter 3. Based on the information acquired from the sensors, the vehicle itself acts to prevent or mitigate collisions. Typical actions the systems can take to mitigate a collision are issuing a warning to the driver, applying the brakes, and changing the course of the vehicle by applying torque to the steering wheel. Other possible countermeasures might be activating the brake lights to avoid being hit from behind, early airbag inﬂation or adjusting the vehicle’s height to increase crash compatibility. Several systems exist or have been proposed with CM functionality: • Lane-keeping aid systems [75, 89] monitor the lane markings. Using the observations of the lane markings an estimate of the vehicle’s position in the lane can be obtained. Should the vehicle swerve out of the lane a warning can be issued or a steering intervention executed. • Lane change aid systems monitor the blind spot and some distance behind the car. The system can then warn or intervene by adding steering wheel torque to avoid a collision when switching lane. • Forward collision mitigation systems [10, 20, 26, 119] monitor what is in front of the host vehicle and intervene to prevent or mitigate a frontal collision. A more detailed introduction to CM systems is given in Section 2.2.3. • Adaptive cruise control works like normal cruise control, but adapts the speed to the vehicle in front; if the driver is closing in on a vehicle in the same lane. ACC is really a CM system. The reason that it is dealt with separately here is that it is marketed as a comfort system that is switched on and oﬀ by the driver examples of ACC system on the market are: Toyota’s Radar cruise control which is based on a lidar and was introduced in 1997; Mercede’s Distronic which is based on a mm-radar and was introduced in 1998; Jaguar’s Adaptive cruise control which is based on a mm-radar and was introduced in 1999, and BMW’s Active Cruise Control which is based on a mm-radar and was introduced in 2002 ). ACC systems are described in [130]. 2.2.3 Forward Collision Mitigation Forward looking CM systems mainly try to avoid or mitigate frontal collisions. The countermeasure that is discussed mainly in the application examples of this thesis is braking, and to some extent warning. Hereafter, forward collision warning systems Section 2.2 Automotive Safety 15 will be referred to as FCW systems and forward collision mitigation by braking systems as CMbB systems. Although FCW and CMbB systems are similar in many ways, they also present diﬀerent challenges. A FCW system, must rely on the driver to react quickly and correctly to the warning. However, this might not always be the case. A warning might actually startle the driver and cause his performance to degrade in a critical situation. A great deal of work is going on to determine how to warn the driver in the best way (e.g. see [131]). The timing in FCW and CMbB systems is also quite diﬀerent. In a FCW system, driver reaction time must be taken into account. In [85] it is stated that 85 % of all drivers are able to react to a warning within 1.18 seconds. Apart from reaction time, one must also take into account that the driver might not perform the optimal avoidance maneuver. Taking this into consideration means that a FCW system has to predict future states several seconds. For CMbB systems we assume that braking will be initiated at the last second, i.e., when a collision is becoming unavoidable or very close to this point. The reason for this is that a driver may in some cases drive in such a way that a collision is “spatially close” without the situation being critical. A FCW system therefore presents the problem of making correct predictions over a longer time horizon. In a CMbB system on the other hand the tolerance for faulty decisions is lower. In a FCW system some faulty interventions are tolerated, whilst in CMbB systems practically no faulty interventions are allowed. 2.2.4 Ideal Collision Mitigation by Braking Performance In this section a study of what performance to expect from the best possible CMbB system, for a head-on collision scenario, using a detailed handling model for the host vehicle will be made. The assumption here is that it is possible to determine exactly when a collision becomes unavoidable and immediately commence with automatic braking. The distance needed to avoid collision (by means of steering or braking) for the head-on collision scenario in is plotted in Figure 5.4. Assuming that there exists no better maneuver than pure steering or braking, the distance at which a collision becomes unavoidable is given by the minimum of the plotted braking distance and the steering avoidance distance in Figure 5.4. The steering avoidance distance and braking distance intersects at approximately 37 km/h, indicating that, to avoid a collision, braking is more eﬃcient at low speeds and steering is more eﬃcient at high speeds. To generate the escape path, a 15 degree-of-freedom handling model that has been veriﬁed against measured data from a Volvo S80 was used. The brake and steering system has a delay of 0.2 second and the brake system rise time is 0.3 seconds. From Figure 2.3 it can be noted that for low speed the dynamics is no longer important to determine the steering avoidance distance. Here, instead, the turning radius of the vehicle limits the possibility to avoid a object by means of steering. If it would be possible to start braking at the distance where a collision becomes unavoidable (by pure braking or steering) the collision speed would be as plotted in Figure 2.4. Thus, at relative speeds below 37 km/h, ideally, a collision could be completely avoided. At speeds above 37 km/h the speed should be reduced by 22 km/h. However, it should be stated that the maneuver 16 Chapter 2 Collision Avoidance Overview Distance neede to avoid collision by braking and steering 120 brakedist steerdist 100 Distance [m] 80 60 40 20 0 0 50 100 150 Speed [km/h] Figure 2.3: Distances required to avoid a stationary object collision by braking and by steering. This plot shows the result from simulation with a 15 degree-of-freedom handling model. Both the object and the host vehicle have a width of 2 meters. The steering maneuver is optimized for each velocity so that the lateral force is maximized. that has been simulated here is a simple one where the driver quickly turns the steering wheel (at 700 ◦ /second) either to the left or the right. The maneuver was optimized in the sense that the steering wheel is turned to the angle which optimizes lateral force for each velocity. It has not been studied if this actually is the most eﬃcient avoidance maneuver (by steering). Furthermore, avoidance at even shorter distances might be possible by a combined steering and braking maneuver. 2.2.5 Automotive Safety History Ever since automobiles were introduced to the mass market in the beginning of the century, safety has been an issue. During the ﬁrst half of the decade, the focus was mainly on energy-absorbing structures, reinforced steel passenger cages and visibility issues. In 1959 Volvo introduced the ﬁrst three-point seat belt. In the 1980s the ﬁrst driver support system was introduced on a large scale. This was the well-known ABS brake system. In the 1990s more driver support systems became common. Examples of such systems are traction control systems, yawcontrol systems etc. The ﬁrst ACC system was introduced in 1999 by Mercedes. In 2002 Nissan introduced the ﬁrst lane-keeping assist system and also the ﬁrst CMbB system. Another major trend during the last two decades is the increased use of airbag systems. This is one of the major reasons for the improved crashworthiness of modern cars. In Appendix C a list of diﬀerent safety designs introduced in Volvo Section 2.3 17 Accident Statistics Collision Speed Collision Speed [km/h] 150 100 50 With CMbB No CMbB 0 0 50 100 Initial Speed [km/h] 150 Figure 2.4: Speed at which the CMbB equipped vehicle would hit the stationary obstacle for diﬀerent initial speeds. The distance at which braking starts is given by the minimum distance for avoidance in Figure 2.3. The width of both the obstacle and the vehicle is assumed to be 2 m. For high closing velocities the speed reduction is 22 km/h. cars is given. However, Volvo was not ﬁrst with all of these designs. 2.3 Accident Statistics To investigate the possible eﬀects of forward looking CM systems, some accident statistics will be examined. A thorough investigation of potential eﬀects of FCW and CMbB systems with respect to accident statistics is presented in [133], other works discussing the potential eﬀects of forward looking CM systems are [19, 77]. In this section data from three American databases is used. The general estimates system (GES), [36], is a sample database of all types of accidents. The national automotive sampling system NASS, [99], database is composed of two data-bases: the crashworthiness data system (CDS), [22], and the GES. The fatality analysis reporting system (FARS) database, [28], records all car accidents with fatal outcome. 2.3.1 Accident Types Given that the ﬁeld of view (FOV) of CM systems is often limited, it is clear that not all accident types can be mitigated. Typical accidents where forward looking CM systems are ineﬀective are those where the principal other vehicle (POV) approaches the host vehicle from the side, so that the POV appears in the 18 Chapter 2 5% 4% 11% Collision Avoidance Overview Other Single vehicle hitting nonfixed object Single vehicle hitting fixed object Sideswipe opposite direction Rear end Head on Angle Sideswipe same direction 35% 16% 1% 2% 26% The diagram show how common diﬀerent types of accident are. The data is from the GES database year 1998. Figure 2.5: sensors’ FOV late or not at all. Figure 2.5 shows how common diﬀerent types of accidents were in the USA in the year 1998. The statistics presented in Figure 2.5 come from the GES database. Categories which are very likely to be aﬀected by a forward looking CM system are rear-end (26 %) and single vehicle hit ﬁxed (16 %) and non-ﬁxed object (11 %) collisions. The reason for this is that in many of these accidents the struck object has been in the sensor’s FOV for a long time (several seconds). The statistics indicate that potentially more than 50 % of all accidents might be aﬀected by a forward looking CM system (assuming that all rear-end and single vehicle accidents can be aﬀected). A very important aspect when considering the eﬀect of an active safety function is to know what the driver did prior to the accident. In [126] this is investigated for rear-end collisions. Rearend collisions were divided into ﬁve groups. It is reported that in more than 68 % of the accidents the driver makes no avoidance maneuver (braking or steering) for all the ﬁve groups. 2.3.2 Eectiveness of Collision Mitigation by Braking Systems It is clear that there is a connection between collision speed and the severity of passenger injury. The kinetic energy of a vehicle is given by Ekin = mv 2 , 2 (2.1) Section 2.3 19 Accident Statistics Fatality risk 1 Joksch,1993 O‘Day and Flora, 1982 0.9 0.8 p(F |∆V ) 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 Figure 2.6: 20 40 60 80 Change in speed, ∆V, [km/h] 100 120 Fatality risk versus collision-induced speed change. where m is the mass of the vehicle and v is the speed. In a (completely non-elastic) collision with a ﬁxed barrier, a CMbB halving the collision speed, thus, cause a decrease in the collision energy by a factor of 4. Clearly, reducing the collision speed has a large potential in injury reduction. Accident research has found that the collision induced speed change (∆V ) and the probability of being injured are strongly correlated. In [69] and [105] the relationship between fatality risk and ∆V is investigated. A good approximation according to [69] and [105] is to describe the relationship between the probability of fatality for a given ∆V , p(F |∆V ), up to some maximum speed, according to p(F |∆V ) ∝ (∆V )4 , (2.2) which is illustrated in Figure 2.6. In real-life accidents the speed change of a vehicle in a collision between two vehicles can be calculated by the equation for conservation of momentum m1 v0,1 + m2 v0,2 = m1 v1 + m2 v2 , (2.3) where v0,i and vi denote the velocity before and after the collision respectively. The speed change for vehicle i is given by ∆V1 = v0,i − vi . (2.4) Example 2.1 Consider a collision where one car hits another stationary car; assume that both vehicles have equal mass. If the collision is completely non-elastic we can calculate 20 Chapter 2 Collision Avoidance Overview ∆V for the two vehicles using (2.3) and (2.4). In this case ∆V for both the vehicles will be v0,1 . (2.5) ∆V = 2 The eﬀectiveness of CMbB systems with respect to reducing the collision speed is one of the main issues of this thesis. In this section a simple assumption that the probability density function (PDF) of the collision speed reduction, p(vr |CM bB), from the CMbB system, has a uniform distribution, p(vr |CM bB) ∈ U(−15, 0), (2.6) where vr denotes the CMbB system induced speed reduction. Hence, the system can reduce the collision speed by up to 15 km/h, with an average expected speed reduction of 7.5 km/h. To study the eﬀectiveness of the CMbB system the probability of sustaining injury in frontal collision will be studied. Injury severity is often quantiﬁed using the abbreviated injury scale (AIS), which range from 1 (minor) to 6 (unsurvivable), sometimes 0 is used to indicate no injury. An AIS2 injury is a moderate injury and AIS2+ denote injuries which are classiﬁed as AIS2 or higher. In Figure 2.7 the probability of sustaining a AIS2+ injury, p(AIS2+|ṽ), in relation to the relative speed between the vehicles ṽ is illustrated. The data is from the NASS database (year 1998-2000) and only includes frontal collisions of cars with similar size and crash performance as Volvo cars. To the data a fourth order polynomial was ﬁtted. Note, that for high and low velocities the polynomial ﬁt is poor, it is only intended to be used from 5 km/h to 105 km/h. For higher velocities there are relatively few accidents and the probability of an injury is relatively constant (≈ 1). For low velocities the probability of a injury is low (≈ 0). Therefore, these accidents are expected to have little eﬀect on the analysis in this section. The probability distribution of rear-end collisions, p(ṽ), with respect to relative velocity ṽ, is illustrated in Figure 2.8. This data is from the GES data-base (year 1998). The eﬀect of the CMbB system is also shown in Figure 2.8. The CMbB system eﬀect on the collision speed is calculated according to ∞ p(ṽ − vr )p(vr |CMbB) dvr , (2.7) p(ṽ|CMbB) = −∞ where p(r|CM bB) ∈ U(−15, 0) denotes the PDF for reducing ṽ. The joint probability density of sustaining AIS2+ injury, p(AIS2+, ṽ), in a rear-end collision can be calculated according to p(AIS2+, ṽ) = p(AIS2+|ṽ)p(ṽ). (2.8) The probability of an injury with and without the CMbB system according to (2.8) is illustrated in Figure 2.9. The eﬀect of the CMbB system is calculated according Section 2.3 21 Accident Statistics Probability of a AIS2+ injury 1 p(AIS2+|ṽ) 0.8 0.6 0.4 NASS data Poly. fit 0.2 0 0 20 40 60 Relative velocity [km/h] 80 100 Figure 2.7: Probability of sustaining a AIS2+ injury versus relative velocity. The accident data is from the NASS data-base. Only cars with similar size and collision performance as Volvo cars are included. A fourth order polynomial was ﬁtted to the data, using least-squares minimization. Distribution of rear−end collisions for different velocitys GES data Poly. fit With CMbB 0.02 p(ṽ) 0.015 0.01 0.005 0 0 20 40 60 80 Relative velocity [km/h] 100 120 Figure 2.8: Distribution of rear-end collisions from GES 1998. Again a fourth order polynomial was ﬁtted to the data. The eﬀect of the CMbB system is also shown. The speed reduction of the CMbB system is assumed to be uniformly distributed according to U(0, −15). 22 Chapter 2 Collision Avoidance Overview Risk of AIS2+ at a certain relative velocity No CMbB With CMbB p(AIS2+, ṽ) 0.02 0.015 0.01 0.005 0 0 20 40 60 Relative velocity [km/h] 80 100 Probability of a AIS2+ injury in a rear-end collision with respect to ∆V . The CMbB system reduces the probability of a fatal rear-end collision by 8.5%̇. Note, that for higher values the polynomial ﬁt becomes negative. The probability should only be considered up to this point. For higher values the number of accidents is relatively small and the probability of injury is relatively constant (≈ 1). Figure 2.9: to 105 0 p(AIS2+, ṽ|CMbB) dṽ . 105 p(AIS2+, ṽ) dṽ 0 (2.9) The CMbB system reduces the probability of an AIS2+ injury by approximately 16 % for all rear-end collisions. Given that rear-end collisions constitute approximately 30 % of all accidents a CMbB system, with an average speed reduction of 7.5 km/h, have the potential of reducing the risk for AIS2+ injuries for all accidents by approximately 5 %. 2.3.3 Weather Conditions Diﬀerent sensors are not aﬀected in the same way by weather and light conditions. Some sensors are more robust to bad weather conditions while other sensors might be more sensitive. However, the sensors that do not cope well with bad weather or poor light conditions, might have other advantages. When designing a CM system it becomes interesting to examine in what conditions accidents occur. The statistics in Table 2.1 and 2.2 come from the GES and the FARS year 1998 and were presented in [133]. It is worth noting that most accidents occur in the day time (68.6 %) and under no adverse weather conditions (82.7 %). It is also clear that for fatal accidents a much larger share occurs during dark conditions. Section 2.3 23 Accident Statistics Table 2.1: Accident frequency for diﬀerent weather conditions. Weather conditions FARS [%] GES [%] No adverse weather condition 87.26 82.72 Rain 8.80 13.22 Sleet (hail) 0.39 0.24 Snow 1.26 1.71 Fog 1.50 0.42 Rain and fog 0.16 0.03 Sleet and fog 0.02 0.01 Other (smog, smoke, dust) 0.19 0.29 Unknown 0.44 1.35 Table 2.2: Accident frequency for diﬀerent light conditions. Light conditions FARS [%] GES [%] Day time 50.55 68.55 Dark 30.00 11.49 Dark but illuminated 14.92 11.76 Dawn 1.96 1.41 Dusk 2.30 2.29 Unknown 0.28 1.49 24 Chapter 2 Collision Avoidance Overview Part II Tracking 25 Chapter 3 Automotive Tracking Sensors In order to track objects and predict collisions the movement of the host vehicle and the movement of the surrounding objects need to be monitored. This chapter will discuss sensors used to detect and observe other objects in the surrounding environment of the host vehicle. These sensors are referred to as “tracking sensors”. The applications in this thesis are mainly from automotive examples. Thus, this chapter discusses automotive tracking sensors. The general description of each sensor should be valid for any application, but the performance will be typical for sensors that are commercially available for automotive applications. For automotive applications in particular, one very important requirement is the price of the sensor. In general, there may exist sensors with better performance and more capabilities than the ones discussed below. 3.1 The Radar Sensor Radio detection and ranging (Radar), is traditionally the most common tracking sensor. Radar has been used in military applications for several decades. Today radar is common in for example aircraft and missile tracking, air traﬃc control, and naval applications. In recent years, radar has been introduced in automotive ACC systems. The most important measurements provided by the radar sensor are range (r), range rate (ṙ), azimuth angle (ϕ), and elevation angle. In Figure 3.1, range and azimuth angle measurements are illustrated. The radar sensor is an active sensor in the sense that it emits electro-magnetic radiation to illuminate targets. Emitting and receiving radar waves is typically achieved by the same antenna, thus, the sensor is constantly switching between the sending and receiving mode. The emitted energy has a particular waveform. By examining the echo of the transmitted signal, information on the environment is gathered. Most long range automotive radars use frequencies in the region 76 − 77 GHz. By their wavelength they are often called millimeter-wavelength radars (hereafter referred to as mm-radars). Other automotive radar sensors operate in the 5 GHz and 24 GHz 27 28 Chapter 3 Automotive Tracking Sensors Figure 3.1: Measurements provided by the radar sensor are range, range rate, and azimuth angle. Due to the relative wide main lobe of the emitted radar pulse, the azimuth resolution is poor. The lobe width is determined by the size of the antenna. region. In [114] the basic theory of radar systems is described. The shape of the radiation pattern is determined by the antenna design. In automotive radars, the antenna is designed so that the main lobe has a conical shape. The beam width is normally 1 − 4◦ both in the azimuth and the elevation direction. To provide azimuth angle information, the sensor either mechanically sweeps the antenna over a range or electronically switches between diﬀerent emission angles. The total ﬁeld of view (FOV) (see Figure 3.1 for an example) is normally 10 − 15◦ for available ACC radar sensors. The techniques that are used to provide azimuth resolution can of course also be used to provide elevation angle information. However, most existing automotive radars do not provide elevation measurements (scanning is only one-dimensional). The radar sensor has both advantages and disadvantages. Advantages: • Bad-weather performance: Frequencies at 30 − 300 GHz are generally lineof-sight and experience attenuation due to absorption in atmospheric gases (0.3 − 0.5 dB/km [1]). The sensor does, however, have the ability to detect objects in darkness, haze, rain and snow for the short distances required for automotive applications (where the range is normally less than 200 m). Automotive radar sensors are also insensitive to dirt deposits (i.e., mud and dirt from the road). • Range and range rate: Automotive radars provide accurate range measurements, in contrast to passive sensors. Some radars also measure the range rate using the Doppler eﬀect. Disadvantages: • Clutter and spurious reﬂections: A number of objects in the environment reﬂect emitted radar signals. Unwanted reﬂections (often called clutter) from the road surface for instance, might give “ghost” obstacles (i.e., obstacles that do not exist). Multipath propagation might also cause such phenomena. Section 3.2 29 The Lidar Sensor • Resolution: Because of the relatively wide lobe, the spatial resolution of the radar is poor. Its ability to measure spatial properties of the observed object is therefore limited. • Elevation: Since automotive radars have no resolution in elevation and a wide beam, it might be hard to discriminate obstacles from low overhead objects (road signs above the road, bridges, tunnels etc). Most available sensors have an update frequency of 10 Hz, i.e., one sweep is completed in 100 ms. In Table 3.1 some basic sensor properties from diﬀerent suppliers are displayed. Range accuracy is often better than 1 m and angular resolution is normally better than 1◦ (for radars with scanning antennas). In [1, 41, 113, 127] mm-radars for automotive purposes are discussed. A thorough description of radar sensors is given in [114]. Table 3.1: Properties of Radar Sensors. Note, that this table was compiled in 2002. Automotive radar sensors are under rapid development. Radar Supplier Fujitsu Ten Mitsubishi Denso Nec Hitachi A.D.C. Bosch Waveform FM-CW FM-Pulse Delphi >120 m 150 m FOV 16 ◦ 12-16 ◦ Mechanical 8 beams ◦ Mechanical 8 beams ? 150 m 20 120 m 16◦ Synchronized 9 beams FSK 120 m 16◦ Monopulse 150 m 10 ◦ 150 m ◦ FM-CW FSK FM-CW 150 m 150 m 8 ? switched 3 beams synchronized 3 beams 12 ◦ Monopulse 16 ◦ Mechanical ◦ Monopulse Monopulse Eaton FSK 150 m 12 Visteon FSK 150 m 12◦ 3.2 Scanning FM-CW FM-CW Autocruise Range The Lidar Sensor A laser radar (lidar) is similar in its basic function to a mm-radar. Lidars provide range, range rate, azimuth and elevation measurements. While most automotive radar sensors provide no elevation measurements, there are some lidars that do. The lidar sensor is also an active sensor, i.e., it uses a laser diode to illuminate obstacles. Lidars operate in the infrared frequency region. Wavelengths are around 850 nm (just above visible light). The wave propagation properties of lidars are similar to visible light. Lidars often consist of a one dimensional (distance) measurement device combined with a mechanical beam deﬂection system (e.g. a rotating mirror) 30 Chapter 3 Automotive Tracking Sensors to provide spatial measurements. Distance is measured by observing time of ﬂight between received and transmitted signals. Lidar systems for automotive purposes are discussed in [120], [113] and [112]. In [127] a comparison between mm-radar and lidar for ACC purposes is presented. Compared to mm-radars, lidars have advantages and disadvantages: Advantages: • Resolution: The beam of a lidar is often quite narrow. There are lidars that measure at several hundred azimuth angles and for several angles of elevation in one sweep (normally 100 ms). They thus provide a pixel map which could potentially give much more detailed information of objects than a mm-radar. • Clutter: Due to the narrow beam and detection, the lidar does not experience clutter and spurious reﬂections to the same extent as a mm-radar. Note that some lidars still have a wide angle beam and detector, and may also experience multipath problems. • Gray scale: The photo detector can easily detect reﬂected intensity, thus providing a gray scale “picture”. This may be used to monitor lane markings. • Light conditions: Lidars are relatively insensitive to light conditions. • Cost: Lidars are less expensive compared to mm-radars. Disadvantages: • Bad weather: In hard rain, fog or snow, the lidars experience performance degradation. • Dirt deposits: Lidars are sensitive to dirt deposits on the lens. Dirt deposits on the tracked vehicle may also cause altered reﬂectivity and thus problems in detection. 3.3 Vision Systems for Obstacle Recognition Vision systems use one or several cameras together with a microprocessor to perform image processing. Since they operate in the visible light region, their capabilities are similar to that of our own eyes. The two main types of system are: • Single camera systems - using either a monochrome or a color camera. One use in automotive applications for single camera systems is to monitor the lane markings in lane-keeping aid systems, [75, 89]. • Stereo camera systems - A stereo camera system provides a 3D image by combining the images from two (or more) cameras. In such a system, range can be measured through triangulation. Because of the 3D information obstacle Section 3.3 31 Vision Systems for Obstacle Recognition Table 3.2: Properties of Lidar Sensors. Note, that this table was compiled in 2002. Automotive lidar sensors are under rapid development. Radar Supplier Mitsubishi Denso Denso (gen.II) Nec Omron Omron (gen II) Range [m] Vertical range FOV scanning 130 4◦ 12◦ 1-D 16 ◦ 2-D 40 ◦ 2-D 20 ◦ 1-D 4.4 ◦ 120 4.4 ◦ 100 ◦ 120 150 150 3 3.3 ◦ 6.5 ◦ ◦ Kansei 120 3.5 A.D.C. 150 ? 10.5 ◦ 20 − 30 ? ◦ 2-D 12 ◦ 1-D 17 ◦ ? detection is easier. Generally, shape or pattern recognition is not needed to the same extent as for a single camera system. Stereo vision systems for automotive applications are discussed in [13, 51] The performance of a vision system depends on among other thing the optics, the size of the sensor, number of pixels, and the dynamic range. The update frequency of many vision sensors is 25 Hz. However, image processing can be very computationally demanding. Therefore, the output from a vision sensor (after image processing) is normally at a lower data rate. Vision system performance in automotive systems is discussed in [23], [14] and [50]. In the VITA II project [121] extensive tests were performed using several vision systems to perform automated driving. Advantages: • High resolution: A pixel array with 640 × 480 pixels is common. This resolution makes it possible to measure spatial properties of the obstacle with good accuracy. Advanced target classiﬁcation is also possible from the detailed images. Disadvantages: • Sensitivity to light conditions: A vision system can suﬀer severe performance degradation in certain light conditions (for example thick fog, heavy rain, or wet roadway combined with backlight). • Sensitivity to dirt: Dirt deposits on/in front of the lens can cause problems to a vision system. • Computational demands: The image processing algorithms are computationally intensive. 32 Chapter 3 Figure 3.2: 3.4 Automotive Tracking Sensors Image displayed by a night vision system. Infrared Vision Sensors Infrared (IR), or thermal, cameras basically give the same information as any normal camera. In many ways an IR vision system thus has the same properties as normal vision systems discussed in the previous chapter. The diﬀerence is that the IR camera is sensitive to wavelengths typical to heat radiation (the same region that lidars normally operate in). In automotive systems, IR cameras have been introduced for night vision systems. Night vision systems are used to enhance the driver’s perceptive abilities (Figure 3.2). These sensors are most sensitive to wavelengths corresponding to the normal body temperature of humans and large animals. So far, IR cameras have not been used in automotive collision mitigation (CM) systems. The ability to measure the temperature of objects is unique to the IR sensor. IR cameras could potentially be quite useful in classifying objects and providing accurate angle measurements. For example, on some cars the exhaust system is visible and easily observed in a heat sensitive camera. Humans and animals are also often easily detected in an IR-image. A disadvantage with the IR sensor is that it costs more than a normal vision sensor for visible light. 3.5 Inter-Vehicle Communication Today, vehicles are more frequently being equipped with navigation systems consisting of GPS receivers and digital maps. If cars were equipped with communica- Section 3.6 Summary 33 tion systems it would be possible to transmit the navigation data to other vehicles. This data could then be used by the tracking system of the other vehicle. Potentially, this could yield very accurate measurements of kinematic properties as well as an accurate object classiﬁcation (for example make, model and performance speciﬁcation can be transmitted to tracking vehicles). 3.6 Summary The two main tasks for tracking sensors are: 1. To detect obstacles and provide measurements for the tracking system. 2. To provide data for object classiﬁcation. In a CM system, objects must be classiﬁed into at least one of the following groups: • Obstacles which the system should react to, i.e., obstacles to be avoided. • Obstacle which the system should ignore, i.e., false obstacles or obstacles that are not harmful to collide with. Today, commercial radars, lidars and mono-camera sensors for automotive use exist. These sensors have diﬀerent strengths and weaknesses. To be able to provide both accurate measurements and correct object classiﬁcation for a CM system, one sensor might not be suﬃcient. This is due to the fact that a sensor might report a target that does not really exist (a false positive). A sensor might also miss detecting an object (false negative). By using more than one sensor the risk for erroneous decision can be reduced. Furthermore, to be able to monitor the function of the sensors themselves (i.e., diagnosis) it might also be necessary to include more than one tracking sensor. How to merge data from several sensors will be discussed further in Section 4.6. 34 Chapter 3 Automotive Tracking Sensors Chapter 4 Tracking for Collision Avoidance Applications The task of a target tracking system is to detect objects and estimate kinematic quantities of these objects. Estimating kinematic quantities (states) of an object is often called ﬁltering or state estimation. Example of quantities that are of interest to estimate in collision avoidance (CA) applications are position, speed, acceleration and turn rates. For automotive and naval applications it is often sufﬁcient to look at motion in the ground plane (i.e., motion in two dimensions only). For aerospace, robotics and underwater application three-dimensional motion often needs to be considered. The coordinate systems to be used in this thesis are deﬁned in Appendix A. A thorough introduction to tracking theory is found in [7, 8, 15, 16]. This chapter will present the most common tracking ﬁlters that appear in the tracking literature. Particular emphasis is on the Kalman ﬁlter (KF), the extended Kalman ﬁlter (EKF) and the particle ﬁlter (PF) which will be used later in Part IV. A brief description of common tracking models is also given. In a CA system, several objects usually have to be tracked simultaneously. In this thesis the tracking system’s representation of one tracked object is referred to as a track. Another common name for this in the tracking literature is a trace. In a multi-target tracking (MTT) system one must determine which measurement belongs to which track. This task is called measurement or data association and will be discussed in Section 4.4. For observing the targets, a CA system can have several tracking sensors. The task of merging the measurement from the diﬀerent sensors will be addressed in Section 4.6. An example of the architecture of a tracking system is shown in Figure 4.1. In general, all the states, xt , that are of interest cannot be measured directly with the tracking sensors. Instead, they have to be estimated from the measurements denoted by y. The measurements are often corrupted by measurement noise. Estimating states that cannot be directly measured from noisy measurements is a well-studied problem. In control theory literature, an algorithm 35 36 Chapter 4 Tracking for Collision Avoidance Applications Figure 4.1: Schematic illustration of a tracking system providing state estimates to a decision and control system. that estimates or reconstructs the non-measurable states from the measurements is called an observer. State estimation is described in [5, 46, 67, 70]. All state estimation is model-based. The model describes the dynamics of the system to be estimated and how the measurements relate to the states of interest. A general model for a dynamic system is given by (4.1). Since the sensors and processing units work with discrete time, discrete time models is used for all tracking algorithms. xt+T = f (xt , νt ). (4.1) In (4.1), xt is the state vector at time t and νt is the process noise (also called system disturbance). In the vehicle tracking examples, the main contributions to νt come from the human or controller input commands to the tracked object. Thus, the driver or controller is modeled as a stochastic process, whose objective is often unknown. The measurement relation is given by (4.2). yt = h(xt , et ), (4.2) were yt is the measurement vector and et is the measurement noise. It is assumed that the process noise distribution pν (·) and the measurement noise distribution pe (·) are known. There are two main approaches that can be taken to state estimation: 1. The deterministic (Fisher) approach: In this approach, it is assumed that there exists a true but unknown value of the states x, and the goal of the estimation is to ﬁnd this true value. The connection between the stochastic observation vector y and the states x is given by the probability density function (PDF) p(y|x). A popular choice of state estimate x̂, is to take x̂ as the value that maximizes the likelihood of the observation (4.3). x̂t = arg max p(Yt |xt ), xt (4.3) where Yt = {y1 , y2 , · · · , yt }. Another common approach is to choose x̂ so that the square of the expected prediction error is minimized: x̂t = arg min(yt − h(x̂t|t−T ))2 . x̂ (4.4) 2. The Bayesian Approach: The observation vector yt is assumed to have a known probability density function p(yt |xt ) which can depend on the states. Section 4.1 37 The Kalman Filter In the Bayesian approach the state vector xt is considered to be a stochastic variable. It is assumed that the random vector xt has a known prior density function p(xt ). The prior distribution encompasses the knowledge of xt before any observation has been made. When a measurement is made, this observation alters our knowledge of xt . The distribution after an observation has been made is called the posterior density function p(xt |Yt ). The posterior density function can be expressed using Bayes’ rule: p(xt |Yt ) = p(Yt |xt )p(xt ) . p(Yt ) (4.5) In (4.5), p(Yt ) is a scalar constant that can be found through marginalization: p(Yt |xt )p(xt ) dx. (4.6) p(Yt ) = Rn The posterior density p(x|Y) is a very general solution to the tracking problem. It can be used to calculate the probability of any characteristics of states. As will be shown in Chapter 7, this approach is very suitable for decision making in CA systems. A motivation for favoring the Bayesian approach is that when you make a decision in a CA system, all possible scenarios should be considered. Therefore, a point estimate of x is not suﬃcient. The general Bayesian solution p(x|Y), on the other hand, contains all information that can be known of the state vector, given the measurements. In many applications, however, one is interested in ﬁnding a point estimate of state vector x̂. Popular choices are the least mean square estimate given by (4.7) or the maximum a posteriori estimate given by (4.8) xp(x|y) dx, (4.7) x̂MMS = Rn x̂MAP = arg max p(x|Y). x (4.8) In the following Sections 4.1 – 4.3 ﬁltering methods that provide a solution to the Bayesian state estimation problem are examined. 4.1 The Kalman Filter The well known KF, ﬁrst introduced in [71], is widely used in tracking applications, and there are numerous examples of its implementations. 4.1.1 The Stationary Kalman Filter This section reviews the results for the stationary Kalman ﬁlter. For the stationary case the linear time discrete dynamic model and the measurement equation can be 38 Chapter 4 Tracking for Collision Avoidance Applications written according to xt+T = Axt + Bνt , yt = Cxt + et , νt ∈ N (0, Q), et ∈ N (0, R). (4.9) (4.10) Thus, in (4.9) and (4.10), A, B and C are constant matrices. Q and R are the covariance matrices of the process noise, νt , and the measurement noise et respectively. The sampling time of the system is T . The subscript t denotes the sample time at sample number n, thus t = nT and t + T = (n + 1)T . A straightforward estimation of xt is given by x̂t|t = Ax̂t|t−T + Kεt , (4.11) εt = yt − C x̂t|t−T . (4.12) where In (4.11) the choice of K will of course aﬀect the ﬁlter. If K is chosen as in (4.13) this gives the time invariant Kalman ﬁlter. In Kalman ﬁltering, K is often referred to as the Kalman gain, (4.13) K = P C T (CP C T + R)−1 . In (4.13) the covariance matrix P is given by the solution to the stationary Riccati Equation (4.14) according to P = AP AT − AP C T (CP C T + R)−1 CP AT + BQB T . (4.14) The KF gives the Bayesian solution p(xt |yt ). Under the assumption that νt ∈ N (0, Q) and et ∈ N (0, R), then x ∈ N (0, P ). The PDF is given by (4.15). T −1 1 p(xt |Yt ) = e−0.5(x−x̂t ) P (x−x̂t ) . p (2π) (det P ) (4.15) The state estimation error covariance P = cov(xt − x̂t ) = E(xt − x̂t )(xt − x̂t )T is obtained by solving (4.14). Note, that in the stationary case the Kalman gain K and state covariance matrix can be calculated oﬀ-line (i.e., they do not depend on the measurements). 4.1.2 The Time-Varying Kalman Filter For the time varying KF a linear time-varying model given by xt+T = At xt + Bt νt , νt ∈ N (0, Qt ), (4.16) yt = Ct xt + et , et ∈ N (0, Rt ), (4.17) is considered. The subscript t of the system matrices A, B and C indicates that the dynamics of the system, and also the measurement equation, vary over time. Also the process noise covariance Qt and measurement noise covariance Rt may vary with time. Due to the time variability of the system the state covariance P Section 4.1 39 The Kalman Filter will now vary with time. The Kalman gain thus needs to be calculated at each time instant. The time-varying Kalman ﬁlter is given by x̂t|t = x̂t|t−T + Pt|t−T CtT (Ct Pt|t−T CtT − Rt )−1 (yt − Ct x̂t|t−T ), Pt|t = Pt|t−T + x̂t+T |t = At x̂t|t , Pt|t−T CtT (Ct Pt|t−T CtT −1 − Rt ) Ct Pt|t−T , Pt+T |t = At Pt ATt + Bt Qt BtT . (4.18a) (4.18b) (4.18c) (4.18d) The Equations (4.18a) – (4.18b) are called the measurement update and (4.18c) – (4.18d) are called the time update. In the measurement update, the state and covariance estimate is revised according to the latest measurement. In the time update step a prediction is made. In a CA system, predictions for several future time instances might be required. Prediction for time t + 2, t + 3, t + 4, · · · are easily obtained by iterating the time update equations. As in the stationary case, the a posteriori density is known if νt ∈ N (0, Qt ) and et ∈ N (0, Rt ). Then p(xt+T |yt ) is given by (4.15), where the state estimate x̂t+T |t and cov(xt+T − x̂t+T |t ) = Pt+T |t are given by the time update (4.18c) and (4.18d). For a deeper study of Kalman ﬁltering, [5, 46, 70] are recommended. 4.1.3 The Extended Kalman Filter In many cases the dynamic model and the measurement equations are nonlinear. This is, for example, the case for many tracking models. The assumption of Gaussian noise might not be fulﬁlled either. In such cases it is hard to ﬁnd an analytical solution to the estimation problem, i.e., ﬁnding p(x|Y). Consider a system that can be written as xt+T = f (xt ) + νt , yt = h(xt ) + et , νt ∈ N (0, Qt ), et ∈ N (0, Rt ). (4.19a) (4.19b) The idea of extended Kalman ﬁltering (EKF) is to solve an approximation of the original problem by linearizing the equations and approximating the noise distributions with Gaussian distributions. There are diﬀerent approaches that can be taken; we start with discussing how to deal with a non-linear measurement equation. Linearizing (4.19b) by Taylor expansion and neglecting higher order terms we get ∂h(x) h(xt ) = h(x̂t|t−T ) + (xt − x̂t|t−T ), (4.20) ∂x x=x̂t|t−T = Ht . A new measurement equation where the Jacobian matrix ∂h(x) ∂x x=x̂t|t−T (4.21) is formed using the expansion (4.20), ȳt = yt − h(x̂t|t−T ) + Ht x̂t|t−T ≈ Ht x̂t|t−T + et . (4.21) The right hand side of (4.21) looks as in the linear case (4.17), and the Kalman ﬁlter measurement update (4.18a),(4.18b) can be applied. 40 Chapter 4 Tracking for Collision Avoidance Applications Another approach to treat a nonlinear measurement equation is to make a nonlinear transformation of the measurements. The aim of the transformation is for the transformed measurements to yield a linear measurement equation. One such transformation is to take ȳ = h−1 (y). In example 4.1, a transformation of nonlinear measurements from a radar sensor is shown. Example 4.1 A radar sensor measuring range and azimuth angle has the following measurement equation y= r ϕ . (4.22) In a tracking system using a Cartesian coordinate system, position measurements in Cartesian coordinates (px and py ) would yield a linear measurement equation. To obtain this we perform a nonlinear transformation of the range and angle measurement from a radar; the transformed measurement equation is given by r cos(ϕ) px = (4.23) ȳ = r sin(ϕ) py The problem with this approach is that the measurement covariance R must also be transformed. This can only be done approximately, and Gaussianity is in general not preserved. To be able to use the Kalman ﬁlter time update equations (4.18c) and (4.18d), f (xt ) must also be linearized. Starting with a nonlinear continuous time model, there are two choices for how to do this. Either by using discretized linearization or linearized discretization. Both these methods are described in [46]. If a discrete time nonlinear function exists as in (4.19a), a Taylor expansion for each component (i) of the state vector yields the estimate 1 (i) xt+T ≈ f (i) (x̂t )+ (f (i) ) (x̂t )(xt − x̂t )+ (xt − x̂t )T (f (i) ) (x̂t )(xt − x̂t )+ νi,t , (4.24) 2 where (f (i) ) denotes a row vector consisting of partial derivatives with respect to all states. Using this expression the time update of the EKF will be x̂t+T |t = f (x̂t|t ) (4.25) T Pt+T |t = f (x̂t|t )Pt|t f (x̂t|t ) + Qt , where (4.26) (f (1) ) .. f = . (f (n) ) (4.27) Section 4.2 41 The Point Mass Filter The EKF will give a sub-optimal solution since the dynamics and measurement equations of the system are an approximation of the true system. The true process and measurement noise might also not be Gaussian, in which case the posteriori distribution given by the EKF is only an approximation of the true density. For highly nonlinear systems or when the PDFs are not well approximated with Gaussian distributions, the EKF is not suﬃcient. To improve tracking performance several techniques using multiple models have been proposed. 4.2 The Point Mass Filter So far we have only discussed the use of linearized models and Gaussian noise processes to deal with nonlinear and non-Gaussian systems. We will now look at methods where we do not linearize the system, but instead we try to solve the estimation problem for the “true” system, [67, 79, 115]. The systems are nonlinear with additive noise, as described by (4.19a) and (4.19b). The process and measurement noise distributions (pν (·) and pe (·)) are assumed to be known, but they are not necessarily Gaussian. As already stated, it is often not possible to ﬁnd an analytical solution to the Bayesian tracking problem. The idea is to solve the problems by numerical approximation. A recursive solution to the Bayesian estimation problem is given by (4.28) – (4.30). p(yt |xt )p(xt |Yt−T ) , p(yt |Yt−T ) p(xt+T |Yt ) = p(xt+T |xt )p(xt |Yt ) dxt , p(xt |Yt ) = (4.28) (4.29) Rn where p(yt |Yt−T ) = Rn p(yt |xt )p(xt |Yt−T ) dxt , (4.30) and the recursion is initiated by p(x0 |Y−T ) = p(x0 ). Inserting the measurement equation (4.19b) into (4.28) we get p(xt |Yt ) = α−1 t pet (yt − h(xt )) p(xt |Yt−T ), (4.31) where αt = p(yt |Yt−T ) = Rn pet (yt − h(xt )) p(xt |Yt−T ) dxt . (4.32) The time update is given by p(xt+T |Yt ) = Rn pνt (xt+T − f (xt )) p(xt |Yt ) dxt . (4.33) One approach to approximately solve the recursive estimation problem is to evaluate the recursion (4.31) – (4.33) for several points around the working point (i.e., the current value of xt ). These points are then updated at each time step through 42 Chapter 4 Tracking for Collision Avoidance Applications the recursion. We thus have a quantization of the state space. Each point is assigned a weight or mass corresponding to the probability that the states assume that particular value. This approach has sometimes been called the point mass ﬁlter (PMF), essentially it means that the PDF is approximated with a ﬁnite number of samples from the distribution and the integrals are approximated with a Riemann sum. Assume that a grid consisting of N points is chosen to approximate the density function p(xt |Yt ). These N points in the state space will be denoted xt (k), k = 1, 2, · · · , N . The associated probability mass for each point is denoted p(xt (k)|Yt ), k = 1, 2, · · · , N . Assuming that the grid is uniform with the distance δ between the points, an integral over a region in the state space can be approximated by a sum N p(xt ) dxt ≈ p (xt (k)) δ n . (4.34) Rn k=1 Applying the PMF approximation yields the recursion: p(xt (k)|Yt ) = α−1 t pet (yt − h (xt (k))) p(xt (k)|Yt−T ), xt+T (k) = f (xt (k)), k = 1, 2, · · · , N p(xt+T (k)|Yt ) = N pνt (xt+T (k) − f (xt (n)) p(xt (n)|Yt−T )) δ n . (4.35a) (4.35b) (4.35c) n=1 In order for the PMF to work, the grid-points must be adapted to the support of the conditional density. One way to do this is to remove points with low mass. Doing this will of course decrease the number of points, so when the number of grid points falls below a lower bound one should increase the number of points. A thorough account of the PMF, with tracking and navigation examples, is given in [11]. 4.3 The Particle Filter One of the main problems with the PMF approach is the computational complexity. The relative error of brute force techniques for approximating the integral (4.29) is shown in [11] to be of the complexity order O(L−1/n ) (where L depends on the distance between grid points and n is the state dimension). For high dimensional problems, the PMF approach thus becomes intractable. The PF approach bears many similarities to the PMF. This also aim at ﬁnding a numerical approximation of the recursive Bayesian solution to the tracking problem in (4.28) – (4.30). The PF approach also uses a set of points to represent the posteriori distribution of the states. Instead of discretizing the state space, the PF depends on the assumption that it is possible to draw samples from the a priori distribution p(xt−T |Yt−T ). Here only a brief description of the theory is given. For more details on PF see [12, 25, 39, 48, 72, 91, 123]. The Bayesian solution given by the particle ﬁlter method provides an approximative solution to (4.5) by approximating the probability density p(xt |Yt ) by a large set of N particles Section 4.4 43 Measurement Association {xt }N i=1 , where each particle has an assigned relative weight, γt , such that all weights sum to unity. The location and weight of each particle reﬂect the value of the density in the region of the state space. When the measurement relation is according to (4.36) yt = h(xt ) + et . (i) (i) The likelihood p(yt |xt ) can be calculated as: γt = p(yt |xt ) = pet (yt − h(xt )). (4.37) Each sample from the prior distribution can then be simulated according to the dynamic model. The resulting swarm of particles represents the posterior density. However, as time evolves the particle cloud is spread over an ever increasing region of the state space. This eventually leads to a poor representation of the true distribution and divergence of the PF. By introducing a resampling step, as proposed in [39], problems with divergence can be alleviated. This is referred to as sampling importance resampling (SIR), and is summarized in Algorithm 4.1. The PF approach is often referred to as simulation-based, because of the fact that it uses Monte Carlo simulations to approximate the solution. 1. Generate N samples {x0 }N i=1 from p(x0 ). (i) (i) (i) 2. Compute γt = pe (yt |xt ) and normalize, i.e., (i) (i) N (j) γ̄t = γt / j=1 γt , i = 1, . . . , N . 3. Generate a new set {xt }N i=1 by resampling with replacement N times from (i) (j) (i) (j) {xt }N , with probability γ̄t = P r{xt = xt }. i=1 (i) (i) (i) 4. Prediction: xt+T = f (xt izations (i) , ut , νt ), i = 1, . . . , N using diﬀerent noise real- (i) νt . 5. Increase t and continue from step 2. Algorithm 4.1: Particle ﬁlter. 4.4 Measurement Association A Multi target tracking (MTT) system’s task is to track several objects simultaneously. The system thus supervises several targets (represented by their estimated (i) state vector xt , i = 1, 2, · · · , J, where J is the maximum number of tracks) at any given time. The tracking sensors will then generally provide measurements (i) originating from several objects, yt , i = 1, 2, · · · , M . Before updating the state estimates of current tracks with new measurements, the system needs to determine which measurement belongs to which track. The task of assigning measurements 44 Chapter 4 Tracking for Collision Avoidance Applications Gating 5 4.5 y 5 4 h(x̂(3) ) y−coordinate [m] 3.5 3 2.5 y3 2 y2 1.5 y1 1 h(x̂(2) ) y 4 (1) h(x̂ ) 0.5 0 −1 Figure 4.2: 0 1 2 x−coordinate [m] 3 4 Gating example in a Cartesian coordinate system. to tracks is called measurement association. Some reference where measurement association is discussed are [7, 9, 16, 49]. Given a dense target environment, noisy measurements, multiple reﬂections on each target and the fact that the sensors might erroneously detect object caused by multi path propagation or spurious reﬂections, this is not always an easy task. 4.4.1 Gating The ﬁrst step in assigning measurements to tracks is called gating. Gating is used to eliminate unlikely observation-to-track pairings, and if no conﬂicts exist, to assign the measurement. For each track, a subset G(i) is deﬁned in the measurement coordinate space, G(i) ⊂ Rm . If a measurement falls within the gate of a track (j) yt ∈ G(p) , then that measurement is considered a candidate for updating that track. Figure 4.2 shows an example of a gating procedure in a 2-D Cartesian space. Clearly there are conﬂicts: There are several measurements falling within the same gate (i.e., {y1 , y2 } ∈ G(1) and ({y2 , y3 , y4 } ∈ G(2) ) and there are also measurements falling within more than one gate (y2 ∈ G(1) and y2 ∈ G(2) ). If a measurement falls in one gate only and there are no other measurements that can be associated to that gate, then that measurement can be directly associated with the track to which the gate belongs. If there are conﬂicts, on the other hand, further correlation logic is needed. If a measurement does not fall into any gate this measurement is Section 4.4 Measurement Association 45 considered a candidate to initiate a new track. 4.4.2 Measurement Association Methods and Techniques In the previous section it was shown that after the gating procedure there might still be conﬂicts regarding which measurement to assign to which track. In this section common techniques to resolve these conﬂicts is reviewed. In tracking literature, the process of solving conﬂicting assignments is referred to as data or measurement association. The Nearest Neighbor Approach The nearest neighbor (NN) approach looks for a unique pairing (i.e., one observation can only be used to update one track). To decide which observation to associate with which track, a distance measure gij is formed. gij denotes the distance from observation i to track j. The idea is then to choose the assignments in such a way that the total distance is minimized. One popular choice of distance measure is the likelihood function associated with the assignment of observation j to track i. In this case the idea is to maximize the total likelihood of all assignments. The Multiple Hypothesis Approach In the multiple hypothesis tracking (MHT) approach, a track is only updated with one measurement, as in the NN approach. If there are conﬂicts, however, the tracks are divided into several tracks and all possible associations are made. That is, one measurement can now be used to update several tracks. The fact that a measurement could also be used to initiate a new track or may be a false measurement is also considered. Example 4.2 In Figure 4.2, there are three tracks (x̂(1) ,x̂(2) ,x̂(3) ) before the measurement association. In the association step track x̂(1) is updated with y1 and y2 . x̂(1) is thus divided into two tracks x̂(11) and x̂(12) . Similarly x̂(2) splits into three tracks and x̂(3) is not split. In the MHT approach it is of course possible that the number of tracks becomes large. This has to be dealt with in order for the solution to be tractable. One way to handle the problem is calculating the probability of the diﬀerent tracks, and terminating tracks with low probability. Clustering Techniques In some applications it is common to receive multiple measurement from the same obstacle. In this case, it is possible to initiate and maintain several tracks that correspond to the same entity. Having several tracks for the same entity might cause confusion in further analysis and waste computational eﬀort. Possibly, the 46 Chapter 4 Tracking for Collision Avoidance Applications Clustering 5 4.5 y−coordinate [m] 4 A 3.5 3 C 2.5 2 B 1.5 0 0.5 1 1.5 2 2.5 x−coordinate [m] 3 3.5 Figure 4.3: Example of measurement clustering in a Cartesian coordinate system. In this case all tracks that are within a minimum distance from others are clustered together. This result in tree clusters of tracks. erroneous allocation of tracks could lead to discrimination of interesting targets. One way to handle this is to cluster tracks or merge tracks that are likely to represent the same target. Figure 4.3 shows an example where several tracks (denoted by crosses) in a Cartesian coordinate system are clustered into three groups A, B and C. How to cluster objects is not clear and will diﬀer for diﬀerent applications. In CA applications one will typical try to estimate the physical size of the object, this estimate can be used to determine if several tracks actually represent the same object. Clustering is sometimes used to track troop movements in military applications. Here the entire troop is considered to be the tracked entity. An introduction and further references to clustering techniques are given in [49]. Another approach to deal with the problem of multiple measurement from the same object is to allow tracks to be updated with several measurements, i.e., several measurements are associated with the same track. One reason for using clustering is to save computational power; in Example 4.3 it is discussed why this might be desirable for CA applications. Section 4.5 Ranking Measurements 47 Traﬃc scenario where the vehicle ahead of the host vehicle (object 2) has come to a stop. Depending on what manoeuvre the driver of the host vehicle decides on, object 1, 2, 3, and the stationary object, 4, on the side of the road can all constitute a threat. Figure 4.4: Example 4.3 The computational power of a commercial CA system will of course be limited. This means that the maximum number of live tracks at any one time will be limited. Here a system that is able to track four diﬀerent objects simultaneously is examined. In Figure 4.4, there are four objects that can become a threat depending on how the host vehicle driver chooses to avoid the stopped vehicle ahead (object 2). Figure 4.5 shows measurements from a commercial radar sensor. The measurement was performed on a large open tarmac area with only one stationary target present (see Figure 4.6). Clearly, in Figure 4.5 the radar detects two targets which are coming from the same physical object. In this case it is certainly possible to initiate two tracks from the two measurements. If this would happen in the scenario from Figure 4.4 two tracks would be used for the stopped vehicle (object 2), and since the maximum number of tracked objects is four, one of the other potential threats will not be tracked. 4.5 Ranking Measurements It has already been stated that there is an upper limit to the number of objects that can be tracked simultaneously (because of limited computational power). The sensor, however, might possibly detect more objects than can be dealt with by the 48 Range [m] Chapter 4 Multiple Detection of a Stationary Target − Range and Azimuth Angle 40 Target 1 Target 2 30 20 10 0 −0.5 Azimuth Angle [degrees] Tracking for Collision Avoidance Applications 0 0.5 1 1.5 Time [s] 2 2.5 3 5 Target 1 Target 2 0 −5 −10 −15 −0.5 0 0.5 1 1.5 Time [s] 2 2.5 3 Range and azimuth angle measurements from a commercial radar sensor. The host vehicle is approaching one lone stationary obstacle at 40 km/h. Two obstacles at the same range and an angular spacing of 3 − 4◦ are detected (between time 0.5 and 1.75), however, only one physical object is present, see Figure 4.6. This is a clear case of multiple reﬂections from one object. Note, that the tracking and measurement association is done internally in the radar sensor and the algorithm is not fully known. Figure 4.5: tracking system. To make the total system as eﬀective as possible, it is desirable to use the most relevant measurements and to remove irrelevant measurements. In this thesis the process of selecting relevant measurements will be called ranking. This section will discuss how ranking of measurements can be done in a CA system. Assume that the sensor whose measurements should be ranked, measures a maximum of M diﬀerent obstacles. The measurements are given in a vector Y = (y (1) , y (2) , · · · , y (M) ). Because of computational limits only i ≤ M measurements are allowed to be passed on to the tracking system. The idea is now to reorder the measurements of Y to get Ỹ = (ỹ (1) , ỹ (2) , · · · , ỹ (M) ), where ỹ (1) is the most relevant measurement and so on. How to perform the ranking in general will of course depend on the type of application considered. Here considerations when ranking the importance of a measurement for diﬀerent CA systems are given. Since the aim of the ranking process is to decrease the computational burden, it must not be computationally demanding itself. The sensors normally measure range, range rate and azimuth angle, so the ranking will be based on these quantities. Section 4.5 Ranking Measurements 49 The test area where the measurement of Figure 4.5 was performed, there are no other objects close to the stationary obstacle. Figure 4.6: 4.5.1 Ranking Measurements by Range Only For CA systems, it might be wise to prioritize the measurements according to range, since objects close to the host vehicle in many cases constitute the greatest threat. For some systems, it is at least possible to say that measurements with a range above a certain value are irrelevant (see Example 4.4). Example 4.4 Consider a CMbB system which performs braking when a collision becomes unavoidable. The collision unavoidable point is closer than 30 m for stationary targets and speeds up to 150 km/h. Assuming that other vehicles have similar handling characteristics, one can say that measurements where the range is much larger than 60 m are irrelevant. For such a system it would be wise to rank measurements below 60 m as more relevant than those above 60 m. The danger in ranking measurements on range alone is that one might miss objects that are serious threats because of a high relative speed. 4.5.2 Ranking by Range Rate and Time-to-Collision Some sensors measure range rate directly. For such sensors it is possible to prioritize objects according to their oncoming speed, which is approximated by the measured Ṙ. An estimate using both range and range rate is time-to-collision (tTTC ) given 50 Chapter 4 Tracking for Collision Avoidance Applications by R , Ṙ < 0. (4.38) Ṙ An alternative is to prioritize measurements with a short tTTC or high closing velocity Ṙ, reasoning that they present a large threat because of their high relative speed. The danger here is of possible discrimination of objects with a short range that are traveling at the same speed or faster then the host vehicle. Such an object can very quickly become a threat if the driver of that vehicle hits the brakes hard. The tracking sensors often pick up a lot of stationary road-side objects which make such discriminations likely. The tTTC for road-side objects is often short but often they constitute no threat. tTTC ≈ − 4.5.3 Ranking Objects by their Azimuth Angle Vision sensors often only measure angle to the object ahead. For such a sensor the measurements need to be ranked based on the angular position only. A simple approach would be to consider objects straight ahead as more dangerous. This approach, of course, also has the risk of missing some threats. For example, an object that is dead ahead but far away and has low oncoming speed will be considered more dangerous than an object nearby with a high relative speed but azimuth angle not equal to zero. In general one should also consider the vehicle’s own turn rate when one uses the azimuth angle to prioritize threats. If the vehicle is turning, objects with an azimuth angle in that direction are probably a greater threat than those with azimuth angle equal to zero. It is clear that real threats will sometimes be missed if one chooses to prioritize objects only after range, tTTC or azimuth angle. A more appropriate and general approach is to use all of the quantities to rank the relevance of each measurement. One suggestion on how this could be done is given in [38]. Of course when designing the ranking algorithm one should always keep in mind that this step is used to decrease computational load. It is therefore desirable to keep the algorithm as computationally simple as possible. All the theory needed to build a tracking system has now been presented. Algorithm 4.2 summarizes the tracking algorithm that will be used later in demonstrator vehicle 1. 4.6 Sensor Fusion In tracking systems, it is often the case that there are several tracking sensors. For example, in a CMbB system it is desirable to have more than one tracking sensor to increase the reliability of the system, i.e., to avoid making faulty interventions caused by invalid obstacles or spurious target observations at one sensor. Now the question arises how to fuse the data from the diﬀerent sensors. This task is often called sensor or data fusion. A thorough treatment of sensor fusion problems is given in [49]. To fuse independent estimates of the same quantity is straightforward. Section 4.6 51 Sensor Fusion 1. Association: The purpose of the association is to assign measurement to the current tracks. This is done for each tracking sensor (in this case the measurements come from a radar sensor). The following steps are performed: (a) Cluster measurements. Measurements from the same sensor that are very close to each other are clustered. (b) Rank measurements. Measurements are reordered (range, tTTC and azimuth angle are here used to determine the importance of each measurement). The ordered measurements are denoted Y = (y (1) , y (2) , · · · , y (M) ). (c) Select the most relevant measurements. (d) Gating. The measurements Y are compared with h(x̂(j) ) to check if they fall in any of the gates G(j) . Here a rectangular gate according to: |r(i) − h1 (x̂(j) )| < rgate y (i) − h(x̂(j) ) ∈ G(j) → |ṙ(i) − h2 (x̂(j) )| < ṙgate (4.39) |ϕ(i) − h3 (x̂(j) )| < ϕgate is used. (e) Measurement association. If there exist conﬂicts after the gating procedure these are resolved using a NN approach. Here a geometric distance measure is used. The measurement i that is associated with track j is denoted y i,j . 2. Filtering: Update state estimates for track j ∈ {1, 2, 3, 4} with associated measurement y i,j . x̂t|t = x̂t|t−T + Pt|t−T CtT (Ct Pt|t−T CtT + Rt )−1 (yt (j) (j) (j) (j) (i,j) (j) − Ct x̂t|t−T ), Pt|t = Pt|t−T + Pt|t−T CtT (Ct Pt|t−T CtT + Rt )−1 Ct Pt|t−T . (j) (j) (j) (j) (j) 3. Prediction: (a) Predict future states. (j) (j) x̂t+T |t = At x̂t|t , Pt+T |t = At Pt ATt + Bt Qt BtT . (j) (j) (b) Iterate the previous step to obtain predictions for as many time instants as desired. Algorithm 4.2: Tracking algorithm used in demonstrator vehicle 1. 52 Chapter 4 Tracking for Collision Avoidance Applications Figure 4.7: Pixel/data level fusion. In this approach the information from each sensor is associated with current tracks and then jointly fused to extract features of the measured object. If X 1 and X 2 are two independent estimates of the same quantity with covariance matrices P 1 and P 2 , Equation (4.40a) will yield the optimal fused estimate, in the sense that the covariance of X fused is minimized. X fused = P ((P 1 )−1 X 1 + (P 2 )−1 X 2 ), 1 −1 P = ((P ) 2 −1 −1 + (P ) ) . (4.40a) (4.40b) The problem of fusing sensor information in tracking systems is that the sensors measure widely diﬀerent quantities. For example, a mm-radar measures reﬂected radar energy at the antenna, a vision sensor measures light intensity at each pixel, and a GPS receiver measures the ﬂight time of coded transmissions from satellites. In many applications one is not only interested in tracking kinetic quantity of detected objects; one might also be interested in inferring a number of diﬀerent features of the object. These features as well as the kinetic quantities can then be used to classify the object. When designing a system one must decide on what level fusion of sensor data should be performed. Figures 4.7 and 4.8 schematically illustrates two extreme approaches to fusion of sensor information. In the pixel level fusion illustrated in Figure 4.7, one tries to fuse the data at the lowest level, i.e., fusing the raw sensor output. This approach is the most general and promises the best results. It is also the computationally most demanding approach. For large systems, pixel level fusion is often infeasible because of the computational cost as well as system complexity issues. The other extreme is the approach illustrated in Figure 4.8. Here the data from each sensor is treated separately and has its own tracking and feature extraction. The estimated states and features are then fused at the top level. This approach has the advantage that it is robust against failure of one sensor node. Should one sensor fail there will still be a state estimate from the other sensors. From a system engineering point of view such a system is also easier to understand and maintain. For example, switching to another sensor will not require a total redesign of the entire system. These two examples are, of course, extremes and any combination between these two is possible. It is also possible to have feedback in the system for sensor management and diagnosis. For example, to decrease the computational load in one sensor information from other sensors Section 4.7 Models for Tracking and Navigation 53 Declaration level fusion. Here each sensor extracts features and maintains its own objects. The tracked objects from each sensor are then fused in the last stage. Figure 4.8: can be used. An example of such a system is given in the example below. Example 4.5 In a CA system it is desirable to determine not only the speed and distance of objects close to the host vehicle. One also wants to determine its size and type. Figure 4.9 shows a system that fuses data from a mm-radar and a camera sensor to achieve this. In this system the radar is used to detect obstacles and determine the range, range rate and azimuth angle. This information is then fed back to the camera sensor, which performs template matching to determine if the object is a car or not. It also provides a width measurement and a more accurate azimuth angle measurement of the obstacle. In CA systems, faulty interventions are not allowed. Since available tracking sensors all have weaknesses that cause them to sometimes report false targets, the sensor fusion is a crucial part of the system. In CA systems, sensor information should be fused in such a way that the risk of passing on false targets as valid ones to the decision making algorithm is minimized. 4.7 Models for Tracking and Navigation It has already been stated that most tracking is model-based. In this section common models that describes the tracked object’s motion will be discussed. Tracking models are further discussed in [86, 87]. The performance of a tracking system will, to a large extent, depend upon how well the dynamics of the true system is described by the model. Generally speaking, a better model yields a better tracking performance. For CA it is of interest to ﬁnd the Bayesian solution p(xt+T |Yt ), so the better the model, the closer the posterior distribution will be to the true distribution of the vehicle’s future states. However, it is also desirable to use as simple a model as possible, both for computational issues and for understanding the system. Modeling vehicle dynamics is a well-explored area. Very detailed dynamic models exist for cars, aircraft, robot manipulators etc. These models all break down the system into several subsystems and model the dynamics of each subsystem; 54 Chapter 4 Tracking for Collision Avoidance Applications Figure 4.9: Fusion of a radar and a camera sensor. The radar is used to detect obstacles and measure kinematic quantities. The detected obstacles (indicated by the points in the sensor image) are fed back to the camera sensor that performs template matching in the pixel image. After a positive matching spatial properties such as width and height can then be measured in the pixel image. thus, providing a very accurate description of the system. A good introduction to automotive vehicle dynamics is given in [37, 128]. Such models are often too complex for tracking purposes. Often they are computationally too demanding and, more importantly, since the tracking sensors cannot measure all the quantities in the models the ﬁnal estimation result may not be much better than for a simpler model. Instead, a model that captures the major characteristics of the vehicle’s dynamics should be used. Apart from deciding which tracking model to choose, it is also necessary to think about which coordinate system to track in. In [4], it is suggested that modiﬁed polar coordinates are better for angle-only tracking. In this thesis, Cartesian coordinate systems will mainly be used, these are described in Appendix A. The host vehicle and other objects are distinguished by a superscript host or object, e.g. host and object position in the x-direction are denoted Pxhost and Pxobj . ω denotes the vehicle turn/yaw rate. Below common models to describe the dynamics of tracked objects are presented for the two-dimensional case. The constant velocity and constant acceleration models are straightforward to extend to higher dimensions. 4.7.1 The Constant Velocity Model One of the simplest models for tracking is given in (4.41). In this model the motion of the vehicle is modeled as having piecewise constant velocity in the x and y direction. The transitions between diﬀerent velocity are modeled by white Section 4.7 Models for Tracking and Navigation 55 random noise, and there is no correlation between motion in the x and y directions. T x = Px Py Vx Vy , (4.41) 1 0 T 0 T 2 /2 0 2 0 1 0 T 0 T /2 xt + νt . (4.42) xt+T = 0 0 0 1 0 T 0 0 0 1 0 T Assuming that the noise is Gaussian distributed and that the covariance matrix is given by 0 σV2 x Qt = . (4.43) 0 σV2 y An assumption of constant velocity means that the motion is described by a straight line between two measurement updates. 4.7.2 The Constant Acceleration Model A model that assumes that tracked objects have constant velocity is, of course, a very simple one. For tracking of maneuvering targets one often wants to model the dynamics better. A simple extension of the model is to add the acceleration as a state, and assume that the acceleration is piecewise constant. This is often called the constant acceleration model (4.44)−(4.46) T , (4.44) x = Px Py Vx Vy Ax Ay 2 0 0 1 0 T 0 T2 T 3 /6 2 3 0 1 0 T 0 T 0 T /6 2 2 0 0 0 0 1 0 T T /2 xt+T = (4.45) xt + νt , 0 0 0 1 0 0 T T 2 /2 0 0 0 0 0 0 1 T 0 0 0 0 0 1 0 T 2 0 σA x Qt = . (4.46) 2 0 σA y A constant acceleration will model the motion as straight lines or parabolic trajectories. 4.7.3 The Coordinated Turn Model The constant velocity and constant acceleration models are very nice to use in the sense that they are both linear models. When modeling the motion of a vehicle, 56 Chapter 4 Tracking for Collision Avoidance Applications there are more aspects that could be considered. A simple assumption is to say that a vehicle can generate forces either longitudinally (in its heading direction) or laterally (orthogonal to the heading direction). If one assumes that the velocity is constant and that a constant lateral force is applied, this results in centripetal motion, with constant turn rate. Thus the motion is described by straight lines and circle segments. T , (4.47) x = Px Py Vx Vy 1−cos(ωT ) sin(ωT ) 1 0 T 2 /2 0 ω ω 1−cos(ωT ) sin(ωT ) 2 0 1 −( 0 T /2 ) − ω ω νt , (4.48) xt+T = xt + cos(ωT ) sin(ωT ) 0 0 0 T 0 0 − cos(ωT ) sin(ωT ) 0 T 0 σV2 x Qν = . (4.49) 0 σV2 y In the coordinated turn model ω denote the vehicle turn rate. Such a model was proposed in [94], and is often called the coordinated turn model. In this model it is assumed that the turn rate of the observed object is known and piecewise constant. In reality, the turn rate is often not known and has to be estimated. 4.7.4 The Nearly Coordinated Turn Model The model, which assumes piecewise constant turn rate and includes ω as a part of the state estimation process, is called the nearly coordinated turn model. The discrete-time nearly coordinated turn model is given by (4.50) – (4.51). T x = Px Py Vx Vy ω (4.50) sin(ωT ) 1−cos(ωT ) 2 1 0 T 0 /2 0 0 ω ω ) ) 0 1 −( 1−cos(ωT 0 ) − sin(ωT 0 T 2 /2 0 ω ω xt+T = cos(ωT ) sin(ωT ) 0 0 0 xt + T νt 0 0 0 0 0 − cos(ωT ) sin(ωT ) 0 T 0 0 Qν = 0 0 σV2 x 0 0 σV2 y 0 0 0 0 0 1 0 0 T (4.51) (4.52) σω2 A comparison between the constant velocity model (piecewise straight line motion), the constant acceleration model (parabolic trajectories), and the nearly coordinated turn model (motion on circle segments), in a tracking application, is given in Example 4.6. Section 4.7 57 Models for Tracking and Navigation Tracked vehicle path 30 Global position [m] 20 10 0 t1 t3 t2 t4 t5 t6 t7 −10 −20 −30 −40 0 20 40 60 80 Global position [m] 100 120 Figure 4.10: Trajectory of a tracked vehicle performing: acceleration–lane change– deceleration–right turn. Acceleration starts at point t1 , lane change at point t3 , deceleration at t5 and right turn starts at t6 . Example 4.6 In Figure 4.11 a comparison between the constant velocity, the constant acceleration model and the nearly coordinated turn model applied in an automotive-like estimation problem is illustrated. In the comparison the manoeuvre described in Figure 4.10 is used. A bicycle model described in Section 4.8.1 was used to generate the true trajectory. For each model, 1000 Monte Carlo simulations are performed (the same measurement noise realizations are used for each model). The EKF parameters (θ) were tuned to give the best possible tracking performance according to N Pt − P̂t . (4.53) θ = arg min θ t=1 The measurements are given by r r er y = h(xt ) + et = ṙ + et = ṙ + eṙ , ϕ ϕ (4.54) eϕ where all the ei :s are independent white Gaussian random variables, with σr = 0.5 m, σṙ = 1 m/s and σϕ = 0.5π/180 rad. The measurement rate and tracking system update frequency in this example is 10 Hz. By looking at Figure 4.11, it appears that the constant acceleration model and the nearly coordinated turn model have similar performance. Looking at the mean square predicted error and the maximum predicted error in Table 4.1, it is clear that the coordinated turn model performs better for both aspects. For all the models an EKF was used, since the measurement equations are nonlinear. The nearly coordinated turn model is 58 Chapter 4 Tracking for Collision Avoidance Applications Tracked path for 3 tracking models (average path from 1000 MC simulations) 4 2 Global position [m] 0 −2 −4 −6 −8 True Path Constant Acceleration Constant Velocity Coordinated Turn −10 −12 −14 −16 0 50 100 Global position [m] 150 Tracked trajectory for the constant velocity model, the constant acceleration model and the nearly coordinated turn model. The ﬁgure illustrates the average path from 1000 MC simulations for each model. Figure 4.11: nonlinear and thus the model needs to be linearized each time step. This is not the case for the constant velocity and the constant acceleration model. The ﬁlter for the nearly coordinated turn model will thus be slightly more computationally demanding. In the Monte Carlo simulations performed here, the required CPU time for the nearly coordinated turn model was 2.8 times more than for the constant velocity model. The constant acceleration model required 1.2 times more CPU time than the constant velocity model. 4.8 Navigation This chapter has mainly discussed the tracking of other vehicles’ dynamic states. The tracking of the host vehicle’s states is sometimes called navigation or positioning. In this thesis the term navigation will be used for the task of estimating the states of the CA host vehicle. Basically the same ﬁltering techniques and vehicle models that are used for tracking other vehicles can be used for this purpose. The only diﬀerence is that other sensors are used to provide information of the kinematic properties of the vehicle. The input to navigation systems comes from accelerometers, gyroscopes, wheel speed sensors, air/water ﬂow sensors and GPS systems etc. Example 4.7 illustrate the potential of using the information from a steering wheel angle sensor, for navigation in a automotive application. The navigation sensors Section 4.8 59 Navigation Table 4.1: Results from a comparison of three tracking models. For each model 1000 Monte Carlo simulations were performed, where the same noise realizations were used for all models. Both the average error in column two and the maximum error in column three are measures of the spatial distance between the true position and the prediction position. The CPU time does not say anything about the actual time to execute each algorithm and should only be used for comparing the algorithms with each other Tracking model RMSE [m] Max error [m] CPU time Constant velocity 0.88 5.2 1 Constant acceleration 0.65 4.7 1.2 Nearly coordinated turn 0.56 3.5 2.8 often have a lower noise level than the tracking sensors. Furthermore, quantities such as acceleration and turn rates can be measured directly. Therefore, it might make sense to use a more accurate model for navigation than for tracking other vehicles. How should a more accurate vehicle model be constructed? Often one tries to describe how the forces aﬀecting the motion are being generated. Example 4.7 Sometimes cars are equipped with a steering wheel position sensor and a brake pedal force and position sensor. These sensors can be used to increase the tracking and prediction performance of the host vehicle. Figure 4.12 shows the relationship between steering wheel angle and yaw rate of a vehicle traveling at 50 km/h. There is a delay in the order of several hundred ms between the steering wheel angel and the yaw rate. Compared with the use of yaw rate measurements only, the use of steering wheel measurements will tell you much earlier when the vehicle will turn. In this case it will clearly make sense to model the dynamics between steering wheel and yaw rate in the navigation system. The performance of a CA system using this information would be signiﬁcantly improved by the “preview” oﬀered by the use of the steering wheel sensor. In some sense this is very similar for cars, aircraft and boats. The forces are generated when the control actuators are moved out of the heading direction of the vehicle. This creates a slip angle between e.g. the wheels or rudder and the heading direction. The force of friction between tire-to-road, rudder-to-air or water generates forces that are used to control the vehicle. It is important to note that the forces that can be generated in this fashion are limited. Thus, there is a fundamental nonlinear constraint to the motion of the vehicle. In the next section a simple handling model for the motion of a car is given. 4.8.1 Bicycle Model A more accurate model for modeling handling properties of a car is the well known bicycle model. In this model the eﬀect of both front and both rear wheels are 60 Chapter 4 Tracking for Collision Avoidance Applications Yaw rate and steering wheel angle for a lane change maneuver 150 Yaw rate Steering wheel angle Yaw rate [deg/s] 100 50 0 −50 −100 −150 0 1 2 3 4 Time [s] 5 6 7 8 Figure 4.12: The yaw rate and steering wheel angle for a vehicle traveling at 50 km/h performing a lane change manoeuvre. The steering wheel angle has been scaled to be of the same size as the yaw rate. The time constant between steering wheel angle and yaw rate is approximately 500 ms. merged to one front and one rear wheel (thus the vehicle is modeled as a bicycle). The motion of the vehicle is then approximated by calculating the forces generated at the front and rear wheel. Figure 4.13 shows a schematic image of this model. In the ﬁgure only the lateral wheel forces are indicated. The longitudinal and lateral forces on the wheels are given as a function of the longitudinal and lateral slip, respectively. The longitudinal slip is given by the slip coeﬃcient s= rwh ωwh − vx , rwh ωwh (4.55) where rwh denote the wheel radius and ωw the wheel turning rate. The relationship between slip and force is a nonlinear function; a model for this is given by Pacejkas’ magic formula [6] which is plotted in Figure 4.14. This kind of model can explain phenomena such as skidding. Therefore it is often used, for example, as a reference model in yaw control systems. For tracking other vehicles, this model is probably too detailed. For tracking the host vehicle (navigation) this model might be appropriate. In fact, in vehicles equipped with a yaw control system, a bicycle model is used as a reference model to detect when the vehicle is skidding. The yaw acceleration is calculated from the torque balance equation Jz ω̇ = cos(δ)Ff a − Fr b, (4.56) where in this case Jz is the moment of inertia and longitudinal wheel forces, pitching motion, roll motion, wind forces, rolling resistance, and suspension dynamics are Section 4.8 61 Navigation A schematic image of the bicycle model, here only the lateral forces on each tire are indicated. The longitudinal forces work along the wheel direction normal to the lateral forces. Figure 4.13: Force as a function of slip 1 0.8 Normalized force µ (F/N) 0.6 0.4 0.2 0 −0.2 −0.4 −0.6 −0.8 −1 −1 −0.5 0 Slip 0.5 1 Figure 4.14: Relationship between slip and force between tire and road surface according to Pacejkas’ magic formula. Here the value is normalized with the normal force on the wheel. The values correspond to warm and dry asphalt. 62 Chapter 4 Tracking for Collision Avoidance Applications neglected. The lateral wheel forces are calculated from the slip angle for the front wheel, αf , and rear wheel, αr , which can be solved from aω + v sin(β) tan(δ − αf ) = (4.57) v cos(β) , bω − v sin(β) tan(αr ) = (4.58) v cos(β) , The bicycle model is used to model the host vehicle dynamics in the simulation environments for the algorithms used in the demonstrator vehicle presented in Section 8.2. A derivation of the dynamic equations for the bicycle model is given in [103], where it is referred to as the single track model. Part III Decision Making 63 Chapter 5 Deterministic Decision Making By deﬁnition the goal of a collision avoidance (CA) system is to avoid or mitigate collisions by an autonomous action or warning. Here the task of deciding on an action is called decision making. The problem of deciding when and how the system should act is complicated by several factors. Among these factors are measurement and process noise. Anther factor is that simpliﬁed motion models must be used for computational reasons. A CA decision is based on the current state estimate of the host vehicle and those of other objects. The estimates are provided by the tracking system, and are used in one or more decision function to determine the appropriate collision avoidance action. Because of the measurement and process noise, the state estimates are uncertain. In this chapter the state estimate uncertainty is neglected. The focus is on how to determine the threat of a collision given that the state of other objects is known. CA strategies in general and various collision avoidance decision functions will be discussed in Section 5.1 and 5.3 respectively. The task of dealing with multiple obstacles will be addressed in Chapter 6. To deal with uncertainty in the decision process, a probabilistic approach, based on statistical decision making, is proposed in Chapter 7, where a general framework for dealing with nonlinear and non-Gaussian models is presented, with diﬀerent alternatives for how to calculate statistical decision making rules. 5.1 Collision Avoidance Decision Strategy The two main desired properties of a CA system are that it should avoid all collisions and that no faulty interventions should occur. The problem with choosing a CA intervention strategy is that these properties are often contradictory. In order to analyze possible strategies, ﬁve diﬀerent states will be deﬁned. In Figure 5.1, these states are visualized for an automotive scenario in which a vehicle is approaching a stationary obstacle. A short description of each state is given below: 65 66 Chapter 5 Normal operation Collision avoidable Deterministic Decision Making Collision unavoidable Collision Post collision Figure 5.1: The ﬁgure shows an example of the ﬁve possible states of a vehicle approaching a stationary obstacle. At ﬁrst the obstacle is far away and presents no imminent threat, this is called normal operation. As the vehicle comes closer to the obstacle it enters the collision avoidable state, here the obstacle is a threat but there still exists an escape path that can be followed by the appropriate driver maneuver. At some point it becomes impossible to avoid the collision; this is called the collision unavoidable state. The collision state is when the vehicle collides. The state after the collision is called the post collision state. 1. Normal operation: In this state there is no imminent threat or risk of a collision. 2. Collision avoidable: Here there is a threat to the vehicle. A non-negligible risk exists that a collision will occur. In this state it is still possible to avoid the imminent collision by an appropriate avoidance maneuver. Typically this state is perceived by a human as dangerous. What is perceived by a CA system as a dangerous situation will of course vary depending upon the CA decision function used. It is normally in this state that collision warning systems are activated. Any system intended to avoid collision has to act in this state. 3. Collision unavoidable: In this state a collision is imminent, and cannot be avoided by any maneuver. Although the collision cannot be avoided, it might still be possible to signiﬁcantly reduce its severity by reducing the collision speed and by taking other mitigating actions. 4. Collision: This is the state when a collision occurs. 5. Post collision: The state when a collision has occurred. If the CA system is still operational, actions to avoid secondary collisions could be considered. It should be noted, since information of the surrounding environment is limited, it is in many cases possible to go directly from the normal operating state to the collision unavoidable state. A common example of such an event is a multiple collision (pileup) on a free-way. In this case a following vehicle is in the normal operating state until the lead vehicle suddenly collides. If the traﬃc is dense and the time gap to Section 5.1 Collision Avoidance Decision Strategy 67 H (a) Overtaking situation, here two vehicles are on a collision course. The appropriate maneuver for the overtaking vehicle is to accelerate and then turn back into the right hand lane to avoid a collision. H (b) Meeting on a narrow road, the two vehicles might be on a collision course. Normally both drivers will steer their vehicle to the right side of the road just before they pass each other. Figure 5.2: Two traﬃc situation examples where the two vehicles involved are on a collision course. Even though the situations are threatening in some sense, there is no danger except if the driver has misjudged the situation (e.g. there is not enough time to ﬁnish the overtaking, or the road is too narrow for the two vehicles to pass). the lead vehicle is small the collision instantaneously becomes unavoidable. The collision unavoidable state is clearly deﬁned by the laws of physics. The collision avoidable state on the other hand is harder to deﬁne. Despite there being many situations in normal driving where a collision is close, these situations are not always perceived as threatening (or at least not as situations where a CA system should intervene). Two examples of such situations, are a overtaking situation and meeting on a narrow road, illustrated in Figure 5.2. The strategy for when to perform an autonomous CA intervention will inevitably be a trade-oﬀ between a system’s eﬀectiveness in avoiding collisions and faulty intervention frequency. The tolerance for faulty intervention will, of course, depend on the type of intervention. For example, an erroneous warning may not be as bad as an erroneous avoidance maneuver. In collision warning (CW) systems, human reaction time and non-ideal performance of the driver must be accounted for. Thus, the decision boundary for a CW system must be in the collision avoidable state; with suﬃcient time to react before the collision unavoidable state is reached. For an inattentive driver to be able to avoid an imminent collision, the warning normally has to come more than one second before the collision unavoidable state is entered (see [85]). 68 Chapter 5 Deterministic Decision Making Avoidance maneuver along a circular path 20 15 host r y P [m] 10 5 0 −5 −5 whost 0 wobj 5 10 15 20 Px [m] Figure 5.3: A steering avoidance maneuver approximated by constant lateral acceleration (9.82 m/s2 ). 5.2 Static Analysis of Collision Mitigation To analyze the potential of a system which intervenes only when the collision becomes unavoidable, the ideal performance of an automotive CMbB system is studied in Section 2.2.4. In this section the same analysis is performed with a simpliﬁed model of the host vehicles motion. The scenario that is studied is the head-on against a stationary obstacle illustrated in Figure 5.1. Both the vehicle and the obstacle are assumed to be 2 meters wide. Furthermore, it is assumed that maximum deceleration and centripetal acceleration can be achieved instantaneously. Thus, the steering maneuver follows a circular path according to Figure 5.3. The coordinate system used here is described in Appendix A. The braking distance and the distance needed to avoid the obstacle by a steering maneuver (with constant centripetal acceleration) are plotted in Figure 5.4. The radius rhost of the circle for the host vehicle traveling with a constant centripetal acceleration ay (here it is assumed that ay = 9.82 m/s2 can be achieved) is given by rhost = vx2 . ay (5.1) Section 5.2 69 Static Analysis of Collision Mitigation Distance required to avoid a collision 50 45 Brake distance Steer distance 40 Distance [m] 35 30 25 20 15 10 5 0 0 20 40 60 80 Initial velocity [km/h] 100 120 Distance required to avoid a collision by braking and by steering. The assumption here is that the vehicle can instantaneously achieve 9.82 m/s2 deceleration or 9.82 m/s2 lateral acceleration. Figure 5.4: The distance to avoid an obstacle of width wobj , assuming that the host vehicles width is whost , is given by the trajectory swept by the right side of the vehicle according to (rhost + whost /2)2 = Px2 + (Py − rhost )2 . (5.2) This is illustrated in Figure 5.3. Setting the longitudinal position, Py , equal to wobj /2 and solving for Px (the longitudinal distance) yields (whost )2 (wobj )2 + rhost wobj − rhost whost + 4 4 v2 (whost )2 − (wobj )2 = ± ( x )(whost + wobj ) + ay 4 Px = ± = Px,steer , (5.3) (5.4) (5.5) where, in this case, the positive solution is of interest. The distance traveled during constant acceleration is given by Px (t) = Px,0 + t Vx,0 + t0 τ a ds dτ, (5.6) t0 1 = Px,0 + Vx,0 t + Ax,0 t2 . 2 (5.7) 70 Chapter 5 Deterministic Decision Making Collision velocity 120 With braking Without braking Collision velocity [Km/h] 100 80 60 40 20 0 0 20 40 60 80 Initial velocity [km/h] 100 120 Figure 5.5: The collision speed for the head-on scenario with and without autonomous braking. The assumption is that a deceleration of 9.82 m/s2 can be applied instantaneously when the collision unavoidable state is entered. This deceleration is applied at the minimum of the braking and steering avoidance distance, which are plotted in Figure 5.4. Similarly the velocity is given by Vx (t) = Vx,0 + t a dτ, (5.8) t0 = Vx,0 + Ax,0 t. (5.9) By setting the ﬁnal velocity Vx (t) to 0, t can be eliminated and the braking distance can be calculated from 2 vx,0 . (5.10) Px,brake = 2ax Thus, the braking distance and steering avoidance distance are 2 vx,0 , 2a x v2 (whost )2 − (wobj )2 . = ( x )(whost + wobj ) + ay 4 Px,brake = (5.11) Px,steer (5.12) As can be seen from both the equations and the graph, the braking distance is proportional to the square of the velocity, whilst the steering distance is basically linearly proportional to the velocity. Furthermore, for low speeds (below 40 km/h), Section 5.3 Collision Avoidance Decision Functions 71 braking is more eﬃcient and for high speeds (above 40 km/h), steering is more eﬃcient to avoid a collision. If braking is applied exactly at the collision unavoidable boundary, the collision speed for this scenario would be as illustrated in Figure 5.5. For scenarios where the initial speed is below 40 km/h the accident is completely avoided. For scenarios where the initial speed is above 40 km/h, the collision speed is reduced by 25 km/h. The results from the analysis carried out in this section is thus very similar to the once presented in Section 2.2.4, which are based on a detailed handling model. 5.3 Collision Avoidance Decision Functions As already stated, a CA decision function, g(X), that measures the threat other objects constitute is required to make any decision. The two main factors to consider when deriving a decision function are what dynamic model to use for the host vehicle and other objects and the assumption of future actions of the host and obstacle. The model describing dynamic properties and future action is here called the decision model, x̃ = xobj − xhost , x̃˙ = f (x̃), (5.13a) (5.13b) where x̃ is the relative state vector and f (·) describes the dynamics of the relative motion. Note, that while the tracking model is naturally in discrete time, because of the discrete time measurement updates, the decision model is given in continuous time. The decision model is used to calculate the future trajectory (or possibly trajectories) according t f (x̃)dτ + x̃(t0 ). (5.14) x̃(t) = t0 Often the same models that are used for tracking are also used for decision making. Below the time continuous constant velocity, constant acceleration and coordinated turn model are summarized. Note, that the constant acceleration and the constant velocity also hold for relative states. • Constant velocity: T x = PT VT , 0 I ẋ = xt . 0 0 (5.15a) (5.15b) The solution for the position and velocity state is given by P(t) = P0 + V0 t, (5.16a) V(t) = V0 . (5.16b) 72 Chapter 5 Deterministic Decision Making • Constant acceleration: T x = PT VT AT . 0 I 0 ẋ = 0 0 I xt . 0 0 (5.17a) (5.17b) 0 The solution for the position and velocity state is given by P(t) = P0 + V0 t + 1 A0 t2 , 2 V(t) = V0 + A0 t. (5.18a) (5.18b) • Coordinated turn (two dimensions): x = Px 0 0 ẋ = 0 0 0 1 0 0 0 0 0 1 −ω 0 ω 0 T ω , 0 0 0 xt . 0 0 0 0 0 0 Py Vx Vy (5.19a) (5.19b) The solution for the position and velocity state is given by sin(ωt) 1 − cos(ωt) Vx,0 + Vy,0 , ω ω 1 − cos(ωt) sin(ωt) Vx,0 − Vy,0 , Py (t) = Py,0 − ω ω Vx (t) = cos(ωt)Vx,0 + sin(ωt)Vy,0 , Px (t) = Px,0 + Vy (t) = − cos(ωt)Vx,0 + sin(ωt)Vy,0 . (5.20a) (5.20b) (5.20c) (5.20d) Given the trajectory of each object a function to measure the threat is formulated. Below several CA decision function are derived for the most commonly assumed decision models. Also novel alternatives to deal with piecewise constant motion, actuator dynamics, constant centripetal acceleration, and general 2-dimensional acceleration are presented. 5.3.1 Headway and Headway Time The headway (pHW ) is the distance to the obstacle in the path ahead. However, this does not always correspond to the radial distance (r) often measured by the sensors. One example when they diﬀer is when the vehicle is traveling along a Section 5.3 73 Collision Avoidance Decision Functions curved path. A thorough treatment of curved coordinate systems is given in [27]. The headway time tHW is given by tHW = solt {Phost (t + t0 ) = Pobj (t0 )}, (5.21) where solt {} denotes the solution with respect to t such that the expression within the brackets is fulﬁlled. Assuming both vehicles travel at equal and constant speed, v host = v obj , ahost = aobj = 0, (5.22) the headway time (tHW ) is given by tHW = pHW . v0host (5.23) This metric is often used to evaluate the hazard level for how a car is being driven with respect to the vehicles in front of it, see e.g., [124]. 5.3.2 Time to Collision The time to collision (tTTC ) is deﬁned as follows tTTC = solt {p̃x (t) = 0}. (5.24) When calculating time to collision the relative acceleration is often assumed to be constant according to (5.17b). With a constant acceleration decision model of the vehicles motion the solution to (5.24) reduces to solving 0 = p̃x,0 + ṽx,0 t + The solution is given by p̃ − ṽx,0 , x,0 √ 2 −2p̃ ṽx,0 x,0 ãx,0 ṽ x,0 , − ãx,0 − √ ãx,0 2 −2p̃ ṽ ã x,0 x,0 tTTC = − ṽx,0 + x,0 , ãx,0 ãx,0 Undeﬁned Undeﬁned ãx,0 t2 . 2 (5.25) ṽx,0 < 0 and ãx,0 = 0, ṽx,0 < 0 and ãx,0 = 0, ṽx,0 ≥ 0 and ãx,0 < 0, ṽx,0 ≥ 0 and ãx,0 ≥ 0, 2 ṽx,0 (5.26) − 2p̃x,0 ã < 0. 2 − 2p̃x,0 ãx,0 < 0 or Thus, tTTC is given by the smallest positive solution. If ṽx,0 (ṽx,0 ≥ 0 and ãx,0 ≥ 0) no solution exist. This means that the velocity and acceleration is such that no collision occurs. 5.3.3 Closest Point of Approach The headway and time to collision decision functions deal with motion in one dimension only (obstacles that are determined to be in the path of host vehicle). 74 Chapter 5 Deterministic Decision Making The closest point of approach (pCP A ) is often used in two-dimensional motion (often for marine or ATC applications). It denotes the predicted minimum separation between two vehicles, PCP A = mint>t0 (P̃(t)). (5.27) Often an assumption of constant velocity according to (5.15b) is assumed when deriving the PCP A , in this case PCP A (for 2D motion) can be derived as follows. The separation is given by " P̃(t) = P̃ 2 (t) + P̃ 2 (t), x y (5.28) where the relative distance in each direction, P̃i (t), is given by (5.16a). For constant velocity the relative separation becomes " P̃(t) = (P̃x,0 + Ṽx,0 t)2 + (P̃y,0 + Ṽy,0 t)2 . (5.29) The time for the closest point of approach (tCP A ) is obtained by solving d|P̃(t)|2 = 0. dt (5.30) This yields P̃x,0 Ṽx,0 + P̃y,0 Ṽy,0 . 2 + Ṽ 2 Ṽx,0 y,0 (5.31) (P̃y,0 Ṽx,0 − P̃x,0 Ṽy,0 )2 . 2 + Ṽ 2 Ṽx,0 y,0 (5.32) tCP A = − Thus PCP A = 5.3.4 Potential Force In potential ﬁeld algorithms an artiﬁcial potential force acting on the host vehicle is calculated. This force is used to generate a control input to avoid any obstacle. This potential force can also be viewed as a collision decision function. A common and simple expression to calculate the potential force is given by c F = − i P̃, P̃ (5.33) where c is a constant, and i ∈ N. Potential ﬁeld algorithms are common in robotic and autonomous vehicle applications. This approach was ﬁrst introduced in [76], references to diﬀerent applications and more sophisticated potential ﬁeld algorithms than (5.33) are [17, 35, 42, 43, 92, 93, 95, 97, 98, 122]. Section 5.3 75 Collision Avoidance Decision Functions 5.3.5 Required Longitudinal Acceleration The required longitudinal acceleration, ax , measures the longitudinal acceleration required to bring the relative velocity to zero at the time of the collision # T T $ host = 0 0 ax,req = solahost , (5.34) p̃x (t) ṽx (t) x {· · · } denotes the solution with respect to the host vehicle acceleration where solahost x ahost where p̃ x (t) ≥ 0 for all t. With the assumption of constant acceleration the x solution to (5.34) is given by solving the equation system % 0= 0= ṽx,0 + ãx t, 2 p̃x,0 + ṽx,0 t + ãx2t , (5.35) This yields the constant acceleration required to avoid a collision, according to obj obj ahost x,req = ax,0 − ãx = ax,0 − |ṽx,0 |ṽx,0 . 2p̃x,0 (5.36) 5.3.6 Required Longitudinal Acceleration for Piecewise Constant Motion In some cases a better motion model is to assume that all vehicles will remain stationary when zero velocity is reached. Thus, instead of a constant acceleration, a piecewise constant acceleration is assumed. For a situation where the obstacle comes to a stop before the collision occurs solving (5.34) means solving the equation system 0 = v host + ahost t, x,0 x (5.37) ahost t2 p̃ host x = v t + , x,final x,0 2 where p̃x,final = p̃x,0 − obj 2 (vx,0 ) aobj x,0 . (5.38) With the assumption of a constant host acceleration the required acceleration to avoid a collision is given by host 2 ) (vx,0 . ahost = − x,req obj 2 (vx,0 ) 2 p̃x,0 − 2aobj (5.39) x,0 This CA decision function is also discussed for automotive application in [54]. 76 Chapter 5 Deterministic Decision Making 5.3.7 Required Longitudinal Acceleration with Actuator Dynamics The equations (5.36) and (5.39) were derived assuming that the maximum deceleration can be achieved momentaneously. In most vehicles, it will take some time from a control input to a system response. A better CA decision function also considers the actuator dynamics. A simple model is to describe the actuator dynamic (in this case the brake system) by a ﬁrst order model Laplace domain transfer function according to Gbrake (s) = k1 . s/k1 + 1 (5.40) Hence, instead of assuming that the acceleration can be changed instantaneously, the assumption is that the reference acceleration aref , which is the input to the dynamical system (5.40), can be instantaneously changed. The step response for this system yields the following expression for the acceleration ã(t) = aref − aref e−k1 t (5.41) where aref is the input requested acceleration. With the acceleration according to (5.41) the velocity and distance traveled during deceleration can be calculated by aref aref e−k1 t + aref t + , k1 k1 aref e−k1 t aref aref t2 aref − t+ + 2 . p̃(t) = ṽ0 t − 2 k1 2 k1 k1 ṽ(t) = ṽ0 − (5.42) (5.43) The time to bring the velocity to zero is given by the solution to (5.42), which yields tstop −ṽ0 k1 + aref + L(−e = aref k1 (ṽ0 k1 −aref ) aref )aref , (5.44) where w = L(x) denotes the solution to wew = x. By combining (5.44) and (5.43) the stopping distance to be calculated. A decision function accounting for actuator dynamics is given by the stopping distance calculated for aref = −aa , where aa denotes the maximum possible acceleration. This can be compared to the measured distance, to determine any action. In Figure 5.6 a comparison of the braking distance according to (5.36) and (5.43) is shown. Example 5.1: Brake System Model To calculate the stopping distance according to the model above, the brake system characteristics need to be known. A complete brake system model is very complex. However, looking at overall vehicle performance, a simple yet accurate description can be obtained for the vehicle’s acceleration by the ﬁrst order system in (5.40). Figure 5.7 shows measurement data from ten brake maneuvers with a Volvo V70 Section 5.3 77 Collision Avoidance Decision Functions Brake distance Alarm delay 60 0.14 (5.39) (5.43) 0.12 40 0.1 Alarm delay [s] Brake distance [m] 50 30 0.08 20 0.06 10 0.04 0 0 50 100 150 Initial velocity [km/h] 0.02 0 50 100 150 Initial velocity [km/h] Figure 5.6: The left ﬁgure shows the braking distance according to (5.36) and (5.43), respectively. The maximum acceleration is assumed to be 9.82 m/s2 and the rise time for ﬁrst order brake system is 0.3 seconds. If the actuator dynamics is neglected in the decision making, as in (5.36), the CA action will be delayed compared to a criterion that accounts for the actuator dynamics ,(5.43). This delay is illustrated in the right ﬁgure. Measured and simulated braking system Simulated Measured 2 Acceleration [m/s ] 0 −5 −10 −15 0 0.5 1 Time [s] 1.5 2 Distance [m] 20 10 5 0 0 Figure 5.7: Simulated Measured 15 0.5 1 Time [s] 1.5 2 Data from a hard braking maneuver, 110–0 km/h for a Volvo V70. 78 Chapter 5 Deterministic Decision Making and a ﬁrst order approximation. The measurements were performed on warm, dry asphalt. For the simulated data a ﬁrst order system was used with transfer function Gbrake (s) = k1 , s/k1 + 1 (5.45) with k1 = 7 and aref = 11 identiﬁed such that the system rise time is 0.3 s and the stationary value is 11 m/s2 , Comparing distance traveled after 2 seconds the diﬀerence is less than 1 m, which is suﬃcient for CA purposes. More information on typical brake system behavior can be found in [3]. 5.3.8 Required Lateral Acceleration Similar to ahost it is possible to derive the lateral acceleration to avoid a collision. x This means looking for a steering maneuver that makes the host vehicle pass either to the right or to the left of the obstacle. This is given by % Ahost y,i = solAhost y & p̃x (t) 0 = , whost /2 + wobj /2 |p̃y (t)| (5.46) Here constant acceleration (in the global coordinate system) is assumed, according to (5.17b). The lateral position with constant acceleration is given by (5.18a). The time to collision is given by (5.26). Hence, solution to (5.46) is obtained from solving ±( Ãy t2TTC wobj whost + ) = P̃y,0 + Ṽy,0 tTTC + , 2 2 2 (5.47) which yields obj Ahost y,i = Ay − Ãy host obj 2 −P̃y,0 ± ( w 2 + w 2 ) − Ṽy,0 tTTC = Aobj . y,0 − t2TTC host (5.48) obj The sign in front of ( w 2 + w 2 ) determines the acceleration required to pass to host the left (Ahost y,left ) and right (Ay,right ) of the obstacle, respectively. The minimum lateral acceleration to avoid a collision is given by host host Ahost y,req = min(|Ay,left |, |Ay,right |). (5.49) Note that the acceleration is kept constant in the ground ﬁxed coordinate system. Thus, the resulting vehicle trajectory is parabolic. Section 5.3 79 Collision Avoidance Decision Functions 5.3.9 Required Centripetal Acceleration An alternative way to calculate the acceleration required to avoid a collision by some steering maneuver is to assume a constant lateral acceleration in a vehicle ﬁxed coordinate system. This means ﬁnding a solution to (5.46) for the case where the host vehicle’s motion is now described by a circular path according to the coordinated turn model (constant radial velocity and centripetal acceleration) see Figure 5.3. Finding a solution to (5.20a) can be diﬃcult so here an approximation is used. First the time to collision is calculated according to (5.26). Furthermore, the obstacle moves with constant acceleration according to (5.17b). For t = tTTC the position of the obstacle’s right or left edge is given by 2 Aobj x tTTC , 2 2 Aobj wobj y tTTC ± . = P̃y,0 + Vyobj tTTC + 2 2 obj Px,TTC = P̃x,0 + Vxobj tTTC + (5.50) obj Py,TTC (5.51) Now the static reasoning that was used in Chapter 5.2 can be used. The radius of the circle is given by (v host )2 (5.52) rhost = xhost . ay Assuming the center of the vehicle passes through the origin, the trajectory swept by the right side of the vehicle is given by (rhost + whost /2)2 = Px2 + (Py − rhost )2 . (5.53) Thus the circle radius is given by host 2 r host Px2 + Py2 − (w 4 = whost + 2Py ) . (5.54) Inserting (5.52) into (5.54) and solving for the acceleration yields the required centripetal acceleration to pass the obstacle on the left side. ahost y,l = (vxhost )2 (whost + 2Py,TTC ) 2 2 Px,TTC + Py,TTC − (w host )2 4 . (5.55) For the right maneuver the acceleration is instead given by ahost y,r = (vxhost )2 (whost − 2Py,TTC ) 2 2 Px,TTC + Py,TTC − (w host )2 4 . (5.56) For the special case that the obstacle is stationary and has the same width as the host vehicle (5.55) and (5.48) yield the same expression. 80 Chapter 5 Deterministic Decision Making 5.3.10 Multi-Dimensional Acceleration to Avoid Collision The required accelerations (5.48) and (5.55) are restricted to work only in one predeﬁned direction (longitudinal or lateral). A simple way to extend these onedimensional metrics to consider both longitudinal and lateral maneuvers, is simply to take |, |Ahost |). Areq = min(|Ahost x y (5.57) A CA decision algorithm based on (5.57) will be called a single obstacle constant control (SOCC) algorithm, since it only consider one obstacle and escape maneuvers with a constant acceleration. Note that for the SOCC algorithm an avoidance maneuver is restricted to be only in the lateral or longitudinal direction. Furthermore, there is no distinction between diﬀerent directions i.e., it is assumed that the bound for lateral and longitudinal acceleration is the same. A tire’s ability to generate forces in an arbitrary direction is normally described by an ellipse, the friction ellipse (Ef ), see e.g. [37, 103, 128]. An alternative to thresholding (5.57) is to determine if the acceleration vector in the vehicles longitudinal direction, Ahost x,req , or in the lateral direction, Ahost , lies within the friction ellipse, y,req Ahost x,req ∈ Ef or Ahost y,req ∈ Ef . (5.58) In order to evaluate (5.58) the coeﬃcient of friction, µ, must be known. The estimation of tire-to-road friction is discussed in [44, 45]. For general two-dimensional motion, it is possible to calculate, analytically, the minimum constant acceleration vector (for an arbitrary direction) required to avoid a collision. The value of the acceleration vector can be calculated from 2 2 2 (Ahost req ) = Ax + Ay,i , (5.59) where Ay,i , (5.48), can be expressed as a function of Ax , where tTTC is calculated according to (5.26). Thus, (5.59) is diﬀerentiated with respect to Ax 2 ∂(A2x + A2y (Ax )) ∂(Ahost req ) (Ax ) = , ∂Ax ∂Ax and the min and max points are found by the solution to 2 ∂(Ahost req ) (Ax ) = 0. ∂Ax (5.60) (5.61) The solution yields min and max points for the acceleration vector Ahost req . Note that the solution has to be calculated both for passing to the left and to the right of the object. The special case with only braking (given by (5.36)) should of course also be considered. A collision can be avoided if Ahost req ∈ Ef . (5.62) Finally it should be noted that a solution to (5.61) is easily obtained by any symbolic interpreting equation solver. The expression is however complicated and not easily written down. Figure 5.8 shows the diﬀerence in alarm distance between (5.57) and the optimal solution from (5.61). Section 5.3 81 Collision Avoidance Decision Functions Alarm distance Alarm delay 18 16 0.16 (5.57) (5.61) 0.14 0.12 Alarm delay [s] Alarm distance [m] 14 12 10 8 0.1 0.08 0.06 6 0.04 4 0.02 2 0 50 100 Initial velocity [km/h] 0 0 50 100 Initial velocity [km/h] Figure 5.8: Braking distance according to (5.57) and (5.61). The maximum acceleration is assumed to be 9.82 m/s2 laterally and longitudinally. The right plot shows how much later an alarm is issued according to (5.61) compared to (5.57) if these functions are used for a CA decision. 82 Chapter 5 Deterministic Decision Making Chapter 6 Multiple Obstacles Decision Making How is the threat of a collision in a multiple object scenario evaluated? In this section this question is simpliﬁed. The alternative question is: can a conﬂict be avoided or not? The set of input signals that avoid conﬂicts will be called the set of admissible maneuvers. For the multiple object case the admissible maneuvers are given by (6.1) Ut = Ut1 ∩ Ut2 ∩ · · · ∩ Utn , An for n observed objects. If Ut = ∅, the empty set, a conﬂict is unavoidable. alarm is issued if too much control eﬀort is required (minUt (maxt Ut ) > umax ) or no admissible control sequence is found, Ut = ∅. In this section three diﬀerent approaches to ﬁnd Ut , with increasing computational complexity are proposed. Perfect state knowledge is assumed, i.e., process and measurement noise will be neglected. It is stressed that the main objective in this section is not to actually control the vehicle, but merely to determine if a conﬂict can be avoided or not. 6.1 Constant Control The ﬁrst approach will be called the constant control method. The idea here is to try to ﬁnd a constant control signal uhost = uhost that avoids conﬂicts. Allowing t t0 only constant avoidance maneuvers is of course a severe restriction but it may allow the derivation of an analytical decision function. The decision functions derived in Section 5.3 used an assumption of constant control (constant velocity or constant acceleration). The closest point of approach (5.32), which is common in marine and ATC applications [17, 83], was derived under the assumption of a constant velocity. The longitudinal acceleration required to avoid a collision (5.36), which is often used in automotive applications [2, 20, 24, 119, 130], was derived under the assumption of constant acceleration. 83 84 Chapter 6 Multiple Obstacles Decision Making ay E F2 F F1 Figure 6.1: The host vehicle is approaching two stationary obstacles. Only constant avoidance maneuvers according to (5.48) or (5.36) are considered. Thus, all considered escape paths are parabolic trajectories or possibly straight lines. Each obstacle deﬁnes a set of “forbidden” lateral accelerations. The union of all sets (Fi ) deﬁnes the set F of maneuvers leading to a collision. 6.1.1 Multiple Objects Constant Acceleration In this section the notion of constant longitudinal and lateral acceleration to avoid collision (5.57) to deal with multiple object scenarios. To do this a set of “forbidden” lateral accelerations is deﬁned according to host , (6.2) Fj = ahost , a j,y,left j,y,right for each obstacle j = 1, · · · , n. A new set is then formed F = F1 ∪ · · · ∪ Fn . (6.3) / F, Ut ∈ (6.4) Furthermore denote all lateral accelerations that avoid collision and E = [−aa , aa ], (6.5) denotes the set of all feasible lateral accelerations. If Ut ∩ E = ∅, (6.6) there exists no (constant) lateral acceleration that avoids collision. A multiple object scenario and the formation of F is illustrated in Figure 6.1. The lateral Section 6.2 85 Heuristic Control acceleration to avoid collision in the multiple object case is given by ay,multi = min(|Ut |). (6.7) The required longitudinal acceleration in the multiple object case is given by ax,multi = min(aj,x ), (6.8) where {j : 0 ∈ Fj }. The multiple object acceleration to avoid a collision is thus amulti = min(|ax,multi|, ay,multi ). (6.9) The underlying motion model for this criterion is (4.44). Because of the assumption of constant acceleration (either in the longitudinal or lateral direction) the escape paths are always parabolic trajectories or straight lines. A decision algorithm based on (6.9) will be called a multiple obstacle constant control (MOCC) algorithm in this thesis. The MOCC algorithm is also described in [60]. 6.1.2 Summary of the Constant Control Approach Summarizing the constant control approach, its strength is that a closed form decision function such as (6.9) may be found. The drawback of the approach is that if a more complex maneuver is required to avoid collision (see for instance Figure 6.2) the algorithm will fail to ﬁnd this maneuver. 6.2 Heuristic Control In the second approach, the heuristic control method, a control sequence Ut that avoids collision is searched for by simulating the future movements and using a heuristic control scheme ut = J(Xt ) to control the host vehicle. For each time instance, proximity to obstacles is checked. If there is a conﬂict an alarm is issued. An outline of a general heuristic control algorithm is given in Algorithm 6.1. There are several areas of application where a controller that avoids collision has to be designed. Robot manipulators and autonomous moving robots are applications where much work has been devoted to ﬁnding control strategies that avoid collision. In these applications, reactive control strategies [43, 92, 93, 95, 97, 98, 122] and path planning schemes [74, 84] have been extensively researched. Note that here the controller is only used in a simulation to see if a conﬂict occurs, not to actually control the vehicle. Here an alternative strategy is proposed. The strategy uses the acceleration required to avoid collision according to (5.57) from the previous section. This algorithm will be referred to as the multiple obstacles heuristic control (MOHC) algorithm. The heuristic control approach oﬀers the advantage in that it can ﬁnd complex maneuvers that avoid collision. The disadvantage on the other hand is that it requires more computations than the constant control approach (i.e., simulating the whole event), and there is no guarantee that an existing feasible avoidance maneuver will be found. This means that just as for the constant control approach the algorithm may issue a warning even though a maneuver that avoids conﬂicts exists. 86 Chapter 6 Multiple Obstacles Decision Making In this scenario the relative velocity is too high for braking to avoid collision. Just steering will cause the host vehicle to collide with the vehicles in the adjacent lanes. However, a hard braking maneuver followed by a left steering maneuver will avoid the collision. Thus, the constant control strategy will fail (i.e., give an alarm even though the collision is not unavoidable). The algorithm from Example 6.2, on the other hand, detects that as the oncoming vehicle moves forward, steering will eventually be a possibility to avoid the collision. Figure 6.2: T 1. Calculate the control input ut = J(Xt ); Xt = xhost , xobj1 , · · · , xobjn . 2. Simulate the time updates Xt+T , using ut according to the decision model (5.13). 3. Check proximity between the host and obstacles. If there is a conﬂict discontinue the simulation and issue an alarm. 4. If t = t0 + N T the simulation is discontinued. A conﬂict free path has been found. 5. Go back to 1. Algorithm 6.1: A general heuristic control algorithm. Section 6.3 Exhaustive Search 87 The basic idea is to choose an obvious escape path (constant acceleration) if it exists. Otherwise, full braking is applied until an obvious escape path appears or the vehicle collides. Here is an outline of the algorithm. 1. Calculate the acceleration required to avoid collision according to (5.57). If it is below the threshold for a collision being unavoidable an escape path exists and the simulation can be aborted. = −aa . 2. Calculate the time updates Xt+T , using uhost x 3. Check the proximity to obstacles, dispatch an alarm if a conﬂict is detected. 4. If t = t0 + N T the simulation is discontinued without an alarm. 5. Otherwise go back to 1. Algorithm 6.2: Multiple Obstacles Heuristic Control 6.3 Exhaustive Search It is desirable to have a method that guarantees ﬁnding any existing avoidance maneuver. This is not achieved by the previous two approaches. However, ﬁnding a conﬂict-free escape path is clearly not a convex problem. Thus, gradient search methods may not be eﬃcient. As a third alternative an exhaustive search over all possible maneuvers is therefore proposed. This will theoretically guarantee ﬁnding a solution, if it exists. In practice though, both time and the control space have to be discretized. This means that ut is approximated with a ﬁnite number of control sequences ūt ∈ J ∗ = {(ut0 , · · · , utN )|uti ∈ {u(1) , · · · , u(M) }} ⊂ J, where u(i) are the possible values of the discretized control signal. Thus, only a ﬁnite set of M possible control inputs is used at each time instant, and the time interval (from t = t0 to t = tN ), is divided into N segments of length T . This means that the exhaustive search may also fail to ﬁnd an existing solution for speciﬁc scenarios if the discretization is made too sparse. How should N and M be chosen? To answer this we need to know how well an arbitrary trajectory generated with ut ∈ (k) [umin , umax ] can be described, by the best approximating control sequence Ut , n where k = 1 · · · M . Below, results for this are given for a constant acceleration model according to (4.44) and where uti ∈ {−aa , 0, aa }. The tree of possible paths to approximate the true vehicle trajectory with this model is shown in Figure 6.3. The error at time tN = N T is given by pN (u) − pN (ū) = pN − p̄N = ∆pN where 88 Chapter 6 Multiple Obstacles Decision Making Tree of possible paths 0.8 Leaf 0.6 Node Position [m] 0.4 0.2 Branch umax u0 0 u min −0.2 −0.4 −0.6 −0.8 0 0.05 0.1 0.15 0.2 0.25 Time [s] 0.3 0.35 0.4 Figure 6.3: Tree of possible vehicle path approximations (1-dimensional motion). At each time instant three possible maneuvers are allowed ut ∈ {umin , 0, umax } . NT p̄N = p̄0 + v̄0 + 0 τ ā(s) ds dτ 0 = /p̄0 = 0, v̄0 = 0/ N T τ ā(s) ds dτ = 0 0 = /a(s) piecewise constant/ τ N −1 (i+1)T ā(s) ds dτ = v̄i + i=0 = N −1 i=0 = N −1 i=0 = iT (i+1)T iT iT i āj T + j=0 2 a ¯ T i āj T 2 + 2 j=0 (N − 1 − i)āi T 2 + i=0 = N −1 i=0 (2N − 2i − 1) i=0 ā(s) ds dτ iT i N −1 N −1 τ a¯i T 2 2 2 āi T . 2 (6.10) Section 6.3 89 Exhaustive Search Assume a constant acceleration model according to (4.44), where motion is independent for each direction. The acceleration is determined by the control input u(t) ∈ [−aa , aa ], where aa is the maximum available acceleration. An arbitrary trajectory generated by u(t) can then be approximated with a piecewise constant input ū(t) = {ū0 , · · · , ūN −1 }, ui ∈ {−aa , 0, aa } for time t = t0 to tN , ti+1 − ti = T . ūi , i = 0, · · · , N − 1 can be chosen such that the error at time tN Theorem 6.1 tN 0 t u(s) dsdt − ∆pN = 0 0 tN t ū(s) dsdt ≤ 0 aa T 2 . 4 (6.11) Proof of Theorem 6.1 Follows directly from (6.10), which gives all possible positions of the nodes in the tree. The nodes are equidistant with a separation of aa T 2 2 , as shown in Figure 6.3. Assume a constant acceleration model according to (4.44), where motion is independent for each direction. The acceleration is determined by the control input u(t) ∈ [−aa , aa ], where aa is the maximum available acceleration. An arbitrary trajectory generated by u(t) can then be approximated with a piecewise constant input ū(t) = {ū0 , · · · , ūN −1 }, ui ∈ {−aa , 0, aa } for time t = t0 to tN , ti+1 − ti = T . ūi , i = 0, · · · , N − 1 can be chosen such that the maximum position error for all i = 0 · · · N is Theorem 6.2 aa (tN − t0 )2 ≤ . ( max (∆p )) min i ūi ∈{−aa ,0,aa } 1≤i≤N 2N (6.12) Proof of Theorem 6.2 It is always possible to choose a strategy ui so that t |∆vi | < aa T /2, ∀i, where ∆vi = vi − v̄i and v̄i = t0i (ū(t)). This is shown by induction 1. ∆v0 = 0 2. Suppose 0 ≤ |∆vn | ≤ aa T /2. 3. Then it is true that ūn can be chosen so that 0 ≤ |∆vn+1 | ≤ aa T /2. ∆vn+1 = ∆vn + ∆un T = ∆vn + (un − ūn )T . We have |∆vn + un T | ≤ |aa T /2| + |aaT | = 3aa T /2 thus, there are three cases: (a) aa T /2 ≤ ∆vn + un T ≤ 3aa T /2. In this case choose ūn = −aa ⇒ |∆vn+1 | ≤ aa T /2. (b) −aa T /2 < ∆vn +un T < aa T /2. In this case choose ūn = 0 ⇒ |∆vn+1 | < aa T /2. (c) −3aa T /2 ≤ ∆vn + un T ≤ −aa T /2. In this case choose ūn = aa ⇒ |∆vn+1 | ≤ aa T /2. 90 Chapter 6 Multiple Obstacles Decision Making Thus, with this choice of ū, one has ti ∆v(t)dt| ≤ |∆pi | = | ti |∆v(t)|dt (6.13) aa T /2dt (6.14) = N aa T 2 /2 = aa (tN − t0 )2 /2N. (6.15) ≤ t0 tN t0 |∆v(t)|dt ≤ t0 tN t0 Proposition The bound given in Theorem 6.2 may actually be reduced a factor 2 by choosing a smarter strategy that not only tries to limit the absolute value of ∆v but also makes sure that ∆v changes sign (if necessary) so that |∆p| ﬁrst increases and then decreases. An exhaustive search quickly becomes computationally expensive as the discretization is made ﬁner. Thus, this method may not be useful for on-line applications. However, it may be useful for oﬀ-line applications as a reference for automatic testing of other decision making algorithms. In particular for some applications (such as CMbB) it is suﬃcient to ﬁnd one escape path, i.e., the entire tree of possible manoeuvres does not need to be searched. When a escape path has been found the search can be discontinued. In this case an eﬃcient strategy for searching the tree of possible manoeuvres may provide signiﬁcant reduction to the average time of traversing the tree. In Section 9.1 a comparison between the three approaches, to ﬁnding a manoeuvre that avoids collision, is presented. A general discussion on the computational complexity of ﬁnding collision free paths can be found in [111]. An alternative approach to multiple obstacle collision avoidance decision making is to view the problem as a path planning problem [74, 84, 95, 122]. A diﬀerence between a typical path planning problem and the CA decision problem (as posed in this thesis) is that for the path planning problem there is a well deﬁned goal state while for the CA problem the goal state is not of importance. Chapter 7 Statistical Decision Making The decision making algorithm of a CA system bases its decision on the output from the tracking algorithm. This output is associated with some uncertainty. Thus, the CA decision is based on uncertain parameters. Furthermore, in general several actions a could be considered. The CA strategy can be described by a rule data-base (DB), that for each possible state determines what action to take. Here deterministic decision making functions as described in Section 5.3 are often used. Let the rule database be denoted by the nonlinear function (7.1) r(Xt ) = a, T obj1 obj2 objn where Xt = xhost . Each state is nonlinearly mapped x x · · · x t t t t to one action a. The action a can be continuous or discrete. In the sequel it is assumed to be discrete and take on discrete values ak . The nonlinear function r(Xt ) will be referred to as the CA strategy. Typically, the CA strategy is based on thresholding one or more decision function g(Xt ) to determine on an action, e.g. g(Xt ) < g1 ⇒ a1 , g ≤ g(X ) < g ⇒ a2 , 1 t 2 r(Xt ) = (7.2) 3 ≤ g(X ) < g ⇒ a , g 2 t 3 g ≤ g(X ) ⇒ a4 , t 3 Where gi denotes threshold values chosen to get the appropriate CA action. The problem is that the state estimates are uncertain causing the decision to be uncertain. An overview of the decision problem is given in Figure 7.1. In Example 7.1 a decision making problem based on uncertain state estimates is described. Example 7.1 In this example a simple one-dimensional collision avoidance problem is considered. The distance required to avoid a collision with a stationary object, using a deceleration of 1 g ≈ 9.82 m/s2 , for diﬀerent initial velocities is shown in Figure 7.2. 91 92 Chapter 7 Sensors yt ✲ Filter ) p(xt |Yt✲ ? Statistical Decision Making x✲ Action✲akt t CA decision The problem: how to interface uncertain state estimates with deterministic decision making? Figure 7.1: Braking distance versus relative speed. The elliptical region represents the uncertain state estimate and the shaded region deﬁnes the collision unavoidable state, which should lead to an automatic braking action. Figure 7.2: The CA system is supposed to perform an autonomous action when a deceleration (according to (5.36) larger than 9.82 m/s2 is required to avoid the collision. Hence, the two CA actions, a1 and a2 , are braking and no braking. Suppose an object has been detected close to the decision region. An uncertainty region for that object is calculated as indicated in Figure 7.2. Should the system perform an autonomous action in this case? This is a strategic decision for the CA system designer. If the tolerance for false alarms is low, one should require a high conﬁdence of the braking action to be correct. Thus, in this case one should not take any action. Conversely, if one wants to maximize safety one should take action even when there is only a slight possibility that the collision is unavoidable. If the uncertainty region is time varying the probability of making the correct decision will vary. In this case decision strategy r(Xt ) needs to be augmented to deal with the uncertainty associated with the decision. To calculate the probability for each action Prob(ak |Yt ), given the measurements Section 7.1 93 Numerical Approximation Yt , one needs to evaluate Prob(ak |Yt ) = Ir(Xt )=ak p(Xt |Yt ) dx., where Ir(Xt )=ak is an indicator function according to 1 if r(X ) = ak , t Ir(Xt )=ak = 0 otherwise. (7.3) (7.4) Probabilistic decision making for automotive CA systems is discussed [52, 53, 61, 62, 65, 73], for airspace applications it is discussed in [108, 109] and in [81] which give a excellent overview of methods used airspace applications, in [97] a probabilistic potential ﬁeld algorithm is discussed, and in [96] path planning with a probabilistic description of future positions is presented. 7.1 Numerical Approximation In most cases an analytical solution for (7.3) cannot be found. Using stochastic integration the integral (7.3) can be approximated by k Prob(a |Yt ) = Ir(Xt )=ak p(Xt |Yt ) dx ≈ N 1 I k, N i=1 r(Xt )=a xi ∈ p(Xt |Yt ), (7.5) where the xi ’s are samples of the posterior distribution. In Example 7.2 an approximation of the PDF for the decision function time to collision (5.24) is generated by drawing a large number of samples from the state vector distribution. By evaluating (7.5) a speciﬁc action can be performed with a predeﬁned conﬁdence according to Prob(ak ) > 1 − α, (7.6) where α determines the conﬁdence level. Thus, one may now decide to take each action with a certain conﬁdence, as shown in Figure 7.3. An example where stochastic numerical integration is used to calculate the probability of a collision avoidance action is given in Section 9.2 and is also described in [66]. Example 7.2 In many cases it is impossible to calculate the PDF of the decision function analytically. In this example the time to collision tTTC according to (5.26) will be studied. Assume that the states are independent and p̃x N (10, 0.52 ) x̃ = ṽx ∈ N (−3, 0.52) . ãx N (−4, 22 ) 94 Chapter 7 Statistical Decision Making p(xt |Yt✲ Action ai with conﬁdence✲ ) pi ✲Supervisor yt Sensors Figure 7.3: ✲ Filter ✲ Rule {xkt }N k=1 DB {akt }N k=1 The solution: a supervisory algorithm that assesses conﬁdence in action decisions. The probability density for the tTTC can be approximated by drawing a large number of samples from the PDF of x̃ and for each sample calculate the tTTC . This is shown in Figure 7.4. In this case, the resulting PDF is not Gaussian, and it is also skewed so that the mean value is tTTC = 1.70 s while the nominal tTTC = 1.61 s. PDF for tTTC 1.6 1.4 Probability density 1.2 1 0.8 0.6 0.4 0.2 0 0 0.5 1 1.5 2 2.5 Time−to−collision [s] 3 3.5 4 The PDF for the tTTC (5.26). p̃x ∈ N (10, 0.52 ), ṽx ∈ N (−3, 0.52 ), ãx ∈ N (−4, 2 ). The resulting PDF is not Gaussian and it is skewed toward higher tTTC ’s. The mean value is tTTC = 1.70 s while the true tTTC = 1.61 s. Figure 7.4: 2 Section 7.2 7.2 95 Analytical Approximation Analytical Approximation In the previous section statistical decision making based on stochastic numerical integration was discussed. The reason for the numerical approach is that an analytical solution for (7.6) is often impossible to ﬁnd. The disadvantage of the general solution, provided by the numerical approach (7.5), is that the resulting algorithm is computationally expensive. In this section an alternative decision rule is proposed. A simple CA strategy with only one CA action according to (g(X̂t ) > gth ) ⇒ a, (7.7) is considered. Accounting for the uncertainty leads to the following CA strategy Prob(g(X (7.8) t ) > gth ) > 1 − α ⇒ a, where α determines the conﬁdence. A CA strategy according to (7.7) is computationally cheap, but if p(Xt ) varies a lot the probability of making the right decision might also vary signiﬁcantly. A CA strategy according to (7.8) is, on the other hand, computationally expensive. An alternative CA strategy is given by g(X̂t ) − C1 B̂ (g(Xt )) > gth + C2 D̂ (g(Xt )) ⇒ a. (7.9) Here B(·) and D(·) denote bias and standard deviation respectively, and Ci are design parameters. A CA strategy according to (7.9) relies on an assumption that the distribution p(g(x̂t )) is well approximated by a Gaussian distribution. If g(x̂t ) truly is Gaussian, (7.9) corresponds to evaluating (7.8), and the decision will be made with a certain conﬁdence, which is determined by C1 and C2 . 7.2.1 Estimating Statistical Properties of the Decision Function How are the bias B (g(Xt )) and the standard deviation D (g(Xt )) calculated? The bias and standard deviation of (7.9) is deﬁned in the following way B (g(X)) = E (g(X)) − g(X), " 2 D(g(Xt )) = E (g 2 (Xt )) − E (g(Xt )) , (7.10) (7.11) ' ( where E(·) denotes the expectation value. E g j (Xt ) is known as the j:th order zero point moment of g(Xt ), which is deﬁned by j E(g (Xt )) = g j (Xt )p(g(Xt )) dXt , (7.12) Rn where p(g(Xt )) is the PDF of g(Xt ). Considering that g(·) can be a quite complicated function, the integral (7.12) or even the PDF p (g(Xt )) might be hard to compute analytically and computationally costly to approximate by numerical integration. Hence, an approximation of the expected value in (7.12) will be used, 96 Chapter 7 Statistical Decision Making where an assumption of a Gaussian distributed Xt is used. The approximation is obtained from the expectation value of a Taylor expansion of g j (Xt ) given by g j (Xt ) = g j (X̂t ) + n n n ∂g j ∂ 2gj X̄i + X̄i X̄j + · · · , ∂Xi ∂Xi ∂Xj i=1 i=1 j=1 (7.13) where X̄i = Xi − X̂i and Xi denotes the i:th element of the state vector Xt . An approximation of the expectation E(g j (Xt )) can thus be found from the expression E(g j (X)) = g j (X̂) + n n n ∂g j ∂ 2gj E(X̄i ) + E(X̄i X̄k ) + · · · . ∂Xi ∂Xi ∂Xk i=1 i=1 (7.14) k=1 By choosing how many terms of the series expansion to consider the accuracy and computational burden can be traded oﬀ. If terms of the second order and higher is neglected, this results in the well known Gauss approximation formula. To evaluate (7.14) it is clear that higher order moments of Xi need to be known. In the case Xt ∈ N (X̂t , Pt ) these can be calculated from the characteristic function T Ψ(t) = eb X̂+ X T PX 2 . (7.15) T and P is the covariance matrix of X. The j:th order Here b = b1 b2 · · · bn moments of Xi are given by E(Xij ) = ∂ j Ψ(t) ∂bji b=0 . (7.16) Thus, inserting the approximation from (7.14) into (7.10) and (7.11) an approximation of B (g(Xt )) and D (g(Xt )) can be calculated. The same reasoning can of course also be used to estimate other interesting statistical properties such as skewness and kurtosis. In Section 9.3 the application of (7.7) and (7.9) to the longitudinal acceleration to avoid a collision, (5.36), is compared. 7.3 Probability of Collision In the previous section a method for calculating the probability for each possible CA decision was proposed. The decision is based on a decision function where each action can be made with a predeﬁned conﬁdence. Sometimes an appropriate decision function cannot be described by an analytic expression. This may for instance be the case if one is interested in using a complex dynamic model for the vehicles involved. In this case an alternative is to simulate the system for future time instants and calculate the probability for a collision in each time instant. Thus, p(xt+T |Yt ), · · · ,p(xt+T N |Yt ) must be calculated for each vehicle. The number of predictions N is determined by how ﬁne time is discretized and how far in the Section 7.3 97 Probability of Collision H p̃ = p̃x T p̃y D The set D corresponds to all possible conﬁgurations between the two vehicles so that their bodies intersect, i.e., they collide. The area indicated in the ﬁgure is not actually an area since it not only depends on the relative position between the two vehicles, but also on their heading angles. So the indicated area is actually a 3-dimensional volume of the vehicle’s relative position and orientation. It is over this volume the PDF should be integrated to obtain the collision probability. Figure 7.5: future one wish to check for collisions. The probability of a collision can then be calculated as Prob(collision) = max (Prob(xt+iT ∈ D)) , i i = 1, · · · , N, (7.17) where D denote the state space region corresponding to a collision. Below an example is presented for calculating the probability of collision in a two dimensional case. The probability of collision for each time instant can be calculated according to Prob(collision at time t + T ) = Prob(p̃x,t+T , p̃y,t+T , ψ̃t+T ∈ D) (7.18) = pt+T (p̃x , p̃y , ψ̃|Yt ) dxdydψ, p̃x ,p̃y ,ψ̃∈D where D (Figure 7.5) is the set of all possible vehicle conﬁgurations corresponding to a collision, p̃x is the relative longitudinal position, p̃y is the relative lateral position, and ψ̃ is the relative heading angle. The probability density pt+T (p̃x , p̃y , ψ̃|Yt ) of the vehicles’ relative position is obtained directly from the Bayesian solution to the tracking problem, if relative coordinates are chosen. If ground ﬁxed coordinates are used, the density of the relative position has to be calculated before the collision probability can be calculated. Assuming xhost and xobj are independent, the density of x̃ = xhost − xobj is given by the convolution (7.19) ∞ pxobj (x)px̃host (x̃ − x) dx. (7.19) pX̃ (x̃) = −∞ 98 Chapter 7 Statistical Decision Making PDF for a vehicle traveling at 60 km/h 6 5 4 3 2 1 0 4 2 0 −2 −4 global y coordinate [m] 90 100 98 96 94 92 102 104 global x coordinate [m] Figure 7.6: Probability density of a car traveling at 60 km/h, where Qt has been chosen to correspond to typical handling properties of the car. The plot shows the PDF at time t+0.7, t+0.8, t+0.9 and t+1.0 seconds respectively. In general, calculating (7.19) and (7.18) may be computationally demanding. For the special but common case where xhost and xobj are Gaussian and independent the relative states are also Gaussian, x̂host − x̂obj ∈ N (xhost − xobj , P host + P obj ) = N (x̃, P̃ ). An example of how the probability density can evolve over time for a Kalman ﬁlter estimate is shown in Figure 7.6. If the relative heading angle is disregarded, D can be approximated with a rectangle, the width of which is whost + ŵobj and length is lhost . Here w denotes the width of the object and lhost denotes the host vehicle’s length. The length of the obstacle is neglected since it may not be known. With the assumption that movement in the x and y direction is independent the probability of a collision can be calculated according to pt+iT (p̃x , p̃y |Yt ) dxdy Prob(collision at time t + i) = p̃∈D = whost +ŵobj 2 −(whost +ŵobj ) 2 w = (Φ( where − host +ŵ 2 obj σy (y−p̃y )2 e 2Π̃yy " dy 2π Π̃yy − py 2 0 −lhost ) − Φ(− − (x−p̃x ) e 2Π̃xx dx 2π Π̃xx w host +ŵ obj 2 σy Π̃xx Π̃ = 0 − py ))(Φ( 0 − px lhost − px ) − Φ(− )), σx σx (7.20) 0 Π̃yy (7.21) Section 7.3 99 Probability of Collision denotes the state error covariance matrix for the relative position. Algorithm 7.1 summarizes decision making using probability of collision to determine if the system should be activated. In the algorithm an assumption that movement in the x and y direction is independent is made. The area D is approximated with a rectangular area. This algorithm will be used in the ﬁrst demonstrator vehicle which (j) (j) 1. x̂host x̂t P̂thost and P̂t where there are j = 1, · · · , n objects and t = t T, · · · , N T , is calculated using Algorithm 4.2. 2. Transform the position and its covariance matrix to a host vehicle ﬁxed coordinate system (if relative coordinates are used this step is not needed). sin(ψ ) − cos(ψ ) t t host ); Γt = p̃ = Γt (p̂obj t − p̂t cos(ψt ) sin(ψt ) Π̃t = Γt (Πobj + Πhost )ΓTt t t 3. Calculate the collision probability for each time, t, and each obstacle, j. Prob(collision at time t + iT ) = whost +ŵobj whost +ŵobj −py,t −py,t 0−px,t lhost −px,t 2 2 Φ( = Φ( ) − Φ(− ) ) − Φ(− ) , σy,t σy,t σx,t σx,t where Φ is the cumulative function of the Gaussian distribution. 4. Calculate maximum probability for all objects and prediction times. Prob(collision) = maxi,j (Probt+i,obstaclej (collision)) 5. Threshold the probability to determine if the system should activate. If(Prob(collision) > Threshold) Activate system Algorithm 7.1: Probability of Collision is presented in Section 8.3. 100 Chapter 7 Statistical Decision Making Part IV Evaluation 101 Chapter 8 Collision Mitigation by Braking System Testing In this chapter two collision mitigation by braking systems will be described. One system uses the probability of collision, according to Algorithm 7.1, to decide when to perform braking interventions. It only considers each detected object individually and not in relation to other objects. The other system has a multiple obstacle decision algorithm according to (6.9). The strategy for both systems is to reduce collision speed as much as possible under the constraint that no faulty interventions are allowed. This means that the system should engage the brakes as soon as the collision unavoidable driving state is entered. Performance of the systems has been evaluated both by simulation and by ﬁeld tests with demonstrator vehicles. A schematic overview of the simulation environment is presented in Figure 8.1. The simulation environment has been designed to [Obstacles] Environment Driver_Command Driver_in Obstacles Traking Sensor Measurements Obstacles Predictions Driver Inputs Collision Requested Brake Vehicle state Virtual Driver/Chassi control Vehicle Kinematic States Virtual_in Host Vehicle Navigation Sensor Measurements Sensor Model In Collision Probability Host Vehicle Tracking System Decision Making Host Vehicle Model Simulation model for a CMbB system, real time code is directly generated from the same code used in the tracking and decision block of the model. Figure 8.1: bear as much resemblance as possible to the demonstrator vehicle and its sensors. 103 104 Chapter 8 Collision Mitigation by Braking System Testing This enables the use of the same code (for the tracking system and decision making system) in both the simulation environment and in the demonstrator vehicle. 8.1 Simulation Environment The simulation environment in Figure 8.1 has been implemented using Matlab and Simulink. From the Simulink code it is possible to auto-generate real time code. Thus the tracking system and decision making blocks can be directly implemented in the demonstrator vehicle, using Real-time Workshop to produce the real time code. The main purpose of a simulation environment is to test performance of the algorithm in diﬀerent scenarios and to see whether or not it is possible to provoke a faulty intervention, especially for high speed collisions, where physical testing with a demonstrator vehicle might be too dangerous or too costly. Here is a short description of the blocks that the model consists of (corresponding to the blocks of Figure 8.1): Driver Inputs: The driver inputs are described by variables that specify the driver’s steering wheel, brake, and accelerator commands. These are all speciﬁed before the simulation starts, and are not aﬀected by the CMbB system’s brake intervention. The driver will thus continue the same maneuver as he was attempting before the system was activated. The CMbB system will of course (when activated) override the drivers accelerator and braking commands. Environment: The environment describes the traﬃc environment of the host vehicle. It is speciﬁed by variables describing the objects surrounding the host vehicle. These are also speciﬁed before the simulation starts and are not aﬀected by each other or by the host vehicle movement. Together with the driver inputs, the environment variables deﬁne a speciﬁc scenario. Typical objects are other vehicles and stationary objects. There is currently no information about lane geometries, changes in road conditions or infrastructure. Host Vehicle Model: The host vehicle model is a dynamic handling model that describes the physical limitations of the handling performance of the host vehicle. It takes driver commands such as steering wheel angle, brake and accelerator commands as input. The outputs are position, velocity, acceleration and turn rate (yaw rate) of the host vehicle. For most simulations, a bicycle model (as described in Section 4.8.1) was used. This model has no actuator dynamics except for the pressure build-up in the braking system. The pressure build-up is modeled as a ramp which has a rise time of 300 ms to achieve maximum brake force. To model tire to road friction, Pacejkas magic formula, [6], was used. For the simulations in Section 2.2.4, a more detailed model was used. This model has 15 degrees of freedom and has been veriﬁed against a Volvo S80. Section 8.2 105 The Demonstrator Vehicles Sensor model: The sensor model simulates the host vehicle’s tracking and navigation sensors. It takes inputs from the environment block and from the host vehicle. It calculates range, range rate, and azimuth angle to the surrounding objects. To these “ideal” measurements, noise is added according to r (8.1) y = ṙ + et . ϕ The additive sensor noise et is assumed to be white Gaussian and independent. The form of the sensor output is the same in that from the sensors in the demonstrator vehicles. This block also adds noise to the navigation sensor measurement (yaw rate and speed of the host vehicle) Tracking system: The tracking systems are quite diﬀerent for each of the two vehicles. In the ﬁrst vehicle the tracking and fusion algorithms were developed within this research project. The tracking system uses nearest-neighbor measurement association, an EKF tracking ﬁlter and a nearly coordinated turn model for the vehicles dynamics. In the second vehicle an existing system partially developed by a sensor supplier was used. Thus, the exact design of the tracking ﬁlters is unknown. Decision Making: The decision making algorithm takes, as input, data from the tracking system and outputs a binary value (true or false) to indicate whether or not the state for autonomous braking has been reached. The ﬁrst system uses a probabilistic decision criteria. The second system has a multiple object decision making algorithm. Virtual Driver/Chassis Control: This block performs braking when the decision making system determines that the collision risk is too high. In the prototype systems discussed in this chapter, maximum braking is always performed when the intervention threshold is reached. The rise time of the brake system is 300 ms, and the maximum deceleration is 8 m/s2 . There is also a system delay of 100 ms from the time that the threshold value is exceeded and brake actuation is performed. This performance is similar to the performance of the brake system of demonstrator vehicle 1. 8.2 The Demonstrator Vehicles The demonstrator vehicles in both cases are standard Volvo V70s. They have been equipped with two tracking sensors and one dSPACE Autobox. The Autobox (Figure 8.2) is used to perform necessary computations for the tracking and decision making system and is also used to actuate the brakes. The processor of the Autobox is a 400 MHz Power PC. To evaluate the system’s performance in collisions, an inﬂatable car (Figure 8.3) is used. For safety and cost reasons, collision tests have been made at speeds up to 70 km/h only. 106 Figure 8.2: Chapter 8 Collision Mitigation by Braking System Testing The dSPACE unit is installed under the ﬂoor of the luggage compartment Figure 8.3: Inﬂatable car used for collision test, maximum collision speeds 70 km/h. Section 8.2 107 The Demonstrator Vehicles 8.2.1 Demonstrator Vehicle Tracking Sensors The tracking sensors in the demonstrators are a mm-radar installed in the front bumper (see Figure 8.4) in combination with, a lidar for the ﬁrst demonstrator and Figure 8.4: The mm-radar installed in the front bumper of the vehicle. a vision sensor for the second demonstrator, installed behind the windshield close to the rear view mirror (Figure 8.5). The radar and lidar sensors are production-like units that are used for ACC systems. The measurement vector (for one obstacle) that is used in the tracking algorithm from each sensors is given by yradar r r ṙ = ϕleft ϕcenter ṙ r , ylidar = , yfused = ϕleft ṙ ϕcenter ϕcenter ϕright ϕright , (8.2) where ϕleft is the azimuth angle to the left edge of the detected object, ϕcenter is the azimuth angle to the center of the detected object, ϕright is the azimuth angle to the right edge of the detected object. For the second demonstrator only a fused observation of the radar and vision sensor was used, this is indicated by the measurement vector yfused. The lidar and radar sensors also provide additional measurement on reﬂected energy etc. This information is, however, not used. Both the radar and lidar can detect obstacles at ranges beyond 150 m and have an update frequency of 10 Hz. The ﬁeld of view (FOV) is 13.6◦ for the mm-radar, 14◦ for the laser radar and 48◦ for the vision sensor. The narrow FOV provided by radar and lidar is often suﬃcient for ACC systems. For the ﬁrst demonstrator the performance of 108 Figure 8.5: Chapter 8 Collision Mitigation by Braking System Testing The laser radar installed behind the windshield of the demonstrator vehicle. the CMbB system will degrade because of the narrow FOV. To check the range accuracy of the radar and lidar sensors in the ﬁrst demonstrator vehicle, stationary measurements were performed. In these tests, the distance to a parked vehicle was measured when the host vehicle was standing still. Both the laser and the radar measured the true distance to an accuracy within 0.1 m. However this is not the case when the host vehicle is moving. Figure 8.6 shows the range measurements from the laser and the mm-radar as a stationary target is approached head-on at 40 km/h. There is a clear discrepancy between the range measurements from the laser and the mm-radar (the diﬀerence is approximately two meters). In dynamic scenarios, the measurements for each sensor is consistent but the discrepancy between the sensors will cause problems when fusing the sensor data (in the tracking algorithm this is solved by using large gates in the measurement association step). This problem is believed to be caused by an unknown and varying delay in the sensors. There was no time stamp of a measurement or any way to synchronize the sensors. Another problem with the lidar and radar used here is that none of them provides accurate measurements over short distances. In most cases the sensor “loses” sight of objects when the range is shorter than 10 meters (see e.g. Figure 8.6 at time 28). Furthermore, as the measured object moves out of the FOV of the mm-radar the azimuth angle measurement deteriorates. This means that in many cases the azimuth angle measurement is erroneous when the entire object is not in the FOV of the sensor. Figure 8.7 shows typical performance for the sensor in a head-on scenario. At time 3 the range is approximately 10 m and the entire object is not within the sensors FOV; the azimuth angle grows larger until the time when the object is lost by the sensor (at time 3.8). In this scenario it looks as if the measured object moves out of the path of the host vehicle, even though it really Section 8.2 109 The Demonstrator Vehicles Range measurements 45 Lidar mm−radar 40 35 Range [km/h] 30 25 20 15 10 5 0 −5 0 5 10 15 20 Time [s] 25 30 35 40 Figure 8.6: In dynamic scenarios there is often a discrepancy between range measurements from the lidar and the mm-radar. In this scenario an inﬂatable car is approached head-on at 40 km/h. The collision occurs at time 33 s. At time 28 s the laser radar loses track of the obstacle. The diﬀerence in the range measurements is approximately two meters. For other test runs under the same conditions the diﬀerence is diﬀerent. was stationary and was hit head-on by the host vehicle. The behavior illustrated in Figure 8.7 causes problems in head-on collisions at low speed (less than 30 km/h) where the distance for intervention is much shorter than 10 m. Clearly there is a lower limit at which collision tests can be performed with reliable results. This limit lies around 30 km/h. In many of the tests with the demonstrator vehicles, an inﬂatable car was used as an obstacle. It is therefore interesting to examine if its radar cross section diﬀers from that of ordinary vehicles. To investigate this, stationary tests were performed. The measurements were performed as illustrated in Figure 8.8 (picture taken from the viewpoint of the host vehicle). Here both the host vehicle and the measured objects are stationary. From these measurements it was concluded that both obstacles were detected with the same accuracy and that the reﬂected signal strength from the rear end of a Volvo V70 and from the rear end of the inﬂatable car were very similar. 8.2.2 The Braking System of the First Demonstrator Vehicle The braking system is of great importance for the kind of CMbB system that is considered here (intervention only when the collision unavoidable state has been entered). This is due to the fact that an intervention is issued very late. In many cases less than 1 second before collision. Because of this short time, it is crucial that 110 Chapter 8 Collision Mitigation by Braking System Testing Degeneration of Azimuth Angle Measurement 50 Range [m] 40 30 20 10 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 2.5 3 3.5 4 4.5 Time [s] Azimuth Angle [Degrees] 0 −2 −4 −6 −8 −10 −12 −14 0 0.5 1 1.5 2 Time [s] Figure 8.7: Range and azimuth angle measurements provided by the mm-radar, from a stationary obstacle straight ahead. The obstacle is approached head-on without any steering maneuvers before the collision occurs, at time 3.9 s. At time 3 the range to the obstacle is approximately 10 m. After this point the azimuth measurement clearly deteriorates. Figure 8.8: In measuring the radar response from the back of a Volvo V70 and from the inﬂatable car, it was found that the radar echo was as strong from the inﬂatable car as from the Volvo in this particular set up. Section 8.2 111 The Demonstrator Vehicles the brake system quickly reaches maximum deceleration. The ﬁrst demonstrator vehicle is equipped with a production ABS system. Brake actuation (when CMbB system initiated) is achieved by opening a solenoid valve in the brake booster. There is also a vacuum pump that can be manually switched on and oﬀ to ensure vacuum in the brake booster at intervention. Performance of the brake system is plotted in Figure 8.9. The plot shows a CMbB system induced brake maneuver, i.e., the driver does not touch the pedal. The notch in the acceleration curve between time 15 s and time 16 s comes from activation of the ABS system. Both the speed and acceleration are derived from the wheel speed sensor. The rise time for the CMbB system actuated braking maneuver is typically more than 500 ms. The maximum deceleration achieved in the test, illustrated in Figure 8.9, is approximately 6.3 m/s2 . The condition when this test was performed was wet asphalt, the Velocity [km/h] Velocity and acceleration under a CMBB system induced brake manouver 60 40 20 0 13 14 15 16 17 Time [s] 18 19 20 14 15 16 17 Time [s] 18 19 20 2 Acceleration [m/s ] 5 0 −5 −10 13 The speed and acceleration of the host vehicle when the system intervenes (initial speed 50 km/h). The brakes are applied by opening a solenoid valve in the brake booster, maximum deceleration achieved is 6.3 m/s2 . Figure 8.9: temperature was between 0◦ and 5◦ and the vehicle was equipped with non-studded winter tires. This explains the low maximum deceleration reached. It should be stressed that brake actuation through the opening of a solenoid valve in the brake booster is speciﬁc to this demonstrator vehicle, and that better alternatives exist. For the second demonstrator the brakes are not applied at an alarm. The reason for this is to be able to compare performance with a single object algorithm which typically alarms later than the multiple object algorithm. 112 Chapter 8 Collision Mitigation by Braking System Testing Overview of scenarios that are evaluated. Test results refer to test performed with the demonstrator vehicle and an inﬂatable dummy obstacle. In the simulations the same tracking and decision algorithm is used. Table 8.1: Scenario Test results Simulation results Figure 8.11 Figures 8.10, 8.12, 8.13 Lead Vehicle Brakes Hard - Figure 8.15 Host vehicle cut-in - Figure 8.17 Obstacle cut-in - Figures 8.19, 8.20 Drive-by Figure 8.23 Figure 8.22 Late avoidance Figure 8.26 Figure 8.25 Head-on to Stationary Obstacle 8.3 Demonstrator 1 | Probabilistic Decision Making In this section the performance of a CMbB system with a probabilistic decision making algorithm in diﬀerent traﬃc scenarios is studied. The evaluation of diﬀerent scenarios have been performed both with the demonstrator vehicle and inﬂatable dummy car and also in simulations. The test scenarios can be divided into two groups. • The ﬁrst group consists of scenarios where there is a collision. For these scenarios the mitigation performance of the system is of interest, i.e., how much the speed of the host vehicle was reduced at collision, distance between colliding objects when the system activates, etc. • The second group consists of scenarios where there is no collision. These scenarios are used for designing and verifying the CMbB system, i.e., to check if it is possible to provoke a faulty intervention. Table 8.1 gives an overview of all the scenarios that have been evaluated The system should only react when a collision is unavoidable, so any intervention occurring without being followed by a collision will by deﬁnition be a faulty intervention. The important characteristics of the tracking and decision making system for the ﬁrst demonstrator vehicle is summarized in Table 8.2. 8.3.1 Collision Scenarios In all the simulated scenarios, the same settings for the sensors is used. It should be noted however that the FOV of the sensors in the simulated scenarios is larger than the FOV of the sensors in the demonstrator vehicle. The measurement noise in the simulation environment is assumed to be white, Gaussian, and independent. The standard deviation of the range measurement is 0.5 m, the standard deviation of range rate is 0.5 m/s2 and the standard deviation of azimuth angle is 1◦ . These settings are used both for the mm-radar and the lidar. The scenarios that are Section 8.3 Table 8.2: 113 Demonstrator 1 | Probabilistic Decision Making Summary of important characteristics of the ﬁrst Demonstrator vehicle Feature/property Value FOV of the mm-radar 13.6◦ FOV of the lidar 14.0◦ Tracking and decision making system update rate 10 Hz Measurement association Track initialization Tracking ﬁlter Dynamic model NN 3 detections EKF Nearly coord. turn Maximum number of active tracks (tracking alg.) 4 Number of future predictions in the tracking system 20 Temporal spacing between predictions 100 ms Number of future predictions in the tracking system 20 Std. on range measurements in mm-radar model 1m Std. on range rate measurements in mm-radar model 0.5 m/s Std. on azimuth angle measurements in lidar model 1◦ Std. on range measurements in lidar model 1m Std. on range rate measurements in lidar model Std. on azimuth angle measurements in lidar model 1 m/s 1◦ studied in this section are mainly those where a CMbB has a large potential of signiﬁcantly reducing the collision speed. Examples of scenarios where CMbB has low potential for reducing the collision speed are angular accidents (where the angle between colliding vehicles is large e.g. intersection collisions), rear end collisions with a large lateral oﬀset between the colliding vehicles, and frontal collisions where the relative speed between the colliding vehicles is large. Note, that the since demonstrator vehicle test was only performed up to 70 km/h little eﬀort was made to tune the system above this velocity. Therefore, the test results of main interest is those between 0 and 70 km/h also for simulations. Head-on to Stationary Obstacle The ﬁrst scenario to be studied is the head-on against a stationary object, described by Figure 5.1. This scenario has already been discussed in Sections 5.1 and 2.2.4. This type of scenario corresponds well to many real life accidents where the driver of the striking vehicle is inattentive, or even falls asleep, and hits a stationary object. One will also observe this performance for accidents where the host vehicle follows another vehicle that suddenly stops (rear end collisions), in cases where the leading vehicle comes to a stop at a distance greater than the intervention distance. 114 Chapter 8 Collision Mitigation by Braking System Testing Another reason for studying this scenario is its simplicity, meaning that it is easy to perform both simulations and physical testing. The intervention distance will of course depend on the lateral oﬀset between the host vehicle and the obstacle. Figure 8.10 shows the range between the vehicles versus the host vehicle longitudinal speed from simulation results of the head-on scenario with zero lateral oﬀset. Figure 8.11 shows the same results from testing with the demonstrator vehicle. In Range versus range rate − no lateral offset 40 35 30 Range [m] 25 20 15 10 5 0 0 20 40 60 Range rate [km/h] 80 100 Simulation test results: Head-on to stationary object scenario described in Figure 5.1. The ﬁgure displays the average from 20 Monte Carlo simulations, for each velocity. The width of the host vehicle is set to 1.8 m and the width of the obstacle is 1.5 m. Figure 8.10: Figure 8.12 the speed reduction is shown for simulation results for the head-on scenario when there is an 0.5 m oﬀset between the host vehicle and the stationary obstacle. As previously mentioned, the performance of the braking system is crucial to a CMbB system. To evaluate the eﬀect of a more eﬃcient braking system, the head-on scenario with zero lateral oﬀset was simulated again and the braking system performance was changed to powerful state-of-the-art braking systems. The brake pressure rise time is assumed to be 100 ms and the maximum deceleration is assumed to be 9.82 m/s2 . Simulated performance of the CMbB system with a more powerful braking system is shown in Figure 8.13. Rear End Collision, Lead Vehicle Brakes Hard Rear end collision is one of the most common types of accident. Approximately 30 % of all accidents are of this type. A rear end collision is deﬁned as a collision between two vehicles traveling in the same direction, where the trailing vehicle Section 8.3 115 Demonstrator 1 | Probabilistic Decision Making Head on scenario no lateral offset − test result with protype vehicle 60 50 Range [m] 40 30 20 10 0 10 20 30 40 50 Range rate [km/h] 60 70 Figure 8.11: Test with demonstrator vehicle: Head-on to stationary obstacle described in Figure 5.1. Results from testing where the demonstrator vehicle hits the inﬂatable car, which is approximately 1.5 m wide. Range versus range rate − head−on, 0.5 m lateral offset 25 Range [m] 20 15 10 5 0 20 30 40 50 60 70 Range rate [km/h] 80 90 100 Figure 8.12: Simulation test results: Head-on against a stationary object scenario described in Figure 5.1, average results from 20 Monte Carlo simulations. In these simulations there was a 0.5 m lateral oﬀset between the two vehicles. 116 Chapter 8 Collision Mitigation by Braking System Testing Range versus range rate − no lateral offset 30 25 Range [m] 20 15 10 5 0 −20 0 20 40 60 Range rate [km/h] 80 100 Figure 8.13: Simulation test results: Head-on stationary object scenario described in Figure 5.1, both vehicles have a width of 2 m. Here a high performance braking system is used when simulating the CA manoeuver. The thick line shows the average speed from 20 Monte Carlo simulations, and the thinner lines show the performance in single simulations. hits the leading vehicles’ rear end with its front end (Figure 8.14). Even though H v a v Figure 8.14: Rear-end collision scenario. Initially both vehicles travel with the same velocity. Suddenly the leading vehicle brakes hard, achieving a deceleration of 8 m/s2 . the injuries from rear end collisions are not life-threatening most of the time, this type of accident is one of the most costly to society. This is partly because the accident type is so common and partly because the whiplash injuries that are often caused by rear end collision are hard to treat and cause victims pain over long periods of time. The performance of the CMbB system for this type of accident will vary with many factors such as relative speed of the vehicles, headway distance, braking force applied by the lead vehicle, lateral oﬀset, etc. If the head distance is long enough, performance will be the same as in the previous scenario (head-on against a stationary obstacle). In Figure 8.15 simulation results are illustrated for Section 8.3 117 Demonstrator 1 | Probabilistic Decision Making a scenario where the lead vehicle and the following vehicle initially travel at the same speed with a head distance of 1 second. Suddenly the lead vehicle brakes Range versus host vehicle speed − no lateral offset 25 Range [m] 20 15 10 5 0 30 40 50 60 70 80 Host vehicle speed [km/h] 90 100 Figure 8.15: Simulation test results: Rear end collision scenario described in Figure 8.14. The plot shows the average speed from 20 Monte Carlo simulations. hard (8 m/s2 ). There is no lateral oﬀset between the vehicles. Cut-in Behind a Stationary Object In this scenario the host vehicle travels straight ahead, and then makes a sudden lane change. In the lane that is entered there is a stationary obstacle. The lane change maneuver is executed as a sinusoid input to the steering wheel. The duration of the maneuver is 3.14 s for speeds below 40 km/h and 1.57 s for speeds above 40 km/h. The amplitude of the input maneuver is tuned to give a 2 m lateral displacement. The scenario is illustrated in Figure 8.16. In the simulated scenario, H Cut-in scenario — the host vehicle makes a sudden lane change; in the lane that it enters there is a stationary obstacle. Figure 8.16: 118 Chapter 8 Collision Mitigation by Braking System Testing Range versus host vehicle speed 30 25 Range [m] 20 15 10 5 0 0 20 40 60 Host vehicle speed [km/h] 80 100 Figure 8.17: Simulation test results: The driver makes a lane change maneuver (a sinusoid input that cause a 2 m lateral oﬀset of the host vehicle). The stationary obstacle is situated exactly at the point where the maneuver ends in the middle of the lane as illustrated in Figure 8.16. The plot shows the average performance from 20 Monte Carlo simulations. an average speed reduction of approximately 5 km/h for most speeds is attained. For low speeds the reduction is greater, and as the speed becomes greater than 100 km/h there is no speed reduction. This type of scenario was also tested with the demonstrator vehicle. However the result was poor. For low speeds (below 50 km/h) the obstacle will move out of the sensors’ FOV before turning in towards the obstacle. In such cases the tracking system often fails to re-acquire the target and react in time before the collision. The sensors also have problems detecting the stationary target under hard maneuvers. This causes problems even for speeds where the obstacle does not move out of the FOV. Cut-in, in Front of the Host Vehicle This scenario is the opposite of the previous example. Here the host vehicle drives straight ahead and another vehicle traveling at a lower speed enters the host vehicle’s lane. Figure 8.18 gives an overview of the scenario. This scenario tests the ability of the tracking system to track a sudden maneuver of a tracked object. The eﬃciency of the CMbB system will of course depend on several factors such as relative speed between the two vehicles, how close the lead vehicle cuts in front of the host vehicle, etc. This scenario has only been evaluated through simulations. Two variant of the scenarios is studied. The ﬁrst scenario is illustrated in Figure 8.19. Here the relative speed is low. The other vehicle is traveling 20 km/h slower than Section 8.3 119 Demonstrator 1 | Probabilistic Decision Making H Figure 8.18: Cut-in scenario, the other vehicle, that is traveling at a lower speed, enters the same lane as the host vehicle. Range versus host vehicle speed 30 25 Range [m] 20 15 10 5 0 20 30 40 50 60 70 Host vehicle speed [km/h] 80 90 100 Simulation test results: Cut-in scenario illustrated in Figure 8.18. The time to collision when the lead vehicle cuts in is 1 second. The relative speed is 20 km/h, i.e., the lead vehicle travels 20 km/h slower than the host vehicle. Figure 8.19: 120 Chapter 8 Collision Mitigation by Braking System Testing the host vehicle, the cut-in occurs late when the time to collision is one second. As Figure 8.19 shows for initial host speeds up to 60 km/h, the collision speed is reduced almost 10 km/h on the average. For higher speed the tracking system loses track of the lead vehicle as the sudden lane change is performed. In these cases there is no intervention. In the second scenario illustrated in Figure 8.20 the Range versus host vehicle speed 30 25 Range [m] 20 15 10 5 0 50 60 70 80 Host vehicle speed [km/h] 90 100 Simulation test results: Cut-in scenario illustrated in Figure 8.19 The time to collision when the lead vehicle cuts in is 3 seconds. The relative speed is 50 km/h, i.e., the lead vehicle travels 50 km/h slower than the host vehicle. Figure 8.20: relative speed is much greater, 50 km/h, and when the cut-in occurs, the time to collision is 3 seconds. 8.3.2 Scenarios to Provoke Faulty Interventions The scenarios to provoke faulty interventions are those where the host vehicle is close to a collision. Since no collision occurs in these scenarios, any CMbB system intervention is considered a faulty intervention. These scenarios are used for design, i.e., for choosing the intervention threshold. The threshold for CMbB intervention is set as low as possible without giving faulty interventions in any of these near-miss scenarios. Testing with the demonstrator vehicle is also performed to verify that the thresholds are correct. During this work, it has not been possible to make test runs with the demonstrator vehicle against moving obstacles. Therefore, scenarios where the obstacle is stationary is studied. Section 8.3 121 Demonstrator 1 | Probabilistic Decision Making Drive-By Scenario The ﬁrst scenario to be studied is a drive-by scenario. This can be viewed as a special case of the head-on collision scenario that was presented in Section 8.3.1. Here the oﬀset between the host vehicle and the obstacle is just large enough for the host vehicle to pass the stationary object without colliding (see Figure 8.21). From H Figure 8.21: Drive-by scenario, the oﬀset between the host vehicle is just large enough for the two vehicles not to collide. the simulation results in Figure 8.22, it is clear that this particular scenario does not generate high collision probabilities. The threshold for an intervention is when the probability of collision is larger than 0.7, for all the plotted speeds. However, Probability of collision − Drive by scenario, threshold at 0.7 1 0.9 Probability of collisioon 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 0.5 1 1.5 2 Time [s] 2.5 3 3.5 4 Simulation test results: The probability of collision in a drive-by scenario illustrated in Figure 8.21; the plotted probabilities are from 20 Monte Carlo simulations at 30, 40, 50 and 60 km/h. Figure 8.22: when this test was performed with the demonstrator vehicle and the inﬂatable car much greater probabilities were observed. Figure 8.23 shows the probabilities for four drive-by tests at 50 km/h with the demonstrator vehicle and the inﬂatable car. 122 Chapter 8 Collision Mitigation by Braking System Testing The reason for the high probabilities in this scenario was found to be deterioration Probability of collision for a close drive by scenario at 50 km/h 0.7 0.6 Probability 0.5 0.4 0.3 0.2 0.1 0 0 2 4 6 8 10 Time [s] Test with demonstrator vehicle: The probability of collision in a driveby scenario illustrated in Figure 8.21. The results are from 4 tests with the demonstrator vehicle and inﬂatable car at 50 km/h. Figure 8.23: of the sensor measurements for short ranges. This problem was already illustrated in Figure 8.7. When examining the sensor input from these tests one ﬁnds that in many cases the measurements (from the mm-radar) appear to be coming from an obstacle that is moving into the path of the host vehicle. Even though this causes much higher probabilities than from the simulations, none of the drive-by tests caused a faulty intervention. Head-on to Stationary Object Late Avoidance The late avoidance maneuver examined in this section is a maneuver where the host vehicle travels towards a stationary obstacle and avoids the obstacle by steering away as late as possible. This maneuver is illustrated in Figure 8.24. In Figure 8.25 simulation results from this maneuver are illustrated. The estimated probability of collision during the maneuver is plotted (for speeds 30, 40 and 50 km/h). The ﬁrst peak corresponds to simulations at 50 km/h, and the second peak corresponds to speeds of 40 km/h. The peak for simulations at 30 km/h lies exactly between the 40 and 50 km/h peaks, reaching a maximum value of approximately 0.2. The intervention threshold was 0.7 for all these speeds. There is no intervention for the simulated late avoidance scenarios. This should, of course, also be the case since there is no collision. The test results illustrated in Figure 8.26 show probabilities below 0.5 for the late avoidance scenario. However, during trials performing this Section 8.3 Demonstrator 1 | Probabilistic Decision Making 123 (a) Initially, the vehicle approaches the sta- (b) At the last moment the driver makes an tionary obstacle head-on. evasive steering maneuver. (c) The maneuver is suﬃcient to avoid the ob- (d) During the maneuver the driver turned the stacle with the smallest possible margin. steering wheel as fast as possible. The entire maneuver is executed at the limits of this vehicle’s handling capabilities. Figure 8.24: Example of a late avoidance maneuver. The driver steers away from the obstacle at the last moment and manages to avoid a collision. 124 Chapter 8 Collision Mitigation by Braking System Testing Probability of collision − late avoiodance scenario, threshold at 0.7 1 0.9 Probability of collisioon 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 2 4 6 8 10 Time [s] Figure 8.25: Simulation test results: The probability of collision for a late avoidance maneuver scenario illustrated in Figure 8.24. The plotted probabilities are from closing speeds of 30, 40 and 50 km/h, 20 Monte Carlo simulations for each speed are plotted. The peak lateral acceleration during the avoidance maneuver is 4 m/s2 . The intervention threshold is set to 0.7 for all speeds. scenario it has been possible to get faulty interventions. A possible reason for the faulty intervention is the deteriorated performance of the sensors at short range. It is, however, worth noting that there were few complaints about these faulty intervention from diﬀerent test drivers. One possible explanation for this is that the steering avoidance maneuver is so violent that the hard braking is not felt or possibly considered to be appropriate because of the extreme maneuver. Driving on Public Roads Apart from testing the system in speciﬁc scenarios on a test track, the demonstrator vehicle has also been driven on public roads in normal traﬃc situations. In these tests the braking actuation of the CMbB system is shut oﬀ. Any intervention (indicated by a LED head-up display) is considered a faulty intervention as long as an accident does not actually occur. During driving in normal traﬃc it was found that some faulty interventions do occur. Below, some typical situations that have been identiﬁed to cause faulty intervention will be discussed. The causes and possible solutions to avoid these faulty interventions will also be discussed. • Low speed driving: When driving at low speed (below 20 km/h) in target-rich environments, faulty interventions sometimes occur for no apparent reason. Looking closer at the sensor performance and the tracking algorithm, this is Section 8.3 125 Demonstrator 1 | Probabilistic Decision Making Probability of collision − late avoidance scenario 0.5 Probability of collision 0.45 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 0 2 4 6 Time [s] 8 10 12 Figure 8.26: Test with demonstrator vehicle: The probability of collision for a late avoidance maneuver scenario illustrated in Figure 8.24. The plotted probabilities are from tests with the demonstrator vehicle and the inﬂatable car. The closing speeds range from 30 - 70 km/h. not very strange. When traveling at low speeds there are often objects close to the vehicle. At short distances the sensor measurements often deteriorate. There is also an increased risk for spurious reﬂections, i.e., false targets, with a lot of reﬂecting surfaces close to the sensors. In the current algorithm the gates for measurement association are large (several meters). New obstacles are also very “easily” initialized (three consecutively associated samples are required). Due to this, the risk that the algorithm should detect faulty targets is high. The current system could be improved in several ways to attempt to correct this particular problem. By synchronizing the sensors, the gates for measurement association could be kept smaller. Initialization of new objects should possibly be made more “carefully”, i.e., require more than three consecutive measurements to initialize new obstacles. The inferior performance at short range is speciﬁc to the sensors used here. Using other sensors might alleviate the problems with poor measurements at short range; again it is stressed that both sensors of the current system were designed mainly for use in ACC systems. • Spatial sampling: Driving close to structures that have a spatially periodic appearance (for example fences and roadside reﬂector markers) sometimes cause faulty interventions. This happens due to the sensor sampling rate and the equidistant spacing of the reﬂecting objects. For example, a fence might appear to be a target moving at the same speed as the host vehicle at certain 126 Chapter 8 Collision Mitigation by Braking System Testing speeds. Even worse, it might also look like an object moving into the path of the host vehicle. Example 8.1 The sensor sampling frequency is 10 Hz. If the host vehicle is traveling at 30 m/s it will travel 3 m between two consecutive samples. If a structure by the side of the road has a spatial periodicity of 3 meters it may appear as an object moving along with the host vehicle at the same speed. The problem of spatial sampling might be tough to solve using radar sensors only. Of course, by the measured range rate (via Doppler shift) the targets can be determined as stationary and thus be ignored. This would remove all stationary targets, which is not desirable because it decreases the eﬀectiveness of the system. Another sensor, e.g. a vision sensor, could possibly be used to classify this type of target as invalid. Possibly this problem will also be diminished if the gates can be made smaller. • Multiple Reﬂections on Large vehicles: The radar sensor’s image of an object is clearly not the same as our visual image. The strongest point of reﬂection from an object might vary quite a lot between two samples. For large vehicles the risk of receiving measurements from diﬀerent points on the vehicle is particularly high. A typical scenario where this kind of phenomenon has caused faulty interventions is when meeting a large vehicle in a right hand curve. This problem might require sensors that have a better capability of classifying the object than the ones currently used. • Spurious reﬂections: There is always a risk that the sensor reports a ghost target due to spurious reﬂections. This has mainly been a problem with the mm-wavelength radar. An example of how spurious reﬂections might cause a faulty intervention is when driving behind a vehicle. A faulty measurement from the space between the host vehicle and the lead vehicle would look like a hard braking maneuver from the lead vehicle. From the driving on public roads, so far no such faulty interventions have been observed. However, for an ACC system using the same radar sensor, speed control has been observed when there is clearly no obstacle ahead on which to perform control. Again, by using smaller gates the probability of faulty associations decreases. By the use of two sensors (operating at diﬀerent wavelengths) this problem is alleviated to a great extent. 8.3.3 Test Results In this a speciﬁc CM system that uses the probability of collision as a metric for determining when to perform braking actuation is studied. The question is if this metric provides correct decisions for any type of traﬃc situation. This has been tested by simulation and with a demonstrator vehicle. In all the simulations and Section 8.3 Demonstrator 1 | Probabilistic Decision Making 127 test drives, no faulty intervention was found to be caused by an erroneous decision. However some faulty interventions did occur during tests with the demonstrator vehicle. These interventions were found to be caused by tracking system inadequacies. It should be mentioned that the tracking system was not designed primarily to suppress spurious reﬂections and faulty measurements from the sensor; it was designed rather for fast detection of suddenly appearing targets. Also, the sensor’s behavior when a objects is moving out of the FOV is not correctly modeled. The design strategy to ascertain that the CMbB system avoids making faulty decisions was to use a model of the vehicle that overestimates the handling capabilities of the vehicle. The probability threshold is then set high enough not to make faulty decisions. Using this design method it becomes interesting to study the eﬀect of the system in collision scenarios, i.e., will there be any signiﬁcant reduction of the collision speed. The simulation and testing in this chapter shows that signiﬁcant speed reduction is possible for certain types of accident, e.g. rear-end collisions and head-on collisions with stationary obstacles. In many cases the speed is reduced by around 10 km/h, and in the best case it is reduced by approximately 15 km/h. This reduction is achieved with a brake system that is far from optimal (both in the simulation environment and in the demonstrator vehicle). Also most of the demonstrator vehicle testing was performed in late December with temperatures between 0 and 5◦ Celsius, and on wet asphalt. There are several obvious improvements that could be made to achieve better performance than with the tested system. Improvements of importance is to use a state of the art braking system; the incorporation of steering wheel and brake pedal sensors in the navigation system; synchronization of the tracking sensors and reducing the system delay from decision to brake actuation. In simulations with a high performance braking system (Figure 8.13) an average speed reduction of almost 20 km/h was achieved for certain scenarios. Reducing the collision speed is particularly eﬀective for low relative speeds. For high relative speeds (well above 100 km/h) the probability of serious injuries and death will still be very high even if the collision speed is reduced by 20 km/h. Looking at the distance at which interventions occur for relative speeds below 100 km/h one ﬁnds that the range is less than 20 m. For a CMbB system like the one studied in this chapter, a maximum sensor range of 50 m is therefore probably suﬃcient. The current sensors detect obstacles at ranges beyond 150 m, which is much more than needed. The sensors’ narrow FOV, however, creates problems, as measurements deteriorate at short ranges. This causes bad performance (especially at low relative speeds) and also possible faulty interventions. In some of the scenarios that try to provoke faulty interventions, much higher collision probabilities are observed in tests with the demonstrator vehicle than from simulations. This phenomenon has been found to be caused mainly by inconsistent sensor performance at short ranges. 128 8.4 Chapter 8 Collision Mitigation by Braking System Testing Demonstrator 2 | Multiple Object Decision Making In this section, tests with a second demonstrator is presented. The main purpose is to compare a multiple object algorithm with a single object algorithm on realistic tracking input. However in this case the tracking system is provided by a sensor supplier and not fully known. The demonstrator vehicles are similar in that they are both a Volvo V70s, and that they use a dual sensor tracking system. The tracking system installed in demonstrator 2 is provided by a sensor supplier. Therefore, the exact function of the tracking system is not known. Some important characteristics of the tracking and decision making system of the second demonstrator vehicle are summarized in Table 8.3. Table 8.3: Summary of important characteristics of the second demonstrator vehicle. Feature/property Value FOV of the mm-radar 13.6◦ 48◦ FOV of the camera Tracking and decision making system update rate 10 Hz Maximum number of active tracks (tracking alg.) 30 Decision bound for lateral acceleration 7 m/s2 Decision bound for longitudinal acceleration 10 m/s2 8.4.1 Test Results Performance was evaluated for two simple scenarios. The ﬁrst scenario is a head-on collision scenario with three stationary obstacles (Figure 8.27) at approximately 70 km/h. Figure 8.28 and 8.29 show the relative positions and the intervention distance for the multiple object and the single object algorithm, respectively, for two test runs. In these two test runs the intervention is 400 and 300 ms earlier than the single object algorithm. Using a brake system model as Example 5.1 the collision speed is 31.0 and 35.5 km/h for the multiple object algorithm. For the single object algorithm the collision speed is 54.7 and 50.8 km/h. In the second test scenario that is studied, the host vehicle also travels headon against three stationary objects, but in this case it brakes hard to avoid the collision. Figure 8.30 illustrates the range, range rate and the normalized required acceleration to avoid colliding for both algorithms. The normalized required acceleration is referred to as the threat number (T N ), and is deﬁned according to T N = min(| Ahost Ahost y x |, | |), Aa,x Aa,y (8.3) Section 8.4 Demonstrator 2 | Multiple Object Decision Making (a) Three obstacles 70 km/h. are approached at (b) The left obstacle is a Volvo V70. The center and right obstacle are inﬂatable dummies. (c) At approximately 10 m the driver brakes to reduce the collision speed. Figure 8.27: 129 (d) The center obstacle is struck head on. Collision scenario seen from the vision sensor’s view. The speed is 70 km/h. where Aa,x = 9.82 m/s2 is the maximum longitudinal acceleration and Aa,y = 7 m/s2 is the maximum lateral acceleration that the host vehicle can achieve. While the multiple object algorithm indicate that a high acceleration is required to avoid a collision, the single object algorithm displays very moderate threat number values. None of the algorithms are close to 1, which is the threshold for an intervention. 130 Chapter 8 Collision Mitigation by Braking System Testing Alarm distance for multiple and single object algorithms at 71.5 km/h 5 Alarm multiple alg. 4 Obstacle1 Relative lateral position [m] 3 2 Alarm single alg. 1 Obstacle2 0 −1 Obstacle3 −2 −3 −4 −5 0 5 10 15 20 25 30 Relative longitudinal position [m] 35 40 Figure 8.28: Test results from a collision test at 70 km/h. The vertical lines indicate the distance at which each algorithm alarms. Automatic braking at 18.2 and 10.2 meters correspond to a collision speed of 30.9 and 54.7 km/h respectively. The faint, dashed lines indicate the estimated right and left side of each obstacle. The tracking system loses track of the left and right obstacle at approximately 15 meters. Alarm distance for multiple and single object algorithms at 70 km/h 5 Alarm multiple alg. 4 Obstacle1 Relative lateral position [m] 3 2 Alarm single alg. 1 Obstacle2 0 −1 Obstacle3 −2 −3 −4 −5 0 5 10 15 20 25 30 Relative longitudinal position [m] 35 40 Figure 8.29: Test results from a collision test at 70 km/h. The vertical lines indicate the distance at which each algorithm alarms. Automatic braking at 17 and 11.8 meters correspond to a collision speed of 35.5 and 50.8 km/h respectively. The faint, dashed lines indicate the estimated right and left side of each obstacle. The tracking system loses track of the left and right obstacle at approximately 15 meters. Section 8.4 131 Demonstrator 2 | Multiple Object Decision Making Hard braking to avoid a collision 40 20 0 0 100 0.5 1 1.5 2 2.5 3 0.5 1 1.5 2 2.5 3 50 0 0 1 TN multiple obj. TN single obj. 2 TN (m/s ) Rangerate [km/h] Range [m] 60 0.5 0 0 0.5 1 1.5 Time [s] 2 2.5 3 2 Figure 8.30: Test results from a hard braking maneuver (−7.5 m/s ), close to three stationary obstacles. An alarm is triggered when T N ≥ 1 (the required longitudinal acceleration is less than −9.82 m/s2 and the required lateral acceleration is larger than 7 m/s2 . 132 Chapter 8 Collision Mitigation by Braking System Testing Chapter 9 Supplementary Simulation Studies 9.1 Comparison of Multiple Object Algorithms One problem with multiple object decision making algorithms is that they become more complex both from a computational and an understanding perspective. For single object decision algorithms it is normally quite easy to predict the behavior of the algorithm. For a multiple object algorithm, on the other hand, it might be harder to forecast the behavior. It is therefore desirable to have at method to automatically search for errors or unexpected behavior. This section discusses an exhaustive search method to compare CA decision algorithms. A comparison of four decision making algorithms is presented in an extensive simulation study. The four algorithms to be compared are • Single obstacle constant control (SOCC) described in Section 5.3.10, • Multiple obstacles constant control (MOCC) described in Section 6.1.1, • Multiple obstacles heuristic control (MOHC) described in Algorithm 6.2, • Multiple obstacles exhaustive search (MOES) described in Section 6.3. The focus is on ﬁnding situations where the algorithms perform diﬀerently. In this evaluation full state knowledge of all objects is assumed, i.e., no tracking system uncertainty. A more thorough account is given in [78]. 9.1.1 Test Method and Specications The basic idea of the approach taken here is to perform an exhaustive search over all possible situations that the decision algorithm can encounter. Each decision making algorithm is then evaluated for each situation. Of course, this is not possible since there are an inﬁnite number of possible situations. Instead, the possible 133 134 Chapter 9 Supplementary Simulation Studies states for each object are discretized and then all possible combinations are formed. Here, it is assumed that each object is described by a constant acceleration model. The state vector and dynamical model for an object is, thus, given by (4.46). The number of situations that can be generated in this fashion grows very rapidly. Assuming that there are n objects and that there are k possible state vectors for each k! diﬀerent situations can be formed. However, many of the situations object n!(k−n)! created in this manner are uninteresting to examine and can be discarded before the evaluation starts. Below strategies used to remove uninteresting situations are described. • Symmetry — This strategy relies on the decision making algorithm responding exactly the same to scenarios around the host vehicles longitudinal axis. Thus, all symmetric scenarios can be removed. • Deriveability — If one set of object states causes an alarm, all supersets (adding more objects) will also cause an alarm, for the algorithms considered here. Starting with only one object the response for all supersets, of scenarios where there is an alarm, is known and need not to be checked. By starting with the situation including only one object a large number of multiple object situations is also checked. Then one proceeds with the remaining situations where there are two objects, and so on. • Minimum distance — If no object comes within a minimum distance of the host vehicle one may assume that there is no alarm. • Path crossing — If none of the objects crosses the current path of the host vehicle there should not be an alarm since keeping that maneuver constant will avoid collision. Since all situations are automatically generated, there is no before hand information whether there should be an alarm or not. In general it is not possible to say if an alarm was correct or not. However, it is possible to compare the response from diﬀerent algorithms. By using a more advanced decision algorithm as a reference, it is thus possible to ﬁnd many (but not all) situations where there are false alarms. In the test results discussed below four decision making algorithms (SOCC, MOCC, MOHC and MOES) are compared. The test speciﬁcation is given in Table 9.1. Initially, there is a huge number (574965720) of situations to evaluate. By using the strategies described above, only 435 + 77642 + 12265925 had to be evaluated. This could be done in less than 100 hours on two desktop computers. 9.1.2 Test Results Test results are summarized in Table 9.2. There are some results which might seem counter intuitive. First, there is a number of scenarios where MOES alarms but MOCC does not. This seem counter-intuitive since MOES searches a large set of diﬀerent escape paths. However, since only a ﬁnite number of paths are examined the solution has a limited spatial resolution. Thus, narrow passages are diﬃcult Section 9.1 135 Comparison of Multiple Object Algorithms Speciﬁcations for automatic situation generation and testing comparing decision making algorithms. The 1512 states come from combining 84 positions, 9 velocities, and 2 accelerations Table 9.1: Feature/property Value Maximum number of obstacles 3 Number of discrete states 1512 Number of scenarios included in the test 574965720 Obstacle velocity range ± 54 km/h Number of trajectories searched by the MOES alg. Table 9.2: 50625 Test results comparing decision making algorithms in 12344002 diﬀerent situ- ations. Feature/property Value Situations where MOCC alarms but MOHC does not 0.21 % Situations where MOCC alarms but MOES does not 2.98 % Situations where MOES alarms but MOCC does not 1.37 % Situations where MOHC alarms but MOES does not 2.80 % Situations where SOCC alarms but MOES does not 1.86 % to ﬁnd. In this respect the MOCC and MOHC algorithms are superior since they are based on an analytical solution and will ﬁnd any existing narrow passage. The problem is very similar to the narrow passage or narrow corridor problem for path planing applications [74, 84, 110, 122]. Secondly, in some situations SOCC alarms but MOES does not. The reason for this is that MOES tests escape path with combined lateral and longitudinal acceleration. This sometimes provides a more eﬃcient avoidance maneuver than pure longitudinal or lateral acceleration which is assumed in the SOCC algorithm. In fact, a large portion of all scenarios where MOCC and MOHC alarms but MOES does not, is due to the fact that a slightly better avoidance maneuver can be found by combining lateral and longitudinal acceleration. To assess the general beneﬁt of the multiple object algorithm, a comparison with the SOCC algorithm was performed. This comparison is conducted on a subset of the scenarios and only multiple object scenarios are included. Situations where either only SOCC or only the multiple object algorithms alarms are considered. These situations are used as initial condition and are simulated forward and backward in time to ﬁnd the timing diﬀerence between when the algorithms ﬁrst alarm. The result is shown in Table 9.3 136 Chapter 9 Table 9.3: Supplementary Simulation Studies Average diﬀerence in alarm timing between SOCC and the multiple object algorithms. Algorithm Alarm time diﬀerence 9.2 MOCC MOHC MOES 0.47 s 0.41 s 0.26 s Statistical Decision Making by Stochastic Numerical Integration In this section statistical decision making by stochastic numerical integration according to (7.5) is considered in a simulation study. The objective of the evaluation is threefold. The ﬁrst objective is a comparison between EKF and PF for some diﬀerent measurement noise distributions. The second objective is to evaluate stochastic integration when using an EKF. Finally, the real time performance of a numerical solution is considered. The comparison is made for a single scenario, the head-on collision scenario illustrated in Figure 5.1, when traveling at 60 km/h. Furthermore, this section also presents some measurement results from an automotive radar. 9.2.1 Tracking Model The general tracking model takes the form of a nonlinear state space model for the vehicle dynamics and sensor measurements (4.1) and (4.2). Here, the linear constant acceleration model (4.46) will be used to model the dynamic properties of all vehicles. The measurement relation (8.1) comes from radar measurements of range, range rate and bearing. Where the measurement noise, et , typically includes clutter, multi-path and multiple reﬂection points in the vehicle ahead. 9.2.2 Estimation Approach The vehicle dynamic model (4.46) to be used in the tracking ﬁlters is linear. However, the measurement noise and the process noise (driver inputs) are not Gaussian. Thus, a recursive estimation method that can handle this is required. Here the PF (given in Section 4.1), that provide a general nonlinear and non-Gaussian Bayesian solution to the estimation problem, is compared with EKF (4.25) — (4.26). In the EKF, approach the true noise distributions are approximated with Gaussian distributions (4.15). 9.2.3 State Noise Model The classical assumption in target tracking is to assume a Gaussian distribution for the process noise νt . In Figure 9.1, recorded driving data is used to make histograms of the longitudinal component of the acceleration noise, νt . The data is, of course, highly correlated in time, since the acceleration does not ﬂuctuate Section 9.2 Statistical Decision Making by Stochastic Numerical Integration 137 rapidly. This could be modeled by introducing a Markov chain based on the empirical data. However, for braking situations a distribution based on accelerations collected close to rapid decelerations should probably be used instead. A further Empirical distribution of vehicle acceleration 0.18 0.16 0.14 Probability 0.12 0.1 0.08 0.06 0.04 0.02 0 −6 Figure 9.1: −4 −2 0 Acceleration [m/s2] 2 4 Acceleration histogram of a vehicle driven in urban traﬃc for 45 minutes. and natural option is to switch between diﬀerent distributions depending on the driving situation. In urban traﬃc one distribution is used, on the highway another one, etc. This leads to a mode dependent distribution pν (ν; ξ). Here the mode can be determined from the host vehicle speed, it may depend on the state vector, external information from navigation and telematics systems, etc. A simple approach is to use one mode when v < 70 km/h and another one for v > 70 km/h. In the sequel in this section, the histogram illustrated in Figure 9.1 is used to model the process noise in the PF. For the EKF, a Gaussian equivalent is used. 9.2.4 Measurement Noise Model The measurement equation for the radar involves range, range rate and relative angle measurements to the vehicle ahead (which in this case is a stationary obstacle). In many applications, Gaussian noise is used to approximate the measurement noise. Here some other noise distributions will be examined. An experiment was performed by towing a radar-equipped vehicle, which then measure the range to the towing vehicle under driving conditions with an almost constant range. Data is collected using an FM-CW radar at 40 m and constant speed 60 km/h on a Volvo S80 sedan. The range data is correlated, probably due to some oscillations in the rope and from internal ﬁlters in the radar sensor. A second order state space model is ﬁtted to data using the n4sid method in Matlab system identiﬁcation toolbox, 138 Chapter 9 Supplementary Simulation Studies [88], in order to produce de-correlated range data. This PDF range error is presented in Figure 9.2 together with a Gaussian approximation. The measurement Range pdf, towrope: 40 [m] speed: 60[km/h]. Target vehicle: Volvo S80 25 Probability density function (pdf) 20 15 10 5 0 −0.2 −0.15 −0.1 −0.05 0 0.05 Range [m] 0.1 0.15 0.2 0.25 Measurement range noise PDF around the nominal range r = 40 m for the target vehicle Volvo S80 sedan and an equivalent Gaussian approximation. Figure 9.2: noise from the experiment is collected under nearly ideal circumstances. For instance, the data was collected from behind, i.e., no aspect angle to the car, and on a smooth test track. In many cases, due to road curvatures, other vehicles, uneven road surfaces relative lateral position and lateral movement, the measurement range distribution might be diﬀerent. Also, the exact reﬂection point is uncertain since the azimuth angle is not very exact, so for medium distances the main reﬂection point may be located on diﬀerent parts of the car. Hence, for other driving situations the measurement noise PDF may have a larger variance and diﬀerent shape. A bi-Gaussian PDF example is presented in Figure 9.3. This measurement noise model is motivated by modeling the vehicle by two-point reﬂectors, which is a simpliﬁed way of describing both multi-path propagation and complex reﬂection geometry. The exact appearance of the PDF will vary with many factors such as target vehicle, sensor and traﬃc situation. Here only a bi-modal distribution with highly separated peaks is tested. This is to cover many situations with diﬀerent aspect angles, where the density is due to multi-path propagation, and multiple reﬂections points, for instance in the rear-end, wheel housing, and rear-view mirror. More measurements and experiments are needed to establish the empirical PDF for diﬀerent driving situations, so the proposed density should just be an example of non-Gaussian PDF inﬂuence. In the simulation study diﬀerent assumptions of the PDF are compared. Section 9.2 Statistical Decision Making by Stochastic Numerical Integration 139 1 Histogram Gauss sum pdf Normal approx. 0.9 0.8 Probability 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 −3 −2 −1 0 Range [m] 1 2 3 Figure 9.3: Bimodal range measurement noise and Gaussian approximation of range density pe = 0.75N (0, 0.42 ) + 0.25N (1.6, 0.42 ). 9.2.5 Extended Kalman Filter The Bayesian recursions in (4.28) do not in general have an analytical solution. For the special case of linear dynamics, linear measurements and Gaussian noise, there exists a solution, which is retrieved by the Kalman ﬁlter (Section 4.1). For many nonlinear problems the noise assumptions are such that a linearized solution will be a good approximation. This is the idea behind the EKF, where the model is linearized around the previous estimate. When a Gaussian mixture measurement PDF is used et ∈ M pi N (µi , σi2 ), (9.1) i=1 where N (µ, σ 2 ) denotes a Gaussian density with mean (µ) and covariance (σ 2 ), the measurement update equation must be modiﬁed as x̂t|t = x̂t|t−T + Kt (yt − h(x̂t|t−T ) − m̄), (9.2) M where m̄ = i=1 pi µi . If the measurement PDF is not known analytically, the approximation using a single Gaussian PDF introduces a bias (since m̄ = 0). The decision criterion (9.3) is not analytically solvable when the state variables are considered to be Gaussian. By sampling the position and velocity distribution around the estimates, a Monte Carlo integration technique can solve the decision criterion. Hence, part of the computational burden present in the particle ﬁlter is introduce in this step for the EKF method. 140 Chapter 9 Table 9.4: Supplementary Simulation Studies Measurement noise models used in the simulation. Range distributions (pet ) (1) pet = 0.75N (0, 0.42 ) + 0.25N (1.6, 0.42 ) (2) pet = 0.75N (0, 0.22 ) + 0.25N (1, 0.42 ) (3) pet = Empirical (Figure 9.2) 9.2.6 Simulations and Tests The simulation study compares the traditional EKF tracking ﬁlter with the particle ﬁlter for diﬀerent sensor noise assumptions. The acceleration required to avoid collision (5.36) is used in a hypothesis test to determine when to start braking, according to obj Pr{aobj x − ãx = ax − |ṽx |ṽx } > 1 − α, 2p̃x,0 (9.3) where ath is the acceleration threshold for when an accident is considered to be imminent. The collision speed is calculated using a ﬁrst-order system according to the one described in Example 5.1 The real-time performance in a hardware collision avoidance test platform is also tested. To simplify analysis of the system performance for diﬀerent noise distributions, a single scenario is studied. The scenario to be studied is, again, one where the target object is not moving and the vehicle with the CMbB system is approaching with constant velocity (60 km/h). The motion model of the vehicles is a constant acceleration according to (4.44). Two main models for the measurement sensor are considered. In the ﬁrst the measured range distribution from Figure 9.2, based on measurements collected from experimental data on a Volvo S80 is used. It is concluded that the range density is narrow and not as bimodal as expected. This may be due to the ideal circumstances of the experiment, as described in Section 9.2.4. Other driving situations are of course also of interested. Hence the second model of the measurement range noise is a bi-modal Gaussian, i.e., et ∈ p1 N (µ1 , σ12 ) + p2 N (µ2 , σ22 ), (9.4) where N (µ, σ 2 ) denotes a Gaussian density with mean (µ) and covariance (σ 2 ). A sample of et belongs to one of the distributions, with probability pi . The three distributions used are collected in Table 9.4. The simulation results are given in are given in Table 9.5. Here p1 = 0.75 and p2 = 0.25, since the rear-end of the vehicle is assumed to have a larger radar cross section and constitute the main reﬂector. The range rate and azimuth were found to be well approximated by Gaussian distributions, with σṙ = 0.2 and σϕ = 0.01. The longitudinal acceleration process noise, Q ≈ 0.5/T is used to model driving behavior and model imperfections. A simple brake system model according to (5.45) is used to calculate the collision speed. The parameters k1 and k2 are chosen to give a brake system rise-time of 300 ms and a Section 9.2 141 Statistical Decision Making by Stochastic Numerical Integration Table 9.5: Results from 1000 Monte Carlo simulations and diﬀerent cases. pet (r) RMSE (p̃) RMSE (ṽ) Coll. speed σ Coll. speed Case Filter I PF pet 0.11 0.29 -6.59 0.51 EKF (1) pet 0.16 0.29 -6.61 0.53 0.08 0.29 -6.56 0.49 0.11 0.29 -6.56 0.51 0.06 0.29 -6.52 0.47 0.06 0.29 -6.52 0.47 I II PF II EKF III PF III EKF (1) (2) pet (2) pet (3) pet (3) pet maximum deceleration of 9.82 m/s2 . The somewhat lower maximum deceleration (compared to Section 9.2) is due to the fact that the test track exhibited very good frictional conditions during the brake system tests. The brake decision is based on a hypothesis test on the expected required acceleration according to (5.36). A acceleration threshold ath = −8 m/s2 and the conﬁdence level to α = 0.05 have been chosen. For the PF approach, the hypothesis is evaluated for each particle. In the particle ﬁlter N = 5000 particles is used, and the same amount of samples are drawn to perform the stochastic integration for the EKF. 1000 Monte Carlo simulations are performed for both PF and EKF. 9.2.7 Simulation Results The diﬀerent simulations from the Monte Carlo study are summarized in Table 9.5 together with tracking and CMbB system performance. Three diﬀerent cases were considered. In cases I and II two diﬀerent bi-modal range distributions were used and in case III the empirical range distribution from Figure 9.2 was used. All other values were the same for all the simulation cases. For non-Gaussian range distributions, such as the bimodal Gaussian mixture proposed for the measurement noise, the PF method increases estimation performance (position RMSE) slightly. The mean diﬀerence in collision speed is basically not aﬀected, however, the EKF has a somewhat larger collision speed variance, but this is probably insigniﬁcant. The result is under the assumptions of quite accurate range rate and azimuth measurements and a high measurement update rate, f = 1/T = 20 Hz. Hence, for systems that do not measure the relative speed more signiﬁcant diﬀerences are expected. 9.2.8 Hardware and Real-Time Issues In the simulations presented in Section 4.7, the entire algorithm was implemented in standard Matlab code. However, the ultimate goal with the particle ﬁlter based approach is to incorporate the algorithm and test the idea in an on-line application in a collision avoidance system. The test platform uses dSPACE hardware (equipped with a 1 GHz Power PC) together with Simulink and Real-Time Work- 142 Chapter 9 Supplementary Simulation Studies shop (RTW). Hence, the algorithm must be written in C-code and Matlab/MEX functions for Simulink. The entire environment is then compiled using the RTW compiler to the dedicated hardware platform. In Figure 9.4, the Simulink diagram is shown for the underlying MEX C-code functions i.e., the particle ﬁlter and the decision making algorithm. These are incorporated in the testbed using the RTW-compiler. The entire algorithm runs faster than real-time on the dedicated hardware, with T = 0.05 s and N = 5000 particles. x To Workspace Scope2 Out1 inter 1 y Measurements z (par) Unit Delay Parameters In1 Out1 In2 Out2 Particle Filter x xhat xhat To Workspace3 In1 Out1 In2 Out2 Scope1 DecisionMaking acc_req_est_hyp Scope3 Scope4 y To Workspace2 Simulink diagram of the CMbB system model used to generate RTW code for the dedicated hardware. Figure 9.4: 9.2.9 Conclusions A decision rule in a CMbB system for late braking using a hypothesis test based on estimates of the relative longitudinal dynamics have been implemented. Both an EKF and a particle ﬁlter were evaluated for diﬀerent noise assumptions. For nonGaussian range distributions such as the bimodal-Gaussian proposed for the measurement noise, the PF method increases estimation performance slightly. However, the range error is in the order of decimeters for all tested scenarios, so the mean diﬀerence in collision speed is basically not aﬀected. The EKF has a somewhat larger collision speed variance, but this is probably insigniﬁcant. The result is under the assumption of quite accurate range rate and azimuth measurements, and a moderate process noise. Hence, a greater diﬀerence might occur if, for instance, the range rate is not measured or if more maneuverability i.e., larger process noise is considered. The computational cost of the EKF method is somewhat smaller than for the PF, but rather close. This is due to the fact that the EKF uses a stochastic integration to calculate the probability of collision. So for a decision rule, such as (7.5), a particle ﬁlter approach might be preferable. It should also be kept in mind that the scenario studied here is very simple (from a tracking sense); for more complex scenarios with maneuvering target and tracking platform the diﬀerence between the methods may be larger. In addition, the particle ﬁlter method is more ﬂexible, so if a more complex state-space model is used, linearization errors in the dynamical model can be avoided. Section 9.3 Statistical Decision Making Using a Gaussian Approximation 143 In the simulation study the simulations for both the PF and EKF approach were run faster than real-time on an ordinary desk-top computer. The particle ﬁlter was also implemented in the Volvo testbed hardware (used in vehicle tests) using the Matlab Simulink RTW compiler. The tracking ﬁlter and decision algorithm executed faster than the real-time constraint, T = 0.05, using N = 5000 particles for the tracking model described earlier. 9.3 Statistical Decision Making Using a Gaussian Approximation In the previous section a CA system using statistical decision making based on stochastic numerical integration was discussed. In this section a similar system based on the same decision function, the longitudinal acceleration required to avoid a collision (5.36), is studied. Two CA decision strategies compared. The ﬁrst strategy is according (7.7) which neglect the uncertainty of the state estimate. This gives the following decision rule |ṽˆx,0 |ṽˆx,0 obj host < ath1 ⇒ a, (9.5) ax,req = âx,0 − 2p̃ˆx,0 where the action a is full emergency braking, ath1 is the acceleration threshold, ˆ ˆ âobj x,0 , ṽx,0 , p̃x,0 denote estimated object acceleration, estimated relative velocity, and estimated relative distance, respectively. The second CA decision strategy is according to (7.9), yielding the following decision rule ' host ( ' ' host (( ax,req − C1 B ahost ⇒ a. (9.6) x,req < ath2 + C2 D ax,req To evaluate the two decision strategies the same head-on scenario considered in previous sections (Figure 5.1) will be studied. First, the probability of performing an intervention during the sample interval when the host vehicle is exactly at the intervention boundary will be studied i.e., at the instant when a collision is becoming imminent. Here a collision is deﬁned as being imminent when a deceleration of 8 m/s2 or more is required to avoid colliding. In Figure 9.5 the probability of an intervention for the criterion (9.5) and (9.6) can be compared at diﬀerent closing velocities; the parameters for the calculation can be found in Summary 9.3. Note, in Figure 9.5 the probability of an intervention is illustrated only for a certain time instant (i.e., when the collision is becoming imminent). Next, system performance in terms of reduction of the collision speed and the probability of making a too early intervention will be studied. Too early an intervention is deﬁned as a CMbB system-induced brake maneuver before the collision imminent boundary is reached. The process noise, ν, and measurement noise, e, are assumed to be independent and Gaussian. The values of the noise variances were chosen to correspond to the measured PDFs presented in the previous section and illustrated in Figures 9.1 and 9.2. The brake 144 Chapter 9 Supplementary Simulation Studies Probability of intervention at decision boundary: Std.(p) = 0.21, Std.(v) = 0.25 0.45 Criterion (9.5) Criterion (9.6) 0.4 Probability of intervention 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 0 10 20 30 40 Relative speed [km/h] 50 60 Figure 9.5: Probability of performing a CMbB intervention at the sampling interval when the collision imminent boundary is crossed. The required acceleration is −8 m/s2 . system model is according to the model described in Example 5.1. However, the maximum deceleration is somewhat lower since the perfect conditions on the test tracks are not always present on a normal road. Both decision criterion functions are applied to the tracking system output. A summary of the simulation parameters is given in Table 9.3. The simulation results are presented in Table 9.7 for speeds from 5 - 60 km/h. For each speed, 2000 Monte Carlo simulations were performed, and for each scenario both decision criteria were applied to the same tracking data. At speeds higher than 60 km/h a steering avoidance maneuver is much more eﬃcient than a braking maneuver, therefore higher velocities are not as interesting to examine. Table 9.8 presents the same simulation scenarios as Table 9.7, but here ˆ 1) and ṽ ∈ N (ṽ, ˆ 1) the measurement noise was doubled, so that p̃ ∈ N (p̃, 9.3.1 Analysis How well does the deterministic criterion (9.5) work, compared to statistical decision criterion (9.6)? In Figure 9.5 the probability of an intervention at a certain time instant is compared for diﬀerent velocities. From the Figure 9.5 it is clear that for the criterion (9.5) the probability for an intervention varies while for (9.6) it is kept constant (for speeds above 15 km/h). Why is it that the decision criterion (9.6) manages to maintain a constant risk level? To understand this the criterion for intervention is studied more closely. Rewriting the estimated required Section 9.3 Statistical Decision Making Using a Gaussian Approximation Table 9.6: 145 Summary of important simulation parameters. SIMULATION SUMMARY 1. Decision criterion (9.5) parameters: ath1 = −8.5 m/s2 . 2. Decision criterion (9.6) parameters: C1 = 1, C2 = 1 and ath2 = −8 m/s2 ]. 3. State noise. ˆ 0.012 ) N (ã, ˆ 0.252 ) and ã ∈ ˆ 0.252 ), ṽ ∈ N (ṽ, p̃ ∈ N (p̃, 4. Sensor and tracking system update rate 10 Hz. 5. Decision making process data rate 100 Hz. 6. Measurement noise. et ∈ N (0, 0.252 ). 7. Brake system model. k1 = 7, ⇒ rise time of 300 ms. Table 9.7: Head-on scenario simulation results. ṽ is the initial closing velocity, ṽc is the speed at the collision instant and Prob(faulty) denotes the probability of a faulty (i.e., too early) intervention. Criterion (9.5) ṽ [km/h] Criterion (9.6) ṽc [km/h] Prob(faulty) ṽc [km/h] Prob(faulty) 5 5 0 5 0 10 7.3 0.28 10 0 15 10.6 0.18 14.5 0.013 20 12.8 0.11 14.4 0.035 25 14.2 0.073 15.3 0.024 30 15.1 0.033 15.9 0.016 35 15.7 0.019 15.6 0.017 40 15.8 0.014 15.5 0.019 45 16.1 0.006 15.0 0.015 50 15.8 0.005 13.8 0.012 55 15.4 0.003 12.3 0.017 60 14.2 0 10.4 0.009 146 Chapter 9 Supplementary Simulation Studies Table 9.8: Head-on scenario simulation results. Here the standard deviation of the measurement noise was doubled. Criterion (9.5) ṽ [km/h] Criterion (9.6) ṽc [km/h] Prob(faulty) ṽc [km/h] Prob(faulty) 5 3.5 0.329 5.0 0 10 7.1 0.340 10.0 0 15 10.3 0.249 15.0 0 20 12.3 0.199 17.2 0.028 25 13.4 0.138 16.4 0.038 30 14.9 0.078 16.7 0.033 35 15.4 0.060 16.7 0.031 40 15.6 0.037 16.6 0.025 45 15.8 0.021 16.0 0.021 50 15.3 0.017 14.6 0.027 55 14.9 0.011 13.4 0.026 60 14.3 0.005 11.7 0.020 acceleration as ˆ2 obj ˆ = âobj − ṽ âhost − ã x,req = â 2p̃ˆ (ṽ + νv )2 = aobj + νa − 2(p̃ + νp ) = aobj + νa − (ṽ 2 + 2ṽνv + νv2 ) , 2(p̃ + νp ) (9.7) where in our case νp , νv and νa ∈ N (0, σi2 ). When p̃ and ṽ are large compared to the noise terms νv and νp (9.7) is approximately according to obj âhost + νa − x,req ≈ a (ṽ 2 + 2ṽνv ) . 2p̃ (9.8) Equation (9.8) has a Gaussian distribution since it consists of a sum of Gaussians. Thus, when p̃ and ṽ are large compared to their uncertainty, âhost x,req ∈ ṽ 2 ṽ 2 obj 2 2 N (a + 2p̃ , σa + ( p̃ ) σv ) is a good approximation. The criterion (9.6) will thus be equivalent to the hypothesis test according to (7.8), the parameters C1 and C2 will determine the conﬁdence level of the test. What are the implications of this in terms of collision mitigation performance? Looking at the simulation results of Table 9.7, it shows that for speeds of 30 km/h and above the probability of a faulty Section 9.3 Statistical Decision Making Using a Gaussian Approximation 147 intervention is kept constant for the criterion (9.6). For criterion (9.5), on the other hand, the probability of a faulty intervention decreases as the closing velocity becomes higher. At velocities around 35 km/h the performance is the same for both criteria. This is also the velocity at which they cross in Figure 9.5. Both criteria performed badly at low speed. This is mainly due to the fact that the dynamic of the brake system is not captured in the collision avoidance decision function (5.36). The assumption of a constant measurement noise causes the assumptions of p̃ and ṽ being large compared to the noise to be violated at low closing velocities. The required acceleration (5.36) will thus not be Gaussian. This causes the probability of intervention to vary signiﬁcantly for low speeds. 9.3.2 Conclusions In this section, statistical decision making, by approximating the distribution of the decision function by a Gaussian distribution, applied to automotive collision avoidance has been discussed. A criterion that explicitly uses an approximation of the statistical properties of the CA decision function has been simulated and analyzed. To compute the approximation, the estimated tracking noise covariance (which was assumed to be Gaussian) was used. The main reason for this criterion was to be able to deal with changing uncertainty of the decision function without having to evaluate the entire PDF. The criterion is motivated by properties of the Gaussian distribution; for Gaussian distributed variables the criterion corresponds to a hypothesis test. It was shown that the CA decision function (5.36) studied can be considered to be Gaussian under certain circumstances. In head-on collision scenarios where the assumption of Gaussianity holds (v > 25 km/h), the criterion (9.6) applied to (5.36) keeps the risk of a faulty intervention at a relatively constant level. However, comparing the results in Tables 9.7 and 9.8 one can observe a change in that risk level. For the criterion (9.5) the risk of a faulty intervention is changed more signiﬁcantly as the measurement noise is increased. For scenarios where the assumptions do not hold (e.g. at low speeds) the criterion (9.6) becomes too cautious, causing braking to be too late. Bad performance at low speeds is also caused by the fact that the decision function used does not account for the brake system dynamics. 148 Chapter 9 Supplementary Simulation Studies Chapter 10 Summary This thesis discusses theory for collision avoidance decision making and its application to automotive collision mitigation by braking. In the introduction a rough estimation of the eﬀect of a CMbB system, which reduces the collision speed in rear-end collisions, is calculated. For a system with a uniformly distributed collision speed reduction, between 0 and 15 km/h, AIS2+ classiﬁed in rear-end collisions are estimated to be reduced by 16 %. This corresponds to a reduction of all AIS2+ injuries of approximately 5 %. Part II deals with target tracking. Automotive tracking sensors are discussed in Chapter 3. In Chapter 4 tracking theory is reviewed. A speciﬁc tracking algorithm (Algorithm 4.2) which is used in the ﬁrst demonstrator vehicle is presented. A comparison of diﬀerent tracking models in a speciﬁc automotive scenario is presented in Section 4.7. In Part III, CA decision making is discussed. Diﬀerent decision functions are discussed in Chapter 5. A decision function describes the deterministic part of the CA system’s decision i.e., how is the threat, posed by another object, quantiﬁed. One common decision function quantiﬁes the threat from another vehicle by the longitudinal acceleration required to avoid collision (5.36). In situations where the object stops before a collision occurs (5.36) gives incorrect values. To remedy this a decision function (5.39) that assumes that any vehicle remains stationary once it reaches zero velocity is proposed. In Section 5.3.7 a decision function that accounts for the actuator dynamics is proposed. According to Figure 5.6 neglecting the brake system dynamics corresponds to delaying the CA decision by up to 140 ms, assuming a maximum deceleration of 9.82 m/s2 and a brake system rise time of 300 ms. In Section 5.3.10 it is shown how a general two dimensional acceleration vector to avoid a collision can be found. A comparison between the general acceleration and acceleration only in the longitudinal or lateral direction (5.58) for a head-on collision is shown in Figure 5.8. The maximum diﬀerence is achieved at 60 km/h where an avoidance maneuver according to the general two dimensional acceleration can be performed 160 ms later and still avoid the collision. 149 150 Chapter 10 Summary Multiple object collision avoidance decision making is discussed in Chapter 6. The approach presented is to search for a control input that avoids conﬂicts. Three alternatives for how to ﬁnd such an control is proposed: • The ﬁrst alternative (6.9) assumes that the acceleration is kept constant during the entire avoidance maneuver and therefor only explore a very limited set of all possible avoidance manoeuvres. A CA system using this approach is implemented in the second demonstrator vehicle. Performance for two test runs with demonstrator 2, in a head-on collision at 70 km/h with three stationary obstacles was presented in Section 8.4. The multiple object algorithm reduces the collision speed to 30.9 km/h and 35.5 km/h respectively for the two test runs. The single object algorithm reduces the collision speed to 54.7 km/h and 50.8 km/h. • The second alternative described in Algorithm 6.2 also ﬁnds escape paths using constant acceleration but if no escape path is found at t = t0 the system is simulated until an escape path is found or a collision occurs. • The third alternative is to perform a search over all possible maneuvers. To do this the set of possible maneuvers has to be discretized. Theorem 6.1 and 6.2 give a local and global bound on how well an arbitrary escape path can be approximated depending on how ﬁne the discretization is made. A comparison of the three alternative multiple object decision algorithms and a single object algorithm is presented in Section 9.1. Compared with a single object algorithm the three multiple object algorithms (MOCC, MOHC, and MOES) are found to alarm 0.47 s, 0.41 s, and 0.26 s earlier than the single object algorithm, respectively. To deal with uncertainty in the CA decision making process a statistical approach is proposed in Section 7. A general framework to deal with nonlinear decision function and non-Gaussian noise for CA decision making based on Monte Carlo integration is presented. The evaluation of such a system is presented in Section 9.2. Another alternative to statistical decision making is to simulate the motion of each object and determine the probability of a collision for each (simulated) future time instant. The ﬁrst demonstrator vehicle uses this method. In collision tests, in a head-on scenario, this system reduced the collision speed up to 15 km/h. Part V Appendices 151 Appendix A Coordinate Systems In this thesis, Cartesian coordinates according to Figure A.1 will be used. For some models global (ground ﬁxed) coordinates are used. This is indicated by capital letters P , V , and A for position velocity and acceleration respectively. Host vehicle ﬁxed coordinates are denoted by small letters p, v, and a. The origin of the global coordinate system is in the front bumper of the host vehicle at time t0 as indicated in Figure A.1. Thus, at t = t0 the x-direction denotes the host vehicles longitudinal direction and the y-direction is in the host vehicles lateral direction, for both the global and the local coordinate system. As time evolves the local and global coordinate systems are no longer the same (see Figure A.2), unless the host vehicle is stationary. The heading direction of the host vehicle, in the global coordinate system, is denoted ψ and turn rate by ω. Furthermore, relative coordinates is denoted by P̃ = P obj − P host , Ṽ = V obj − V host , and Ã = Aobj − Ahost . Bold font P, V, and A is used to indicate vector valued position, velocity and acceleration. A summary of the coordinate system notations is given in Table A.1 Table A.1: Coordinate systems. Coordinate system Symbols Global position, velocity and acceleration Host ﬁxed position, velocity and acceleration P, V , A p, v, a Relative position, velocity and acceleration P̃ , Ṽ , Ã Vector valued position, velocity and acceleration P, V, A 153 154 Appendix A Coordinate Systems wobj p = px T py x y H whost Figure A.1: Cartesian coordinate system, the x-direction is aligned with the host vehicle’s heading direction. Note that the host is represented by a center point in the very front of the vehicle whereas the obstacle is represented by a center point on the surface closest to the host vehicle (which is what most of the sensors measure). t = t 1 > t0 y H x p(t1 ) = px (t1 ) P = Px T py (t1 ) T Py = p(t0 ) x t = t0 y Figure A.2: aligned. As time evolves the global and the local coordinate systems are no longer Appendix B Notation Symbols A Matrix describing a linear dynamical system aa B Maximum possible acceleration Matrix describing how the control input aﬀects the states Dhead Dbrake et Headway distance to vehicle in front Distance needed to avoid a stationary obstacle by means of braking Distance needed to avoid a stationary obstacle by means of steering Measurement noise ê D Estimate of e Residual vector, innovations f (·) State equation transition mapping (discrete-time) ϕ G(i) Azimuth angle Gate belonging to track i g(·) φ Collision avoidance decision function Host vehicle heading angle Φ(·) h(·) Gaussian probability distribution function Measurement relation H Kt Linearized measurement relation Kalman gain matrix at time t J Maximum number of tracks in the tracking system Dsteer 155 Notation 156 lhost Length of the host vehicle L(.) Lambert’s W function, the solution w = L(x) to wew = x. M Maximum number of measured objects from one tracking sensor Mass of vehicle i mi n N Prob{·} Number of objects Number of grid points in a PMF or particles, samples in a PF Probability p(·) pet Probability density function Measurement noise probability density pνt px 0 Process noise probability density Initial state probability density px , py p x and y positions in a host ﬁxed coordinate system T Position vector px py P Covariance matrix r r(·) R Radial distance, range to another object Decision strategy function, mapping all states to a CA action Measurement noise covariance matrix R Π The set of real numbers Covariance matrix for p σ Q Standard deviation Process noise covariance matrix T Sample period u νt Control input Process noise Yt The cumulative set of measurements up to and including time t, {y1 , y2 , · · · , yt } ω ∆V i Turn rate Speed change of vehicle i during a collision v01 v1 Speed of vehicle 1 before collision Speed of vehicle 1 after collision v host v obj Speed of the host vehicle Observed object’s speed Notation whost 157 Width of the host vehicle obj Width of an observed object State vector at time t xt x1 , . . . , xn (i) States of the ith object in an MTT system State variables x = (x1 · · · xn )T X State vector State vector for own vehicle and all observed objects X = [xhost xobj1 ...xobjn ] xt−T,i Sample state variable drawn from a known prior distribution xdes 1 xref 1 Desired value of x1 Reference value of x1 yt Measurement at time t w xt Acronyms ABS Anti blockier systeme/antilock braking system ATC CA Air traﬃc control Collision avoidance ccd CM Charge coupled device Collision mitigation CMbB CW Collision mitigation by braking Collision warning CPU Central processing unit DOF EHB Degrees of freedom Electro hydraulic brake system EKF FARS Extended Kalman ﬁlter Fatality analysis reporting system FCA FCM Forward collision avoidance Forward collision mitigation FCW FM-CW Forward collision warning Frequency modulated continuous wave FM-Pulse FOV Frequency modulated pulsed wave Field of view FSK GES Frequency shift keying General estimates system Notation 158 GPS Global Positioning System i.i.d. IMM Independent identically distributed Interacting multiple model IR KF Infrared Kalman Filter Lidar LMS Light detection and ranging Least mean square LS MAP Least squares Maximum a posteriori MC Monte Carlo MHT MHz Multiple hypotheses tracking Mega hertz ML MLE Maximum likelihood Maximum likelihood estimate MMSE MTT Minimum mean square error Multiple target tracking NCAP NN New car assessment program Nearest neighbor PDF PF Probability density function Particle ﬁlter PMF Radar Point mass ﬁlter Radio detection and ranging RCS RMSE Radar cross section Root mean square error TTC Time-to-collision THW Time-headway Appendix C Safety Systems Introduced in Volvo Cars 1920-1960 1927 1944 1944 1954 1956 1956 1957 1958 1959 - Safety glass Full safety cage Laminated windshield Front defroster Windshield washers Padded dashboard Front seat safety belt anchorage points Padded door handles Three-point front safety belts 1960-1970 1965 1966 1966 1966 1967 1968 1969 1969 - Power brakes with pressure limiting valves Dual circuit triangular split brake system Front and rear crumple zones Roll-over roof protection bar Two-point rear safety belts Front seat head restraints Three-point inertia reel front seat belts Heated rear window 1970-1980 1972 1972 1972 1972 - Three-point inertia reel rear seat belts Child safety seat Child-proof rear door locks Audio visual seat belt reminder 159 160 1972 1972 1973 1973 1974 1974 1974 1978 1979 Appendix C - Safety Systems Introduced in Volvo Cars Hazard warning lamps Rear head restraints Energy absorbing bumpers Headlamp wiper-washers Isolated fuel tank Multi stage impact-absorbing steering column Stepped bore master cylinder braking system Front and rear fog lights Wide angle rear view mirrors 1980-1990 1982 1982 1984 1985 1986 1987 1987 - Anti-submarining seat protection Door warning lamps ABS antilock braking system ETC electronic traction control High level brake lamp Automatic safety belt tensioner SRS driver side airbag 1990-2000 1990 1990 1991 1991 1992 1994 1998 1998 1998 1998 1998 1999 1999 1999 - Three-point inertia reel rear center seat belt Integrated rear child seat SIPS side impact protection system Automatic height adjusting seat belts SRS passenger side airbag SIPS front seat side impact airbags ROPS roll-over protection system Boron steel windshield A-Pillars SIPS II front seat side impact airbags WHIPS whiplash protection system EBD electronic brake force distribution DSTC dynamic stability and traction control IC inﬂatable curtain airbags HID xenon headlights 2000- 2001 2002 2002 2002 - EBA electronic brake assist RSC roll stability control Boron steel reinforced roof structure Lower compatibility cross-member Bibliography [1] A review of automotive radar systems – devices and regulatory framework. Technical Report SP 4/01, Australian Communication Authority, April 2001. URL http://www.atnf.CSIRO.AU/SKA/intmit/autoradar.doc. [2] Automotive collision avoidance system ﬁeld operational test. Technical Report DOT HS 809 453, NHTSA, May 2002. [3] U. Adler. Automotive Handbook. Robert Bosch GmbH, 5th edition, 2000. ISBN 0-7680-0669-4. [4] V. Aidala and S. Hammel. Utilization of modiﬁed polar coordinates for bearing-only tracking. IEEE Transactions on Automatic Control, 1983. [5] B.D.O. Anderson and J. B. Moore. Optimal Filtering. Prentice Hall, Englewood Cliﬀs, NJ, 1979. [6] E. Bakker, L. Lidner, and H. B. Pacejka. A new tire model with an application in vehicle dynamics studies. In SAE technical paper 890087, Detroit, MI, USA, 1989. Society of Automotive Engineers. [7] Y. Bar-Shalom and T. Fortmann. Tracking and Data Association, volume 179 of Mathematics in Science and Engineering. Academic Press, 1988. [8] Y. Bar-Shalom and X. R. Li. Estimation and Tracking: Principles, Techniques, and Software. Artech House, 1993. [9] Yaakov Bar-Shalom, editor. Multitarget-Multisensor Tracking: Applications and Advances. Artech House, 1992. ISBN 0-89006-517-9. [10] P. Barber and N. Clarke. Advanced collision warning systems. Industrial Automation and Control: Applications in the Automotive Industry, April 1998. [11] N. Bergman. Recursive Bayesian Estimation: Navigation and Tracking Applications. Linköping Studies in Science and Technology. Dissertations No. 579, Linköping University, Linköping, Sweden, 1999. 161 162 Bibliography [12] N. Bergman and A. Doucet. Markov Chain Monte Carlo Data Association for Target Tracking. In Proceedings IEEE Conference on Acoustics, Speech and Signal Processing, 2000. [13] M. Bertozzi, A. Broggi, and A. Fascioli. A stereo vision system for real-time automotive obstacle detection. In Proceedings of the 3:d IEEE International Conference on Image Processing, Lausanne, CH, September 1996. [14] M. Betke, E. Haritaoglu, and L. S. Davis. Multiple vehicle detection and tracking in hard real time. In Symposium on Intelligent Vehicles, pp. 351356, September 1996. [15] S. S. Blackman. Multiple-Target Tracking with Radar Applications. Artech House, Norwood, MA, 1986. [16] S. S. Blackman and R. Popoli. Design and Analysis of Modern Tracking Systems. Artech House, 1999. [17] A. Bole and W. Dineley. Radar and ARPA Manual. Butterworth-Heineman, Oxford, Great Britain, 1992. ISBN 0750608188. [18] J. Borenstein, B. Everett, and L. Feng. Navigating Mobile Robots: Systems and Techniques. A. K. Peters, Ltd., 1996. ISBN 1-56881-058-X. [19] J. Breen and R. Condon. Possibilities for future developments in primary safety requirements. In IMechE, volume 210, 1996. [20] A.L. Burgett, A. Carter, R.J. Miller, W.G. Najim, and D.L. Smith. A collision warning algorithm for rear-end collisions. In 16th International Techical Conference on Enhanced Safety of Vehicles, May 1998. [21] J. Castellanos and J. Tardos. Mobile Robot Localization and Map Building: A Multisensor Fusion Approach. Kluwer Academic Publishers, 2000. ISBN 0792377893. [22] CDS. Crashworthiness data system, 2005. URL http://www-nrd.nhtsa. dot.gov/departments/nrd-30/ncsa/NASS.html. [23] E. Dickmanns. Computer vision and highway automation. Vehicle System Dynamics, 31:325–343, 1999. [24] A. Doi, T. Butsuen, T. Niibe, T. Takagi, Y. Yamamoto, and H. Seni. Development of a rear-end collision avoidance system with automatic brake control. In JSAE Review, volume 15, pages 335–340. SAE, 1994. [25] A. Doucet, N. de Freitas, and N. Gordon, editors. Sequential Monte Carlo Methods in Practice. Springer Verlag, 2001. [26] C. Eberhard, D. Moﬀa, J. Philip, and W. R. Swihart. Collision warning. Automotive Engineering (Warrendale, Pennsylvania), 105(3):86–90, March 1997. Bibliography 163 [27] A. Eidehall. An Automotive Lane Guidance System. Linköping Studies in Science and Technology. Licentiate Thesis No. 1122, Linköping University, Linköping, Sweden, October 2004. [28] FARS. Fatality analysis reporting system, 2002. URL http://www-nrd. nhtsa.dot.gov/departments/nrd-30/ncsa/FARS.html. [29] J. Folkesson and H. I. Christensen. Graphical slam - a self-correcting map. In ICRA-04, New Orleans, April 2004. IEEE. [30] J. Folkesson and H. I. Christensen. Robust SLAM. In IFAC – 5th IAV-2004, Lisboa, PT, July 5-7 2004. [31] Folksam. Säkerhetsrankning, 2002. forskning/index.htm. URL http://www.folksam.se/ [32] Folksam. Hur säker är bilen?, 2005. URL http://www.folksam.se. [33] Insurance Institute for Highway Safety, 2002. URL http://www.hwysafety. org/. [34] A. Fujimori and S. Tani. A navigation of mobile robot with collision avoidance for moving obstacles. In IEEE International Conference on Industrial Technology, 2002, 2002. [35] Williams G., Lagace G., and Woodﬁn A. A collision avoidance controller for autonomous underwater vehicles. In Proceedings of the Symposium on Autonomous Underwater Vehicle Technology, pages 206–212, 1990. [36] GES. General estimates system, 2002. URL http://www-nrd.nhtsa.dot. gov/departments/nrd-30/ncsa/GES.html. [37] T. D. Gillespie. Fundamentals of Vehicle Dynamics. Society of Automotive Engineers, Detroit, MI, USA, February 1992. ISBN 1-56091-199-9. [38] H. Glawing. Measurement data selection and association in a collision mitigation system. Master’s Thesis LiTH-ISY-EX-3213-2002, Department of Electrical Engineering. Linköping University, Linköping, Sweden, 2002. In English. [39] N. J. Gordon, D. J. Salmond, and A.F.M. Smith. A novel approach to nonlinear/non-Gaussian Bayesian state estimation. In IEE Proceedings on Radar and Signal Processing, volume 140, pages 107–113, 1993. [40] S. Gorski. The last-minute manoeuvre. European Journal of Navigation, pages 40–48, 2004. 3. [41] H.P. Groll and J. Detlefsen. History of automotive anticollision radars and ﬁnal experimental results of a mm-wave car radar developed by the Technical University of Munich . IEEE Aerospace and Electronics Systems Magazine, 12(8):15–19, 1997. 164 Bibliography [42] J. Guldner, V. Utkin, and R. Bauer. Mobile robots in complex environments: a three-layered hierarchical path control system. In IEEE/RSJ/GI International Conference on Intelligent Robots and Systems ’94, volume 3, pages 1891–1898, 1994. [43] J. Guldner, V. Utkin, H. Hashimoto, and F. Harashima. Obstacle avoidance in rn based on artiﬁcial harmonic potential ﬁelds. In IEEE International Conference on Robotics and Automation, volume 3, pages 3051–3056, 1995. [44] F. Gustafsson. Slip-based estimation of tire – road friction. Automatica, 33 (6):1087–1099, 1997. [45] F. Gustafsson. Estimation and change detection of tire – road friction using the wheel slip. IEEE Control System Magazine, 18(4):42–49, 1998. [46] F. Gustafsson. Adaptive Filtering and Change Detection. John Wiley & Sons Ltd, 2000. ISBN 0-471-49287-6. [47] F. Gustafsson, M. Drevö, U. Forssell, M. Löfgren, N. Persson, and H. Quicklund. Virtual sensors of tire pressure and road friction. In SAE technical paper 2001-01-0796, Detroit, MI, USA, March 2001. Society of Automotive Engineers 2001 World Congress. [48] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, and P-J Nordlund. Particle ﬁlters for positioning, navigation and tracking. IEEE Transactions on Signal Processing, 50:425–437, February 2002. [49] D.L. Hall and J. Llinas, editors. Handbook of multisensor data fusion. CRC Press, 2001. [50] U. Handmann, T. Kalinke, C. Tzomakas, M. Werner, and W. v. Seelen. Computer vision for driver assistance systems. In Proceedings of SPIE, volume 3364, Orlando, USA, 1998. [51] K. Honda, H. Higashida, R. Nakamura, M. Hitotsuya, and N. Sima. Fusion sensor for an assist system for low speed in traﬃc congestion by using millimeter-wave radar and image recognition sensor. In Intelligent Vehicle Initiative (IVI). Technology and Navigation Systems, volume SAE SP-1593, SAE Technical Paper 2001-01-0800, Detroit, MI, USA, 2001. Society of Automotive Engineers 2001 World Congress. [52] J. Jansson. Tracking and Decision Making for Automotive Collision Avoidance. Linköping Studies in Science and Technology. Licentiate Thesis No. 965, Linköping University, Linköping, Sweden, September 2002. [53] J. Jansson. Advanced Microsystems for Automotive Applications 2004, chapter Dealing with Uncertainty in Automotive Collision Avoidance, pages 165– 180. Springer, Berlin, Germany, March 2004. ISBN 3-540-20586-1. Bibliography 165 [54] J. Jansson and W. Birk. An alternative brake criterion for collision avoidance. Submitted to the European patent authority, patent pending, 2005. [55] J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. Accelerator actuated emergency braking system. U.S. Patent No. 6655749, 2001. [56] J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. Collision mitigation by braking system. U.S. Patent No. 6607255, 2001. [57] J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. Radar-guided collision mitigation system with a passive safety feature. U.S. Patent No. 6617172, 2001. [58] J. Jansson, J. Bond, J. Engelman, J. Johansson, N. Tarabishy, and L. Tellis. System for exerting an ampliﬁed braking force. U.S. Patent No. 6659572, 2001. [59] J. Jansson and J. Ekmark. A method and a computer readable storage device for estimating tire-to-road friction. U.S. Application No. 10/ 858896, patent pending, 2004. [60] J. Jansson and J. Ekmark. A multiple object collision avoidance algorithm. Submitted to the European patent authority, patent pending, 2005. [61] J. Jansson, J. Ekmark, and F. Gustafsson. Decision making for collision avoidance systems. In SAE technical paper 2002-01-0403, volume SP-1662, Detroit, MI, USA, March 2002. Society of Automotive Engineers 2002 World Congress. [62] J. Jansson and F. Gustafsson. A probabilistic approach to collision risk estimation for passenger vehicles. In Proceedings of the 15th International Federation of Automatic Control World Congress, pages T–Th–M05:Model– based fault diagnosis of automotive systems/Area code 8b : Automotive Control, Barcelona, Spain, July 2002. [63] J. Jansson and F. Gustafsson. A framework and overview of collision avoidance decision making. Submitted to International Federation of Automatic Control Journal: Automatica, 2005. [64] J. Jansson and F. Gustafsson. Multiple object collision avoidance decision making. Submitted to International Federation of Automatic Control Journal: Control engineering practice, 2005. [65] J. Jansson and J. Johansson. A probabilistic framework for decision making in collision avoidance. Swedish/German/UK Patent No. 273930, 2001. 166 Bibliography [66] J. Jansson, R. Karlsson, and F. Gustafsson. Model-based statistical tracking and decision making for collision avoidance application. Technical report LiTH-ISY-R-2492, Department of Electrical Engineering Linköping University, S-581 83 Linköping, Sweden, 2003. [67] A. H. Jazwinski. Stochastic Processes and Filtering Theory, volume 64 of Mathematics in Science and Engineering. Academic Press, 1970. [68] JNCAP. New car assessment program, 2002. URL http://www.jafmate. co.jp/anzen/ncap/. [69] H. Joksch. Velocity change and fatality risk in a crash – a rule of thumb. Accident Analysis and Prevention, 25(25(1)), February 1993. [70] T. Kailath, A.H. Sayed, and B. Hassibi. Linear Estimation. Information and System Sciences. Prentice Hall, Upper Saddle River, New Jersey, 2000. [71] R. E. Kalman. A new approach to linear ﬁltering and prediction problems. Transactions of the AMSE–Journal of Basic Engineering, 82:35–45, 1960. [72] R. Karlsson. Simulation Based Methods for Target Tracking. Linköping Studies in Science and Technology. Licentiate Thesis No. 930, Linköping University, Linköping, Sweden, February 2002. [73] R. Karlsson, J. Jansson, and F. Gustafsson. Model-based statistical tracking and decision making for collision avoidance application. In Proceedings of the American Control Conference, Boston, MA, USA, July 2004. [74] L. Kavraki and J. Latombe. Randomized preprocessing of conﬁguration space for fast path planning. In IEEE international conference on robotics and automation, volume 3, pages 2138–2145, San Diego, CA, USA, May 1994. [75] H. Kawazoe, T. Murakami, O. Sadano, K. Suda, and H. Ono. Development of a lane-keeping assistance system. In Intelligent Vehicle Initiative (IVI). Technology and Navigation Systems, volume SAE SP-1593, Detroit, MI, USA, March 2001. Society of Automotive Engineers 2001 World Congress. SAE Technical Paper 2001-01-0797. [76] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots. The International Journal of Robotic Research, 5, 1986. [77] R. Kiefer, D. LeBlanc, M. Palmer, J. Salinger, R. Deering, and M. Shulman. Development and validation of functional deﬁnitions and evaluation procedures for collision warning. Technical report, U.S. Department of Transportation, 1999. [78] A. Kivrikis and J. Tjernström. Testing and veriﬁcation of multiple object collision avoidance algorithms. Masters Thesis LiTH-ISY-EX-3570, Department of Electrical Engineering, Linköping University, 2004. Bibliography 167 [79] S. C. Kramer and H. W. Sorenson. Bayesian parameter estimation. IEEE Transactions on Automatic Control, 33:217–222, 1988. [80] J. Krozel, M. Peters, and G. Hunter. Conﬂict detection and resolution for future air transportation management. Technical report, NASA, 1997. [81] J. Kuchar and L. Yang. A review of conﬂict detection and resolution modeling methods. IEEE Transactions on Intelligent Transportation Systems, 1(4): 179–189, December 2000. [82] L. Küpper and J. Schug. Active night vision systems. In Society of Automotive Engineers 2002 World Congress, SAE technical paper 2002-01-0013, Detroit, MI, USA, March 2002. [83] W. Lamb. The calculation of marine collision risk. Journal of Navigation, 38 (3):365–374, 1985. [84] J. Latombe. Robot Motion Planning. Kluwer Academic Publisher, 1991. [85] D. J. LeBlanc, R. J. Kiefer, R. K. Deering, M. A. Schulman, M. D. Palmer, and J. Salinger. Forward collision warning: Preliminary requirements for crash alert timing. In Intelligent Vehicle Initiative (Ivi) Technology and Navigation Systems, volume SP-1593, SAE Technical Paper 2001-01-0462, Detroit, MI, USA, March 2001. Society of Automotive Engineers 2001 World Congress. [86] X. R. Li and V. P. Jilkov. A survey of maneuvering target tracking–part III: Measurement models. In Proceedings of SPIE Conference on signal and data processing of small targets, volume 4473, pages 423–446, July 2001. [87] X. R. Li and V.P. Jilkov. A survey of maneuvering target tracking: Dynamics models. In Proceedings of SPIE Conference on signal and data processing of small targets, volume 4048, pages 212–235, April 2000. [88] L. Ljung. System Identiﬁcation, Theory for the User. Prentice Hall, Englewood Cliﬀs, New Jersey, second edition, 1999. [89] R. W. Lorenz, J. Coleman, and S. Mehring. Lane Departure Warning for CVO in the USA. In International Truck and Bus Meeting and Exposition, SAE Technical Paper 982779, Indianapolis, IN, USA, November 1998. Society of Automotive Engineers. [90] N. Martinelli and S. A. Boulanger. Cadillac deville thermal imaging night vision system. In SAE technical paper 2002-01-0323, Detroit, MI, USA, March 2002. Society of Automotive Engineers 2002 World Congress. [91] S. Maskell and N. Gordon. A tutorial on particle ﬁlter for on-line nonlinear/non-Gaussian Bayesian tracking. In IEE Workshop. Target Tracking: Algorithms and Applications, Enschede, The Netherlands, October 2001. 168 Bibliography [92] S. Masoud and A. Masoud. Constrained motion control using vector potential ﬁelds. IEEE Transactions on Systems, Man, and Cybernetics, 30:251–272, 2000. [93] S. Masoud and A. Masoud. Motion planning in the presence of directional and regional avoidance constraints using nonlinear anisotropic, harmonic potential ﬁelds: a physical metaphor. IEEE Transactions on Systems, Man, and Cybernetics, 32:705–723, 2002. [94] P. Maybeck, W. Worsely, and P. Flynn. Investigation of constant turn-rate dynamics for airborne vehicle tracking. In NAECON, pages 896–903, 1982. [95] J. Minguez, L. Montano, and O. Khatib. Reactive collision avoidance for navigation with dynamic constraints. In IEEE/RSJ International Conference on Intelligent Robots and System, volume 1, pages 588–594, 2002. [96] J. Miura, H. Uozumi, and Y. Shirai. Mobile robot motion planning considering the motion of moving obstacles. In IEEE Int. Confrence on Systems, Man, and Cybernetics, pages 692–698, Tokyo, Japan, October 1999. [97] Y. Nam, B. Lee, and N. Ko. An analytic approach to moving obstacle avoidance using an artiﬁcial potential ﬁeld. In International Conference on Intelligent Robots and Systems, volume 2, pages 482–487, 1995. [98] Y. Nam, B. Lee, and N. Ko. A view-time based potential ﬁeld method for moving obstacle avoidance. In Proceedings of the 34th SICE Annual Conference, pages 1463–1468, 1995. [99] NASS. National automotive sampling system, 2005. URL http://www-nrd. nhtsa.dot.gov/departments/nrd-30/ncsa/NASS.html. [100] Australian NCAP. New car assessment program, 2002. URL http://www. nrma.com.au/. [101] Euro NCAP. New car assessment program, 2002. euroncap.com/. URL http://www. [102] U.S. NCAP. New car assessment program, 2002. URL http://www.nhtsa. dot.gov/cars/testing/ncap/. [103] L. Nielsen and U. Kiencke. Automotive Control System. Springer, 2000. ISBN 3-540-66922-1. [104] M. Nolan. Fundamentals of Air Traﬃc Control. Brooks Cole, 3rd edition, 1998. ISBN 0534393888. [105] J. O’Day and J. Flora. Alternative measures of restraint system eﬀectiveness: interaction with crash severity factors. In SAE technical paper 820798, Detroit, MI, USA, 1982. Society of Automotive Engineers. Bibliography 169 [106] N. Persson, M. Drevö, and F. Gustafsson. Indirect tire pressure monitoring using sensor fusion. In SAE technical paper 2002-01-1250, Detroit, MI, USA, March 2002. Society of Automotive Engineers 2002 World Congress. [107] N. Persson, F. Gustafsson, U. Forssell, and S. Ahlqvist. Low tire pressure warning using sensor fusion. In In proceedings of ATTCE 2002, 2001. 01 ATT-272. [108] M. Prandini, J. Hu, J. Lygeros, and S. Sastry. A probabilistic approach to aircraft conﬂict detection. IEEE Transactions on Intelligent Transportation Systems, 1:199–220, 2000. [109] M. Prandini, J. Lygeros, A. Nilim, and S. Sastry. A probabilistic framework for aircraft conﬂict detection. In Proceedings of 38th CDC, Phoenix, AZ,, Phoenix, AZ, USA, December 1999. [110] J. Reif and M. Sharir. Motion planning in the presence of moving obstacles. In 26th Annual IEEE Symposium on Foundations of Computer Science,, 1985. [111] J.H. Reif. Complexity of the mover’s problem and generalizations. In 20th Annual IEEE Symposium on Foundations of Computer Science, pages 421– 427, San Juan, Puerto Rico, October 1979. [112] A. Samman, L. Rimai, J. McBridge, R. Carter, W. Weber, C. Gmachl, F. Capasso, A. Hutchinson, D. Sivco, and A. Cho. Potential use of near, mid and far infrared laser diodes in automotive LIDAR applications. In Proceedings of the 52nd. IEEE conference on Vehicular Technology Conference, Boston, MA, USA, September 2000. [113] E. D. R. Shearman, E. G. Hoare, and A. Hutton. Trials of automotive radar and lidar in road spray. Technical report, IEE, 1998. [114] M. Skolnik. Introduction To Radar Systems. McGraw-Hill, 1983. ISBN 0-07-066572-9. [115] H. W. Sorenson and A. R. Stubberud. Nonlinear ﬁltering by approximation of the a posterior density. International Journal of Control, 8(1):33–51, 1968. [116] J. Sporrong and P. Uhlin. Mcas- a mid air collision avoidance system in gripen. Master Thesis LiTH-ISY-EX-3031, Dept of Elec. Eng. Linköping University, S-581 83 Linköping, Sweden, 2001. [117] M. Strandberg. Robot Path Planning: An Object-Oriented Approach. Dissertations No. TRITA-S3-REG-0401, Kungliga Tekniska Högskolan, October 2004. [118] A. Takahashi and N. Asanuma. Improvement of visibility for vulnerable parties in traﬃc accidents. In Proceedings of the 17th International Conference on the Enhanced Safety of Vehicles, Amsterdam, The Netherlands, June 2001. NHTSA. 170 Bibliography [119] M. Tamura, H. Inoue, T. Wtanabe, and N. Maruko. Research on a brake assist system with a preview function. In SAE Technical Paper 2001-01-0357, Detroit, MI, USA, March 2001. Society of Automotive Engineers 2001 World Congress. [120] S. Tanaka, T. Nakagawa, M. Akasu, Y. Fukushima, and W. Bracken. Development of a compact scan laser radar. In SAE Technical Paper 1999-01-1044, Detroit, MI, USA, March 1999. Society of Automotive Engineers 1999 World Congress. [121] B. Ulmer. VITA II - Active Collision Avoidance in Real Traﬃc. In Proceedings of the Intelligent Vehicles ’94 Symposium, 1994. [122] P. Vadakkepat, C. Kay, and M-L. Wang. Evolutionary artiﬁcial potential ﬁelds and their application in real time robot path planning. In Proceedings of the 2000 Congress on Evolutionary Computation, volume 1, pages 256–263, 2000. [123] R. van der Merwe, A. Doucet, N. de Freitas, and E. Wan. The unscented particle ﬁlter. Technical Report CUED/F-INFENG/TR 380, Cambridge University Department of Engineering, Cambridge, UK, May 2000. [124] K. Vogel. Modeling Driver Behaviour - A Control Theory Based Approach. Linköping Studies in Science and Technology. Dissertations No. 751, Linköping University, Linköping, Sweden, 2002. [125] K. Wallace and C. Rockwell. Conﬂict detection and alerting for separation assurance systems. In 18th Digital Avionics Systems Conf., 1999. [126] C. J. Wiacek and W. G. Najm. Driver/vehicle characteristics in rear-end precrash scenarios based on the general estimates system (ges). In SAE technical paper 1999-01-0817, Detroit, MI, USA, March 1999. Society of Automotive Engineers 1999 World Congress. [127] G. R. Widmann, M. K. Daniels, L. Hamilton, L. Humm, B. Riley, J. K. Schiﬀmann, D. E. Schnelker, and W. H. Wishon. Comparison of lidar-based and radar-based adaptive cruise control systems. In SAE Technical Paper 2000-01-0345, SAE Technical Paper 2000-01-0345, Detroit, MI, USA, March 2000. Society of Automotive Engineers 2000 World Congress. [128] J.Y. Wong. Theory of Ground Vehicles. John Wiley and Sons, 1993. [129] M. Yamamoto, M. Shimada, and A. Mohri. Online navigation of mobile robot under the existence of dynamically moving multiple obstacles. In Proceedings of the IEEE International Symposium on Assembly and Task Planning, pages 13 – 18, 28–29 May 2001. Bibliography 171 [130] Y. Yamamura, M. Tabe, and T. Murakami M. Kanehira. Development of an adaptive cruise control system with stop-and-go capability. In SAE Technical Paper 2001-01-0798, Detroit, MI, USA, March 2001. Society of Automotive Engineers 2001 World Congress. [131] P.L. Zandor, S.A. Krawchuk, and R.B. Voas. Final Report - Automotive Collision Avoidance Systems (ACAS) Program. Technical Report DOT HS 809 080, U.S. Department of Transportation, 2000. [132] K. Zeghal. A review of diﬀerent approaches based on force ﬁelds for airborne conﬂict resolution. In Proc. AIAA Guidance, Navigation, and Control Conf., 1998. [133] B. Zhu. Potential eﬀects on accidents from forward collision warning/avoidance system. Master’s Thesis LiTH-ITN-EX-150-SE, Linköping University, Linköping, Sweden, 2001. PhD Dissertations, Division of Automatic Control, Linköping University M. Millnert: Identiﬁcation and control of systems subject to abrupt changes. Thesis no. 82, 1982. ISBN 91-7372-542-0. A.J.M. van Overbeek: On-line structure selection for the identiﬁcation of multivariable systems. Thesis no. 86, 1982. ISBN 91-7372-586-2. B. Bengtsson: On some control problems for queues. Thesis no. 87, 1982. ISBN 91-7372-593-5. S. Ljung: Fast algorithms for integral equations and least squares identiﬁcation problems. Thesis no. 93, 1983. ISBN 91-7372-641-9. H. Jonson: A Newton method for solving non-linear optimal control problems with general constraints. Thesis no. 104, 1983. ISBN 91-7372-718-0. E. Trulsson: Adaptive control based on explicit criterion minimization. Thesis no. 106, 1983. ISBN 91-7372-728-8. K. Nordström: Uncertainty, robustness and sensitivity reduction in the design of single input control systems. Thesis no. 162, 1987. ISBN 91-7870-170-8. B. Wahlberg: On the identiﬁcation and approximation of linear systems. Thesis no. 163, 1987. ISBN 91-7870-175-9. S. Gunnarsson: Frequency domain aspects of modeling and control in adaptive systems. Thesis no. 194, 1988. ISBN 91-7870-380-8. A. Isaksson: On system identiﬁcation in one and two dimensions with signal processing applications. Thesis no. 196, 1988. ISBN 91-7870-383-2. M. Viberg: Subspace ﬁtting concepts in sensor array processing. Thesis no. 217, 1989. ISBN 91-7870-529-0. K. Forsman: Constructive commutative algebra in nonlinear control theory. Thesis no. 261, 1991. ISBN 91-7870-827-3. F. Gustafsson: Estimation of discrete parameters in linear systems. Thesis no. 271, 1992. ISBN 91-7870-876-1. P. Nagy: Tools for knowledge-based signal processing with applications to system identiﬁcation. Thesis no. 280, 1992. ISBN 91-7870-962-8. T. Svensson: Mathematical tools and software for analysis and design of nonlinear control systems. Thesis no. 285, 1992. ISBN 91-7870-989-X. S. Andersson: On dimension reduction in sensor array signal processing. Thesis no. 290, 1992. ISBN 91-7871-015-4. H. Hjalmarsson: Aspects on incomplete modeling in system identiﬁcation. Thesis no. 298, 1993. ISBN 91-7871-070-7. I. Klein: Automatic synthesis of sequential control schemes. Thesis no. 305, 1993. ISBN 91-7871-090-1. J.-E. Strömberg: A mode switching modelling philosophy. Thesis no. 353, 1994. ISBN 91-7871-430-3. K. Wang Chen: Transformation and symbolic calculations in ﬁltering and control. Thesis no. 361, 1994. ISBN 91-7871-467-2. T. McKelvey: Identiﬁcation of state-space models from time and frequency data. Thesis no. 380, 1995. ISBN 91-7871-531-8. J. Sjöberg: Non-linear system identiﬁcation with neural networks. Thesis no. 381, 1995. ISBN 91-7871-534-2. R. Germundsson: Symbolic systems – theory, computation and applications. Thesis no. 389, 1995. ISBN 91-7871-578-4. P. Pucar: Modeling and segmentation using multiple models. Thesis no. 405, 1995. ISBN 91-7871-627-6. H. Fortell: Algebraic approaches to normal forms and zero dynamics. Thesis no. 407, 1995. ISBN 91-7871-629-2. A. Helmersson: Methods for robust gain scheduling. Thesis no. 406, 1995. ISBN 91-7871-628-4. P. Lindskog: Methods, algorithms and tools for system identiﬁcation based on prior knowledge. Thesis no. 436, 1996. ISBN 91-7871-424-8. J. Gunnarsson: Symbolic methods and tools for discrete event dynamic systems. Thesis no. 477, 1997. ISBN 91-7871-917-8. M. Jirstrand: Constructive methods for inequality constraints in control. Thesis no. 527, 1998. ISBN 91-7219-187-2. U. Forssell: Closed-loop identiﬁcation: Methods, theory, and applications. Thesis no. 566, 1999. ISBN 91-7219-432-4. A. Stenman: Model on demand: Algorithms, analysis and applications. Thesis no. 571, 1999. ISBN 91-7219-450-2. N. Bergman: Recursive Bayesian estimation: Navigation and tracking applications. Thesis no. 579, 1999. ISBN 91-7219-473-1. K. Edström: Switched bond graphs: Simulation and analysis. Thesis no. 586, 1999. ISBN 91-7219-493-6. M. Larsson: Behavioral and structural model based approaches to discrete diagnosis. Thesis no. 608, 1999. ISBN 91-7219-615-5. F. Gunnarsson: Power control in cellular radio systems: Analysis, design and estimation. Thesis no. 623, 2000. ISBN 91-7219-689-0. V. Einarsson: Model checking methods for mode switching systems. Thesis no. 652, 2000. ISBN 91-7219-836-2. M. Norrlöf: Iterative learning control: Analysis, design, and experiments. Thesis no. 653, 2000. ISBN 91-7219-837-0. F. Tjärnström: Variance expressions and model reduction in system identiﬁcation. Thesis no. 730, 2002. ISBN 91-7373-253-2. J. Löfberg: Minimax approaches to robust model predictive control. Thesis no. 812, 2003. ISBN 91-7373-622-8. J. Roll: Local and piecewise aﬃne approaches to system identiﬁcation. Thesis no. 802, 2003. ISBN 91-7373-608-2. J. Elbornsson: Analysis, estimation and compensation of mismatch eﬀects in A/D converters. Thesis no. 811, 2003. ISBN 91-7373-621-X. O. Härkegård: Backstepping and control allocation with applications to ﬂight control. Thesis no. 820, 2003. ISBN 91-7373-647-3. R. Wallin: Optimization algorithms for system analysis and identiﬁcation. Thesis no. 919, 2004. ISBN 91-85297-19-4. D. Lindgren: Projection methods for classiﬁcation and identiﬁcation. Thesis no. 915, 2005. ISBN 91-85297-06-2. R. Karlsson: Particle Filtering for Positioning and Tracking Applications. Thesis no. 924, 2005. ISBN 91-85297-34-8.

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement