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

作者：王苗苗; 魏国亮^*; 蔡洁; 栾小珍

来源：信息与控制, 2023, 52(03): 334-342.

DOI：10.13976/j.cnki.xk.2023.2233

• 位姿图优化
• 3D SLAM
• 噪声
• 偏差矩阵
• 线性最小二乘

位姿图优化(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噪声数据集时，较为鲁棒，具有一定的可伸缩性；与迭代初始化算法相比，可对应较差的初始化场景。

• 单位
  上海理工大学