李國(guó)民 宿夢(mèng)瑤 朱代先

摘 要：針對(duì)視覺(jué)SLAM后端非線性?xún)?yōu)化存在計(jì)算速度慢、優(yōu)化效果差等問(wèn)題，采用BA圖優(yōu)化方法，對(duì)其求解策略列文伯格-馬夸爾特（LM）算法進(jìn)行改進(jìn)，改善LM算法計(jì)算過(guò)程中雅可比矩陣可能存在的奇異性或病態(tài)問(wèn)題，增強(qiáng)LM算法的穩(wěn)定性，提高LM算法的效率和BA-SLAM的精度。改進(jìn)的算法將前一次的迭代結(jié)果引入到后一次信賴(lài)域半徑的計(jì)算中，可減小因當(dāng)前解遠(yuǎn)離解集時(shí)目標(biāo)函數(shù)較大所產(chǎn)生的影響，同時(shí)在不假設(shè)雅可比矩陣非奇異的條件下，使其具有二階收斂性，提高算法的穩(wěn)定性和計(jì)算速度，提升光束平差法中LM算法的穩(wěn)健性與效率。仿真實(shí)驗(yàn)結(jié)果表明，提出的改進(jìn)LM算法與傳統(tǒng)LM算法和文獻(xiàn)[6]提出的改進(jìn)LM算法相比，在相同精度時(shí)使用的迭代次數(shù)更少，計(jì)算效率高;采用改進(jìn)LM算法的光束平差法具有更高的優(yōu)化精度和穩(wěn)健性。

關(guān)鍵詞：后端優(yōu)化;光束平差法;重投影誤差;最小二乘;LM算法

中圖分類(lèi)號(hào)：TP 242??????????? 文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1672-9315（2022）01-0152-08

DOI：10.13800/j.cnki.xakjdxxb.2022.0120開(kāi)放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（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]，也稱(chēng)作束調(diào)整、捆集調(diào)整，是目前視覺(jué)SLAM（simultaneous localization and mapping，同時(shí)定位與地圖構(gòu)建）后端優(yōu)化中一種重要的非線性?xún)?yōu)化方法。在以圖優(yōu)化為主體框架的視覺(jué)SLAM中，BA將一個(gè)復(fù)雜的最小二乘問(wèn)題轉(zhuǎn)變成由節(jié)點(diǎn)和邊構(gòu)成的問(wèn)題，進(jìn)而將SLAM中復(fù)雜的非線性最小二乘問(wèn)題通過(guò)圖的方式直觀表達(dá)，便于后期優(yōu)化[2]。BA的核心思想是通過(guò)計(jì)算三維空間中特征點(diǎn)的像素坐標(biāo)與重投影坐標(biāo)之間的誤差作為重投影誤差，求其最小時(shí)對(duì)應(yīng)的路標(biāo)點(diǎn)坐標(biāo)和相機(jī)參數(shù)。具體實(shí)現(xiàn)是利用重投影誤差構(gòu)成最小二乘問(wèn)題，再采取迭代法進(jìn)行求解。傳統(tǒng)迭代方法有梯度下降法、牛頓法、高斯牛頓法（gauss newton，GN）和LM（levenberg marquardt）法[3]等。目前，LM迭代算法的應(yīng)用最為廣泛。但LM算法在計(jì)算時(shí)，引入的信賴(lài)域半徑過(guò)大或過(guò)小都會(huì)對(duì)算法的計(jì)算效率產(chǎn)生較大影響：信賴(lài)域半徑過(guò)大時(shí)，LM算法近似于梯度下降法，導(dǎo)致算法的迭代速度變慢，效率低;信賴(lài)域半徑過(guò)小時(shí)，算法近似于高斯牛頓法，穩(wěn)定性差，計(jì)算時(shí)要求函數(shù)的雅可比矩陣列滿(mǎn)秩。因此，如何選取一個(gè)合適信賴(lài)域半徑是LM算法研究過(guò)程中的難點(diǎn)。針對(duì)上述問(wèn)題，一些學(xué)者提出很多策略對(duì)其進(jìn)行改進(jìn)，通過(guò)對(duì)信賴(lài)域半徑采用不同的計(jì)算方式，以達(dá)到保證算法收斂，提高計(jì)算效率的目的。LIANG采用2步加速的LM算法，可有效提高算法的計(jì)算精度，但使算法復(fù)雜，增加計(jì)算成本[4];UMAR提出迭代參數(shù)分別為

