基于多源信息融合的SLAM算法研究

鹿道璽

(重慶交通大學(xué)，重慶 400074)

0 引言

近年來(lái)，基于多傳感器融合的定位與建圖算法發(fā)展迅速，并有大量針對(duì)性的研究。由于相機(jī)、IMU和GNSS具有成本低、體積小、易于布置的優(yōu)點(diǎn)，基于三者融合的方法受到廣泛關(guān)注。

全球?qū)Ш叫l(wèi)星系統(tǒng)可以提供基于世界坐標(biāo)系的絕對(duì)測(cè)量，因此將GNSS數(shù)據(jù)與視覺(jué)慣性SLAM算法的融合得到了廣泛關(guān)注。張曉娜等[1]針對(duì)室內(nèi)動(dòng)態(tài)目標(biāo)定位不能及時(shí)探測(cè)與修復(fù)周跳值的問(wèn)題，采用EMD分解含噪信號(hào)，并從分解分量中獲取噪聲能量，由此解決了室內(nèi)定位誤差大的問(wèn)題。Li等[2]將精確點(diǎn)定位(PPP)的結(jié)果與視覺(jué)慣性里程計(jì)結(jié)合，以實(shí)現(xiàn)低漂移的位姿估計(jì)。Gakne等[3]以EKF方式緊密融合偽距測(cè)量和視覺(jué)觀測(cè)來(lái)解決漂移問(wèn)題，但不足的是其工作是在車輛坐標(biāo)系與世界坐標(biāo)系的轉(zhuǎn)換是已知的條件下進(jìn)行的。朱葉青等[4]引入圖像中的線特征來(lái)提高定位的精度，然后采用多源信息融合算法解決了大尺度漂移問(wèn)題。馮祎等[5]提出利用GNSS偽距觀測(cè)數(shù)據(jù)預(yù)圖像數(shù)據(jù)采用EKF濾波算法融合的方法，提高了GNSS弱信號(hào)的定位精度。Cao等[6]提出了一種由粗到細(xì)的多級(jí)GNSS初始化策略，對(duì)GNSS碼偽距和多普勒頻移測(cè)量值以及視覺(jué)和慣性信息建模，并采用因子圖進(jìn)行狀態(tài)優(yōu)化。

本文的目標(biāo)是利用GNSS原始測(cè)量值、視覺(jué)數(shù)據(jù)和慣性數(shù)據(jù)緊密融合，通過(guò)利用全球?qū)Ш叫l(wèi)星系統(tǒng)的全球測(cè)量，消除視覺(jué)慣性系統(tǒng)的累積誤差。為此，本文的主要設(shè)計(jì)包括：(1)采用了GNSS多級(jí)初始化策略，將GNSS測(cè)量結(jié)果與VIO輸出結(jié)果對(duì)齊；(2)采用動(dòng)態(tài)邊緣化的策略，減小累計(jì)誤差的產(chǎn)生，提高算法定位的精確度。

1 算法框架

本文提出的算法框架如圖1所示，針對(duì)輸入的GNSS、IMU以及圖像數(shù)據(jù)首先進(jìn)行預(yù)處理，然后根據(jù)相鄰圖像幀估計(jì)相機(jī)位姿，同時(shí)對(duì)IMU預(yù)積分以及GNSS高程數(shù)據(jù)濾波，根據(jù)視覺(jué)慣性對(duì)齊結(jié)果完成GNSS初始化，后端采用動(dòng)態(tài)邊緣化策略保持系統(tǒng)的一致性，最后采用全局位姿圖優(yōu)化完成估計(jì)位姿的優(yōu)化。

圖1 算法框架

1.1 光度因子

前端特征提取與匹配策略與DSO相同，首先對(duì)光度處理后的圖像創(chuàng)建梯度直方圖，采用自適應(yīng)閾值提取梯度變化明顯的像素點(diǎn)，然后采用多層光流估計(jì)相鄰圖像幀的相機(jī)位姿。

(1)

其中，Np表示點(diǎn)p周圍的像素集合，Ii和Ij表示各自幀的圖像數(shù)據(jù)，ti和tj表示第i，j幀圖像的曝光時(shí)間，ai,bi,aj,bj表示光度仿射系數(shù)，γ表示光度Huber范數(shù)，wp表示與梯度相關(guān)的權(quán)重，p′表示Ij圖像中的投影點(diǎn)。

據(jù)此，光度誤差可以表示為：

(2)

其中，F(xiàn)表示滑窗窗口中關(guān)鍵幀集合，P表示關(guān)鍵幀i中的梯度點(diǎn)集合，obs(p)表示關(guān)鍵幀中某一梯度點(diǎn)在滑動(dòng)窗口中其他關(guān)鍵幀的觀測(cè)值。

1.2 IMU因子

(3)

1.3 GNSS因子

1.3.1 偽距測(cè)量因子

