基于偏差矩阵的3D SLAM位姿图优化算法

Deviation Matrix Based for 3D SLAM Pose Graph Optimization

  • 摘要: 位姿图优化(pose graph optimization,PGO)是3D SLAM(simultaneous localization and mapping)后端优化方法之一,其精确求解依赖于良好的初始值。针对PGO噪声数据集初始化,首先提出一种新的PGO目标公式——CN(chordal with noise)模型,此模型考虑噪声影响下产生的旋转偏差,将偏差矩阵设为参数;其次,提出ORDM(optimize rotation with the deviation matrix)算法求解CN模型,此算法在位姿图子图中,分别建立关于偏差矩阵的相对旋转测量方程,最终将CN模型化为矩阵形式,并采用线性最小二乘求出偏差矩阵的封闭解,以此修正旋转方向。实验证明,ORDM算法在面对PGO噪声数据集时,较为鲁棒,具有一定的可伸缩性;与迭代初始化算法相比,可对应较差的初始化场景。

     

    Abstract: Pose graph optimization (PGO) is a back-end optimization method for 3D simultaneous localization and mapping, and its accuracy depends on good initial values. We present a chordal with noise (CN) model, which is a new PGO target formula for initializing PGO noise datasets. This proposed model considers the rotation deviation caused by noise, and the deviation matrix is set as a parameter. The optimize rotation with the deviation matrix (ORDM) algorithm is proposed to solve the CN model. The algorithm builds the relative rotation measurement equation of the deviation matrix in the subgraph of the pose graph and finally models the CN into the matrix form. A closed solution of the deviation matrix is obtained by using linear least squares to correct the direction of rotation. Our experimental results show that the ORDM algorithm is robust and scalable in PGO noise data sets. In addition, the ORDM algorithm can correspond to poor initialization scenarios when compared with the iterative initialization algorithm.

     

/

返回文章
返回