光束平差法中的一種改進(jìn)LM算法

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

(西安科技大學(xué) 通信與信息工程學(xué)院，陜西 西安 710060)

0 引 言

光束平差法(bundle adjustment，BA)[1]，也稱(chēng)作束調(diào)整、捆集調(diào)整，是目前視覺(jué)SLAM(simultaneous localization and mapping，同時(shí)定位與地圖構(gòu)建)后端優(yōu)化中一種重要的非線(xiàn)性?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ù)雜的非線(xiàn)性最小二乘問(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)。

受上述改進(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)化的非線(xiàn)性?xún)?yōu)化，也是目前SLAM后端優(yōu)化的主流方法[14-15]。

光束平差法的核心思想是最小化重投影誤差。如圖1所示，表示光束平差法優(yōu)化問(wèn)題[16]。

圖1 光束平差法優(yōu)化問(wèn)題

圖中Y1，Y2…為路標(biāo)的三維坐標(biāo)，X1，X2，…為相機(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)建非線(xiàn)性最小二乘問(wèn)題[18]，見(jiàn)式(1)

(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í)，其迭代方向呈折線(xiàn)形，導(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)題

(2)

式中x∈Rn，f為任意非線(xiàn)性函數(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最小，即求

(4)

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

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

(5)

為便于記憶，將上式記為

Δx=H-1g

(6)

式中g(shù)=-J(x)Tf(x),H為海塞陣，這里用J(x)TJ(x)近似表示。由式(5)和式(6)知，計(jì)算時(shí)J(x)矩陣必須為列滿(mǎn)秩矩陣，以保證計(jì)算時(shí)的近似H矩陣可逆且正定，但實(shí)際數(shù)據(jù)計(jì)算得到的J(x)TJ(x)只有半正定性，因此在使用高斯牛頓法時(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ì)角線(xiàn)上增加非負(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算法步驟如下

步驟1：

參數(shù)初始化：允許誤差e，k=0，x0，λ0，最大迭代次數(shù)kmax=1 000。

步驟2：

計(jì)算H=JTJ，G=-JTf。

步驟3：

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

步驟4：

若‖JTf‖≤e或達(dá)到最大循環(huán)次數(shù)，則結(jié)束循環(huán)。

步驟5：

計(jì)算

xnew=x+Δx

(8)

(9)

式中F(x)為目標(biāo)函數(shù)；L(x)為近似模型函數(shù)；r為兩者的比率，用來(lái)判斷迭代的有效性。

步驟6：

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

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

步驟7：

若‖JTf‖≤e，結(jié)束循環(huán)，否則令k=k+1，轉(zhuǎn)到步驟3。

目前大部分BA求解都是用LM算法，它是解決非線(xiàn)性最小二乘問(wèn)題最為常用的方法，采用LM算法的求解方式，可在一定程度上避免線(xiàn)性方程組的系數(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)域半徑定義為

(10)

筆者在文獻(xiàn)[6]的基礎(chǔ)上進(jìn)行改進(jìn)，使其在保證收斂的同時(shí)，進(jìn)一步提高算法的效率，故將算法的信賴(lài)域半徑定義為

(11)

由式(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ì)算效率。由式(11)可知，當(dāng)xk遠(yuǎn)離解集時(shí)，F(xiàn)k較大，‖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

若rk

若rk>p2，令μ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

(12)

式中h為迭代步長(zhǎng)；ρ(·)函數(shù)的定義為

(13)

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

(14)

故得

ρ(λ)<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)

(16)

結(jié)果如圖2、圖3所示。

圖2 數(shù)據(jù)一均方根誤差

圖3 數(shù)據(jù)二均方根誤差

圖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 數(shù)據(jù)一3種算法結(jié)果對(duì)比

表2 數(shù)據(jù)二3種算法結(jié)果對(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 LM算法軌跡圖優(yōu)化

圖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)色線(xiàn)條表示初始構(gòu)建的軌跡圖，綠色線(xiàn)條表示一次優(yōu)化后的軌跡圖，紅色線(xiàn)條表示2次優(yōu)化后的軌跡圖，紫色線(xiàn)條表示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 文獻(xiàn)[6]改進(jìn)LM算法軌跡圖優(yōu)化

圖6 文中改進(jìn)LM算法軌跡圖優(yōu)化

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)化。