鹿道璽

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

0 引言

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

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

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

1 算法框架

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

圖1 算法框架

1.1 光度因子

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

(1)

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

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

(2)

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

1.2 IMU因子

(3)

1.3 GNSS因子

1.3.1 偽距測量因子

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

(4)

(5)

其中，rk表示tk時的GNSS接收機。

1.3.2 多普勒因子

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

(6)

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

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

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

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

Gba=Gmetric∪Gvisual∪Mcurr

(7)

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

Gfull=Gmetric∪Gvisual

(8)

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

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

(9)

(10)

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

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

(11)

2 實驗

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

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

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

3 結(jié)語

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