λk=‖JTkJk‖/‖JTkJk‖2，

λk=‖JTkJk‖，λk=‖Jk‖/‖Jk‖2

，以處理更加廣泛的問(wèn)題，但存在由奇異點(diǎn)導(dǎo)致的不穩(wěn)定問(wèn)題[5];

當(dāng)λk=（μk‖F(xiàn)k‖）/（1+‖F(xiàn)k‖）時(shí)，LM算法可在不必假設(shè)Jk為非奇異的局部誤差界的條件約束下具有二階收斂性[6];

WANG等通過(guò)信任區(qū)域技術(shù)進(jìn)行迭代更新，證明了當(dāng)

λk=ηk‖JTkFk‖α，（0≤α≤2）

時(shí)，算法全局收斂，但選取的點(diǎn)遠(yuǎn)離解集時(shí)，F(xiàn)k較大，會(huì)使LM算法的計(jì)算效率比較低[7];

CHEN和MA等提出多步迭代的方法，節(jié)省雅克比矩陣的計(jì)算量，有效地提高了計(jì)算效率[8-9]。

受上述改進(jìn)方法的啟發(fā)，

筆者在文獻(xiàn)[6]改進(jìn)的基礎(chǔ)上，對(duì)參數(shù)的計(jì)算方法進(jìn)行重新定義，將前一次的迭代結(jié)果引入到后一次信賴(lài)域半徑的計(jì)算中[10]，可有效減小因

xk遠(yuǎn)離解集時(shí)Fk較大產(chǎn)生的影響，使LM算法在計(jì)算過(guò)程中在不必假設(shè)Jk為非奇異的局部誤差界的條件約束下具有二階收斂性，進(jìn)一步加快計(jì)算，提高算法的穩(wěn)定性和效率[11-12]。

1 光束平差算法光束平差法（BA）是視覺(jué)SLAM后端優(yōu)化的一種優(yōu)化方法[13]，屬于基于圖優(yōu)化的非線性?xún)?yōu)化，也是目前SLAM后端優(yōu)化的主流方法[14-15]。光束平差法的核心思想是最小化重投影誤差。如圖1所示，表示光束平差法優(yōu)化問(wèn)題[16]。

…為相機(jī)的位姿。如圖1所示，不同地圖點(diǎn)在不同相機(jī)位姿（幀）中對(duì)應(yīng)不同的投影特征點(diǎn)。根據(jù)圖1，光束平差優(yōu)化問(wèn)題可以描述為：機(jī)器人在運(yùn)動(dòng)過(guò)程中，可以獲得一系列的路標(biāo)地圖點(diǎn)在不同時(shí)刻相機(jī)位姿中對(duì)應(yīng)的像素坐標(biāo)，而通過(guò)特征點(diǎn)匹配方法可以計(jì)算得到同一點(diǎn)在對(duì)應(yīng)圖片中的匹配投影點(diǎn)[17]。理論上2個(gè)像素點(diǎn)應(yīng)該是重合的，但由于測(cè)量誤差與外界環(huán)境等因素的影響，實(shí)際情況中2個(gè)像素點(diǎn)坐標(biāo)之間存在差異，2個(gè)坐標(biāo)的差異值稱(chēng)為重投影誤差，以其作為目標(biāo)函數(shù)，平方作為代價(jià)函數(shù)，構(gòu)建非線性最小二乘問(wèn)題[18]，見(jiàn)式（

1）

min

XjYi

∑ni=1

∑mj=1

e（Q（Xj，Yi），xi，j）2

（

1）

式（

1）表示n個(gè)地圖點(diǎn)在m個(gè)幀中;向量

