魏 彤，李 新

(北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院，北京 100191)

0 引言

視覺的缺失對盲人的出行造成巨大困擾[1-2]。對于盲人出行困難的問題，穿戴式視覺導(dǎo)盲儀具有使用方便、成本較低等優(yōu)勢。而視覺即時定位與建圖(visual simultaneous localization and mapping，VSLAM)算法被認(rèn)為是實現(xiàn)移動機器人真正自主的關(guān)鍵[3-4]，能夠提供位姿和環(huán)境地圖，已成為研制視覺導(dǎo)盲儀的關(guān)鍵技術(shù)。目前，VSLAM已經(jīng)能夠?qū)崿F(xiàn)小范圍內(nèi)的定位與建圖[5-6]，但在大范圍場景中，還存在魯棒性差和誤差累計的問題。

有研究者提出采用視覺與慣性器件組合來提高VSLAM的魯棒性[7-9]，通過融合不包含累積誤差的測量數(shù)據(jù)來消除視VSLAM的累積誤差[10-12]。Daniel提出的IMU/GPS/SLAM緊耦合算法[13]，得到的定位結(jié)果精度較高，但GPS需要固定的參考基站，且算法結(jié)構(gòu)復(fù)雜。Jonghyuk提出的SLAM輔助IMU/GPS組合導(dǎo)航系統(tǒng)有效降低了算法復(fù)雜度[14]，但該定位結(jié)果沒有融合視覺數(shù)據(jù)，精度較低。

1 算法結(jié)構(gòu)

針對目前IMU/GPS/VSLAM融合算法存在的問題以及視覺導(dǎo)盲儀的應(yīng)用需求，本文提出了一種利用IMU/GPS組合導(dǎo)航位姿數(shù)據(jù)輔助的視覺導(dǎo)盲儀定位與建圖算法，改進了ORB-SLAM的運動模型。另外，在定位和建圖的優(yōu)化過程中，都融合了IMU/GPS組合定位數(shù)據(jù)觀測量，以提升定位精度和建圖效果，在降低了算法復(fù)雜度的同時，保證了定位與建圖精度。

雙目ORB-SLAM主要包含三個線程：跟蹤、局部建圖和回環(huán)檢測[6]。跟蹤線程通過匹配當(dāng)前幀的特征點和局部地圖中的路標(biāo)點，并最小化投影誤差來定位當(dāng)前幀的相機位姿；局部建圖線程管理局部地圖，并通過局部捆集調(diào)整來優(yōu)化地圖；回環(huán)檢測線程檢測是否形成回環(huán)，并通過位姿圖優(yōu)化來抑制累積誤差。本文將IMU/GPS組合定位數(shù)據(jù)融合進跟蹤線程和局部建圖線程，去除了雙目ORB-SLAM的回環(huán)檢測線程。IMU/GPS組合導(dǎo)航輔助的視覺導(dǎo)盲儀定位與建圖如圖1所示。

圖1 視覺導(dǎo)盲儀定位與建圖

在跟蹤線程中，IMU/GPS組合系統(tǒng)輸出定位數(shù)據(jù)，根據(jù)定位數(shù)據(jù)預(yù)測當(dāng)前幀的相機位姿并進行特征點匹配，提高跟蹤魯棒性；同時，將位姿數(shù)據(jù)加入跟蹤局部地圖時的非線性優(yōu)化。通過最小化路標(biāo)點投影誤差和位姿誤差，優(yōu)化當(dāng)前幀的相機位姿。在局部建圖線程中，路標(biāo)點和關(guān)鍵幀特征點構(gòu)成視覺誤差量，關(guān)鍵幀之間的位姿變化量構(gòu)成位姿誤差量，局部捆集調(diào)整通過最小化視覺誤差量和位姿誤差量對局部地圖進行優(yōu)化。

在跟蹤線程和局部建圖線程中，IMU/GPS組合定位數(shù)據(jù)都起到了抑制累積誤差的作用，最終能夠在室外大范圍環(huán)境中穩(wěn)定地進行定位與建圖。

