一種基于深度特征的室外環(huán)境下激光地圖輔助視覺定位方法

李海標(biāo), 時 君, 田春月

(桂林電子科技大學(xué)機(jī)電工程學(xué)院，桂林 541000)

自主定位是機(jī)器人領(lǐng)域中的一個重要技術(shù)，也是實現(xiàn)自主導(dǎo)航的重要前提。移動設(shè)備通過自身安裝的傳感器，獲取相關(guān)的環(huán)境信息，然后通過對數(shù)據(jù)的處理，最后推算出自身的位置。一般情況下，室外的移動設(shè)備依靠GPS就可以進(jìn)行定位，GPS可以提供無漂移的全局定位。然而，GPS定位經(jīng)常會受到由多徑效應(yīng)引起的間歇性誤差的影響，比如在城市、峽谷和室內(nèi)環(huán)境下，無法很好發(fā)提供定位。

激光雷達(dá)可以提供準(zhǔn)確的3D矢量信息，因此可以使用掃描匹配技術(shù)迭代最近點(iterated closest point,ICP)[1]在給定地圖和當(dāng)前掃描之間進(jìn)行直接匹配，目前在室外環(huán)境下，自動駕駛車輛也多采用激光雷達(dá)進(jìn)行地圖的創(chuàng)建與定位。由于成本和硬件要求，考慮到經(jīng)濟(jì)效益，大范圍的使用激光雷達(dá)并不是最好的選擇。所以研究人員更傾向于利用廉價的相機(jī)來實現(xiàn)位姿的估計。

利用相機(jī)和激光數(shù)據(jù)融合進(jìn)行定位存在一定困難，主要是因為相機(jī)和激光雷達(dá)數(shù)據(jù)是兩種不同形式的數(shù)據(jù)，實現(xiàn)不同傳感器數(shù)據(jù)的自動標(biāo)定和融合也是解決同時定位與地圖創(chuàng)建多傳感器融合的關(guān)鍵。Frossard等[2]提出了通過端到端的方式，利用卷積神經(jīng)網(wǎng)絡(luò)將視覺和激光雷達(dá)數(shù)據(jù)直接生成3D軌跡。浙江大學(xué)團(tuán)隊利用基于激光地圖的幾何信息，提出一種混合BA(bundle adjustment)框架，該方法可以估計相機(jī)相對于激光地圖的位姿，同時優(yōu)化了視覺慣導(dǎo)里程計中的狀態(tài)變量，為了更準(zhǔn)確地進(jìn)行交叉模式數(shù)據(jù)關(guān)聯(lián)，使用了多會話激光和視覺數(shù)據(jù)來優(yōu)化激光地圖以提取用于視覺定位的顯著且穩(wěn)定的子集[3]。

與上述方法不同的是本文算法不關(guān)注全局的尺度定位，采用了一種在給定的激光地圖中進(jìn)行視覺定位的方法，系統(tǒng)結(jié)構(gòu)如圖1所示。定位系統(tǒng)由四個模塊組成，預(yù)處理過程是對從地圖和圖像流中獲取的原始數(shù)據(jù)進(jìn)行處理，用于之后的跟蹤和定位模塊。深度圖是利用立體視差產(chǎn)生。在局部地圖提取中，將從全局激光地圖中提取與視覺深度匹配的局部3D地圖。系統(tǒng)開始時，需要在給定的激光地圖上假定相機(jī)的初始位姿，為了確定初始假定相機(jī)位姿的合理性，在定位之前需要進(jìn)行視覺跟蹤，該模塊對連續(xù)圖像幀之間的相對姿態(tài)進(jìn)行估計，然后利用跟蹤過程中的局部圖、深度圖和假定位姿，對6自由度相機(jī)姿態(tài)進(jìn)行估計。

圖1 系統(tǒng)結(jié)構(gòu)

1 基于激光地圖的視覺定位

1.1 符號定義

首先定義二維點和三維點的歸一化坐標(biāo)如下：

(1)

相機(jī)姿態(tài)由SE(3)來表示：

(2)

式(2)中：R表示旋轉(zhuǎn)；t表示平移；SE表示李群。

T與SE(3)上的指數(shù)映射有關(guān)：

(3)

