慣導(dǎo)衛(wèi)星組合導(dǎo)航輔助的視覺(jué)導(dǎo)盲儀定位建圖

魏 彤，李 新

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

0 引言

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

有研究者提出采用視覺(jué)與慣性器件組合來(lái)提高VSLAM的魯棒性[7-9]，通過(guò)融合不包含累積誤差的測(cè)量數(shù)據(jù)來(lái)消除視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é)果沒(méi)有融合視覺(jué)數(shù)據(jù)，精度較低。

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

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

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

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

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

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

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

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

(1)

觀測(cè)量表示如下：

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

(2)

3 跟蹤線程運(yùn)動(dòng)模型與位姿優(yōu)化

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

3.1 跟蹤線程運(yùn)動(dòng)模型

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

跟蹤線程運(yùn)動(dòng)模型如圖2所示。

圖2 跟蹤線程運(yùn)動(dòng)模型

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

(3)

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

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

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

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

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

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

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

(4)

(5)

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

對(duì)于某一特征點(diǎn)k，其對(duì)應(yīng)的視覺(jué)觀測(cè)誤差EC定義為：

(6)

(7)

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

IMU/GPS組合觀測(cè)誤差E1定義為：

(8)

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

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

(9)

(10)

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

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

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

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

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

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

(11)

優(yōu)化方程為：

(12)

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

5 試驗(yàn)驗(yàn)證

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

5.1 試驗(yàn)設(shè)備

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

5.2 定位精度分析

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

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

表1 平移誤差對(duì)比

表1中，X、Y方向誤差為整個(gè)試驗(yàn)中定位結(jié)果與真實(shí)定位的平均誤差，相對(duì)誤差為定位誤差與運(yùn)動(dòng)路程的比值。

各算法的軌跡對(duì)比如圖5所示。

圖5 各算法的軌跡對(duì)比圖

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

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

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

5.3 建圖效果分析

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

通過(guò)對(duì)比兩種算法的三維點(diǎn)云圖中地面上地磚線的建圖效果和相機(jī)軌跡，可以對(duì)兩種算法的建圖效果進(jìn)行定性的比較分析。點(diǎn)云圖效果對(duì)比如圖6所示。

圖6 點(diǎn)云圖效果對(duì)比圖

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

6 結(jié)束語(yǔ)

本文針對(duì)穿戴式視覺(jué)導(dǎo)盲儀對(duì)定位與建圖精度的要求，提出了一種基于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的累積誤差，擺脫了對(duì)回環(huán)檢測(cè)的依賴，并且提高了VSLAM的建圖精度。同時(shí)，VSLAM能夠有效抑制全球定位系統(tǒng)(global position system,GPS)的定位偏移誤差。試驗(yàn)結(jié)果顯示，本文提出的融合算法在人行道環(huán)境下定位精度達(dá)到0.47 m，基本實(shí)現(xiàn)了在室外大環(huán)境下能夠?qū)崟r(shí)穩(wěn)定運(yùn)行的定位與建圖系統(tǒng)，并在穿戴式視覺(jué)導(dǎo)盲儀研制項(xiàng)目中成功得到應(yīng)用。

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