High Accuracy and Robust Robotic Inspection by Constrained Pose Graph Optimization

Pose graph optimization (PGO) methods are extensively used in robotic inspection (RI) to estimate accurate 3-D scanner poses. However, some inaccurate pairwise relative measurements decrease the accuracy of PGO. In this article, we propose a new framework called robust constrained pose graph optimization (RCPGO) to achieve a robust and accurate RI. Specifically, some additional landmarks are utilized to construct a constrained pose graph (CPG) and improve the accuracy of PGO by CPG's spatial geometry consistency. Furthermore, the symmetric (SYMM) distance function is introduced to define the objective function of RCPGO, which has a higher D-optimality design metric upper bound and contributes to the robustness and accuracy of RCPGO. To solve the SYMM-RCPGO problem in <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\boldsymbol{SE_{3}^{N}}$</tex-math></inline-formula>, we linearize the problem into a quadratically constrained quadratic program (QCQP) and solve the perturbation of <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\boldsymbol{SE_{3}^{N}}$</tex-math></inline-formula> by the interior-point method. Moreover, a dynamic constraints boundaries algorithm is proposed to prevent inaccurate linearization caused by large perturbation. To assess the effectiveness of SYMM-RCPGO, the comparative evaluations with traditional PGO methods are conducted on simulated cylinder surfaces, a high-speed train, and a rotor engine. The results validate the robustness and accuracy of SYMM-RCPGO and find that a few constraints can significantly reduce pose error.