融合信赖域的SLAM后端优化算法*

王华宇，雷 斌

(武汉科技大学 a.机械传动与制造工程湖北省重点实验室；b.机器人与智能系统研究院，武汉 430081)

0 引言

视觉与惯性导航融合里程计(Visual-Inertial Odometry, VIO)是当前解决SLAM高精地图与多场景适用的主流方案之一。VIO可以根据传感器数据融合框架分为松耦合与紧耦合[1]，也可以根据优化方式分为滤波法与优化法[2]。松耦合忽略不同传感器数据之间的相关性，视觉运动估计和IMU运动估计结果相互独立，将估计结果进行融合，损失了部分数据信息，精度较低；紧耦合则使用不同传感器原始数据共同进行估计，一次性建模所有的运动和测量信息，更容易得到最优结果，是目前研究的重点。

MSCKF[3]是基于卡尔曼滤波的紧耦合方法，将多帧图像的相机位姿加入卡尔曼状态向量。该方法鲁棒性高，计算复杂度低，但没有全局优化和回环检测。MSF[4]是基于滤波法的EKF松耦合框架，该方法极大地提升了系统运行速度，系统拓展性好，可接入GPS、激光雷达、码盘等其它传感器，但精度很差。ROVIO[5]是基于稀疏图像块和EKF滤波的VIO，惯性测量用于状态传递过程，视觉信息用于更新；其速度与MSCKF相似，精度较高，计算量小，但不同设备需要调整参数，没有回环检测。OKVIS[6]将视觉观测与IMU观测一起优化求解位姿与3D地图点，前端采用Harris角点检测和BRISK特征描述子进行匹配，计算资源消耗大，不支持重定位和闭环检测。该方法采用紧耦合策略和非线性优化，精度高但速度慢。

VINS-Mono[7]在精度上可以媲美OKVIS，具有比OKVIS更加鲁棒和完善的初始化以及闭环检测过程。但随着SLAM轻量化趋势的发展，一些小型设备开始支持SLAM技术[8]，要在有限的计算资源条件下，发挥出更加优秀的实时性能，VINS-Mono仍然存在提升的空间。

文章在保证原系统精度的前提下，对VINS-Mono系统优化过程中耗时较多的部分进行优化，使用融合信赖域改进的TR算法替换Levenberg-Marquardt算法，对最小二乘问题求解部分进行优化，采用并行运算对信息矩阵拼接部分进行加速。实验结果表明，优化后的算法提高了系统运行速度、改善了系统的实时性能，更加适合大规模的后端优化问题。

1 后端优化速度提升原理分析

经典视觉SLAM系统主要包括4个模块：前端视觉里程计(Visual Odometry)、后端优化、回环检测、地图构建。后端非线性优化过程中的集束调整[10](Bundle Adjustment, BA)是除前端特征提取与跟踪之外，最消耗计算资源的部分。

BA可以归结为最小化图像点观测值和预测值之间的重投影误差。重投影误差表现为大量非线性实值函数的平方和，通过Levenberg-Marquardt[11](LM)等最小二乘算法来最小化重投影误差是当前最常见的做法。相对容易的实现过程和有效的阻尼策略能够让LM在大范围的初始估计中迅速收敛，使得LM算法成为众多BA实现的基准。通过迭代线性化函数，使其在当前估计邻域内最小化，LM算法涉及到线性系统的解，即正规方程。在求解BA中的最小化问题时，由于不同的三维点和相机参数之间缺乏相互作用，使得正规方程矩阵具有稀疏块结构。利用正规方程矩阵的稀疏性，可以加速正规方程的求解过程，从而加快BA的优化速度。

LM算法是Gauss-Newton和最速下降法结合的衍生算法，通过迭代的方式求解多个非线性实值函数的平方和组成的多元函数的局部极小值。LM算法在当前解远离局部极小值时表现出最速下降法的性质：收敛缓慢，但保证收敛性；在当前解靠近局部极小值时表现出Gauss-Newton法的性质：迅速收敛。

忽略二次项，将残差函数进行一阶泰勒近似展开：

f(p+δ)≈l(p)≡f(p)+J(p)δ

(1)

J(p)=∂f(p)/∂p为残差函数的雅克比(Jacobian)矩阵，δ为迭代步长。将残差函数代入损失函数：

(2)

如此，就将损失函数近似成一个二次函数线性模型L，如果雅克比矩阵满秩，则JTJ正定，损失函数有最小值。令损失函数一阶导为零，可得高斯牛顿方程：

