基于優(yōu)化列文伯格－馬夸爾特法的SLAM圖優(yōu)化

　　摘要：針對(duì)目前的視覺SLAM技術(shù)中存在的非線性優(yōu)化方法過程復(fù)雜、優(yōu)化速度慢、優(yōu)化精度低等缺點(diǎn)，在廣泛應(yīng)用的BA非線性優(yōu)化方法的框架基礎(chǔ)之上，對(duì)其核心下降策略列文伯格-馬夸爾特法進(jìn)行優(yōu)化，以便改善傳統(tǒng)的列文伯格-馬夸爾特法在BA非線性優(yōu)化中的不足之處。首先，初始化待優(yōu)化點(diǎn)處的信賴區(qū)域半徑;其次，擬定一個(gè)擴(kuò)大倍數(shù)，并設(shè)定閾值;最后，通過擬定的近似范圍限定每次迭代的信賴區(qū)間，以達(dá)到非線性優(yōu)化的目的。通過設(shè)置對(duì)比實(shí)驗(yàn)和分析實(shí)驗(yàn)的仿真結(jié)果，可以得出經(jīng)過優(yōu)化后的列文伯格-馬夸爾特法下降策略能夠加快優(yōu)化速度，具有優(yōu)化建圖的效果。

　　本文源自戴天虹; 李志成， 哈爾濱理工大學(xué)學(xué)報(bào) 發(fā)表時(shí)間：2021-04-25《哈爾濱理工大學(xué)學(xué)報(bào)》雜志，于1979年經(jīng)國(guó)家新聞出版總署批準(zhǔn)正式創(chuàng)刊，CN:23-1404/N，本刊在國(guó)內(nèi)外有廣泛的覆蓋面，題材新穎，信息量大、時(shí)效性強(qiáng)的特點(diǎn)，其中主要欄目有：測(cè)控技術(shù)與通信工程、計(jì)算機(jī)與控制工程、數(shù)理科學(xué)等。

　　關(guān)鍵詞：BA非線性優(yōu)化;列文伯格-馬夸爾特法下降策略;信賴區(qū)域

　　0　引言

　　自身定位和地圖構(gòu)建是研究移動(dòng)機(jī)器人領(lǐng)域的一項(xiàng)基本任務(wù)。隨著移動(dòng)機(jī)器人關(guān)鍵技術(shù)的發(fā)展，移動(dòng)機(jī)器人的自主導(dǎo)航成為該領(lǐng)域的研究熱點(diǎn)之一。移動(dòng)機(jī)器人在未知的環(huán)境中根據(jù)傳感器獲取的數(shù)據(jù)并同時(shí)估計(jì)自身的位姿和周圍的環(huán)境地圖，這個(gè)問題被稱之為即時(shí)定位與地圖創(chuàng)建[1-2](Simulta?neousLocalizationandMappingSLAM)。移動(dòng)機(jī)器人的即時(shí)定位與地圖創(chuàng)建是以可移動(dòng)的機(jī)器人為主體，搭載特定的傳感器，在無先驗(yàn)信息的環(huán)境下，從一個(gè)未知位置開始運(yùn)動(dòng)，在運(yùn)動(dòng)過程中根據(jù)傳感器信息估計(jì)自身位置，并同時(shí)建立周圍環(huán)境模型的關(guān)鍵技術(shù)。早期的SLAM技術(shù)中一般采用激光測(cè)距儀[3-4]作為傳感器，利用基于濾波的方法優(yōu)化數(shù)據(jù)集。近年來，由于計(jì)算機(jī)視覺的不斷發(fā)展，搭載視覺傳感器的SLAM已經(jīng)成為主流，主要有單目相機(jī)[5-6]，雙目相機(jī)[7]，多目相機(jī)[8]和RGB?D傳感器[9]等。隨著圖優(yōu)化理論的應(yīng)用，非線性優(yōu)化的方法逐漸代替基于濾波優(yōu)化數(shù)據(jù)集的方法受到了研究者的廣泛關(guān)注。相對(duì)于傳統(tǒng)SLAM技術(shù)中SmithR等人提出的基于擴(kuò)展卡爾曼濾波方法(extendedKalmanfilterEKF)[10]，圖優(yōu)化技術(shù)在G2O(generalgraphoptimization)[11]、Ceres[12]等框架下的實(shí)現(xiàn)既提高了移動(dòng)機(jī)器人獲取相對(duì)位姿的準(zhǔn)確性，又能有效提高計(jì)算效率，但仍然存在特征點(diǎn)的檢測(cè)和匹配精度低、優(yōu)化速度慢、算法復(fù)雜、建圖效果不顯著等諸多有待解決的問題。

