Heading Reference-Assisted Pose Estimation for Ground Vehicles

Abstract

In this paper, heading reference-assisted pose estimation (HRPE) has been proposed to compensate inherent drift of visual odometry (VO) on ground vehicles, where an estimation error is prone to grow while the vehicle is making turns or in environments with poor features. By introducing a particular orientation as “heading reference,” a pose estimation framework has been presented to incorporate measurements from heading reference sensors into VO. A graph formulation is then proposed to represent the pose estimation problem under the commonly used graph optimization model. Simulations and experiments on KITTI data set and our self-collected sequences have been conducted to verify the accuracy and robustness of the proposed scheme. KITTI sequences and manually generated heading measurement with Gaussian noises are used in simulation, where rotational drift error is observed to be bounded. Compared with a pure VO, the proposed approach greatly reduces average translational localization error from 153.85 to 24.29 m and 23.80 m in self-collected stereo visual sequences with traveling distance over 4.5 km at the processing rates of 19.7 and 11.1 Hz, for the loosely coupled and tightly coupled models, respectively. Note to Practitioners —When the Global Positioning System is not available or reliable, visual odometry (VO) on ground vehicles is an efficient tool for estimating the pose, which involves translation and rotation. However, VO inherently suffers from drifting issue due to constant iterations. By adding a low-cost heading reference sensor, this paper first introduces graph optimization formulation of pose estimation and then presents a pose estimation framework which incorporates heading measurements to VO such that long-term translation and rotation estimation errors can be greatly reduced in real-time computation. As a supplementary to VO, performance may still deteriorate in environments with poor illumination conditions and high-frequency movements. The proposed approach may be further improved by fusing heading measurements from more sensors or being used to build a heading reference-assisted simultaneous localization and mapping (SLAM) system on any off-the-shelf SLAM framework.