Improved iterative pose estimation algorithm using three-dimensional feature points

Abstract

In a real-time vision navigation system, an accurate and fast convergent pose estimation algorithm is required for the video guidance sensor. The orthogonal iteration (OI) algorithm is fast and globally convergent, but its results have a large translation error at a close range, and sometimes it fails to give a correct rotation matrix when the data are severely corrupted, when using the 3-D feature points. When the rotation matrix solution in the OI algorithm has been refined, an efficient pose estimation algorithm is derived. Simulation of the improved algorithm shows that the rotation matrix is always proper, which in turn improves the accuracy of the translation vector. The noise resistance and the outlier tolerance are enhanced by using the improved algorithm. The two algorithms are applied to our experimental system for an unmanned vehicle rendezvous and docking simulation separately. The comparison experiments show that the relative distance error is less than 0.28% from 1.5 to 5 m, and the rotation angle error is within ±0.7 deg in 5 m using the improved algorithm. These are better than the results using the OI algorithm.