　　針對(duì)視覺SLAM后端處理中，優(yōu)化過程冗雜、優(yōu)化速度慢、優(yōu)化精度較低等缺點(diǎn)，本文首先介紹了視覺SLAM系統(tǒng)和SLAM系統(tǒng)下的狀態(tài)描述;其次，闡述傳統(tǒng)的非線性優(yōu)化方法和經(jīng)過優(yōu)化后的非線性優(yōu)化方法理論，并通過仿真實(shí)驗(yàn)進(jìn)行對(duì)比說明。最后，通過完成對(duì)原始的三維點(diǎn)云地圖的優(yōu)化實(shí)驗(yàn)對(duì)比分析，并進(jìn)行相應(yīng)的說明。

　　1　視覺SLAM系統(tǒng)

　　經(jīng)過長(zhǎng)達(dá)十多年的發(fā)展，視覺SLAM的基本框架已經(jīng)日漸成熟。視覺SLAM主體框架由傳感器部分、視覺里程計(jì)(visualodometry)、后端優(yōu)化(optimi?zation)、回環(huán)檢測(cè)(loopclosing)和建圖(mapping)5個(gè)部分組成。

　　傳感器部分在視覺SLAM中主要負(fù)責(zé)RGB圖像和Depth圖像的獲取，并且進(jìn)行一定的預(yù)處理操作。SLAM前端主要任務(wù)是根據(jù)移動(dòng)機(jī)器人在場(chǎng)景中不斷變化的位置時(shí)視覺傳感器的輸出結(jié)果，通過特征匹配和特征檢測(cè)，估算相鄰圖像間的旋轉(zhuǎn)矩陣和平移向量[13]，并且可以通過視覺傳感器輸入的視頻流[14]，獲得不斷更新的局部地圖。SLAM后端是指SLAM技術(shù)中的后端優(yōu)化，這一部分是為了處理SLAM中的噪聲問題而設(shè)置的。現(xiàn)實(shí)環(huán)境中，視覺傳感器獲取的數(shù)據(jù)信息帶有大量的噪聲，即便是精度再高的傳感器也無法避免這項(xiàng)缺陷。后端優(yōu)化是依據(jù)視覺里程計(jì)測(cè)量的不同時(shí)刻下機(jī)器人位姿信息，通過濾波、非線性優(yōu)化算法等方法對(duì)它們進(jìn)行優(yōu)化的過程，是獲取全局地圖的必要條件之一。回環(huán)檢測(cè)[15]是利用圖像信息豐富的優(yōu)勢(shì)，通過兩幀圖像間的相似性對(duì)比，判斷移動(dòng)機(jī)器人是否到達(dá)過之前已經(jīng)到達(dá)過的位置。回環(huán)檢測(cè)的設(shè)置，是為了解決機(jī)器人在移動(dòng)的過程中位姿估計(jì)隨時(shí)間漂移的問題。SLAM中引入回環(huán)檢測(cè)，即引入時(shí)間間隔更久遠(yuǎn)的約束，并將此約束傳遞給后端進(jìn)行優(yōu)化，從而得到全局一致性的位姿估計(jì)，保證了在長(zhǎng)時(shí)間的運(yùn)動(dòng)過程中所估計(jì)出的機(jī)器人運(yùn)動(dòng)軌跡和地圖的正確性[16]。建圖是構(gòu)建地圖的過程，大體可分為構(gòu)建拓?fù)涞貓D和構(gòu)建度量地圖。

　　1、1　基于圖優(yōu)化的視覺SLAM系統(tǒng)