(JTJ)δgn=-JTf

(3)

式(3)也可以称为正规方程，即用Gauss-Newton步长δgn作为正规方程的解。雅克比矩阵J与海塞(Hessian)矩阵H=JTJ均具有稀疏结构，求解正规方程是整个优化问题的核心。

Levenberg和Marquardt对Gauss-Newton法进行的改进，是在式(3)的基础上引入阻尼因子μ：

(JTJ+μI)δlm=-JTf,μ≥0

(4)

式(4)也被称为增广正规方程，I为单位矩阵。对JTJ做特征值分解后有：

(5)

半正定的信息矩阵特征值{λi}和对应的特征向量为{Vj}。阻尼因子μ的大小是相对于信息矩阵JTJ的元素而言的，一个简单的初始值策略为μ0=τ·max{(JTJ)ii}，通常τ～[10-8,1]。

使用增益ρ控制迭代更新：

(6)

2 融合信赖域改进的后端优化方法

当前大部分的集束调整方法均是采用的LM算法，很少有除利用正规方程矩阵稀疏性之外的集束调整加速方法被提出。但LM算法需要对每一个待估计参数求偏导，而且求解过程中存在矩阵求逆运算，如果目标函数f非常复杂，或者有大量的待估计参数，LM算法就不再是集束调整最优的选择。针对这一问题，提出了一种融合信赖域(Trust Region)的优化方法—TR算法，用于SLAM系统中的后端优化过程。

2.1 TR算法

在一个信赖域框架内，构造目标函数的二次函数模型，该函数模型在当前点附近的行为与目标函数相似，但该函数模型仅对以当前点为中心、半径为Δ的超球面上的点精确表示目标函数，因此被称为信赖区域，Δ为信赖域半径。近似线性化模型为：

f(p+αδ)≈f(p)+αJδ

(7)

(8)

其中，

(9)

Gauss-Newton法的迭代步δgn由正规方程(3)给出，最速下降法的迭代步δsd由δsd=-JTf给出，此时改进的LM算法迭代步长δtr有两个候选迭代步可供选择：δtr=αδsd与δtr=δgn。在信赖域半径为Δ的条件下使用以下策略来选择迭代步：

如果δgn≤Δ,δtr=δgn；

LM算法中使用增益ρ控制阻尼参数更新，而在TR算法中，增益ρ被用来控制信赖域半径Δ更新，如式(10)所示：

(10)

其中，

(11)

p的更新策略为：

(12)

其中，ρ的取值范围划分采用的是经验值。ρ比较大，说明线性化模型效果很好，可以适当增大信赖域半径来增大迭代步长，靠近Gauss-Newton迭代方向；若ρ比较小，则需要减小信赖域半径来减小迭代步长，使迭代靠近最速下降迭代方向。

相较于原有的LM算法，TR算法不仅能从正规方程矩阵的稀疏性当中受益，在大规模问题求解上，TR算法无需对待估计参数求偏导，所需的计算量更低，这一点对于SLAM中一幅图像数百个特征点构建的超大维度线性方程求解尤为重要。

在迭代的过程中，如果遇到当前迭代步没有使计算结果收敛的情况，LM算法需要重新计算Hessian矩阵来进行下一次迭代；而TR算法分别求解了Gauss-Newton法和梯度下降法的迭代步，在信赖域内使用一定的策略来进行选择，当遇到坏的迭代步时不再需要像LM算法一样重新计算Hessian矩阵。因此，TR算法更适用于SLAM问题中的大规模BA优化过程。

2.2 并行运算加速信息矩阵拼接

随着SLAM系统在新环境中的探索不断深入，越来越多的相机与IMU姿态以及环境图像特征被加入到残差函数中，最小二乘残差就会不断增多，信息矩阵数量越来越大，计算量也随之加大。为了保持优化变量的个数在一定范围内，采用滑窗优化动态增加或移除优化变量，是当前流行的处理方法。滑窗优化中，增加或者移除优化变量，需要将对应残差的信息矩阵拼接起来。而在进行矩阵拼接的时候需要进行大量计算和频繁的存储器访问。

如图1a所示，当前的计算机处理器是基于x86指令集的串行架构设计的，在设计思路上是为了尽快地完成一个任务，因此在处理矩阵拼接这种需要高度并行、运算密集、分多个阶段执行的问题时，其缺陷显而易见。使用并发编程模型可以有效解决这个问题。如图1b所示，并发编程可以通过共享变量进行数据交换来共享内存，按照for循环将单线程的单个任务使用多线程进行处理，能够大幅提升信息矩阵的拼接速度，减少运算时间，提升程序实际运行效率。