(4)

式中，把平移記為ρ，把旋轉(zhuǎn)記為φ，se表示李代數(shù)。位姿更新表示為

T←T(ξ)T

(5)

所有增量均采用左乘的方式[4]。

1.2 局部地圖的提取

在全局的點云地圖中，對相機(jī)可觀察到的局部區(qū)域進(jìn)行提取，將提取的局部區(qū)域定義為局部地圖，使用基于八叉樹[5]的半徑內(nèi)近鄰搜索實現(xiàn)局部地圖的提取。

八叉樹是一種基于樹的數(shù)據(jù)結(jié)構(gòu)，通常用于處理三維點云數(shù)據(jù)。八叉樹中的每個節(jié)點都有八個子節(jié)點，代表八個子多維數(shù)據(jù)集，所以利用八叉樹可以快速實現(xiàn)空間劃分和搜索。

1.3 生成深度地圖

本系統(tǒng)實現(xiàn)跟蹤和定位任務(wù)都是依據(jù)深度圖來完成。首先，利用OpenCV[6]中的SGMB算法[7]獲得視差圖。SGMB是一種立體匹配算法，通過最小化基于互信息構(gòu)成的能量函數(shù)來估計差異，利用該方法可以獲得高密度的立體視差圖。通過選取每個像素點的差值，組成一個差值圖，設(shè)置一個和差值圖相關(guān)的全局能量函數(shù)，使這個能量函數(shù)最小化，以達(dá)到求解每個像素最優(yōu)差異值的目的，能量函數(shù)定義如下：

(6)

式(6)中：p、q為圖像中的像素；Np是像素p的相鄰像素點；C(p,Dp)是當(dāng)像素點為Dp時，該像素點的損失函數(shù)；P1、P2為懲罰系數(shù)；I(·)是一個判斷函數(shù)。

最后，采用文獻(xiàn)[8-9]中的方法估計場景深度，通過重新統(tǒng)計三角測量特征分布，并且對級數(shù)進(jìn)行展開，消除三角測量中與距離相關(guān)的統(tǒng)計偏差。

1.4 視覺跟蹤

視覺跟蹤可以為定位提供初始值。當(dāng)系統(tǒng)執(zhí)行定位任務(wù)時，由于基于深度的定位系統(tǒng)不依賴于特定的跟蹤算法，可按照特定要求選擇合適的跟蹤算法。現(xiàn)采用一種基于高斯-牛頓光度誤差最小化的視覺跟蹤算法來實現(xiàn)定位[10]。首先利用相機(jī)位姿估計的初始值，根據(jù)像素的梯度，尋找到兩個對應(yīng)像素點的位置，通過優(yōu)化點之間的光度誤差，最終求解出相機(jī)位姿。

(7)

In[Xi]

(8)

式(8)中Xi=[ui,vi,1]T，它是一個三維向量，表示在齊次坐標(biāo)系下的圖像中第i個像素的坐標(biāo)。第i個像素的深度表示為di。三維空間中的點的坐標(biāo)P=[x,y,z,1]T，通過投影函數(shù)π(·)將三維空間中的點投影到圖像平面上：

X=π(P)

(9)

π-1(·)表示圖像投影函數(shù)的倒數(shù)。當(dāng)前幀的圖像強(qiáng)度為In，上一幀圖像強(qiáng)度為In-1，I[X]表示在圖像點X處的強(qiáng)度。

(10)

其中的增量ξ是來自方程:

JTΩJξ=-JTΩJr(0)

(11)

式(11)中：J由Ji組成，Ji是rI,j的雅可比矩陣;信息矩陣Ω的對角由誤差項方差的倒數(shù)組成;此函數(shù)的求解是一個非線性優(yōu)化問題，利用g2o[11]庫求解上述問題，它是一種強(qiáng)大的圖優(yōu)化框架。

1.5 定位

視覺定位通過匹配從雙目像機(jī)獲得的地圖點和當(dāng)前激光深度地圖來實現(xiàn)。地圖點的深度與相應(yīng)相機(jī)深度之間差定義為深度差rD，具體定義如下：

(12)

優(yōu)化的執(zhí)行方式與跟蹤模塊中的執(zhí)行方式類似。深度殘差的雅可比矩陣為

