光束平差法中的一种改进LM算法
2022-03-17李国民宿梦瑶朱代先
李国民 宿梦瑶 朱代先
摘 要：针对视觉SLAM后端非线性优化存在计算速度慢、优化效果差等问题，采用BA图优化方法，对其求解策略列文伯格-马夸尔特（LM）算法进行改进，改善LM算法计算过程中雅可比矩阵可能存在的奇异性或病态问题，增强LM算法的稳定性，提高LM算法的效率和BA-SLAM的精度。改进的算法将前一次的迭代结果引入到后一次信赖域半径的计算中，可减小因当前解远离解集时目标函数较大所产生的影响，同时在不假设雅可比矩阵非奇异的条件下，使其具有二阶收敛性，提高算法的稳定性和计算速度，提升光束平差法中LM算法的稳健性与效率。仿真实验结果表明，提出的改进LM算法与传统LM算法和文献[6]提出的改进LM算法相比，在相同精度时使用的迭代次数更少，计算效率高;采用改进LM算法的光束平差法具有更高的优化精度和稳健性。
关键词：后端优化;光束平差法;重投影误差;最小二乘;LM算法
中图分类号：TP 242 文献标志码：A
文章编号：1672-9315（2022）01-0152-08
DOI：10.13800/j.cnki.xakjdxxb.2022.0120开放科学（资源服务）标识码（OSID）：
An improved LM algorithm in bundle adjustment
LI Guomin，SU Mengyao，ZHU Daixian
（College of Communication and Information Engineering，Xi’an University of Science and Technology，Xi’an 710054，China）Abstract：In order to solve the problems of slow calculation speed and poor optimization effect in the non-linear back-end optimization of the visual SLAM，the bundle adjustment（BA）graph optimization method is adopted to improve the solution strategy，Levenberg-Marquardt（LM）algorithm，thus ameliorating the singularity or ill-conditioned problems that may exist in Jacobian matrix during the calculation process and with the stability of the LM algorithm enhanced.And the efficiency of the LM algorithm is improved and the accuracy of BA-SLAM is promoted.In the improved algorithm the results of the previous iteration
are substituted
into the calculation of the next trust region radius，and
the impact of the larger objective function is accordingly reduced when the current solution is far away from the solution set.At the same time，it has second-order convergence without assuming that the Jacobian matrix is not singular，an improvement of the stability and calculation speed of the algorithm，by which the robustness and efficiency of the LM algorithm in the bundle adjustment method are improved.The simulation results show that compared with the traditional LM algorithm and the improved LM algorithm proposed in literature[6]，the improved LM algorithm uses fewer iterations are used at the same accuracy in the improved LM algorithm with computational efficiency higher.The bundle adjustment method using the improved LM algorithm has higher optimization accuracy and robustness.Key words：back-end optimization;bundle adjustment;re-projection error;least squares;LM algorithm
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算法研究过程中的难点。针对上述问题，一些学者提出很多策略对其进行改进，通过对信赖域半径采用不同的计算方式，以达到保证算法收敛，提高计算效率的目的。LIANG采用2步加速的LM算法，可有效提高算法的计算精度，但使算法复杂，增加计算成本[4];UMAR提出迭代参数分别为
λk=‖JTkJk‖/‖JTkJk‖2，
λk=‖JTkJk‖，λk=‖Jk‖/‖Jk‖2
，以处理更加广泛的问题，但存在由奇异点导致的不稳定问题[5];
当λk=（μk‖Fk‖）/（1+‖Fk‖）时，LM算法可在不必假设Jk为非奇异的局部误差界的条件约束下具有二阶收敛性[6];
WANG等通过信任区域技术进行迭代更新，证明了当
λk=ηk‖JTkFk‖α，（0≤α≤2）
时，算法全局收敛，但选取的点远离解集时，Fk较大，会使LM算法的计算效率比较低[7];
CHEN和MA等提出多步迭代的方法，节省雅克比矩阵的计算量，有效地提高了计算效率[8-9]。
受上述改进方法的启发，
笔者在文献[6]改进的基础上，对参数的计算方法进行重新定义，将前一次的迭代结果引入到后一次信赖域半径的计算中[10]，可有效减小因
xk远离解集时Fk较大产生的影响，使LM算法在计算过程中在不必假设Jk为非奇异的局部误差界的条件约束下具有二阶收敛性，进一步加快计算，提高算法的稳定性和效率[11-12]。
1 光束平差算法光束平差法（BA）是视觉SLAM后端优化的一种优化方法[13]，属于基于图优化的非线性优化，也是目前SLAM后端优化的主流方法[14-15]。光束平差法的核心思想是最小化重投影误差。如图1所示，表示光束平差法优化问题[16]。
…为相机的位姿。如图1所示，不同地图点在不同相机位姿（帧）中对应不同的投影特征点。根据图1，光束平差优化问题可以描述为：机器人在运动过程中，可以获得一系列的路标地图点在不同时刻相机位姿中对应的像素坐标，而通过特征点匹配方法可以计算得到同一点在对应图片中的匹配投影点[17]。理论上2个像素点应该是重合的，但由于测量误差与外界环境等因素的影响，实际情况中2个像素点坐标之间存在差异，2个坐标的差异值称为重投影误差，以其作为目标函数，平方作为代价函数，构建非线性最小二乘问题[18]，见式（
1）
min
XjYi
∑ni=1
∑mj=1
e（Q（Xj，Yi），xi，j）2
（
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算法对于一个最小二乘问题
min
x=12‖f（x）‖22
（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最小，即求
Δx=argminΔx
12
‖f（x）+J（x）Δx‖2
（4）式（4）对Δx求导，并令导数为零，得到增量Δx为
Δx=-（J（x）TJ（x））-1
J（x）Tf（x）
（5）为便于记忆，将上式记为
Δx=H-1g
只有半正定性，因此在使用高斯牛顿法时可能会出现H为奇异矩阵或病态的情况，导致求出的增量过大，这样无法保证迭代的收敛，也会导致采用的局部近似不够准确[23]。为克服高斯牛顿法增量过大导致迭代不稳定的缺点，同时结合梯度下降法和高斯牛顿法的优点，列文伯格提出通过在对角线上增加非负常数λ来修改增量方程，得到列文伯格·马夸尔特（levenberg marquardt，LM）算法[24]，其增量方程为
Δx=（H+λD）-1g
（7）一般地，λ称为信赖域半径或阻尼因子，
D为对增量进行转换的矩阵，在LM算法中，为简化计算，常用单位矩阵I代替。由式（7）可知，当λ=0时，就得到了高斯牛顿法;當λ很大时，近似得到梯度下降法，因此，可以将LM算法看作是梯度下降法和高斯牛顿法的一种结合。LM算法允许多次迭代至收敛，迭代步长被控制在一个高斯牛顿所生成的二次多项式近似被信任的区域内，因此，LM算法又被称为信赖域方法。LM算法步骤如下
步骤7：若‖JTf‖≤e，结束循环，否则令k=k+1，转到步骤3。目前大部分BA求解都是用LM算法，它是解决非线性最小二乘问题最为常用的方法，采用LM算法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量
Δx。受文献[6]的启发，笔者对LM算法进行改进，使LM算法在不必假设
Jk
为非奇异的局部误差界的条件约束下具有二阶收敛性，并且可以有效减小因xk远离解集时Fk较大所产生的影响。
3 改进的LM算法
3.1 改进算法文献[6]中的改进方法，是将信赖域半径定義为
式中 μk为一个可调参数，随每次迭代的相对变化程度改变;
‖Fk‖=fTkfk
，表示目标函数，用来计算迭代后函数的相对下降，以此判断迭代步长的有效性以及信赖域半径是否需要更新。通过以上运算，LM算法的运算过程中，在保证收敛的条件下，可避免出现因雅克比可能存在的奇异性导致算法不收敛的问题，提高运算速度。笔者在文献[6]的基础上进行改进，使其在保证收敛的同时，进一步提高算法的效率，故将算法的信赖域半径定义为
1）由式（10）可知，增加‖Fk‖的幂进行计算可以更明显表示出迭代后目标函数的相对变化程度，影响迭代增量的计算，提高算法的迭代效率，但幂指数过大会使计算量增多，增加计算时间，降低算法效率。受文献[6]和[10]的启发，文中通过计算
‖Fk‖的平方加快迭代，有效提高计算效率。由式（1
1）可知，当xk远离解集时，
‖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
通过以上2步运算，可以判断迭代效果的好坏以及迭代是否可取，进而调节相应参数，提高算法效率。
3.2 收敛性推导已知定理：对任意函数和任意初始变量的迭代算法，迭代收敛的充分必要条件是
ρ（h）<1
（13）在文中，迭代步长由信赖域半径决定，且计算过程中的可调参数μk∈（0，
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）
图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可知，使用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～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算法的光束平差法效率更高，也更稳定。
5 结 论
1）对传统LM算法系数矩阵可能存在的问题进行分析，并参考反馈原理，提出一种通过重新定义信赖域半径计算方式进而改进LM的方法。2）每次信赖域半径的计算根据前一次的迭代效果自动调整，使LM算法在更稳定的前提下二阶收敛。通过不同的数据实验，文中改进的算法相较于改进前与文献[6]的改进，其迭代次数平均减小约40.1%和38.1%，用时最短，表明该算法更高效、更稳定。3）仿真实验表明，相较于文献[6]的改进，文中提出基于改进LM算法的BA效率更高，可以更稳定、更快完成后端优化。
参考文献（References）：
[1] BUSTOS P，CHIN T J，ERIKSSON A，et al.Visual SLAM：Why bundle adjust？[C]//2019 International Conference on Robotics and Automation（ICRA），2019：2385-2391.
[2]戴天虹，李志成.基于优化列文伯格-马夸尔特法的SLAM图优化[J].哈尔滨理工大学学报，2021，26（2）：68-74.DAI Tianhong，LI Zhicheng.SLAM graph optimization based on optimized Levenberg-Marquardt method[J].Journal of Harbin University of Science and Technology，2021，26（2）：68-74.
[3]MA C，LIU X，WEN Z W.Globally convergent levenberg-marquardt method for phase retrieval[J].IEEE Transactions on Information Theory，2019，65（4）：2343-2359.
[4]LIANG M L，ZHENG B，ZHENG Y T，et al.A two-step accelerated Levenberg Marquardt method for solving multilinear systems in tensor-train format[J].Journal of Computational and Applied Mathematics，2021，382：113069.
[5]UMAR A O，SULAIMAN I M，MAMAT M，et al.On damping parameters of Levenberg-Marquardt algorithm for nonlinear least square problems[J].Journal of Physics（Conference Series），2021，1734（
1）：012018（9pp）.
[6]AMINI K，ROSTAMI F，CARISTI G.An efficient Levenberg-Marquardt method with a new LM parameter for systems of nonlinear equations[J].Optimization，2018，67（5）：637-650.
[7]WANG H Y，FAN J Y.Convergence rate of the Levenberg-Marquardt method under Hlderian local error bound[J].Optimization Methods and Software，2020，35（4）：767-786.
[8]FAN J Y，HUANG J C，PAN J Y.An adaptive multi-step levenberg-marquardt method[J].Journal of Scientific Computing，2019，78（
1）：531
-548.
[9]CHEN L，MA Y F.Shamanskii-like levenberg-marquardt method with a new line search for systems of nonlinear equations[J].Journal of Systems Science and Complexity，2020，33（5）：1694-1707.
[10]王华宇，雷斌.融合信赖域的SLAM后端优化算法[J].组合机床与自动化加工技术，2021（
1）：9-12.WANG Huayu，LEI Bin.SLAM back-end optimization algorithm fused with trust region[J].Modular Machine Tool and Automated Processing Technology，2021（
1）：9-12.
[11]伍珍香，陈亮，周童.一种改进的求解非线性方程组的Levenberg-Marquardt方法[J].延边大学学报（自然科学版），2020，46（4）：302-307，332.WU Zhenxiang，CHEN Liang，ZHOU Tong.An improved Levenberg-Marquardt method for solving nonlinear equations[J].Journal of Yanbian University（Natural Science Edition），2020，46（4）：302-307，332.
[12]周童，陈亮，伍珍香.一种求解非线性方程组的Levenberg-Marquardt方法及其收敛性[J].淮北师范大学学报（自然科学版），2021，42（
1）：1-7.ZHOU Tong，CHEN Liang，WU Zhenxiang.A levenberg-marquardt method for solving nonlinear equations and its convergence[J].Journal of Huaibei Normal University（Natural Science Edition），2021，42（1）：1-7.
[13]季晨，宋燕燕，秦军，等.非线性优化同时定位与地图创建问题[J].软件工程，2020，23（9）：39-42.JI Chen，SONG Yanyan，QIN Jun，et al.Simultaneous positioning and map creation for nonlinear optimization[J].Software Engineering，2020，23（9）：39-42.
[14]张洪华，刘璇，陈付豪，等.基于图优化的SLAM后端优化研究与发展[J].计算机应用研究，2019，36（1）：11-17.ZHANG Honghua，LIU Xuan，CHEN Fuhao，et al.Research and development of SLAM back-end optimization based on graph optimization[J].Computer Application Research，2019，36（1）：11-17.
[15]高沛林，高赟.移动机器人多信标SLAM技术[J].西安科技大学学报，2019，39（5）：905-911.
GAO Peilin，GAO Yun.Multi-beacon SLAM technology for mobile robots[J].Journal of Xi’an University of Science and Technology，2019，39（5）：905-911.
[16]刘康.大尺度视觉SLAM的光束平差算法研究[D].北京：北京邮电大学，2018.
LIU Kang.Research on bundle adjustment algorithm of large-scale visual SLAM[D].Beijing：Beijing University of Posts and Telecommunications，2018.
[17]薛旭升，张旭辉，毛清华，等.基于双目视觉的掘进机器人定位定向方法研究[J].西安科技大学学报，2020，40（5）：781-789.XUE Xusheng，ZHANG Xuhui，MAO Qinghua，et al.Research on the positioning and orientation method of road tunneling robot based on binocular vision[J].Journal of Xi’an University of Science and Technology，2020，40（5）：781-789.
[18]LUO K，PAN H，ZHANG Y，et al.Partial bundle adjustment for accurate three-dimensional reconstruction[J].IET Computer Vision，2019，13（7）：666-675.
[19]LIU K，SUN H X，YE P.Research on bundle adjustment for visual SLAM under large-scale scene[C]//2017 4th International Conference on Systems and Informatics（ICSAI），2017：220-224.
[20]廖仕良.基于g2o的SLAM后端图优化研究及其应用[D].广州：广东工业大学，2019.LIAO Shiliang.Research and application of SLAM back-end graph optimization based on g2o[D].Guangzhou：Guangdong University of Technology，2019.
[21]林利蒙，王梅.改進的光束平差优化算法[J].工业控制计算机，2019，32（4）：98-100.
LIN Limeng，WANG Mei.Improved bundle adjustment optimization algorithm[J].Industrial Control Computer，2019，32（4）：98-100.
[22]林国聪，薛斌强，王冬青.基于图优化的SLAM后端优化算法研究[J].电子设计工程，2020，28（24）：6-10，16.
LIN Guocong，XUE Binqiang，WANG Dongqing.Research on SLAM back-end optimization algorithm based on graph optimization[J].Electronic Design Engineering，2020，28（24）：6-10，16.
[23]OVECHKIN V，INDELMAN V.BAFS：Bundle adjustment with feature scale constraints for enhanced estimation accuracy[J].IEEE Robotics and Automation Letters，2018，3（2）：804-810.
[24]王丽，陈震，刘奇龙.应用改进的Levenberg-Marquardt方法求解一类多线性系统[J].四川师范大学学报（自然科学版），2020，43（
1）：39-44.WANG Li，CHEN Zhen，LIU Qilong.Applying the improved Levenberg-Marquardt method to solve a class of multilinear systems[J].Journal of Sichuan Normal University（Natural Science Edition），2020，43（1）：39-44.
1424501705368