xi，j為幀j上的第i個(gè)點(diǎn)投影坐標(biāo)，這個(gè)是實(shí)際點(diǎn)測(cè)量坐標(biāo);Q（Xj，Yi）為點(diǎn)i在幀j上的預(yù)測(cè)投影坐標(biāo)，這個(gè)是理想點(diǎn)坐標(biāo);e（x，y）為向量x，y的誤差，即重投影誤差。公式的意義就是最小化n個(gè)點(diǎn)的重投影誤差，即求解由重投影誤差構(gòu)成的最小二乘問(wèn)題。由此，光束平差問(wèn)題就化簡(jiǎn)成了使得所有的特征點(diǎn)相關(guān)的歐式距離和最小的最小二乘問(wèn)題，其目的是使代價(jià)函數(shù)和最小，求得代價(jià)函數(shù)和最小時(shí)對(duì)應(yīng)的路標(biāo)點(diǎn)的坐標(biāo)和相機(jī)運(yùn)動(dòng)參數(shù)[19-20]。對(duì)于最小二乘問(wèn)題的求解，由于機(jī)器人位姿由李代數(shù)表示，難以直接求解，因此常采用迭代的方式求解[21]。常用的迭代方法有梯度下降法、牛頓法、高斯牛頓法和LM法。梯度下降法迭代速度快，但接近最優(yōu)點(diǎn)時(shí)，其迭代方向呈折線形，導(dǎo)致收斂速度慢，計(jì)算效率低;高斯牛頓法計(jì)算簡(jiǎn)單且效率高，但計(jì)算時(shí)要求計(jì)算過(guò)程中采用的近似矩陣列滿(mǎn)秩，否則會(huì)出現(xiàn)奇異矩陣或病態(tài)問(wèn)題，導(dǎo)致迭代不收斂;LM算法引入信賴(lài)域問(wèn)題，可以看作是對(duì)高斯牛頓法的改進(jìn)，克服高斯牛頓法的不足，得到更穩(wěn)定更準(zhǔn)確的增量，因此應(yīng)用最為廣泛[22]。

2 LM算法對(duì)于一個(gè)最小二乘問(wèn)題

min

x=12‖f（x）‖22

（2）

式中 x∈Rn，f為任意非線性函數(shù)。對(duì)于復(fù)雜f函數(shù)，常采用迭代法進(jìn)行求解。求解時(shí)，高斯牛頓法是最優(yōu)算法中最簡(jiǎn)單的方法之一，它將f（x）一階泰勒展開(kāi)。

f（x+Δx）≈f（x）+J（x）Δx

（3）

式中 J（x）為雅可比矩陣，表示f（x）關(guān)于x的導(dǎo)數(shù)。當(dāng)前需求解增量Δx，使‖f（x+Δx）‖2最小，即求

Δx=argminΔx

12

‖f（x）+J（x）Δx‖2

（4）式（4）對(duì)Δx求導(dǎo)，并令導(dǎo)數(shù)為零，得到增量Δx為

Δx=-（J（x）TJ（x））-1

J（x）Tf（x）

（5）為便于記憶，將上式記為

Δx=H-1g

只有半正定性，因此在使用高斯牛頓法時(shí)可能會(huì)出現(xiàn)H為奇異矩陣或病態(tài)的情況，導(dǎo)致求出的增量過(guò)大，這樣無(wú)法保證迭代的收斂，也會(huì)導(dǎo)致采用的局部近似不夠準(zhǔn)確[23]。為克服高斯牛頓法增量過(guò)大導(dǎo)致迭代不穩(wěn)定的缺點(diǎn)，同時(shí)結(jié)合梯度下降法和高斯牛頓法的優(yōu)點(diǎn)，列文伯格提出通過(guò)在對(duì)角線上增加非負(fù)常數(shù)λ來(lái)修改增量方程，得到列文伯格·馬夸爾特（levenberg marquardt，LM）算法[24]，其增量方程為

Δx=（H+λD）-1g

（7）一般地，λ稱(chēng)為信賴(lài)域半徑或阻尼因子，

D為對(duì)增量進(jìn)行轉(zhuǎn)換的矩陣，在LM算法中，為簡(jiǎn)化計(jì)算，常用單位矩陣I代替。由式（7）可知，當(dāng)λ=0時(shí)，就得到了高斯牛頓法;當(dāng)λ很大時(shí)，近似得到梯度下降法，因此，可以將LM算法看作是梯度下降法和高斯牛頓法的一種結(jié)合。LM算法允許多次迭代至收斂，迭代步長(zhǎng)被控制在一個(gè)高斯牛頓所生成的二次多項(xiàng)式近似被信任的區(qū)域內(nèi)，因此，LM算法又被稱(chēng)為信賴(lài)域方法。LM算法步驟如下