2 狀態(tài)量與觀測量

本質(zhì)上，基于IMU/GPS組合系統(tǒng)輔助的VSLAM算法的核心是狀態(tài)優(yōu)化問題，即優(yōu)化相機的狀態(tài)量。為了求解該狀態(tài)優(yōu)化問題，需要明確其狀態(tài)變量和觀測量，進而構(gòu)建優(yōu)化模型來進行求解。在ORB-SLAM基礎(chǔ)上加入IMU/GPS組合數(shù)據(jù)之后，對狀態(tài)量和觀測量也進行了重新定義。

(1)

觀測量表示如下：

Zi=[Ci,Ii]=[{Cik},Ii]

(2)

3 跟蹤線程運動模型與位姿優(yōu)化

考慮到算法實時性與輸出結(jié)果的連續(xù)性，本文提出的同時定位與建圖算法采用了ORB-SLAM的多線程結(jié)構(gòu)，主要包括跟蹤線程和局部建圖線程。

3.1 跟蹤線程運動模型

跟蹤線程同時接收圖像數(shù)據(jù)和IMU/GPS組合數(shù)據(jù)，對當(dāng)前幀的相機位姿進行估計。由于算法采用非線性優(yōu)化方法估計位姿，因此需要給出當(dāng)前幀位姿的初步估計值。

跟蹤線程運動模型如圖2所示。

圖2 跟蹤線程運動模型

圖2中：上角標(biāo)j為前一幀圖像；上角標(biāo)b為當(dāng)前幀圖像；下角標(biāo)e為IMU/GPS組合器件固連坐標(biāo)系；下角標(biāo)c為地理坐標(biāo)系；下角標(biāo)c為相機坐標(biāo)系；下角標(biāo)w為初始相機坐標(biāo)系。

(3)

式中：Rcb為相機與IMU/GPS組合器件的安裝矩陣；Rew為初始矩陣。

3.2 跟蹤線程位姿優(yōu)化

一旦模型預(yù)測出當(dāng)前幀相機位姿，就能將前一幀檢測到的地圖點投影到當(dāng)前幀中，并在投影點附近根據(jù)描述子距離選取最優(yōu)匹配。然后通過最小化相機觀測誤差及組合器件觀測誤差來優(yōu)化當(dāng)前幀位姿。

具體優(yōu)化過程根據(jù)地圖是否在局部建圖線程中被更新而有所區(qū)別。

跟蹤線程位姿優(yōu)化如圖3所示。

圖3 跟蹤線程位姿優(yōu)化圖

當(dāng)?shù)貓D更新時，即前一幀為關(guān)鍵幀，則認(rèn)為前一幀的位姿數(shù)據(jù)是可靠的，因此只優(yōu)化當(dāng)前幀位姿。此時待優(yōu)化的狀態(tài)變量和優(yōu)化變量為：

(4)

(5)

式中：EC為視覺觀測誤差(反投影誤差)；E1為IMU/GPS組合觀測誤差。

對于某一特征點k，其對應(yīng)的視覺觀測誤差EC定義為：

(6)

(7)

“玩陰術(shù)”表面隱晦莫測，其實不堪一擊，乃典型的“見光死”。無論是政府機構(gòu)還是各行各業(yè)乃至民間交往，只要一切事務(wù)均在法規(guī)之下公然運作，堵塞“暗箱操作”“私相授受”漏洞，履行各類監(jiān)督機制，建立各類誠信檔案，使違法違規(guī)者付出相應(yīng)代價——“玩陰術(shù)”自然會失去滋生的土壤和瘋長的空間。

IMU/GPS組合觀測誤差E1定義為：

(8)

∑1為組合觀測數(shù)據(jù)的信息矩陣，從IMU/GPS組合模塊中讀取。

記IMU/GPS組合觀測數(shù)據(jù)為ΔR和Δt，那么，觀測數(shù)據(jù)的相應(yīng)殘差可表示為：