　　目前SLAM中的主流優(yōu)化方法是基于圖優(yōu)化技術(shù)建立位姿圖，即對(duì)環(huán)境中特征點(diǎn)的觀測(cè)通過邊緣化的方法轉(zhuǎn)換為機(jī)器人不同時(shí)刻位姿間的約束，通過對(duì)機(jī)器人的位姿及環(huán)境中的特征點(diǎn)位置序列進(jìn)行估計(jì)來簡(jiǎn)化優(yōu)化。其中，位姿圖中的節(jié)點(diǎn)對(duì)應(yīng)于每個(gè)時(shí)刻機(jī)器人的姿勢(shì)和當(dāng)下時(shí)刻觀測(cè)到環(huán)境中的特征點(diǎn)，位姿圖中的邊對(duì)應(yīng)于節(jié)點(diǎn)之間的空間約束。通過不斷調(diào)整位姿圖中節(jié)點(diǎn)的位置，使其符合邊的空間約束，進(jìn)而得到機(jī)器人的運(yùn)動(dòng)軌跡與所觀測(cè)到的環(huán)境地圖。基于圖優(yōu)化的視覺SLAM系統(tǒng)，如圖1所示。

　　BA(bundleadjustment)[17]，又名光速平差法，是指能夠從視覺重建模型之中提取出相應(yīng)的相機(jī)內(nèi)、外參數(shù)以及最優(yōu)的三維模型的一種方法。BA優(yōu)化作為圖優(yōu)化框架下的視覺SLAM算法中的核心算法，具有將位姿與路標(biāo)同時(shí)調(diào)整的優(yōu)化能力。早期的BA算法理論，在測(cè)量學(xué)中有廣泛的工程應(yīng)用，很少用于視覺圖像方面。隨著計(jì)算機(jī)視覺技術(shù)的發(fā)展，研究者們才將BA算法逐漸引至視覺圖像研究領(lǐng)域，才使其在視覺圖像方面的應(yīng)用以及理論的創(chuàng)新逐漸稱為研究熱門。直到近十年，由于BA的稀疏特性，該方法才被廣泛應(yīng)用于SLAM問題研究中，并在實(shí)時(shí)的場(chǎng)景中得到很好的應(yīng)用。BA在SLAM技術(shù)中的應(yīng)用，主要體現(xiàn)在SLAM后端的優(yōu)化過程中。BA算法在以圖優(yōu)化為主體框架的視覺SLAM中，可以將一個(gè)復(fù)雜的最小二乘問題轉(zhuǎn)變成由節(jié)點(diǎn)和邊構(gòu)成的問題，能夠直觀的描述相機(jī)運(yùn)動(dòng)行為。這種方法，將SLAM中復(fù)雜的非線性最小二乘問題通過圖論的方式直觀表述，易于后期研究和優(yōu)化，同時(shí)體現(xiàn)了圖優(yōu)化的核心思想。

　　1、2　SLAM狀態(tài)描述

　　SLAM技術(shù)中采用不同的傳感器，會(huì)有不同的參數(shù)化形式。如果要保持其通用性，可以取成通用的抽象形式，即用運(yùn)動(dòng)方程和觀測(cè)方程來描述，

　　其中：x是位姿點(diǎn)描述;xk是在k時(shí)刻的位姿;uk是運(yùn)動(dòng)傳感器的讀數(shù);wk為噪聲;當(dāng)視覺機(jī)器人在xk位置上看到某個(gè)路標(biāo)yj，產(chǎn)生一個(gè)觀測(cè)數(shù)據(jù)zk，j，vk是這次觀測(cè)里的噪聲。運(yùn)動(dòng)方程描述了狀態(tài)xk-1如何變到xk，而觀察方程式描述的是從xk是怎么得到觀察數(shù)據(jù)yj的。

　　運(yùn)動(dòng)方程和觀測(cè)方程都容易受到噪聲的影響，通常把位姿x與路標(biāo)y看成符合概率分布的隨機(jī)變量，假設(shè)狀態(tài)量和噪聲項(xiàng)服從高斯分布。通過計(jì)算對(duì)變量最優(yōu)值的估計(jì)(即均值)以及度量均值的不確定性(即協(xié)方差)，來估計(jì)狀態(tài)量的高斯分布。