步驟7：若‖JTf‖≤e，結(jié)束循環(huán)，否則令k=k+1，轉(zhuǎn)到步驟3。目前大部分BA求解都是用LM算法，它是解決非線性最小二乘問(wèn)題最為常用的方法，采用LM算法的求解方式，可在一定程度上避免線性方程組的系數(shù)矩陣的非奇異和病態(tài)問(wèn)題，提供更穩(wěn)定、更準(zhǔn)確的增量

Δx。受文獻(xiàn)[6]的啟發(fā)，筆者對(duì)LM算法進(jìn)行改進(jìn)，使LM算法在不必假設(shè)

Jk

為非奇異的局部誤差界的條件約束下具有二階收斂性，并且可以有效減小因xk遠(yuǎn)離解集時(shí)Fk較大所產(chǎn)生的影響。

3 改進(jìn)的LM算法

3.1 改進(jìn)算法文獻(xiàn)[6]中的改進(jìn)方法，是將信賴(lài)域半徑定義為

式中 μk為一個(gè)可調(diào)參數(shù)，隨每次迭代的相對(duì)變化程度改變;

‖F(xiàn)k‖=fTkfk

，表示目標(biāo)函數(shù)，用來(lái)計(jì)算迭代后函數(shù)的相對(duì)下降，以此判斷迭代步長(zhǎng)的有效性以及信賴(lài)域半徑是否需要更新。通過(guò)以上運(yùn)算，LM算法的運(yùn)算過(guò)程中，在保證收斂的條件下，可避免出現(xiàn)因雅克比可能存在的奇異性導(dǎo)致算法不收斂的問(wèn)題，提高運(yùn)算速度。筆者在文獻(xiàn)[6]的基礎(chǔ)上進(jìn)行改進(jìn)，使其在保證收斂的同時(shí)，進(jìn)一步提高算法的效率，故將算法的信賴(lài)域半徑定義為

1）由式（10）可知，增加‖F(xiàn)k‖的冪進(jìn)行計(jì)算可以更明顯表示出迭代后目標(biāo)函數(shù)的相對(duì)變化程度，影響迭代增量的計(jì)算，提高算法的迭代效率，但冪指數(shù)過(guò)大會(huì)使計(jì)算量增多，增加計(jì)算時(shí)間，降低算法效率。受文獻(xiàn)[6]和[10]的啟發(fā)，文中通過(guò)計(jì)算

‖F(xiàn)k‖的平方加快迭代，有效提高計(jì)算效率。由式（1

1）可知，當(dāng)xk遠(yuǎn)離解集時(shí)，

‖F(xiàn)k‖2/（1+‖F(xiàn)k‖2）趨近于1，λk接近于μk;當(dāng)xk接近于解集時(shí)，F(xiàn)k趨向于0，λk接近于μk‖F(xiàn)k‖2。改進(jìn)LM算法的計(jì)算步驟與傳統(tǒng)LM算法步驟相似，僅在計(jì)算出相對(duì)下降比率rk后再進(jìn)行以下計(jì)算。

1）判斷rk若rk≥p0，令xk+1=xk+Δx;

否則令xk+1=xk。

2）判斷rk若rkp2，令μk+1=μk/6，否則，令μk+1=μk。

通過(guò)以上2步運(yùn)算，可以判斷迭代效果的好壞以及迭代是否可取，進(jìn)而調(diào)節(jié)相應(yīng)參數(shù)，提高算法效率。

3.2 收斂性推導(dǎo)已知定理：對(duì)任意函數(shù)和任意初始變量的迭代算法，迭代收斂的充分必要條件是

ρ（h）<1

（13）在文中，迭代步長(zhǎng)由信賴(lài)域半徑?jīng)Q定，且計(jì)算過(guò)程中的可調(diào)參數(shù)μk∈（0，

1），由此可以推出

（15）滿(mǎn)足迭代收斂條件，因此改進(jìn)的LM算法收斂。

