From Simultaneous Localization and Mapping to Collaborative Localization for Intelligent Vehicles

Abstract

This paper describes a new method for real-time collaborative localization for intelligent vehicles based on a combination of an extension of simultaneous localization and mapping and track-to-track fusion methods. This strategy aims to overcome the shortcomings of the embedded proprioceptive sensors and solve the localization issues in a cluster of intelligent vehicles, which are equipped with low-cost navigation systems (global position systems, rangefinders, and vehicle to vehicle communication means) in an unknown environment. The strategy improves the vehicles’ localization accuracy and tracks a cluster of vehicles for advanced driver assistance systems and vehicles self-driving. For real-time purposes, the computational complexity of standard solutions, which are based on Kalman filter derivatives, is transformed, herein, from cubic to quadratic with respect to the state vector of the cluster. Furthermore, the correlated data is another challenge that needs to be solved in cooperative localization scenarios. To this end, two independent, local and global, fusion nodes are associated with each vehicle. The local node exploits multiple forms of the extended Kalman filter and its dual extended information filter to estimate the vehicles’ state, information vectors, covariance and information matrices in the moments and information forms, respectively. Then, the vehicles broadcast the result of their respective local fusion node to cluster. The information is used to solve the second problem, which is a track-to-track fusion on distributed architectures, with two methods: covariance intersection and information matrix fusion in the global fusion node. The simulation and the experimental results of the proposed methods, on distributed architectures, are compared to the optimal solution of centralized data fusion, and to the traditional SLAM method in terms of root mean square errors and consistency.