(13)

式(13)中相機(jī)投影函數(shù)的導(dǎo)數(shù)定義如下：

(14)

圖2 場景中三平面

然而，在過于復(fù)雜的環(huán)境中，不同物體的交界會出現(xiàn)深度值的突變，三維邊緣產(chǎn)生的深度突變，會影響位姿估計的精度。這是因為根據(jù)雙目視覺深度估計出的邊的位置有時會產(chǎn)生偏差，由于邊緣引起的深度梯度的變化，使得位姿也會隨之更新，因此會產(chǎn)生突變。為了使系統(tǒng)盡可能不受邊緣影響，將深度殘差的方差設(shè)置為與深度梯度的大小成正比，最終物體邊緣對系統(tǒng)精度的影響將會減小。

2 實驗與分析

為了驗證算法的精度與可靠性，首先使用了KITTI[13]數(shù)據(jù)集驗證本文算法，其次使用LG公司的自動駕駛仿真軟件LGSVL測試本文定位算法，最后為了檢測本算法在真實環(huán)境中的可行性，利用數(shù)據(jù)采集車進(jìn)行驗證。

實驗分為兩部分，測試單獨(dú)的定位模塊和測試完整的SLAM框架。首先通過使用KITTI數(shù)據(jù)集測試驗證視覺定位器模塊。將視覺定位精度與KITTI提供的數(shù)據(jù)進(jìn)行比較，以進(jìn)行定量評估。在仿真軟件和真實場景中，將定位模塊導(dǎo)入了ORB-SLAM[14]框架中，ORB-SLAM中的跟蹤模塊提供的值作為定位器的初始值。

2.1 KITTI數(shù)據(jù)集測試

在數(shù)據(jù)集序列00～10上評估系統(tǒng)的定位算法，由于01序列的場景是高速路上，此場景包含的特征有限，所以文中的算法在這個序列中失敗了。在其他序列數(shù)據(jù)中均取得良好的效果。

數(shù)據(jù)集測試路程選擇在100里內(nèi)。圖3(a)～圖3(c)顯示了序列中的平移和旋轉(zhuǎn)誤差。平移誤差始終小于1.0 m，旋轉(zhuǎn)誤差小于5°，平均平移誤差為0.13 m，平均波動幅度0.1 m。平均轉(zhuǎn)動誤差為0.4°，平均波動幅度0.4°。分析誤差圖時，發(fā)現(xiàn)在十字路口存在較大的誤差，當(dāng)汽車轉(zhuǎn)彎時，相機(jī)旋轉(zhuǎn)，而鄰近的建筑物和停放的汽車忽然缺失，因此推斷，所提出的算法不適用于快速旋轉(zhuǎn)運(yùn)動和無鄰近結(jié)構(gòu)的情況。然而，該方法的性能與其他方法并無明顯差別。序列00的結(jié)果顯示，平均平移和旋轉(zhuǎn)誤差低于文獻(xiàn)[15]中提出的結(jié)果,軌跡圖如圖3(d)所示，本算法的估計值和數(shù)據(jù)集提供的真值基本吻合。

圖3 數(shù)據(jù)集仿真

表1總結(jié)了其他序列的定位誤差，每個序列中的定位誤差以平均值±標(biāo)準(zhǔn)偏差格式給出。平均平移誤差小于0.5 m，平均旋轉(zhuǎn)誤差小于1.0°。算法在序列00上的性能最好，而在序列04上的性能最差，場景中結(jié)構(gòu)的缺失可能是導(dǎo)致算法性能下降的主要原因。

表1 定位誤差(平均值±標(biāo)準(zhǔn)差)

2.2 LGSVL仿真

在利用KITTI數(shù)據(jù)集測試時，評估定位的精度達(dá)到了亞米級。然而，在KITTI數(shù)據(jù)集大多數(shù)場景都是從居民區(qū)或高速公路上拍攝的，場景比較單一。為了在更具挑戰(zhàn)性的場景中進(jìn)一步評估本算法，利用LGSVL仿真系統(tǒng)和SLAM框架，在結(jié)構(gòu)變化大、道路寬、環(huán)境隨時間變化以及動態(tài)物體等場景中進(jìn)行評估。系統(tǒng)在線運(yùn)行基于深度的定位模塊，當(dāng)深度殘差較大時，會選擇地進(jìn)行從雙目相機(jī)和地圖的重建點云之間執(zhí)行局部ICP。