(9)

(10)

當(dāng)局部地圖未更新時，即前一幀為普通幀，則認(rèn)為前一幀的位姿同樣需要再進行優(yōu)化。此時，需要同時對當(dāng)前幀和前一幀進行優(yōu)化，當(dāng)前幀的組合觀測誤差和雅克比矩陣與局部地圖更新時相同，誤差方程對前一幀狀態(tài)量的雅克比矩陣Ji(e1)。

最后，利用(levenberg marquardt,LM)算法對該非線性最小二乘問題進行求解，使得初始值不斷向最優(yōu)估計收斂。

4 局部地圖優(yōu)化

局部建圖線程實現(xiàn)對局部地圖內(nèi)地圖點坐標(biāo)和關(guān)鍵幀位姿的優(yōu)化。當(dāng)插入關(guān)鍵幀時，利用局部捆集調(diào)整算法，對局部窗口內(nèi)的N個關(guān)鍵幀的位姿以及關(guān)鍵幀所觀測到的地圖點坐標(biāo)進行優(yōu)化。N個關(guān)鍵幀由共視關(guān)系圖確定。在局部窗口外但又能觀測到局部地圖點的關(guān)鍵幀也將參與到優(yōu)化過程中，但這些關(guān)鍵幀的狀態(tài)量固定，不再進行優(yōu)化。

圖4 局部建圖線程優(yōu)化圖

在非線性優(yōu)化時，狀態(tài)變量為：

(11)

優(yōu)化方程為：

(12)

式中：i為局部關(guān)鍵幀;j為固定關(guān)鍵幀;k為局部地圖點;Ec(k,i)為局部關(guān)鍵幀與局部地圖點的相機觀測誤差，定義與跟蹤線程中的Ec相同，對關(guān)鍵幀位姿和地圖點坐標(biāo)同時進行優(yōu)化;Ec(k,j)為固定關(guān)鍵幀與局部地圖點的相機觀測誤差，定義與跟蹤線程中的Ec相同，但不對關(guān)鍵幀位姿進行優(yōu)化，只優(yōu)化地圖點坐標(biāo)；EI(i,i+1)為相鄰兩個局部關(guān)鍵幀之間的IMU/GPS組合觀測誤差，定義與跟蹤線程中未更新關(guān)鍵幀時的EI相同，同時對相鄰兩個關(guān)鍵幀的位姿進行優(yōu)化。

5 試驗驗證

在盲人經(jīng)常行走的人行道環(huán)境下，將本文算法的定位性能和參考導(dǎo)航系統(tǒng)的定位性能進行比較，同時對建圖效果進行比較分析，驗證本算法的可行性。

5.1 試驗設(shè)備

利用IMU/GPS組合導(dǎo)航模塊、便攜式計算機和雙目相機，搭建了一套視覺導(dǎo)盲儀。其中，USB攝像頭為廣角攝像頭，最高幀率為30幀/s，采集圖像像素為640×480；便攜式計算機搭載Intel i7 6700HQ(2.6 GHz)處理器，8 GB內(nèi)存。

5.2 定位精度分析

在校園中選取一條25 m×30 m的矩形封閉路線，選取矩形的一個角點作為起點，沿矩形邊行走，最后回到起點。試驗過程中，記錄VSLAM算法、IMU/GPS組合導(dǎo)航算法和本文提出的IMU/GPS輔助VSLAM算法的實時定位數(shù)據(jù)，以及各算法輸出的的建圖數(shù)據(jù)。

試驗結(jié)束后，用Matlab繪制各算法輸出的定位數(shù)據(jù)，并進行分析。平移誤差對比如表1所示。

表1 平移誤差對比

表1中，X、Y方向誤差為整個試驗中定位結(jié)果與真實定位的平均誤差，相對誤差為定位誤差與運動路程的比值。

各算法的軌跡對比如圖5所示。

圖5 各算法的軌跡對比圖