4 實(shí)驗(yàn)結(jié)果及分析實(shí)驗(yàn)的硬件環(huán)境為筆記本電腦（英特爾i5處理器，4G運(yùn)行內(nèi)存），實(shí)驗(yàn)環(huán)境為MATLAB 2016a。

4.1 收斂性分析為驗(yàn)證改進(jìn)LM算法的收斂性，使用2組自建數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)。數(shù)據(jù)集分別是遞增和遞減的2組數(shù)據(jù)，各自包含20個(gè)點(diǎn)和30個(gè)點(diǎn)。對(duì)于2組實(shí)驗(yàn)數(shù)據(jù)，分別使用LM算法、文獻(xiàn)[6]改進(jìn)的LM算法和文中改進(jìn)的LM算法（I-LM）3種方法，在設(shè)置相同參數(shù)的條件下進(jìn)行擬合，計(jì)算不同迭代次數(shù)時(shí)的均方根誤差（RMSE），用來(lái)表示分析算法的收斂性，均方根誤差計(jì)算見(jiàn)式（16）

圖2和圖3顯示了2組數(shù)據(jù)均方根誤差對(duì)比。橫、縱坐標(biāo)分別代表迭代次數(shù)和均方根誤差。從圖2和圖3可以看出，文獻(xiàn)[6]LM算法和I-LM算法，隨迭代次數(shù)增加，RMSE

都不斷減小，并且當(dāng)?shù)螖?shù)達(dá)到一定數(shù)值時(shí)，

RMSE的值不再變化。這表明當(dāng)?shù)_(dá)到一定次數(shù)時(shí)，文獻(xiàn)[6]LM算法和I-LM算法的誤差都不再減小，趨于平穩(wěn)。文獻(xiàn)[6]證明其改進(jìn)的LM算法是收斂的，由此表明，改進(jìn)的LM算法也具有相似的收斂性。

4.2 改進(jìn)LM算法結(jié)果分析為驗(yàn)證文中改進(jìn)LM算法的性能，將LM算法、文獻(xiàn)[6]LM算法和I-LM算法分別應(yīng)用到2組自建數(shù)據(jù)集中，結(jié)果見(jiàn)表1、表2。2張表格分別記錄2組數(shù)據(jù)使用3種方法擬合的迭代次數(shù)、均方根誤差和運(yùn)行時(shí)間，并進(jìn)行對(duì)比分析。

由表1可知，使用LM、文獻(xiàn)[6]LM和I-LM的3種算法，其RMSE相同，表明3種算法的最終計(jì)算結(jié)果相同;但從迭代次數(shù)看，傳統(tǒng)LM算法的數(shù)值最大，文獻(xiàn)[6]LM算法次之，I-LM算法最小，說(shuō)明在達(dá)到相同的迭代效果時(shí)，傳統(tǒng)LM算法需要進(jìn)行運(yùn)算的次數(shù)最多，I-LM算法的次數(shù)最少;從運(yùn)行時(shí)間看，LM算法的運(yùn)行時(shí)間最長(zhǎng)，文獻(xiàn)[6]LM算法次之，I-LM算法最短。通過(guò)以上數(shù)據(jù)對(duì)比，可以得出，I-LM算法較傳統(tǒng)LM算法和文獻(xiàn)[6]LM算法在迭代效率和運(yùn)行時(shí)間上都有改進(jìn)：在達(dá)到相同的計(jì)算結(jié)果的前提下，I-LM算法使用的迭代次數(shù)最少，運(yùn)行的時(shí)間最短，進(jìn)而說(shuō)明改進(jìn)算法的效率更高。