當(dāng)信號(hào)從衛(wèi)星傳播到接收機(jī)的過(guò)程中，會(huì)穿過(guò)電離層和對(duì)流層。由于信號(hào)在兩種介質(zhì)之間的傳輸速度不同，會(huì)以不同的方式到達(dá)接收機(jī)，這種現(xiàn)象稱為多徑效應(yīng)。同時(shí)，接收機(jī)時(shí)鐘也存在誤差，基于此，偽距測(cè)量可以建模為：

(4)

(5)

其中，rk表示tk時(shí)的GNSS接收機(jī)。

1.3.2 多普勒因子

多普勒頻移根據(jù)接收的載波信號(hào)和設(shè)計(jì)的載波信號(hào)之間的差來(lái)測(cè)量，反映了接收機(jī)衛(wèi)星沿信號(hào)傳播路徑的相對(duì)運(yùn)動(dòng)。由于GNSS信號(hào)結(jié)構(gòu)的特性，多普勒測(cè)量的精度要比偽距測(cè)量精度高一個(gè)數(shù)量級(jí)。多普勒頻移可以建模為：

(6)

1.4 基于動(dòng)態(tài)邊緣化的滑窗優(yōu)化

動(dòng)態(tài)邊緣化的策略主要是同時(shí)保持多個(gè)邊緣化先驗(yàn)Mvisual，Mcurr，Mhalf，并在尺度估計(jì)離邊緣化先驗(yàn)中的線性化點(diǎn)太遠(yuǎn)時(shí)重置當(dāng)前使用的先驗(yàn)。其中，Mvisual只包含來(lái)自先前視覺(jué)狀態(tài)的與尺度無(wú)關(guān)的信息，不能用來(lái)推斷全局尺度。Mcurr包含自設(shè)置時(shí)間以來(lái)的所有尺度線性化點(diǎn)信息，Mhalf只包含最近的狀態(tài)，其尺度接近當(dāng)前的估計(jì)。

當(dāng)尺度估計(jì)偏離Mcurr的線性化點(diǎn)太多時(shí)，Mhalf會(huì)被設(shè)置為Mhalf，Mhalf會(huì)被設(shè)置為相應(yīng)線性化點(diǎn)變化的Mvisual，這確保了優(yōu)化始終具有一致的尺度估計(jì)。

本文用圖優(yōu)化來(lái)進(jìn)行狀態(tài)優(yōu)化：

Gba=Gmetric∪Gvisual∪Mcurr

(7)

定義Gmetric僅包含視覺(jué)慣性因子，Gvisual表示除邊緣化因子以外的所有因子，即

Gfull=Gmetric∪Gvisual

(8)

當(dāng)關(guān)鍵幀i被邊緣化時(shí)，采用邊緣化幀i在Gvisual∪Mvisual的信息來(lái)更新Mvisual，這意味著Mvisual包含所有的邊緣化視覺(jué)因子。關(guān)鍵幀i∈M當(dāng)且僅當(dāng)M包含一個(gè)在關(guān)鍵幀i中被邊緣化的IMU因子，基于此，得到了對(duì)于視覺(jué)因子的約束。

?i∈Mcurr:si∈[smiddle/di,smiddle·di]

(9)

(10)

其中，smiddle表示當(dāng)前允許的尺度間隔的中間值，di表示第i時(shí)刻的尺度間隔大小，scurr表示當(dāng)前的尺度估計(jì)值。

其中，di的選擇十分重要，為了保證系統(tǒng)的一致性，di的值不能太小，以便于Mcurr始終含有足夠的慣性因子，因此，通過(guò)動(dòng)態(tài)調(diào)整一下參數(shù)，通過(guò)下式計(jì)算

(11)

2 實(shí)驗(yàn)

本實(shí)驗(yàn)是在公共數(shù)據(jù)集UrbLoco[7]上進(jìn)行的，該數(shù)據(jù)集是在舊金山和香港高度城市化地區(qū)手機(jī)的。本文提出的方法在4個(gè)舊金山序列上進(jìn)行，并與VINS-MONO[8]以及VINS-Fusion做對(duì)比。所有的實(shí)驗(yàn)都是在IntelCorei53.6GHz×6核CPU和32GB內(nèi)存的PC上進(jìn)行的。

表1 不同方法得到的均方根值(RMSE)比較

從表中可以看出，在相同場(chǎng)景下，本文提出的方法在定位精度方面明顯優(yōu)于其他兩種方法。

3 結(jié)語(yǔ)

本文中提出了一種緊耦合的系統(tǒng)，以在基于非線性優(yōu)化的框架下融合來(lái)自相機(jī)、IMU和GNSS接收器的測(cè)量結(jié)果。通過(guò)建立偽距模型以及多普勒頻移模型，在非線性優(yōu)化階段約束系統(tǒng)狀態(tài)。同時(shí)采用動(dòng)態(tài)邊緣化策略，更好地對(duì)IMU進(jìn)行了初始化。結(jié)果表明，本系統(tǒng)有效地消除了累積漂移，并保持了典型VIO系統(tǒng)的局部精度。