從圖5可以看出，VSLAM算法在每一個拐角處都會產(chǎn)生一定誤差；另外，由于VSLAM算法逐幀估計位姿導(dǎo)致位姿誤差不斷累積，平移誤差達(dá)到2.61%。IMU/GPS組合導(dǎo)航算法有GPS數(shù)據(jù)來抑制IMU數(shù)據(jù)的累積誤差，定位結(jié)果相對VSLAM精度更高，但是，GPS同時也容易受到路邊建筑、樹木等高大物體的影響，使得相應(yīng)環(huán)境下的定位結(jié)果產(chǎn)生偏移誤差。試驗中的平移誤差為1.59%。

而本文提出的IMU/GPS輔助的VSLAM算法中，IMU/GPS組合導(dǎo)航的定位結(jié)果可以抑制VSLAM的累積誤差，使得大范圍環(huán)境下的定位誤差不發(fā)散。同時，從軌跡圖中IMU/GPS定位誤差較大的區(qū)域可以看出，VSLAM利用視覺數(shù)據(jù)能夠抑制GPS的偏移誤差對定位結(jié)果的影響，提高IMU/GPS組合導(dǎo)航的定位精度。最終試驗過程中的平移誤差僅有0.43%。

以上試驗結(jié)果證明，本文算法能夠利用IMU/GPS組合導(dǎo)航的絕對定位信息抑制VSLAM的累積誤差，同時利用VSLAM抑制GPS受環(huán)境影響產(chǎn)生的偏移誤差，最終使得IMU/GPS輔助的VSLAM算法在整個試驗過程中的定位精度優(yōu)于0.5 m。

5.3 建圖效果分析

鑒于實際場景中，各物體的位置等數(shù)據(jù)難以獲取真值，建圖效果只對ORB-SLAM算法和本文提出的IMU/GPS輔助VSLAM算法進行定性的分析比較。

通過對比兩種算法的三維點云圖中地面上地磚線的建圖效果和相機軌跡，可以對兩種算法的建圖效果進行定性的比較分析。點云圖效果對比如圖6所示。

圖6 點云圖效果對比圖

雙目ORB-SLAM算法所建的點云圖中，相機軌跡存在漂移和中斷，且終點沒能回到起點，只有一部分的地磚線比較明顯。而IMU/GPS輔助的VSLAM算法所建的點云圖中，相機軌跡更加平滑，終點與起點成功重合，地磚線清晰可見。以上試驗結(jié)果證明，本文提出的IMU/GPS輔助的VSLAM算法建圖效果顯著優(yōu)于雙目ORB-SLAM算法。

6 結(jié)束語

本文針對穿戴式視覺導(dǎo)盲儀對定位與建圖精度的要求，提出了一種基于IMU/GPS組合導(dǎo)航輔助的VSLAM算法。該算法在ORB-SLAM的基礎(chǔ)上，融合了IMU/GPS組合導(dǎo)航系統(tǒng)的位置與姿態(tài)數(shù)據(jù)。IMU/GPS組合導(dǎo)航數(shù)據(jù)能夠抑制VSLAM的累積誤差，擺脫了對回環(huán)檢測的依賴，并且提高了VSLAM的建圖精度。同時，VSLAM能夠有效抑制全球定位系統(tǒng)(global position system,GPS)的定位偏移誤差。試驗結(jié)果顯示，本文提出的融合算法在人行道環(huán)境下定位精度達(dá)到0.47 m，基本實現(xiàn)了在室外大環(huán)境下能夠?qū)崟r穩(wěn)定運行的定位與建圖系統(tǒng)，并在穿戴式視覺導(dǎo)盲儀研制項目中成功得到應(yīng)用。

在本文提出的IMU/GPS輔助VSLAM算法中，VSLAM算法對由環(huán)境引起的GPS定位結(jié)果偏移誤差有抑制作用。但是，當(dāng)GPS受環(huán)境影響偏移誤差較大時，仍然會使得最終的定位結(jié)果產(chǎn)生較大誤差，即該算法對于環(huán)境的魯棒性還有待提高。