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

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

**摘要：**位姿图优化（PGO）是3D SLAM后端优化方法之一，其精确求解依赖于良好的初始值。

针对PGO噪声数据集初始化，首先提出一种新的PGO目标公式——CN（chordal with noise）模型，此模型考虑噪声影响下产生的旋转偏差，将偏差矩阵设为参数；其次，提出ORDM（optimize rotation with the deviation matrix）算法求解CN模型，此算法在位姿图子图中，分别建立关于偏差矩阵的相对旋转测量方程，最终将CN模型化为矩阵形式，并采用线性最小二乘求出偏差矩阵的封闭解，以此修正旋转方向。

实验证明，ORDM算法在面对PGO噪声数据集时，较为鲁棒，具有一定的可伸缩性；与迭代初始化算法相比，可对应较差的初始化场景。

附赠自动驾驶最全的学习资料和量产经验：链接

00  引言

SLAM，即同时定位与地图构建[1]，在AR、VR、无人驾驶、智能家居，路径优化等领域广泛应用。基于图优化的SLAM系统可以分为前端的里程计、后端非线性优化、回环检测以及建图四个阶段（如图1所示），其中位姿图优化（PGO）是SLAM后端优化的方法之一。

图１ 基于位姿图优化的SLAM框架

位姿图优化是根据前端图构建的相对约束来估计机器人的绝对位姿[2]。PGO问题将机器人轨迹用图表示，其中顶点表示所有时刻机器人的绝对位姿，边表示相邻时刻间机器人的相对位姿测量[3]。

PGO的主体算法是将非凸的最大似然估计问题转化为非线性最小二乘问题[4]，并采用高斯牛顿[5-7]、列文伯格-马尔夸特[8-10]等方式求解。其中，SLAM经典的图优化框架g2o[6]也采用迭代算法。由于非线性迭代对初始值依赖性强，如果没有良好的初始值，容易陷入局部极小值，甚至难以收敛，因此，对于迭代方法，一个良好的初始值是至关重要的。

2010年，KONOLIGE等提出了基于最小生成树的PGO初始算法——SPT（SPanning Tree）[11]。该算法复杂度低，且无需初始值。最近，研究者陆续提出精度更高的PGO初始化算法：LAGO[12]、TORO[13]、Cauchy[14]和HIPE[15]。LAGO是一种线性近似技术，可以为2D SLAM提供初始值，但无法处理噪声数据集。TORO算法利用梯度下降法进行迭代收敛，可应对较差初始值。Cauchy算法是一种基于M估计的迭代方法，具有一定的抗噪能力。最后，HIPE算法采用分层思想，并与粗粒度图相结合，可应对大规模PGO数据集。TORO、Cauchy、HIPE同时考虑了机器人的位置和旋转方向，加大了PGO问题的计算复杂度，运行速度慢，而且这3种算法本身也需要初始值。

研究表明，对于PGO优化算法，优先估计旋转方向在兼顾计算复杂度和鲁棒性方面具有显著优势[16]，此思想在3D SLAM中广泛应用[17-18]。

目前，在3D SLAM中，PGO初始化标准算法是MARTINEC和PAJDLA提出的Chordal算法[19]，并经CARLONE等[20]验证该算法优于其他3D SLAM位姿图初始化算法，之后的研究者在形式上对其进行了改进[21]。该算法将同时估计机器人位置和旋转方向的问题转化成优先估计旋转的子问题，大幅度地降低了PGO问题的计算复杂度，加快了其运行速度，但只可应对低噪声的PGO数据集。近期，NASIRI等提出RS（rotation synchronization）算法[22]，该算法基于递归最小二乘，为最新的迭代初始化算法，但此算法需要良好的初始值。

本文为针对噪声数据集，将噪声造成的旋转偏差设为参数，形成新的PGO目标公式，并提出相应的求解算法——ORDM（optimize rotation with the deviation matrix）算法。

ORDM算法具有一定的抗噪能力，可作为PGO的初始化算法。在低噪声和无噪声数据集中，也可直接作为PGO的优化算法。本文贡献如下：

1）考虑噪声产生的旋转偏差，将偏差矩阵设为参数，建立新的PGO目标函数模型——CN模型。

2）提出ORDM算法求解CN模型，此算法在位姿图子图中，分别建立关于偏差矩阵的相对旋转测量方程，最终将CN模型化为矩阵形式，并采用线性最小二乘得出偏差矩阵的封闭解，以此修正旋转方向。

3）利用PGO公开数据集进行实验，实验结果显示ORDM算法相比于Chordal算法在噪声的情况下，更加精确、鲁棒；适用于各类规模的PGO数据集，具有一定的可伸缩性。

4）ORDM算法对初始值无要求，适用于初始值较差的轨迹优化场景，适合与复杂度较低的初始化算法相结合。

01  知识储备

1.1 位姿图

机器人轨迹的时刻节点和时刻间的相对位姿测量边组成有向图，也称为位姿图（如图2）。其中是n+1个时刻节点，表示机器人的位姿点，，定义了节点的位姿，其中旋转矩阵，是包含旋转矩阵的一种特殊正交群，称之为3维旋转群；位置向量；表示m条相对位姿测量边。，表示节点到节点