光束平差法中的一种改进LM算法

2022-03-14李国民宿梦瑶朱代先

西安科技大学学报 2022年1期

李国民，宿梦瑶，朱代先

(西安科技大学通信与信息工程学院，陕西西安 710060)

0 引言

光束平差法(bundle adjustment，BA)[1]，也称作束调整、捆集调整，是目前视觉SLAM(simultaneous localization and mapping，同时定位与地图构建)后端优化中一种重要的非线性优化方法。在以图优化为主体框架的视觉SLAM中，BA将一个复杂的最小二乘问题转变成由节点和边构成的问题，进而将SLAM中复杂的非线性最小二乘问题通过图的方式直观表达，便于后期优化[2]。

BA的核心思想是通过计算三维空间中特征点的像素坐标与重投影坐标之间的误差作为重投影误差，求其最小时对应的路标点坐标和相机参数。具体实现是利用重投影误差构成最小二乘问题，再采取迭代法进行求解。传统迭代方法有梯度下降法、牛顿法、高斯牛顿法(gauss newton，GN)和LM(levenberg marquardt)法[3]等。

目前，LM迭代算法的应用最为广泛。但LM算法在计算时，引入的信赖域半径过大或过小都会对算法的计算效率产生较大影响：信赖域半径过大时，LM算法近似于梯度下降法，导致算法的迭代速度变慢，效率低；信赖域半径过小时，算法近似于高斯牛顿法，稳定性差，计算时要求函数的雅可比矩阵列满秩。因此，如何选取一个合适信赖域半径是LM算法研究过程中的难点。

受上述改进方法的启发，笔者在文献[6]改进的基础上，对参数的计算方法进行重新定义，将前一次的迭代结果引入到后一次信赖域半径的计算中[10]，可有效减小因xk远离解集时Fk较大产生的影响，使LM算法在计算过程中在不必假设Jk为非奇异的局部误差界的条件约束下具有二阶收敛性，进一步加快计算，提高算法的稳定性和效率[11-12]。

1 光束平差算法

光束平差法(BA)是视觉SLAM后端优化的一种优化方法[13]，属于基于图优化的非线性优化，也是目前SLAM后端优化的主流方法[14-15]。

光束平差法的核心思想是最小化重投影误差。如图1所示，表示光束平差法优化问题[16]。

图1 光束平差法优化问题

图中Y1，Y2…为路标的三维坐标，X1，X2，…为相机的位姿。如图1所示，不同地图点在不同相机位姿(帧)中对应不同的投影特征点。

根据图1，光束平差优化问题可以描述为：机器人在运动过程中，可以获得一系列的路标地图点在不同时刻相机位姿中对应的像素坐标，而通过特征点匹配方法可以计算得到同一点在对应图片中的匹配投影点[17]。理论上2个像素点应该是重合的，但由于测量误差与外界环境等因素的影响，实际情况中2个像素点坐标之间存在差异，2个坐标的差异值称为重投影误差，以其作为目标函数，平方作为代价函数，构建非线性最小二乘问题[18]，见式(1)

(1)

式(1)表示n个地图点在m个帧中；向量xi,j为帧j上的第i个点投影坐标，这个是实际点测量坐标；Q(Xj,Yi)为点i在帧j上的预测投影坐标，这个是理想点坐标；e(x,y)为向量x,y的误差，即重投影误差。公式的意义就是最小化n个点的重投影误差，即求解由重投影误差构成的最小二乘问题。由此，光束平差问题就化简成了使得所有的特征点相关的欧式距离和最小的最小二乘问题，其目的是使代价函数和最小，求得代价函数和最小时对应的路标点的坐标和相机运动参数[19-20]。

对于最小二乘问题的求解，由于机器人位姿由李代数表示，难以直接求解，因此常采用迭代的方式求解[21]。常用的迭代方法有梯度下降法、牛顿法、高斯牛顿法和LM法。梯度下降法迭代速度快，但接近最优点时，其迭代方向呈折线形，导致收敛速度慢，计算效率低；高斯牛顿法计算简单且效率高，但计算时要求计算过程中采用的近似矩阵列满秩，否则会出现奇异矩阵或病态问题，导致迭代不收敛；LM算法引入信赖域问题，可以看作是对高斯牛顿法的改进，克服高斯牛顿法的不足，得到更稳定更准确的增量，因此应用最为广泛[22]。

2 LM算法

对于一个最小二乘问题

(2)

式中x∈Rn，f为任意非线性函数。对于复杂f函数，常采用迭代法进行求解。

求解时，高斯牛顿法是最优算法中最简单的方法之一，它将f(x)一阶泰勒展开。

f(x+Δx)≈f(x)+J(x)Δx

(3)

式中J(x)为雅可比矩阵，表示f(x)关于x的导数。当前需求解增量Δx，使‖f(x+Δx)‖2最小，即求

(4)

式(4)对Δx求导，并令导数为零，得到增量Δx为

Δx=-(J(x)TJ(x))-1J(x)Tf(x)

(5)

为便于记忆，将上式记为

Δx=H-1g

(6)