由表2可知，LM算法最終計(jì)算結(jié)果的RMSE比文獻(xiàn)[6]LM算法和I-LM算法的數(shù)值大，表明LM算法的計(jì)算誤差較大，文獻(xiàn)[6]LM算法和I-LM算法的結(jié)果更優(yōu);從迭代次數(shù)看，文獻(xiàn)[6]LM算法的數(shù)值最大，I-LM算法最小，說(shuō)明在達(dá)到相同的迭代效果時(shí)，文獻(xiàn)[6]LM算法需要進(jìn)行運(yùn)算的次數(shù)最多，I-LM算法的次數(shù)最少;從運(yùn)行時(shí)間看，LM算法的運(yùn)行時(shí)間最長(zhǎng)，I-LM算法最短。以上數(shù)據(jù)說(shuō)明，雖然LM算法比文獻(xiàn)[6]LM算法的迭代次數(shù)少，但其RMSE更大，也就是說(shuō)[6]-LM算法迭代多次后可以得到更好的運(yùn)算結(jié)果，效果更好。I-LM算法在RMSE、迭代次數(shù)和運(yùn)行時(shí)間方面的值均為最小值，說(shuō)明I-LM算法相較于LM算法，在進(jìn)行少次迭代的情況下，可以達(dá)到更好的迭代效果，運(yùn)行時(shí)間也最短;相較于文獻(xiàn)[6]LM算法，I-LM算法在達(dá)到相同效果的情況下，需要進(jìn)行運(yùn)算的次數(shù)最少，時(shí)間也最短。綜合以上結(jié)果，改進(jìn)的LM算法可以使用最少的迭代次數(shù)達(dá)到或者優(yōu)于LM算法和文獻(xiàn)[6]LM算法計(jì)算的數(shù)據(jù)結(jié)果，同時(shí)使用的時(shí)間也最短，進(jìn)一步表明改進(jìn)的LM算法可以顯著提高算法的運(yùn)算效率。

4.3 改進(jìn)LM算法應(yīng)用仿真實(shí)驗(yàn)為進(jìn)一步驗(yàn)證改進(jìn)算法的性能，將基于改進(jìn)前后的算法的光束平差法分別應(yīng)用于優(yōu)化仿真，采用2D的2 MIT Killian Court數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)進(jìn)行3組，分別使用基于傳統(tǒng)LM算法（LM-BA）、文獻(xiàn)[6]LM算法（文獻(xiàn)[6]中LM-BA）和文中改進(jìn)的LM算法（I-LM-BA）的光束平差法對(duì)根據(jù)位姿頂點(diǎn)等數(shù)據(jù)構(gòu)建的軌跡圖進(jìn)行優(yōu)化，實(shí)驗(yàn)結(jié)果如圖4～6所示。

圖4～6的橫縱坐標(biāo)分別表示機(jī)器人位置，單位為米（m）。圖4表示LM-BA優(yōu)化，圖5表示文獻(xiàn)[6]中LM-BA優(yōu)化，圖6表示I-LM-BA優(yōu)化，每幅圖中藍(lán)色線條表示初始構(gòu)建的軌跡圖，綠色線條表示一次優(yōu)化后的軌跡圖，紅色線條表示2次優(yōu)化后的軌跡圖，紫色線條表示4次優(yōu)化后的軌跡圖。從圖4可以看出，采用LM-BA對(duì)軌跡圖進(jìn)行優(yōu)化時(shí)，算法沒(méi)有產(chǎn)生奇異性導(dǎo)致優(yōu)化的軌跡圖不準(zhǔn)確的問(wèn)題，但是每次優(yōu)化后的軌跡圖都不相同，說(shuō)明算法的效率比較低，導(dǎo)致每次優(yōu)化計(jì)算都會(huì)產(chǎn)生較上一次更為準(zhǔn)確的軌跡圖;圖5和圖6所示的圖形中，2次優(yōu)化和4次優(yōu)化后的軌跡圖是幾乎重合，表明文獻(xiàn)[6]中LM-BA和I-LM-BA這2種方法都可以更快的完成優(yōu)化，同時(shí)對(duì)比3組實(shí)驗(yàn)中4次優(yōu)化后的軌跡圖，文獻(xiàn)[6]中LM-BA和I-LM-BA這2種方法優(yōu)化的軌跡圖更為精確，表明2種方法速度更快、精度更高;對(duì)比圖5和圖6，圖5中一次優(yōu)化后的軌跡圖產(chǎn)生畸變，這是由于其計(jì)算過(guò)程中出現(xiàn)奇異性問(wèn)題引起的，而圖6中，采用I-LM-BA方法，一次優(yōu)化后的軌跡圖雖然不是完全平滑，但其畸變程度大大減弱，表明I-LM-BA方法更加穩(wěn)定，同時(shí)，采用I-LM-BA方法，2次優(yōu)化和4次優(yōu)化后的軌跡圖幾乎完全重合，而采用文獻(xiàn)[6]中LM-BA方法，2次優(yōu)化后和4次優(yōu)化后的軌跡圖大致重合，但還是明顯可以看出兩者的差異，并且2次優(yōu)化后的軌跡圖仍舊存在畸變，需進(jìn)一步優(yōu)化才可以消除，綜上，基于改進(jìn)LM算法的光束平差法效率更高，也更穩(wěn)定。

