戴天虹，李志成

(東北林業(yè)大學(xué) 機(jī)電工程學(xué)院，哈爾濱 150036)

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

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

1 視覺(jué)SLAM系統(tǒng)

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

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

1.1 基于圖優(yōu)化的視覺(jué)SLAM系統(tǒng)

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

圖1 基于圖優(yōu)化的視覺(jué)SLAM系統(tǒng)Fig.1 Visual SLAM system based on graph optimization

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

1.2 SLAM狀態(tài)描述

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

k=1,…,N;j=1,…,M

其中：x是位姿點(diǎn)描述；xk是在k時(shí)刻的位姿；uk是運(yùn)動(dòng)傳感器的讀數(shù)；wk為噪聲；當(dāng)視覺(jué)機(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)服從高斯分布。通過(guò)計(jì)算對(duì)變量最優(yōu)值的估計(jì)(即均值)以及度量均值的不確定性(即協(xié)方差)，來(lái)估計(jì)狀態(tài)量的高斯分布。

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

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

2.1 傳統(tǒng)的L-M下降策略

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

(1)

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

高斯牛頓法(G-S)[18]是最基本的非線性最小二乘方法，是將目標(biāo)函數(shù)f(x)在x附近進(jìn)行一階泰勒展開(kāi)，如式(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，

將目標(biāo)函數(shù)Δx*求導(dǎo)并令其導(dǎo)數(shù)等于零，得到線性方程：

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

為方便記憶，將左邊的系數(shù)定義為H，右邊的定義為g，記為式(3)：

HΔx=g

(3)

列文伯格-馬夸爾特法(L-M)[19]是基于G-S法基礎(chǔ)上的一種改進(jìn)方法，其核心思想是在每一個(gè)最優(yōu)優(yōu)化算法里，需要通過(guò)迭代來(lái)獲得目標(biāo)函數(shù)的極小值，且在每次迭代中的目標(biāo)函數(shù)的值都是下降的，即給Δx添加了一個(gè)信賴(lài)區(qū)域。在L-M優(yōu)化中，需要將帶有不等式約束的優(yōu)化問(wèn)題變成一個(gè)無(wú)約束的優(yōu)化問(wèn)題，轉(zhuǎn)化中需要引進(jìn)拉格朗日算子λ，

其中，D是信賴(lài)區(qū)域。計(jì)算增量的線性方程類(lèi)似于高斯牛頓法中的處理方法，

(H+λDTD)Δx=g

簡(jiǎn)化形式，

(H+λI)Δx=g

其中D=I。

L-M法中，信賴(lài)區(qū)間的范圍是根據(jù)近似模型和實(shí)際函數(shù)之間的差異確定的，可以通過(guò)式(4)判斷近似的效果好壞。

(4)

若ρ值太小，說(shuō)明實(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ì)于拉格朗日算子λ的取值沒(méi)有限制，會(huì)出現(xiàn)過(guò)大或者過(guò)小取值的情況，因而造成增量Δx的不穩(wěn)定、不精確的情況，設(shè)計(jì)一種改進(jìn)的L-M法下降策略以解決λ取值問(wèn)題。

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

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

2)求解第k次迭代下方程的解并事先確定ρ值，

3)通過(guò)擬定一個(gè)閾值和擴(kuò)大倍數(shù)，檢測(cè)泰勒近似效果是否足夠好，

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

優(yōu)化后的L-M下降策略框架中，近似范圍擴(kuò)大的倍數(shù)和閾值是可自定義的，可使用經(jīng)驗(yàn)值代替。通過(guò)一個(gè)擬定的閾值和擴(kuò)大倍數(shù)檢測(cè)泰勒近似是否足夠好，若不夠滿(mǎn)意則通過(guò)將信賴(lài)區(qū)域進(jìn)行簡(jiǎn)單的放大或者縮小，進(jìn)而繼續(xù)尋找信賴(lài)區(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ù)，將通過(guò)G2O 進(jìn)行曲線擬合，擬合曲線為，

其中，a、b、c為曲線的參數(shù)，w為高斯噪聲。假設(shè)N個(gè)關(guān)于x、y的觀察數(shù)據(jù)點(diǎn)，通過(guò)求解最小二乘問(wèn)題來(lái)估計(jì)曲線參數(shù)，

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

圖2 傳統(tǒng)的L-M下降策略擬合Fig.2 Traditional L-M descending strategy fitting

圖3 改進(jìn)的L-M下降策略擬合Fig.3 Improved L-M descending strategy fitting

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

表1 曲線擬合Tab.1 Curve fitting

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

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

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

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

本文采用來(lái)自于華盛頓大學(xué) GRAIL 實(shí)驗(yàn)室發(fā)布于 GitHub網(wǎng)站上的公開(kāi)數(shù)據(jù)集作為三維點(diǎn)云實(shí)驗(yàn)原始數(shù)據(jù)，從“Community Photo Collections”項(xiàng)目中可獲取該數(shù)據(jù)集?；贐A中的稀疏特性，選用稀疏求解器作為G2O中使用的線性求解器。定義傳統(tǒng)的L-M方法和基于L-M法的優(yōu)化下降策略通過(guò)相同實(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)過(guò) 19 次迭代，每次迭代的時(shí)間大約為 2 s。優(yōu)化后的三維點(diǎn)云地圖結(jié)構(gòu)雖然比較清晰，但依舊存在很多點(diǎn)云模糊的情況。

圖4 未優(yōu)化的三維點(diǎn)云地圖Fig.4 Unoptimized 3d point cloud map

圖5 傳統(tǒng)的L-M法下降策略實(shí)驗(yàn)數(shù)據(jù)Fig.5 Experimental data of traditional l-m descending strategy

圖6 傳統(tǒng)的L-M法下降策略?xún)?yōu)化后的三維點(diǎn)云圖Fig.6 3D point cloud map optimized by the traditional l-m descending strategy

在G2O優(yōu)化框架下，下降策略選用優(yōu)化的列文伯格-馬夸爾特方法。

如圖7和圖8所示，在G2O框架下，改進(jìn)的L-M下降策略經(jīng)過(guò) 16 次迭代，每次迭代的時(shí)間大約為 0.17 s。相比傳統(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。

圖7 改進(jìn)的L-M下降策略實(shí)驗(yàn)數(shù)據(jù)Fig.7 Experimental data of improved l-m descending strategy

圖8 改進(jìn)的L-M下降策略?xún)?yōu)化后的三維點(diǎn)云圖Fig.8 3D point cloud map optimized by improved l-m descending strategy

表2 實(shí)驗(yàn)參數(shù)對(duì)比Tab.2 Comparison of experimental parameters

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

4 總 結(jié)

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