基于偏差矩阵的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后端优化的方法之一。

图片

位姿图优化是根据前端图构建的相对约束来估计机器人的绝对位姿[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 位姿图

机器人轨迹的时刻节点和时刻间的相对位姿测量边组成有向图G=(V,E) ，也称为位姿图（如图2）。其中V=\{V_0,V_1,...,V_n\} 是n+1个时刻节点，表示机器人的位姿点，V_i=(\pmb{R}_i,\pmb{t}_i),i\in\{1,2,...,n\} ，定义了节点i 的位姿，其中旋转矩阵\pmb{R}_i\in SO(3) ，SO(3) 是包含旋转矩阵\pmb{R}_i 的一种特殊正交群，称之为3维旋转群；位置向量\pmb{t}_i \in \mathbb{R}^3 ；E=\{e_1,e_2,...,e_m\} 表示m条相对位姿测量边。e_{k=(i,j)}=(\pmb{\tilde{R}}^j_i,\tilde{t}_{ij})\in E，k \in \{1,2,..,m\} 表示节点i 到节点j 之间的第k 条相对位姿测量边，其中\pmb{\tilde{R}}^j_i 为含有噪声的相对旋转测量，\pmb{\tilde{R}}_{ij} 表示含有噪声的相对位置测量。一般假设，V_0 为参考点[23]，t_0=[0,0,0]^T ，\pmb{R}_0=\pmb{I}_3 ；\bar{A} 是由1、-1、0组成的矩阵，为有向图G 的关联矩阵，A 为A 去除第一列的列满秩矩阵[24]，即A^TA 为可逆矩阵。

图片

1.2 PGO问题公式

PGO是从m 个相对测量值中估计n 个绝对位姿的问题，观测方程如下：

图片

为优化求解机器人n 个位姿点\pmb{R}_i ，\pmb{t}_i ，PGO的问题公式相当于极小化以下目标函数[24]：

图片

本文令\Omega_{m\times m}=[\text{diag}(\omega_k)] ，\Lambda_{m\times m}=[\text{diag}(\lambda_k)] ，k \in \{1,2,...,m\}，分别表示旋转噪声和位置噪声协方差矩阵的逆，即信息矩阵，在PGO问题中起权重作用。SO(3) 是包含旋转矩阵的特殊正交矩阵，也称三维旋转群。

1.3 机器人位姿的旋转方向求解

对于PGO目标函数式（3），文[20]建议优先估计机器人位姿的旋转方向，即利用观测方程式（1）极小化相对旋转测量误差，设为式（5），并将Chordal算法整理如下：

图片

于是，式（5）可进一步的改写为

图片

在这里，假设\gamma^l_i 为\pmb{R}_i 的第l 列，l\in \{1,2,3\} ，式（6）可变为

图片

进而松弛式（7）中的SO(3) 空间约束，将式（7）变为无约束问题：

图片

将\pmb{R}_0=\pmb{I} 作为已知条件，利用线性最小二乘求解式（8），可得n个矩阵M_i=[\gamma^1_i,\gamma^2_i,\gamma^3_i] ，i\in \{1,2,...,n\} 。但由于式（8）为松弛问题，求出的Mi不一定为位姿点的旋转方向，需要进行修正。由此极小化\pmb{M}_i 与\pmb{R}_i 的误差：

图片

文[25]通过SVD算法求解式（9），M_i=S_iD_iV_i^T ，其中D_i 为奇异值组成的矩阵，则：

图片

从上述可知，Chordal算法以向量为参数。在文[21]中，NASIRI等改进了Chordal算法，改进后的算法以旋转矩阵为参数。

1.4 机器人位姿的位置求解

如果位姿点的旋转方向\pmb{R}_i ，i\in \{1,2,...,n\} 已知，由于相对位置测量与相对旋转测量可相互独立，机器人的位姿的绝对位置求解公式如下：

图片

采用线性最小二乘进行求解，得封闭解：

图片

t_{n\times 3}=[t_1,t_2,...,t_n]^T为n个位姿点的绝对位置，\Lambda_{m\times m}=\text{diag}[\lambda_1,\lambda_2,...,\lambda_m] 是位置信息的权重矩阵，\pmb{D}_{m\times 3} 的第k行为\pmb{t}_{ij}^T\pmb{R}^T_i 。

02  ORDM算法

2.1 CN模型建立

Chordal算法没有考虑噪声影响，本质上认为\pmb{\tilde{R}}^j_i=\pmb{R}_j\pmb{R}_i^T 。接下来，考虑噪声对每个位姿点的影响，假设有初始旋转方向\hat{\pmb{R}}_i(i\in \{1,2,...,n\}) ，由于噪声的存在，每个位姿的初始方向都有一定的偏差，称为旋转变化量，即\Psi_i(i\in\{1,2,...,n\})，这里为矩阵形式。本文采用右扰动模型，则每个位姿点的旋转方向应表示为

图片

在理想状态下，式（1）可更改为

图片

因此，本文提出新的目标函数模型——CN（chordal with noise）模型，设为式（15）：

图片

与式（5）相比，CN模型考虑噪声偏差，将偏差矩阵\Psi_i为参数，构建新的残差项。

2.2 ORDM算法思想及流程

为求解CN模型，本文将位姿图G=(V,E) 分为两个子图E_1 、E_2 ，与V_0 相连的位姿点及其相对位姿测量边构成子图E_1 ，不与V_0 相连的位姿点及其相对位姿测量边构成子图E_2 。

由于\pmb{R}_0=\pmb{I}_0 ，\pmb{\Psi}_0=\pmb{I}_3 ，将其代入式（14），则子图中的相对旋转测量方程可记为

图片

式（16）通过矩阵扩维，被整理为

图片

其中，\pmb{0}_{3\times 3} 是零矩阵，\pmb{\Psi} 为3n\times 3 的参数矩阵。

子图中的相对旋转测量方程可改写为

图片

通过矩阵的转置变换和矩阵扩维，可整理为

图片

由于位姿图G 中有m 条相对位姿测量边，分别属于E_1 、E_2 ，因此将m 个相对旋转测量方程依照上述两种形式，通过矩阵扩维分别改写为类似式（17）与式（20）的形式，并组合相加，可得式（21）：

图片

H为3m\times 3n 矩阵，Q 为3m\times 3 的矩阵。

因此，CN模型可以更改为式（22）：

图片

利用线性最小二乘求解，可得：

图片

同样考虑到旋转权重的问题，式（23）可改写为

图片

ORDM（optimize rotation with the deviation matrix）算法将位姿图G 分成两个子图，依照式（14）分别建立相对旋转测量方程，最终将CN模型化为矩阵形式，采用线性最小二乘求解参数矩阵\Psi ，并通过式（13）对初始旋转方向\hat{\pmb{R}} 进行优化。此外，因为计算出的\Psi_i 可能不满足旋转矩阵的要求，所以本文利用SVD算法对于每个\Psi_i 做一个修正。

由上述可知，ORDM算法采用封闭解式（24）求解参数，无需迭代，相比于迭代优化算法，对初值要求不高。此外，当初始旋转方向\hat{\pmb{R}} 较好时，式（24）计算出旋转偏差\Psi_i 接近于单位矩阵；但当初值较差时，计算的\Psi_i 偏差较大，此时更能突出以偏差矩阵为参的意义和优势。算法1为ORDM算法的具体步骤。由于ORDM算法需要初始值，在此采用改进的Chordal算法[21]或是最小生成树搜索（SPT）[11]提供初始值。

图片

03  实验结果与分析

在本节中，通过对比实验主要验证ORDM算法在PGO初始化中的两点优势：

1）向PGO公共数据集添加噪声，评估ORDM算法与Chordal算法[21]的性能，以证明ORDM算法可有效应对噪声数据集，具有一定的鲁棒性，运行时间在可接受范围之内。

2）采用复杂度低的SPT算法[11]为ORDM算法和RS算法[22]提供初始值，在原始的PGO数据集上进行对比实验，验证ORDM算法可应对初始值较差的机器人轨迹优化场景。

所有实验均在CPU为AMD锐龙74800U、内存16G的笔记本电脑上进行，编程语言为Matlab 2017b。

3.1 PGO数据集

在3D SLAM中，PGO公共数据集[20]主要有Sphere_a、Garage、Torus和Cubicle，均为低噪声数据集。Sphere_a、Torus是模拟生成的位姿图；Garage为斯坦福停车场的3D地图，用于研究自动停车；Cubicle是佐治亚理工学院RIM中心提供的3D激光SLAM位姿图。

为检验ORDM算法的棒性和抗噪能力，本文在这些数据集的相对旋转测量中分别加入均值\mu_R=0 ，标准差\sigma_R为0.1rad、0.2rad、0.3rad的噪声。表1是数据集的具体参数，\theta_{\text{noise}} 表示相对旋转测量的噪声（单位：rad），Mean、Std分别为其均值和标准差，m 为位姿图边数，n 为位姿图节点个数。

图片

3.2 实验结果

3.2.1 鲁棒性

在PGO数据集中加入不同水平的噪声，重复实验50次取平均值，记录所得到的损失函数值f。一般认为，损失函数值越低，说明机器人轨迹优化越精确，算法越鲁棒。此次实验采用SPT算法、Chordal算法分别作为ORDM算法的初始化算法，记为ORDM+T、ORDM+C。

表2记录了数据集Sphere_a、Torus、Garage、Cubicle，在添加噪声的情况下，Chordal、ORDM+T、ORDM+C算法对数据集优化得到的损失函数值（表中加黑数据说明对应算法最优）。从表中可知，随着噪声的增大，基于ORDM的两种算法的损失函数值始终低于Chordal算法，说明ORDM算法的鲁棒性较高。在Sphere_a、Cubicle各类噪声数据集中，ORDM+C算法的损失函数值始终保持最低水平，但与ORDM+T优化结果差距较小；在Garage噪声数据集中，ORDM+T算法损失函数值最低，优化效果最好。可知，初始化算法的选择对于ORDM算法的鲁棒性影响较小。即使与复杂度较低，优化效果较差的SPT算法相结合，ORDM算法依旧较为鲁棒。

图片

图3给出了相对测量边数、位姿点数、噪声对3种算法运行时间的影响。从图3（a）、图3（b）可知，在噪声标准差为0.2rad的情况下，边数或点数的增多会导致运行时间的增加，ORDM+T与Chordal算法的运行时间相差很小，ORDM+C的运行时间几乎是Chordal算法的两倍；对于大型PGO数据集，ORDM+T的运行时间也是保持较低水平，说明对于各类规模的数据集，ORDM+T的运行时间在可接受范围之内，具有一定的可伸缩性。从图3（c）可知，在Cubicle数据集中，噪声大小对3种算法的运行时间几乎没有影响。

图片

3.2.2 初值问题

针对ORDM算法无需良好初始值问题，选择目前较新的迭代初始化算法——RS算法，在无添加噪声的PGO公开数据集上进行比较。由于ORDM算法与RS算法均需要初始值，此次实验选择复杂度低，运行速度快的SPT算法提供初始值。

表3记录了Sphere_a、Garage、Torus、Cubicle四个数据集在SPT算法初始化下，ORDM算法与RS算法对数据集的优化结果。可以发现，在不同类型的数据集中，ORDM算法无论是损失函数值，还是运行时间都要优于RS算法。

图片

在损失函数值上，ORDM算法优化Torus和Cubicle得到的损失函数值比RS算法的优化结果降低了1~2个数量级；Garage的损失函数值相比于RS算法降低了4个数量级。在这4种PGO数据集上，ODRM算法相比于SPT算法，其损失函数值的数量级更是大幅度降低。在算法运行时间上，ORDM算法对于相对位姿测量边在10 000以内的数据集（如：Shpere_a、Torus、Garage），运行时间保持在0.5s以内；对于大型数据集Cubicle的运行时间比RS算法少0.451s。

此实验说明ORDM算法对初始值没有要求，更适合与复杂度较低的初始化算法（如SPT）相结合。RS算法属于迭代优化算法，需要良好的初始值。较差的初始值会使得RS一类的迭代算法陷入局部极小值，收敛速度慢，难以达到最优。

图片

图片

图片

图片

图4~图7分别是SPT算法、RS算法以及ORDM算法对Sphere_a、Torus、Garage、Cubicle优化结果。可以看出，ORDM算法优化的轨迹图明显优于SPT算法和RS算法。图4~图7说明ORDM算法无需良好的初始值就可精确优化机器人轨迹，由于SPT算法提供的初始值较差，RS算法不能达到轨迹最优，甚至出现越优化越差的现象。

04  结论

PGO是机器人感知领域中重要的轨迹优化方法。本文提出一种基于偏差矩阵的PGO初始化算法——ORDM。该算法以提出的CN模型为目标公式，致力于将CN模型转化为矩阵的形式，并采用线性最小二乘求出封闭解。ORDM算法无需迭代，对初始值无要求，甚至在初始值不良时，更能发挥其优势。最后，在PGO公开数据集上进行实验，说明了ORDM算法在面对噪声数据集时较为鲁棒；可应对各类规模的数据集，具有一定的可伸缩性；适用于初始值较差的机器人轨迹场景。

ORDM算法中需要矩阵求逆，因此在接下来的工作中，将探讨如何快速矩阵求逆，提高初始化运行速度。其次，希望可以将CN算法运用到g2o框架之中，运用到实际场景。

1. 【代码实现】基于 Gym-CarRacing 的自动驾驶项目：路径训练功能

2. ROS+Gazebo强化学习从虚拟训练到实车部署全流程分析

3. 干货丨26篇值得细读的多机器人协作SLAM论文

4. 空中机器人复杂环境高效自主导航—从单机到集群

5. 高翔博士-『自动驾驶与机器人中的SLAM技术』