Tightly Coupled GNSS with Stereo Camera Navigation Using Graph Optimization

Abstract

To achieve an accurate and robust localization result under Global Navigation Satellite System (GNSS) denied environment, coupling GNSS with other sensors tends to be a promising solution, also known as the sensor fusion system. One of the most challenging scenarios in sensor fusion is to couple vision navigation system with GNSS, especially the tightly-coupled fusion. Compared with those loosely coupled ones, it performs the information fusion on a more intrinsic level, which can benefit the accuracy and robustness of the system. However, the major barrier lying in front of tightly coupling method is the way to unify the heterogeneous and asynchronous GNSS observations with vision observations. To solve this problem, we start from the Vison Simultaneous Localization and Mapping (V-SLAM) method, which has already become the mainstream in vision navigation domain. By utilizing existing attempts on graph-optimization-based GNSS data processing, we propose a joint graph optimization combining vision-SLAM with GNSS pseudorange. It is a tightly coupled fusion of GNSS and stereo camera, implemented by considering GNSS satellites as vision feature points. Based on the framework of mature state-of-art pose-graph SLAM, a graph optimization is applied simultaneously on GNSS and vision positioning, to achieve a robust, accurate, yet versatile fusion system.