Heading Reference-Assisted Pose Estimation for Ground Vehicles

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.