基于優化列文伯格－馬夸爾特法的SLAM圖優化

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

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

　　關鍵詞：BA非線性優化;列文伯格-馬夸爾特法下降策略;信賴區域

　　0　引言

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

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

　　1　視覺SLAM系統

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

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

　　1、1　基于圖優化的視覺SLAM系統

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

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

　　1、2　SLAM狀態描述

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

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

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

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

　　2　優化L?M下降策略

　　2、1　傳統的L?M下降策略

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

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

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

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

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

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

　　2、2　優化的L?M下降策略

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

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

　　1)給定系統的初始值x0以及初始優化信賴區域半徑μ;

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

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

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

　　2、3　優化的L?M法仿真實驗

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

　　在相同的實驗環境下，通過G2O庫優化分別采用傳統的L?M下降策略和優化的L?M下降策略進行仿真實驗，得到的結果如圖2和圖3所示。

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

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

　　3　三維點云的BA優化仿真實驗

　　本文采用的開發環境是Linux系統，開發平臺是Ubuntu16、04。Ubuntu16、04為以Linux系統為基礎開發的圖形界面的操作系統。計算機具體參數為：華碩筆記本，型號：A55V，CPU：Intel酷睿i53210M，CPU主頻：2、5GHz，內存容量：4GB。

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

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

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

　　在G2O優化框架下，下降策略選用傳統的L?M方法，

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

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

　　由表2可知，經過優化后的L?M下降策略，在迭代次數由原來的19次縮減到現在的16次，每次迭代平均時間明顯優于傳統的L?M法下降策略，總體優化效果有顯著提高。

　　4　總結

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