式中g=-J(x)Tf(x),H为海塞阵，这里用J(x)TJ(x)近似表示。由式(5)和式(6)知，计算时J(x)矩阵必须为列满秩矩阵，以保证计算时的近似H矩阵可逆且正定，但实际数据计算得到的J(x)TJ(x)只有半正定性，因此在使用高斯牛顿法时可能会出现H为奇异矩阵或病态的情况，导致求出的增量过大，这样无法保证迭代的收敛，也会导致采用的局部近似不够准确[23]。

为克服高斯牛顿法增量过大导致迭代不稳定的缺点，同时结合梯度下降法和高斯牛顿法的优点，列文伯格提出通过在对角线上增加非负常数λ来修改增量方程，得到列文伯格·马夸尔特(levenberg marquardt，LM)算法[24]，其增量方程为

Δx=(H+λD)-1g

(7)

一般地，λ称为信赖域半径或阻尼因子，D为对增量进行转换的矩阵，在LM算法中，为简化计算，常用单位矩阵I代替。由式(7)可知，当λ=0时，就得到了高斯牛顿法；当λ很大时，近似得到梯度下降法，因此，可以将LM算法看作是梯度下降法和高斯牛顿法的一种结合。

LM算法允许多次迭代至收敛，迭代步长被控制在一个高斯牛顿所生成的二次多项式近似被信任的区域内，因此，LM算法又被称为信赖域方法。

LM算法步骤如下

步骤1：

参数初始化：允许误差e，k=0，x0，λ0，最大迭代次数kmax=1 000。

步骤2：

计算H=JTJ，G=-JTf。

步骤3：

根据增量方程Δx=(H+λI)-1g，求解得出Δx。

步骤4：

若‖JTf‖≤e或达到最大循环次数，则结束循环。

步骤5：

计算

xnew=x+Δx

(8)

(9)

式中F(x)为目标函数；L(x)为近似模型函数；r为两者的比率，用来判断迭代的有效性。

步骤6：

若r>0.75，则x=xnew，H=JTJ，g=-JTf，λ=λ/3。

若r<0.25，则λ=λ*2。

步骤7：

若‖JTf‖≤e，结束循环，否则令k=k+1，转到步骤3。

目前大部分BA求解都是用LM算法，它是解决非线性最小二乘问题最为常用的方法，采用LM算法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量Δx。受文献[6]的启发，笔者对LM算法进行改进，使LM算法在不必假设Jk为非奇异的局部误差界的条件约束下具有二阶收敛性，并且可以有效减小因xk远离解集时Fk较大所产生的影响。

3 改进的LM算法

3.1 改进算法

文献[6]中的改进方法，是将信赖域半径定义为

(10)

笔者在文献[6]的基础上进行改进，使其在保证收敛的同时，进一步提高算法的效率，故将算法的信赖域半径定义为

(11)

由式(10)可知，增加‖Fk‖的幂进行计算可以更明显表示出迭代后目标函数的相对变化程度，影响迭代增量的计算，提高算法的迭代效率，但幂指数过大会使计算量增多，增加计算时间，降低算法效率。受文献[6]和[10]的启发，文中通过计算‖Fk‖的平方加快迭代，有效提高计算效率。由式(11)可知，当xk远离解集时，Fk较大，‖Fk‖2/(1+‖Fk‖2)趋近于1，λk接近于μk；当xk接近于解集时，Fk趋向于0，λk接近于μk‖Fk‖2。

改进LM算法的计算步骤与传统LM算法步骤相似，仅在计算出相对下降比率rk后再进行以下计算。

1)判断rk

若rk≥p0，令xk+1=xk+Δx；

否则令xk+1=xk。

2)判断rk

若rk

若rk>p2，令μk+1=μk/6，

否则，令μk+1=μk。

通过以上2步运算，可以判断迭代效果的好坏以及迭代是否可取，进而调节相应参数，提高算法效率。

3.2 收敛性推导

已知定理：对任意函数和任意初始变量的迭代算法，迭代收敛的充分必要条件是

ρ(h)<1

(12)

式中h为迭代步长；ρ(·)函数的定义为

(13)

在文中，迭代步长由信赖域半径决定，且计算过程中的可调参数μk∈(0,1)，由此可以推出

(14)

故得

ρ(λ)<1

(15)

满足迭代收敛条件，因此改进的LM算法收敛。

4 实验结果及分析

实验的硬件环境为笔记本电脑(英特尔i5处理器，4G运行内存)，实验环境为MATLAB 2016a。

4.1 收敛性分析

为验证改进LM算法的收敛性，使用2组自建数据集进行实验。数据集分别是递增和递减的2组数据，各自包含20个点和30个点。对于2组实验数据，分别使用LM算法、文献[6]改进的LM算法和文中改进的LM算法(I-LM)3种方法，在设置相同参数的条件下进行拟合，计算不同迭代次数时的均方根误差(RMSE)，用来表示分析算法的收敛性，均方根误差计算见式(16)

(16)

结果如图2、图3所示。

图2 数据一均方根误差

图3 数据二均方根误差

