High precision pose measurement for humanoid robot based on PnP and OI algorithms

In this paper, a method combining PnP and OI algorithms is developed to measure the pose of a humanoid robot in high precision in order to play table tennis. The PnP-based algorithm is employed to obtain the rough pose, which is taken as the initial value of the OI algorithm. The OI algorithm optimizes the result in order to ensure the orthogonal orientation matrix of the pose. Considering the real-time, the accuracy, and the robustness of the pose measurement, a sort of fast feature extraction algorithm including edge detection, line extraction and intersection computation is adopted to get the four corners in high precision. The experiments and results verify the effectiveness of the proposed method.