　　在運(yùn)動(dòng)方程和觀測(cè)方程中，通常假設(shè)兩個(gè)噪聲項(xiàng)wk和vk分別滿足N(0，Rk)、N(0，Qk，j)的零均值高斯分布。

　　2　優(yōu)化L?M下降策略

　　2、1　傳統(tǒng)的L?M下降策略

　　因?yàn)樵肼暤拇嬖冢琒LAM中運(yùn)動(dòng)和觀測(cè)方程的狀態(tài)計(jì)算值和系統(tǒng)估計(jì)的軌跡存在一定的誤差，通過優(yōu)化方法對(duì)狀態(tài)計(jì)算值進(jìn)行微調(diào)，使這項(xiàng)誤差下降到一個(gè)極小值，這個(gè)過程是一個(gè)典型非線性優(yōu)化的過程。因此，引入最小二乘的問題，如式(1)：

　　其中f是任意非線性函數(shù)。此公式能計(jì)算出系統(tǒng)的極值點(diǎn)，并通過直接比較可獲得理想值。

　　高斯牛頓法(G?S)[18]是最基本的非線性最小二乘方法，是將目標(biāo)函數(shù)f(x)在x附近進(jìn)行一階泰勒展開，如式(2)：f(x+Δx)≈f(x)+J(x)Δx(2)其中，J(x)是f(x)關(guān)于x的導(dǎo)數(shù)。欲使‖f(x)+Δx‖2達(dá)到最小值，目標(biāo)是尋找最合適的Δx，Δx∗=argminΔx12‖f(x)+J(x)Δx‖2將目標(biāo)函數(shù)Δx∗求導(dǎo)并令其導(dǎo)數(shù)等于零，得到線性方程：

　　為方便記憶，將左邊的系數(shù)定義為H，右邊的定義為g，記為式(3)：HΔx=g(3)列文伯格-馬夸爾特法(L?M)[19]是基于G?S法基礎(chǔ)上的一種改進(jìn)方法，其核心思想是在每一個(gè)最優(yōu)優(yōu)化算法里，需要通過迭代來獲得目標(biāo)函數(shù)的極小值，且在每次迭代中的目標(biāo)函數(shù)的值都是下降的，即給Δx添加了一個(gè)信賴區(qū)域。在L?M優(yōu)化中，需要將帶有不等式約束的優(yōu)化問題變成一個(gè)無約束的優(yōu)化問題，轉(zhuǎn)化中需要引進(jìn)拉格朗日算子λ，minΔxk12‖f(xk)+J(xk)Δxk‖2+λ2‖DΔxk‖2其中，D是信賴區(qū)域。計(jì)算增量的線性方程類似于高斯牛頓法中的處理方法，

　　簡(jiǎn)化形式，(H+λI)Δx=g其中D=I。L?M法中，信賴區(qū)間的范圍是根據(jù)近似模型和實(shí)際函數(shù)之間的差異確定的，可以通過式(4)判斷近似的效果好壞。ρ=f(x+Δx)-f(x)J(x)Δx(4)若ρ值太小，說明實(shí)際減小值遠(yuǎn)小于近似減小值;若ρ值太大則意味著實(shí)際下降值超出預(yù)計(jì)。ρ值接近于1時(shí)，是近似最好的效果。

　　雖然L?M法在G?S法的基礎(chǔ)上進(jìn)行了一定改進(jìn)，但仍存在不足。當(dāng)參數(shù)λ比較小時(shí)，I的影響力遠(yuǎn)遠(yuǎn)小于H占據(jù)的地位，導(dǎo)致二次近似模擬在該范圍內(nèi)更適合，這時(shí)候更接近于G?S法;當(dāng)λ比較大時(shí)，I的影響力遠(yuǎn)遠(yuǎn)大于H占據(jù)的地位，導(dǎo)致二次近似模擬在該范圍內(nèi)不理想。

　　2、2　優(yōu)化的L?M下降策略

