Sensor Fusion for Self-Localisation of Automated Vehicles

Original Article

Abstract

Accurate pose estimation is key for a large number of real world applications. For example, automated cars require fast, recent, accurate, and highly available pose estimates for robust operation. Multiple redundant and complementary localisation systems are therefore installed on most automated vehicles. This work proposes a novel multi-sensor pose fusion approach for generically combining measurements from multiple localisation systems into a single pose estimate. We formulate our approach as a sliding window pose graph and enforce a particular chain graph structure, which enables efficient optimisation and a novel form of marginalisation. Our pose fusion approach scales from a filtering-based to a batch solution by increasing the size of the sliding window. It also adapts online to the available computational resources to guarantee high availability. We evaluate our approach on simulated data as well as on real data gathered with a prototype vehicle and demonstrate that our solution runs at 20 Hz, provides timely estimates, is accurate, and yields high availability.

Keywords

Sensor fusion State estimation Chain pose graph Localization Automated driving 

Zusammenfassung

Sensorfusion für die Positionierung von autonom fahrenden Fahrzeugen. Eine genaue Lokalisierung ist zentral für viele reale Anwendungen. So benötigen beispielsweise automatisierte Autos häufige, aktuelle, präzise und hochverfügbare Schätzungen der Fahrzeugpose. Daher sind auf den meisten automatisierten Fahrzeugen mehrere redundante und komplementäre Ortungssysteme integriert. In diesem Artikel beschreiben wir eine neuartige generische Ortungsfusion zum Zusammenführen der Positionsmessungen von unterschiedlichen Ortungssystemen. Wir konstruieren dazu einen Posengraphen über das Zeitfenster der letzten Odometrie- und Positionsmessungen, so dass der Aufbau des Graphen einer Kettenstruktur entspricht. Diese ermöglicht ein effizientes Optimieren und eine neue Art der Marginalisierung. Unser Ortungsfusionsansatz skaliert von einer filter-basierten bis zur Batch-Lösung durch das Vergrößern des Zeitfensters. Er passt sich außerdem zur Laufzeit an die verfügbaren Rechenressourcen an, um eine hohe Verfügbarkeit der Posenschätzungen zu gewährleisten. Wir evaluieren unseren Ansatz mit Hilfe simulierter und auf einem realen Prototypen aufgezeichneter Daten und zeigen, dass unsere Lösung mit 20 Hz läuft, aktuelle Schätzungen bereitstellt, sowie präzise und hochverfügbar ist.

