李海標, 時 君, 田春月

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

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

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

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

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

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

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

1.1 符號定義

首先定義二維點和三維點的歸一化坐標如下：

(1)

相機姿態(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 局部地圖的提取

在全局的點云地圖中，對相機可觀察到的局部區(qū)域進行提取，將提取的局部區(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ù)最小化，以達到求解每個像素最優(yōu)差異值的目的，能量函數(shù)定義如下：

(6)

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

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

1.4 視覺跟蹤

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

(7)

In[Xi]

(8)

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

X=π(P)

(9)

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

(10)

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

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

(11)

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

1.5 定位

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

(12)

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

(13)

式(13)中相機投影函數(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ù)采集車進行驗證。

實驗分為兩部分，測試單獨的定位模塊和測試完整的SLAM框架。首先通過使用KITTI數(shù)據(jù)集測試驗證視覺定位器模塊。將視覺定位精度與KITTI提供的數(shù)據(jù)進行比較，以進行定量評估。在仿真軟件和真實場景中，將定位模塊導(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)在十字路口存在較大的誤差，當汽車轉(zhuǎn)彎時，相機旋轉(zhuǎn)，而鄰近的建筑物和停放的汽車忽然缺失，因此推斷，所提出的算法不適用于快速旋轉(zhuǎn)運動和無鄰近結(jié)構(gòu)的情況。然而，該方法的性能與其他方法并無明顯差別。序列00的結(jié)果顯示，平均平移和旋轉(zhuǎn)誤差低于文獻[15]中提出的結(jié)果,軌跡圖如圖3(d)所示，本算法的估計值和數(shù)據(jù)集提供的真值基本吻合。

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

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

表1 定位誤差(平均值±標準差)

2.2 LGSVL仿真

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

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

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

圖5 局部地圖

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

實驗車的傳感器系統(tǒng)配備了3D激光雷達，視覺傳感器等，如圖6所示。首先利用激光雷達獲得點云地圖，在此基礎(chǔ)上重建三維激光雷達圖像。為了進行視覺定位，先利用MATLAB相機校準應(yīng)用程序?qū)ο鄼C的內(nèi)外參數(shù)進行了估計，其次，為了確定相機在傳感器系統(tǒng)中的相對姿態(tài)，在相機和三維激光雷達之間也進行了標定。最后為了完成定位，先將相機強度圖像與通過車輛運動重建的局部地圖的強度圖像進行比較。通過優(yōu)化相機與激光雷達深度圖像之間的誤差，最終實現(xiàn)定位。圖7為激光地圖，圖8為實驗車的軌跡圖，其中細綠色線為地圖提供的真值，粗綠色線為相機的運動軌跡。圖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)的精度將極大降低，每當檢測到定位失效時，必須要重新啟動系統(tǒng)。而在結(jié)構(gòu)豐富區(qū)域，系統(tǒng)運行良好。經(jīng)過多次采集數(shù)據(jù)并進一步進行分析，在結(jié)構(gòu)豐富的區(qū)域，稠密或稀疏的激光點云地圖對定位系統(tǒng)精度的影響并不大，而在無豐富結(jié)構(gòu)的區(qū)域，稀疏的激光點云地圖無法很好的輔助視覺定位，運行時，系統(tǒng)頻繁的重啟。

圖6 實驗車

圖7 激光地圖

圖8 軌跡圖

圖9 定位誤差

3 結(jié)論

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

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