基于四元数表示的GraphSLAM位姿初始化算法

Initialization Techniques of Pose Optimization Using the Quaternion Representation for Graph SLAM

位姿估计问题本质上是一个非凸且NP-难问题,难以求解的原因在于位姿中包含特殊正交群,且往往拥有一个较差的初始值。针对此问题,提出了一种基于图的即时定位与建图技术位姿初始化算法。首先,使用两阶段算法将位姿估计问题构建的最大似然估计方程重新拆分为两个独立的子问题,即基于四元数表示的旋转平均问题和无约束的平移向量平均的线性方程问题。然后,通过迹松弛方式将非凸问题转化为线性方程问题。最后,结合平移向量计算出初始化的位姿。仿真实验表明,在高噪声的数据集中,所提方法比基于生成树的目标函数值降低了至少1个数量级,能够鲁棒地为位姿估计问题提供一个良好的初始值。

Pose estimation is essentially a non-convex and NP-hard problem,which is difficult to solve due to the inclusion of the special orthogonal group and often comes with a poor initial value.This paper proposes a Graph SLAM pose initialization algorithm based on quaternion representation.Firstly,a two-stage algorithm is used to decompose the maximum likelihood estimation equation constructed for pose estimation into two independent subproblems,namely the rotation averaging problem based on quaternion representation and linear equation problem of unconstrained average translation vectors.Then,the non-convex problem is transformed into a linear equation problem through trace relaxation.Finally,the initial pose is calculated in combination with the translation vectors.Sim-ulation results show that this method reduces the error value by an order of magnitude compared to classic methods in datasets with noise,and can robustly provide a good initial pose for pose estimation problems.