Situational Awareness and Road Prediction for Trajectory Control Applications

  • Christian Lundquist
  • Thomas B. Schön
  • Fredrik Gustafsson


Situational awareness is of paramount importance in all advanced driver assistance systems. Situational awareness can be split into the tasks of tracking moving vehicles and mapping stationary objects in the immediate surroundings of the vehicle as it moves. This chapter focuses on the map estimation problem. The map is constructed from sensor measurements from radars, lasers and/or cameras, with support from on-board sensors for compensating for the ego-motion.

Four different types of maps are discussed:
  1. (i)

    Feature-based maps are represented by a set of salient features, such as tree trunks, corners of buildings, lampposts and traffic signs.

  2. (ii)

    Road maps make use of the fact that roads are highly structured, since they are built according to clearly specified road construction standards. This allows relatively simple and powerful models of the road to be employed.

  3. (iii)

    Location-based maps consist of a grid, where the value of each element describes the property of the specific coordinate.

  4. (iv)

    Finally, intensity-based maps can be considered as a continuous version of the location-based maps.

The aim is to provide a self-contained presentation of how these maps can be built from measurements. Real data from Swedish roads are used throughout the chapter to illustrate the methods.


Coordinate Frame Adaptive Cruise Control Probability Hypothesis Density Lane Detection Road Edge 
These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.



