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

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

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

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

　　0　引言

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

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

　　1　視覺SLAM系統(tǒng)

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

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

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

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

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

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

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

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

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

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

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

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

　　因為噪聲的存在，SLAM中運動和觀測方程的狀態(tài)計算值和系統(tǒng)估計的軌跡存在一定的誤差，通過優(yōu)化方法對狀態(tài)計算值進行微調(diào)，使這項誤差下降到一個極小值，這個過程是一個典型非線性優(yōu)化的過程。因此，引入最小二乘的問題，如式(1)：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

　　通過上述仿真實驗，對實驗結(jié)果進行總結(jié)，如表1所示。

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

　　3　三維點云的BA優(yōu)化仿真實驗

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

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

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

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

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

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

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

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

　　4　總結(jié)

　　針對SLAM的優(yōu)化問題，在已有的BA非線性優(yōu)化方法的基礎(chǔ)上，采用一種基于改進的L?M下降策略，優(yōu)化后的下降策略核心在于事先確定一個閾值并且可以在擬定參數(shù)的擴大倍數(shù)范圍內(nèi)，限定每次迭代的信賴區(qū)域達到優(yōu)化的效果。通過對GRAIL實驗室在GitHub網(wǎng)站上的公開數(shù)據(jù)集中原始三維點云地圖的優(yōu)化實驗，使用G2O完成對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)化效果不明顯等有待解決的問題。因此，需要進一步的優(yōu)化算法，以達到更好的實時性。