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

李海標, 時 君, 田春月

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

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

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

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

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

圖1 系統結構

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

1.1 符號定義

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

(1)

相機姿態由SE(3)來表示：

(2)

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

T與SE(3)上的指數映射有關：

(3)

(4)

式中，把平移記為ρ，把旋轉記為φ，se表示李代數。位姿更新表示為

T←T(ξ)T

(5)

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

1.2 局部地圖的提取

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

八叉樹是一種基于樹的數據結構，通常用于處理三維點云數據。八叉樹中的每個節點都有八個子節點，代表八個子多維數據集，所以利用八叉樹可以快速實現空間劃分和搜索。

1.3 生成深度地圖

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

(6)

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

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

1.4 視覺跟蹤

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

(7)

In[Xi]

(8)

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

X=π(P)

(9)

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

(10)

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

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

(11)

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

1.5 定位

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

(12)

優化的執行方式與跟蹤模塊中的執行方式類似。深度殘差的雅可比矩陣為

(13)

式(13)中相機投影函數的導數定義如下：

(14)

圖2 場景中三平面

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

2 實驗與分析

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

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

2.1 KITTI數據集測試

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

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

圖3 數據集仿真

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

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

2.2 LGSVL仿真

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

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

圖4 系統仿真軌跡

圖5 局部地圖

2.3 真實環境測試

實驗車的傳感器系統配備了3D激光雷達，視覺傳感器等，如圖6所示。首先利用激光雷達獲得點云地圖，在此基礎上重建三維激光雷達圖像。為了進行視覺定位，先利用MATLAB相機校準應用程序對相機的內外參數進行了估計，其次，為了確定相機在傳感器系統中的相對姿態，在相機和三維激光雷達之間也進行了標定。最后為了完成定位，先將相機強度圖像與通過車輛運動重建的局部地圖的強度圖像進行比較。通過優化相機與激光雷達深度圖像之間的誤差，最終實現定位。圖7為激光地圖，圖8為實驗車的軌跡圖，其中細綠色線為地圖提供的真值，粗綠色線為相機的運動軌跡。圖9為實驗的定位誤差。平均平移誤差為5 m，波動幅度為6.8 m。平均旋轉誤差為5°，波動幅度3.5°。此次數據采集的行駛路程也控制在50 km內。

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

圖6 實驗車

圖7 激光地圖

圖8 軌跡圖

圖9 定位誤差

3 結論

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

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