5 結(jié) 論

1）對(duì)傳統(tǒng)LM算法系數(shù)矩陣可能存在的問(wèn)題進(jìn)行分析，并參考反饋原理，提出一種通過(guò)重新定義信賴(lài)域半徑計(jì)算方式進(jìn)而改進(jìn)LM的方法。2）每次信賴(lài)域半徑的計(jì)算根據(jù)前一次的迭代效果自動(dòng)調(diào)整，使LM算法在更穩(wěn)定的前提下二階收斂。通過(guò)不同的數(shù)據(jù)實(shí)驗(yàn)，文中改進(jìn)的算法相較于改進(jìn)前與文獻(xiàn)[6]的改進(jìn)，其迭代次數(shù)平均減小約40.1%和38.1%，用時(shí)最短，表明該算法更高效、更穩(wěn)定。3）仿真實(shí)驗(yàn)表明，相較于文獻(xiàn)[6]的改進(jìn)，文中提出基于改進(jìn)LM算法的BA效率更高，可以更穩(wěn)定、更快完成后端優(yōu)化。

參考文獻(xiàn)（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]戴天虹，李志成.基于優(yōu)化列文伯格-馬夸爾特法的SLAM圖優(yōu)化[J].哈爾濱理工大學(xué)學(xué)報(bào)，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，F(xiàn)AN 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]王華宇，雷斌.融合信賴(lài)域的SLAM后端優(yōu)化算法[J].組合機(jī)床與自動(dòng)化加工技術(shù)，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]伍珍香，陳亮，周童.一種改進(jìn)的求解非線性方程組的Levenberg-Marquardt方法[J].延邊大學(xué)學(xué)報(bào)（自然科學(xué)版），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].淮北師范大學(xué)學(xué)報(bào)（自然科學(xué)版），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]季晨，宋燕燕，秦軍，等.非線性?xún)?yōu)化同時(shí)定位與地圖創(chuàng)建問(wèn)題[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]張洪華，劉璇，陳付豪，等.基于圖優(yōu)化的SLAM后端優(yōu)化研究與發(fā)展[J].計(jì)算機(jī)應(yīng)用研究，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]高沛林，高赟.移動(dòng)機(jī)器人多信標(biāo)SLAM技術(shù)[J].西安科技大學(xué)學(xué)報(bào)，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]劉康.大尺度視覺(jué)SLAM的光束平差算法研究[D].北京：北京郵電大學(xué)，2018.

LIU Kang.Research on bundle adjustment algorithm of large-scale visual SLAM[D].Beijing：Beijing University of Posts and Telecommunications，2018.

[17]薛旭升，張旭輝，毛清華，等.基于雙目視覺(jué)的掘進(jìn)機(jī)器人定位定向方法研究[J].西安科技大學(xué)學(xué)報(bào)，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后端圖優(yōu)化研究及其應(yīng)用[D].廣州：廣東工業(yè)大學(xué)，2019.LIAO Shiliang.Research and application of SLAM back-end graph optimization based on g2o[D].Guangzhou：Guangdong University of Technology，2019.

[21]林利蒙，王梅.改進(jìn)的光束平差優(yōu)化算法[J].工業(yè)控制計(jì)算機(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]林國(guó)聰，薛斌強(qiáng)，王冬青.基于圖優(yōu)化的SLAM后端優(yōu)化算法研究[J].電子設(shè)計(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]王麗，陳震，劉奇龍.應(yīng)用改進(jìn)的Levenberg-Marquardt方法求解一類(lèi)多線性系統(tǒng)[J].四川師范大學(xué)學(xué)報(bào)（自然科學(xué)版），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