　　本文就傳統(tǒng)的L?M法中對(duì)于拉格朗日算子λ的取值沒有限制，會(huì)出現(xiàn)過大或者過小取值的情況，因而造成增量Δx的不穩(wěn)定、不精確的情況，設(shè)計(jì)一種改進(jìn)的L?M法下降策略以解決λ取值問題。

　　在迭代的過程中，把增量限定于一個(gè)圓球中，該圓球的半徑為μ，增量只有在圓球內(nèi)才被認(rèn)為是有效的。這種設(shè)計(jì)直接把x約束到一個(gè)圓球中，信賴區(qū)域用D表示，并將D取成非負(fù)數(shù)對(duì)角陣，使約束范圍在該梯度方向上稍大一些。本文改進(jìn)的法下降策略方法：

　　1)給定系統(tǒng)的初始值x0以及初始優(yōu)化信賴區(qū)域半徑μ;

　　2)求解第k次迭代下方程的解并事先確定ρ值，minΔxk12‖f(xk)+J(xk)Δxk‖2，‖DΔxk‖2≤μ3)通過擬定一個(gè)閾值和擴(kuò)大倍數(shù)，檢測(cè)泰勒近似效果是否足夠好，當(dāng)ρ≥45，μ=2μ當(dāng)ρ<15，μ=12μ

　　4)當(dāng)ρ大于等于事先選定的某個(gè)閾值并且當(dāng)下算法收斂，則可認(rèn)為該近似是可行的，迭代結(jié)束;否則，令xk+1=xk+Δx重復(fù)上述步驟。

　　優(yōu)化后的L?M下降策略框架中，近似范圍擴(kuò)大的倍數(shù)和閾值是可自定義的，可使用經(jīng)驗(yàn)值代替。通過一個(gè)擬定的閾值和擴(kuò)大倍數(shù)檢測(cè)泰勒近似是否足夠好，若不夠滿意則通過將信賴區(qū)域進(jìn)行簡(jiǎn)單的放大或者縮小，進(jìn)而繼續(xù)尋找信賴區(qū)域內(nèi)的最優(yōu)下降策略。這種基于L?M法的優(yōu)化下降策略能提高優(yōu)化系統(tǒng)的效率和系統(tǒng)的魯棒性，具有良好的工程實(shí)踐應(yīng)用性。

　　2、3　優(yōu)化的L?M法仿真實(shí)驗(yàn)

　　本實(shí)驗(yàn)基于觀測(cè)數(shù)據(jù)，將通過G2O進(jìn)行曲線擬合，擬合曲線為，y=exp(ax2i+bxi+c)+w其中，a、b、c為曲線的參數(shù)，w為高斯噪聲。假設(shè)N個(gè)關(guān)于x、y的觀察數(shù)據(jù)點(diǎn)，通過求解最小二乘問題來估計(jì)曲線參數(shù)，mina，b，c12∑Ni=1‖yi-exp(ax2i+bxi+c)‖2

　　在相同的實(shí)驗(yàn)環(huán)境下，通過G2O庫優(yōu)化分別采用傳統(tǒng)的L?M下降策略和優(yōu)化的L?M下降策略進(jìn)行仿真實(shí)驗(yàn)，得到的結(jié)果如圖2和圖3所示。

　　通過上述仿真實(shí)驗(yàn)，對(duì)實(shí)驗(yàn)結(jié)果進(jìn)行總結(jié)，如表1所示。

　　在同等實(shí)驗(yàn)環(huán)境的梯度下降策略中，相比于傳統(tǒng)的L?M法下降策略，改進(jìn)后的L?M法下降策略所需迭代次數(shù)更少，迭代時(shí)間更短，效率有所提高。

　　3　三維點(diǎn)云的BA優(yōu)化仿真實(shí)驗(yàn)

　　本文采用的開發(fā)環(huán)境是Linux系統(tǒng)，開發(fā)平臺(tái)是Ubuntu16、04。Ubuntu16、04為以Linux系統(tǒng)為基礎(chǔ)開發(fā)的圖形界面的操作系統(tǒng)。計(jì)算機(jī)具體參數(shù)為：華碩筆記本，型號(hào)：A55V，CPU：Intel酷睿i53210M，CPU主頻：2、5GHz，內(nèi)存容量：4GB。