The authors would like to thank the SEnsor Fusion for Safety (SEFS) project within the Intelligent Vehicle Safety Systems (IVSS) program, the strategic research center MOVIII, funded by the Swedish Foundation for Strategic Research (SSF) and CADICS, a Linneaus Center funded by be Swedish Research Council for financial support.


  1. Björck Å (1996) Numerical methods for least squares problems. SIAM, PhiladelphiaMATHCrossRefGoogle Scholar
  2. Blackman SS, Popoli R (1999) Design and Analysis of Modern Tracking Systems. Artech House, NorwoodMATHGoogle Scholar
  3. Borenstein J, Koren Y (1991) The vector field histogram-fast obstacle avoidance for mobile robots. IEEE Trans Robot Automation 7(3):278–288CrossRefGoogle Scholar
  4. Buehler M, Iagnemma K, Singh S (eds) (2008) Special issue on the 2007 DARPA urban challenge, Part I-III. J Field Rob 25:8–10Google Scholar
  5. Caveney D (2010) Cooperative vehicular safety applications. IEEE Control Syst Mag 30(4):38–53MathSciNetCrossRefGoogle Scholar
  6. Civera J, Davison A, Montiel J (2008) Inverse depth parametrization for monocular SLAM. IEEE Trans Rob 24(5):932–945CrossRefGoogle Scholar
  7. Daley DJ, Vere-Jones D (2003) An introduction to the theory of point processes, vol 1, 2nd edn, Elementary theory and method. Springer, New YorkMATHGoogle Scholar
  8. Davison AJ, Reid I, Molton N, Strasse O (2007) MonoSLAM: Real-time single camera SLAM. IEEE Trans Patterns Anal Mach Intell 29(6):1052–1067CrossRefGoogle Scholar
  9. Dickmanns E (1988) Dynamic computer vision for mobile robot control. In: Proceedings of the international symposium on industrial robots, SydneyGoogle Scholar
  10. Dickmanns ED (2007) Dynamic vision for perception and control of motion. Springer, LondonGoogle Scholar
  11. Dickmanns ED, Mysliwetz BD (1992) Recursive 3-D road and relative ego-state recognition. IEEE Trans Pattern Anal Mach Intell 14(2):199–213CrossRefGoogle Scholar
  12. Diversi R, Guidorzi R, Soverini U (2005) Kalman filtering in extended noise environments. IEEE Trans Autom Control 50(9):1396–1402MathSciNetCrossRefGoogle Scholar
  13. Duda RO, Hart PE (1972) Use of the Hough transformation to detect lines and curves in pictures. Commun ACM 15(1):11–15CrossRefGoogle Scholar
  14. Eidehall A, Pohl J, Gustafsson F, Ekmark J (2007) Toward autonomous collision avoidance by steering. IEEE Trans Intell Transp Syst 8(1):84–94CrossRefGoogle Scholar
  15. Elfes A (1987) Sonar-based real-world mapping and navigation. IEEE J Robot Automation 3(3):249–265CrossRefGoogle Scholar
  16. Erdinc O, Willett P, Bar-Shalom Y (2009) The bin-occupancy filter and its connection to the PHD filters. IEEE Trans Signal Process 57(11):4232–4246MathSciNetCrossRefGoogle Scholar
  17. Gilholm K, Salmond D (2005) Spatial distribution model for tracking extended objects. IEE Proc Radar Sonar Navigation 152(5):364–371CrossRefGoogle Scholar
  18. Gill PE, Murray W, Saunders MA, Wright MH (1991) Inertia-controlling methods for general quadratic programming. SIAM Rev 33(1):1–36MathSciNetMATHCrossRefGoogle Scholar
  19. Goldfarb D, Idnani A (1983) A numerically stable dual method for solving strictly convex quadratic programs. Math Program 27(1):1–33MathSciNetMATHCrossRefGoogle Scholar
  20. Gordon NJ, Salmond DJ, Smith AFM (1993) Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proc Radar Signal Process 140(5):107–113CrossRefGoogle Scholar
  21. Guiducci A (1999) Parametric model of the perspective projection of a road with applications to lane keeping and 3D road reconstruction. Comput Vis Image Underst 73(3):414–427MATHCrossRefGoogle Scholar
  22. Guiducci A (2000) Camera calibration for road applications. Comput Vis Image Underst 79(2):250–266CrossRefGoogle Scholar
  23. Harris C, Stephens M (1988) A combined corner and edge detector. In: Proceedings of the Alvey vision conference, Manchester, pp 147–151Google Scholar
  24. Hough PVC (1962) A method and means for recognizing complex patterns. US Patent 3,069,654Google Scholar
  25. Jung CR, Kelber CR (2005) Lane following and lane departure using a linear-parabolic model. Image Vis Comput 23(13):1192–1202CrossRefGoogle Scholar
  26. Kim ZW (2008) Robust lane detection and tracking in challenging scenarios. IEEE Trans Intell Transp Syst 9(1):16–26CrossRefGoogle Scholar
  27. Lee JW (2002) A machine vision system for lane-departure detection. Comput Vis Image Underst 86(1):52–78MATHCrossRefGoogle Scholar
  28. Lowe DG (2004) Distinctive image features from scale-invariant keypoints. Int J Comput Vision 60(2):91–110CrossRefGoogle Scholar
  29. Lundquist C, Schön TB (2011) Joint ego-motion and road geometry estimation. Inf Fusion 12(4):253–263CrossRefGoogle Scholar
  30. Lundquist C, Hammarstrand L, Gustafsson F (2011a) Road intensity based mapping using radar measurements with a probability hypothesis density filter. IEEE Trans Signal Process 59(4):1397–1408Google Scholar
  31. Lundquist C, Orguner U, Gustafsson F (2011b) Extended target tracking using polynomials with applications to road-map estimation. IEEE Trans Signal Process 59(1):15–26MathSciNetCrossRefGoogle Scholar
  32. Mahler RPS (2003) Multitarget Bayes filtering via first-order multitarget moments. IEEE Trans Aerosp Electron Syst 39(4):1152–1178CrossRefGoogle Scholar
  33. Mahler RPS (2007) Statistical multisource-multitarget information fusion. Artech House, BostonMATHGoogle Scholar
  34. Matas J, Chum O, Urban M, Pajdla T (2004) Robust wide baseline stereo from maximally stable extremal regions. Image Vis Comput 22(10):731–767Google Scholar
  35. McCall JC, Trivedi MM (2006) Video-based lane estimation and tracking for driver assistance: servey, system, and evaluation. IEEE Trans Intell Transp Syst 7(1):20–37CrossRefGoogle Scholar
  36. Moravec H (1988) Sensor fusion in certainty grids for mobile robots. AI Mag 9(2):61–74Google Scholar
  37. Nistér D, Naroditsky O, Bergen J (2006) Visual odometry for ground vehicle applications. J Field Rob 23(1):3–20MATHCrossRefGoogle Scholar
  38. Powell M (1985) On the quadratic programming algorithm of Goldfarb and idnani. Math Program Study 25(1):46–61MATHCrossRefGoogle Scholar
  39. Rohling H, Meinecke MM (2001) Waveform design principles for automotive radar systems. In: Proceedings on CIE international conference on radar, Beijing, pp 1–4Google Scholar
  40. Rohling H, Möller C (2008) Radar waveform for automotive radar systems and applications. In: IEEE radar conference, Rome, pp 1–4Google Scholar
  41. Sidenbladh H (2003) Multi-target particle filtering for the probability hypothesis density. In: Proceedings of the international conference on information fusion, Cairns, vol 2, pp 800–806Google Scholar
  42. Söderström T (2007) Survey paper: errors-in-variables methods in system identification. Automatica 43(6):939–958MathSciNetMATHCrossRefGoogle Scholar
  43. Szeliski R (2010) Computer vision: algorithms and applications. Springer, New YorkGoogle Scholar
  44. Thrun S, Burgard W, Fox D (2005) Probabilistic robotics. The MIT Press, CambridgeMATHGoogle Scholar
  45. Vo B-N, Ma W-K (2006) The Gaussian mixture probability hypothesis density filter. IEEE Trans Signal Process 54(11):4091–4104CrossRefGoogle Scholar
  46. Vo B-N, Singh S, Doucet A (2003) Random finite sets and sequential monte carlo methods in multi-target tracking. In: Proceedings of the international radar conference, Adelaide, pp 486–491Google Scholar
  47. Vu TD, Aycard O, Appenrodt N (2007) Online localization and mapping with moving object tracking in dynamic outdoor environments. In: Proceedings of the IEEE intelligent vehicles symposium. Istanbul, pp 190–195Google Scholar
  48. Wang Y, Bai L, Fairhurst M (2008) Robust road modeling and tracking using condensation. IEEE Trans Intell Transp Syst 9(4):570–579CrossRefGoogle Scholar
  49. Waxman A, LeMoigne J, Davis L, Srinivasan B, Kushner T, Liang E, Siddalingaiah T (1987) A visual navigation system for autonomous land vehicles. IEEE J Robot Automation 3(2):124–141CrossRefGoogle Scholar
  50. Zhou Y, Xu R, Hu X, Ye Q (2006) A robust lane detection and tracking method based on computer vision. Meas Sci Technol 17(4):736–745CrossRefGoogle Scholar

Copyright information

© Springer-Verlag London Ltd. 2012

Authors and Affiliations

  • Christian Lundquist
    • 1
  • Thomas B. Schön
    • 1
  • Fredrik Gustafsson
    • 1
  1. 1.Department of Electrical Engineering, Division of Automatic ControlLinköping UniversityLinköpingSweden

Personalised recommendations