图2和图3显示了2组数据均方根误差对比。横、纵坐标分别代表迭代次数和均方根误差。从图2和图3可以看出，文献[6]LM算法和I-LM算法，随迭代次数增加，RMSE都不断减小，并且当迭代次数达到一定数值时，RMSE的值不再变化。这表明当迭代达到一定次数时，文献[6]LM算法和I-LM算法的误差都不再减小，趋于平稳。文献[6]证明其改进的LM算法是收敛的，由此表明，改进的LM算法也具有相似的收敛性。

4.2 改进LM算法结果分析

为验证文中改进LM算法的性能，将LM算法、文献[6]LM算法和I-LM算法分别应用到2组自建数据集中，结果见表1、表2。2张表格分别记录2组数据使用3种方法拟合的迭代次数、均方根误差和运行时间，并进行对比分析。

表1 数据一3种算法结果对比

表2 数据二3种算法结果对比

由表1可知，使用LM、文献[6]LM和I-LM的3种算法，其RMSE相同，表明3种算法的最终计算结果相同；但从迭代次数看，传统LM算法的数值最大，文献[6]LM算法次之，I-LM算法最小，说明在达到相同的迭代效果时，传统LM算法需要进行运算的次数最多，I-LM算法的次数最少；从运行时间看，LM算法的运行时间最长，文献[6]LM算法次之，I-LM算法最短。通过以上数据对比，可以得出，I-LM算法较传统LM算法和文献[6]LM算法在迭代效率和运行时间上都有改进：在达到相同的计算结果的前提下，I-LM算法使用的迭代次数最少，运行的时间最短，进而说明改进算法的效率更高。

由表2可知，LM算法最终计算结果的RMSE比文献[6]LM算法和I-LM算法的数值大，表明LM算法的计算误差较大，文献[6]LM算法和I-LM算法的结果更优；从迭代次数看，文献[6]LM算法的数值最大，I-LM算法最小，说明在达到相同的迭代效果时，文献[6]LM算法需要进行运算的次数最多，I-LM算法的次数最少；从运行时间看，LM算法的运行时间最长，I-LM算法最短。以上数据说明，虽然LM算法比文献[6]LM算法的迭代次数少，但其RMSE更大，也就是说[6]-LM算法迭代多次后可以得到更好的运算结果，效果更好。I-LM算法在RMSE、迭代次数和运行时间方面的值均为最小值，说明I-LM算法相较于LM算法，在进行少次迭代的情况下，可以达到更好的迭代效果，运行时间也最短；相较于文献[6]LM算法，I-LM算法在达到相同效果的情况下，需要进行运算的次数最少，时间也最短。

综合以上结果，改进的LM算法可以使用最少的迭代次数达到或者优于LM算法和文献[6]LM算法计算的数据结果，同时使用的时间也最短，进一步表明改进的LM算法可以显著提高算法的运算效率。

4.3 改进LM算法应用仿真实验

为进一步验证改进算法的性能，将基于改进前后的算法的光束平差法分别应用于优化仿真，采用2D的2 MIT Killian Court数据集进行实验。

实验进行3组，分别使用基于传统LM算法(LM-BA)、文献[6]LM算法(文献[6]中LM-BA)和文中改进的LM算法(I-LM-BA)的光束平差法对根据位姿顶点等数据构建的轨迹图进行优化，实验结果如图4～6所示。

图4 LM算法轨迹图优化

图4～6的横纵坐标分别表示机器人位置，单位为米(m)。图4表示LM-BA优化，图5表示文献[6]中LM-BA优化，图6表示I-LM-BA优化，每幅图中蓝色线条表示初始构建的轨迹图，绿色线条表示一次优化后的轨迹图，红色线条表示2次优化后的轨迹图，紫色线条表示4次优化后的轨迹图。从图4可以看出，采用LM-BA对轨迹图进行优化时，算法没有产生奇异性导致优化的轨迹图不准确的问题，但是每次优化后的轨迹图都不相同，说明算法的效率比较低，导致每次优化计算都会产生较上一次更为准确的轨迹图；图5和图6所示的图形中，2次优化和4次优化后的轨迹图是几乎重合，表明文献[6]中LM-BA和I-LM-BA这2种方法都可以更快的完成优化，同时对比3组实验中4次优化后的轨迹图，文献[6]中LM-BA和I-LM-BA这2种方法优化的轨迹图更为精确，表明2种方法速度更快、精度更高；对比图5和图6，图5中一次优化后的轨迹图产生畸变，这是由于其计算过程中出现奇异性问题引起的，而图6中，采用I-LM-BA方法，一次优化后的轨迹图虽然不是完全平滑，但其畸变程度大大减弱，表明I-LM-BA方法更加稳定，同时，采用I-LM-BA方法，2次优化和4次优化后的轨迹图几乎完全重合，而采用文献[6]中LM-BA方法，2次优化后和4次优化后的轨迹图大致重合，但还是明显可以看出两者的差异，并且2次优化后的轨迹图仍旧存在畸变，需进一步优化才可以消除，综上，基于改进LM算法的光束平差法效率更高，也更稳定。