References

  1. Agarwal P, Olson E (2012) Variable reordering strategies for SLAM. In: Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (IROS), pp 3844–3850Google Scholar
  2. Agarwal P, Burgard W, Stachniss C (2014a) Helmert’s and Bowie’s geodetic mapping methods and their relation to graph-based SLAM. In: Proceedings of the IEEE international conference on robotics and automation (ICRA). IEEE, pp 3619–3625Google Scholar
  3. Agarwal P, Burgard W, Stachniss C (2014b) A survey of geodetic approaches to mapping and the relationship to graph-based SLAM. IEEE Robot Autom Mag 21(3):63–80CrossRefGoogle Scholar
  4. Barfoot TD (2016) State estimation for robotics: a matrix-Lie-group approach. Draft in preparation for publication by Cambridge University Press, CambridgeGoogle Scholar
  5. Bar-Shalom Y (2002) Update with out-of-sequence measurements in tracking: exact solution. IEEE Trans Aerosp Electron Syst 38(3):769–778CrossRefGoogle Scholar
  6. Bell B, Cathey F (1993) The iterated Kalman filter update as a Gauss–Newton method. IEEE Trans Autom Control 38(2):294–297CrossRefGoogle Scholar
  7. Chiu HP, Williams S, Dellaert F, Samarasekera S, Kumar R (2013) Robust vision-aided navigation using sliding-window factor graphs. In: Proceedings of the IEEE international conference on robotics and automation (ICRA), pp 46–53Google Scholar
  8. Cucci DA, Matteucci M (2013) A flexible framework for mobile robot pose estimation and multi-sensor self-calibration. In: Proceedings of the international conference on informatics in control, automation and robotics (ICINCO), pp 361–368Google Scholar
  9. Golfarelli M, Maio D, Rizzi S (1998) Elastic correction of dead-reckoning errors in map building. In: Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (IROS), pp 905–911Google Scholar
  10. Hartley R, Zisserman A (2004) Multiple view geometry in computer vision, 2nd edn. Cambridge University Press, CambridgeCrossRefGoogle Scholar
  11. Indelman V, Williams S, Kaess M, Dellaert F (2012) Factor graph based incremental smoothing in inertial navigation systems. In: Proceedings of the international conference on information fusion (FUSION), pp 2154–2161Google Scholar
  12. Julier SJ, Uhlmann JK (1997) A non-divergent estimation algorithm in the presence of unknown correlations. In: Proceedings of the American control conference (ACC), pp 2369–2373Google Scholar
  13. Julier SJ, Uhlmann JK (2007) Using covariance intersection for SLAM. Robot Auton Syst 55(1):3–20CrossRefGoogle Scholar
  14. Kaess M, Johannsson H, Roberts R, Ila V, Leonard J, Dellaert F (2012) iSAM2: Incremental smoothing and mapping using the Bayes tree. Int J Robot Res 31(2):216–235CrossRefGoogle Scholar
  15. Kubelka V, Oswald L, Pomerleau F, Colas F, Svoboda T, Reinstein M (2015) Robust data fusion of multimodal sensory information for mobile robots. J Field Robot 32(4):447–473CrossRefGoogle Scholar
  16. Kümmerle R (2013) State estimation and optimization for mobile robot navigation. Ph.D. dissertation. University of FreiburgGoogle Scholar
  17. Kümmerle R, Grisetti G, Strasdat H, Konolige K, Burgard W (2011) g2o: A general framework for graph optimization. In: Proceedings of the IEEE international conference on robotics and automation (ICRA), pp 3607–3613Google Scholar
  18. Lynen S, Achtelik MW, Weiss S, Chli M, Siegwart R (2013) A robust and modular multi-sensor fusion approach applied to MAV navigation. In: Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (IROS), pp 3923–3929Google Scholar
  19. Merfels C, Stachniss C (2016) Pose fusion with chain pose graphs for automated driving. In: Proceedings of the IEEE/RSJ international conference on intelligent robots and systems (IROS), pp 3116–3123Google Scholar
  20. Merfels C, Riemenschneider T, Stachniss C (2016) Pose fusion with biased and dependent data for automated driving. In: Proceedings of the positioning and navigation for intelligent transportation systems conference (POSNAV). ISSN: 2191-8287Google Scholar
  21. Olson E, Leonard J, Teller SJ (2006) Fast iterative alignment of pose graphs with poor initial estimates. In: Proceedings of the IEEE international conference on robotics and automation (ICRA), pp 2262–2269Google Scholar
  22. Reinhardt M, Noack B, Hanebeck UD (2012) Closed-form optimization of covariance intersection for low-dimensional matrices. In: Proceedings of the international conference on information fusion (FUSION). IEEE, pp 1891–1896Google Scholar
  23. Schlegel S, Korn N, Scheuermann G (2012) On the interpolation of data with normally distributed uncertainty for visualization. IEEE Trans Vis Comput Graph 18(12):2305–2314CrossRefGoogle Scholar
  24. Sibley G, Matthies L, Sukhatme G (2010) Sliding window filter with application to planetary landing. J Field Robot 27(5):587–608CrossRefGoogle Scholar
  25. Strasdat H, Montiel J, Davison AJ (2012) Visual SLAM: why filter? Image Vis Comput 30(2):65–77CrossRefGoogle Scholar
  26. Weiss S, Achtelik MW, Chli M, Siegwart R (2012) Versatile distributed pose estimation and sensor self-calibration for an autonomous MAV. In: Proceedings of the IEEE international conference on robotics and automation (ICRA), pp 31–38Google Scholar

Copyright information

© Deutsche Gesellschaft für Photogrammetrie, Fernerkundung und Geoinformation (DGPF) e.V. 2017

Authors and Affiliations

  1. 1.Automated Driving, Volkswagen Group ResearchWolfsburgGermany
  2. 2.Institute of Geodesy and GeoinformationUniversity of BonnBonnGermany

Personalised recommendations