利用仿真軟件，使汽車行駛一定的距離，將相機(jī)的運(yùn)動軌跡與地圖提供的真值進(jìn)行對比。在汽車行駛過程中，當(dāng)系統(tǒng)出現(xiàn)問題時，如無法進(jìn)行定位，將會重啟系統(tǒng)，圖4(a)中C、D、E處表示重啟，而結(jié)構(gòu)豐富的A、B處，系統(tǒng)運(yùn)行良好。圖5是行駛過程中的部分地圖。

圖4 系統(tǒng)仿真軌跡

圖5 局部地圖

2.3 真實環(huán)境測試

實驗車的傳感器系統(tǒng)配備了3D激光雷達(dá)，視覺傳感器等，如圖6所示。首先利用激光雷達(dá)獲得點云地圖，在此基礎(chǔ)上重建三維激光雷達(dá)圖像。為了進(jìn)行視覺定位，先利用MATLAB相機(jī)校準(zhǔn)應(yīng)用程序?qū)ο鄼C(jī)的內(nèi)外參數(shù)進(jìn)行了估計，其次，為了確定相機(jī)在傳感器系統(tǒng)中的相對姿態(tài)，在相機(jī)和三維激光雷達(dá)之間也進(jìn)行了標(biāo)定。最后為了完成定位，先將相機(jī)強(qiáng)度圖像與通過車輛運(yùn)動重建的局部地圖的強(qiáng)度圖像進(jìn)行比較。通過優(yōu)化相機(jī)與激光雷達(dá)深度圖像之間的誤差，最終實現(xiàn)定位。圖7為激光地圖，圖8為實驗車的軌跡圖，其中細(xì)綠色線為地圖提供的真值，粗綠色線為相機(jī)的運(yùn)動軌跡。圖9為實驗的定位誤差。平均平移誤差為5 m，波動幅度為6.8 m。平均旋轉(zhuǎn)誤差為5°，波動幅度3.5°。此次數(shù)據(jù)采集的行駛路程也控制在50 km內(nèi)。

通過對實驗結(jié)果分析發(fā)現(xiàn)，在無結(jié)構(gòu)區(qū)域定位極易失效。在道路寬闊的地帶，由于附近的結(jié)構(gòu)信息稀缺，定位系統(tǒng)的精度將極大降低，每當(dāng)檢測到定位失效時，必須要重新啟動系統(tǒng)。而在結(jié)構(gòu)豐富區(qū)域，系統(tǒng)運(yùn)行良好。經(jīng)過多次采集數(shù)據(jù)并進(jìn)一步進(jìn)行分析，在結(jié)構(gòu)豐富的區(qū)域，稠密或稀疏的激光點云地圖對定位系統(tǒng)精度的影響并不大，而在無豐富結(jié)構(gòu)的區(qū)域，稀疏的激光點云地圖無法很好的輔助視覺定位，運(yùn)行時，系統(tǒng)頻繁的重啟。

圖6 實驗車

圖7 激光地圖

圖8 軌跡圖

圖9 定位誤差

3 結(jié)論

采用了一種利用激光三維地圖輔助相機(jī)定位的算法。利用視覺跟蹤的初始估計，通過最小化深度殘差估計地圖中的6自由度相機(jī)位姿。各種數(shù)據(jù)集的結(jié)果表明，方法具有可行性。本文方法可以作為無GPS傳感器下的完成車輛定位的補(bǔ)充解決方案，尤其是在復(fù)雜城市區(qū)域內(nèi)的狹窄街道，能夠達(dá)到較好的定位效果。

針對在無豐富場景結(jié)構(gòu)下，定位系統(tǒng)無法很好工作的問題，下一步將研究文中的定位傳感器與其他傳感器(如GPS和慣性測量單元IMU)進(jìn)行融合來增強(qiáng)定位系統(tǒng)的魯棒性，進(jìn)一步考慮光度變化和動態(tài)物體下的系統(tǒng)的精度。