(a) 串行运算示意图

(b) 并行运算示意图图1 串并行运算示意图

3 实验

实验基于VINS-Mono开源框架完成，实验所需视觉及IMU数据采用EuRoC公开数据集中的MH_05_difficult序列。实验平台为Intel i7-8750H@2.20GHz (16GB RAM)。采用绝对轨迹误差(ATE)测试系统在精度和速度上的变化。绝对轨迹误差(ATE)是估计值与地面真实轨迹之间的绝对距离，将估计值与地面真值根据位姿时间戳对齐，计算每对位姿之间的差值并以图表的形式输出。ATE具有直观的可视化性能，非常适合用于评估视觉SLAM系统的性能。

3.1 LM算法改进前后对比实验

该部分主要进行了三组对比实验，第一组为原始LM算法与地面真值(GT)，第二组为TR算法与地面真值，第三组为TR算法与原始LM算法。使用SLAM性能评估工具EVO对系统轨迹绝对误差进行测试，测试项目为最大轨迹误差max、轨迹误差平均值mean、轨迹误差中值median、轨迹误差最小值min、均方根误差rmse、误差平方和sse以及标准差std。表1中给出了算法改进前后绝对轨迹对比实验的各项数据。

如表1所示，原始LM算法与地面真值的最大轨迹误差约为0.483 m，误差平均值约为0.226 m，均方根误差约为0.242 m；TR算法与地面真值的最大轨迹误差约为0.426 m，误差平均值约为0.226 m，均方根误差约为0.239 m；改进前后的轨迹误差最大值约为0.105 m，误差平均值约为0.033 m。改进前后的系统精度几乎保持不变。

表1 绝对轨迹误差对比实验 (m)

图2所示为TR算法与地面真值的轨迹对比结果。虚线为地面轨迹真值，靠右部分柱状图中刻度从下往上依次为绝对轨迹误差最小值、中值、最大值。图3所示为融合信赖域改进前后的算法时间消耗对比结果。横坐标为图像帧序列，纵坐标为处理该帧所消耗的时间。原始LM算法的平均时间消耗约为89.34 ms，TR算法的平均时间消耗为62.43 ms，速度提升了近30.12%。实验结果表明，TR算法在以极小的精度损失为代价的情况下，节省了大量计算资源，极大地减少了优化过程的时间消耗，提升了后端优化算法的性能。

图2 TR算法与地面真值轨迹对比实验结果

图3 改进前后LM算法与TR算法时间消耗对比

3.2 并行加速对比实验

使用多线程编程对系统代码进行优化，提升信息矩阵拼接速度，使用EVO工具测试优化前后系统的精度和速度变化。表2所示为并行加速优化算法与TR算法、并行加速优化算法与原始LM算法两组对比实验的结果。由表可知，并行加速后的轨迹与地面真值的最大轨迹误差约为0.426 m，平均误差约为0.226 m，均方根误差约为0.238 m；与TR算法的轨迹相比对，最大误差约为0.105 m，平均误差约为0.033 m，均方根误差约为0.035 m；即并行计算加速前后系统的精度损失极小。图4所示为并行运算加速前后的算法Hessian矩阵拼接部分耗时对比结果。加速前Hessian矩阵拼接平均耗时32.09 ms，加速后平均耗时为7.90 ms，速度提升近75.38%。

如表2和图4所示，并行加速以极小的系统精度为代价，获得了明显的速度提升。实验结果表明，并行运算能够加速后端优化过程中的矩阵拼接过程，提高系统运行速度。

表2 并行加速实验绝对轨迹误差结果 (m)

图4 并行加速优化前后时间消耗对比

4 结论

本研究针对轻量化SLAM系统中，后端优化消耗大量计算资源与时间这一问题，提出了一种融合信赖域改进的后端优化方法。该方法通过优化最小二乘法求解步骤，能够显著提升最小二乘问题求解速度，加快后端优化速度；同时，使用并行运算加快信息矩阵拼接速度，能够在不降低精度的前提下，提升SLAM系统的实时性能。实验结果表明，经该方案优化的集束调整更加适合大规模的SLAM后端优化问题，优化后的算法迭代求解速度提升了近30.12%，实时性能表现更佳。