　　G2O(GeneralGraphicOptimization)是一個(gè)用C++開發(fā)的用于求解非線性最小二乘問題的開源庫[20]。它提供了大量的頂點(diǎn)和邊的類型，是一個(gè)基于圖優(yōu)化的庫。在工程實(shí)踐中，G2O可以求解很多將優(yōu)化表達(dá)成圖的問題，對(duì)于相機(jī)位姿估計(jì)問題十分方便。

　　本文采用來自于華盛頓大學(xué)GRAIL實(shí)驗(yàn)室發(fā)布于GitHub網(wǎng)站上的公開數(shù)據(jù)集作為三維點(diǎn)云實(shí)驗(yàn)原始數(shù)據(jù)，從“CommunityPhotoCollections”項(xiàng)目中可獲取該數(shù)據(jù)集。基于BA中的稀疏特性，選用稀疏求解器作為G2O中使用的線性求解器。定義傳統(tǒng)的L?M方法和基于L?M法的優(yōu)化下降策略通過相同實(shí)驗(yàn)環(huán)境的對(duì)比，觀察和分析三維點(diǎn)云的重建效果。

　　如圖4所示，是一張未優(yōu)化的三維點(diǎn)云地圖。地圖中，像素點(diǎn)散亂，不具有可觀性。

　　在G2O優(yōu)化框架下，下降策略選用傳統(tǒng)的L?M方法，

　　如圖5和圖6所示，在G2O框架下，傳統(tǒng)的L?M法下降策略經(jīng)過19次迭代，每次迭代的時(shí)間大約為2s。優(yōu)化后的三維點(diǎn)云地圖結(jié)構(gòu)雖然比較清晰，但依舊存在很多點(diǎn)云模糊的情況。在G2O優(yōu)化框架下，下降策略選用優(yōu)化的列文伯格-馬夸爾特方法。

　　如圖7和圖8所示，在G2O框架下，改進(jìn)的L?M下降策略經(jīng)過16次迭代，每次迭代的時(shí)間大約為0、17s。相比傳統(tǒng)的L?M下降策略，優(yōu)化后的三維點(diǎn)云地圖結(jié)構(gòu)更加清晰明了，混亂的點(diǎn)云扎堆的情況減少了很多，達(dá)到了優(yōu)化改進(jìn)的效果。將使用傳統(tǒng)的L?M法和優(yōu)化后的L?M法的處理結(jié)果匯總于表2。

　　由表2可知，經(jīng)過優(yōu)化后的L?M下降策略，在迭代次數(shù)由原來的19次縮減到現(xiàn)在的16次，每次迭代平均時(shí)間明顯優(yōu)于傳統(tǒng)的L?M法下降策略，總體優(yōu)化效果有顯著提高。

　　4　總結(jié)

　　針對(duì)SLAM的優(yōu)化問題，在已有的BA非線性優(yōu)化方法的基礎(chǔ)上，采用一種基于改進(jìn)的L?M下降策略，優(yōu)化后的下降策略核心在于事先確定一個(gè)閾值并且可以在擬定參數(shù)的擴(kuò)大倍數(shù)范圍內(nèi)，限定每次迭代的信賴區(qū)域達(dá)到優(yōu)化的效果。通過對(duì)GRAIL實(shí)驗(yàn)室在GitHub網(wǎng)站上的公開數(shù)據(jù)集中原始三維點(diǎn)云地圖的優(yōu)化實(shí)驗(yàn)，使用G2O完成對(duì)BA優(yōu)化方法的研究，分析其仿真結(jié)果得出經(jīng)過優(yōu)化后的L?M法下降策略能夠提高優(yōu)化速度，可以顯著提升建圖效果，并能應(yīng)用于三維重建和視覺SLAM圖優(yōu)化技術(shù)中的結(jié)論。但SLAM技術(shù)中仍有諸多環(huán)節(jié)存在精度不高，優(yōu)化效果不明顯等有待解決的問題。因此，需要進(jìn)一步的優(yōu)化算法，以達(dá)到更好的實(shí)時(shí)性。