基于激光與視覺融合的車輛自主定位與建圖算法

余祖俊，張晨光，郭保青,b

(北京交通大學，a.機械與電子控制工程學院；b.智慧高鐵系統(tǒng)前沿科學中心，北京100044)

0 引言

隨著計算機技術的快速發(fā)展，自主駕駛成為車輛智能化的重要發(fā)展趨勢，而車輛自身定位與周圍環(huán)境感知是實現(xiàn)自主駕駛的基礎，其準確度和穩(wěn)定性是智能導航、避障檢測、路徑規(guī)劃和車輛控制[1]的基本保障，對提升自主駕駛車輛的安全性有至關重要的作用。

傳統(tǒng)的車輛定位技術大多需要借助GPS/INS組合系統(tǒng)[2-3]來實現(xiàn)，但是GPS系統(tǒng)存在信號不穩(wěn)定的缺陷，在城市高樓或隧道等信號遮擋的地點常會失效，此外，GPS 還存在地圖版本與實際路況不匹配的情況。INS(Inertial Navigation System)慣導系統(tǒng)由于陀螺儀零點漂移嚴重和車輛震動等因素，無法通過直接積分加速度獲得高精度的方位和速度等信息，需要定期糾正累積誤差。SLAM(Simultaneous Localization And Mapping)，即時定位與地圖構建，近年來被更多地用于自主駕駛定位模塊，其相關算法研究經歷了30多年的發(fā)展，所使用的傳感方式主要分為激光和視覺兩類。早期的激光SLAM 算法多以2D 激光雷達配合濾波算法為主，主要包括：卡爾曼濾波和粒子濾波。后期隨著硬件技術的發(fā)展和圖優(yōu)化理論的健全，3D 激光雷達配合優(yōu)化算法成為新的主流方式，ZHANG 等[4]提出的LOAM(Lidar Odometry and Mapping)算法將SLAM 問題分為高頻率、低精度的里程計問題和高精度、低頻率的建圖問題。JI等[5]提出的LLOAM(Lidar Odometry and Mapping with Loopclosure Detection)算法在LOAM 的基礎上添加了基于點云分割匹配的回環(huán)檢測和基于圖優(yōu)化的后端，建立了完整的3D 激光SLAM 算法框架。SHAN 等[6]根據(jù)LOAM 提出一種輕量級的激光雷達測程法LeGO-LOAM(Lightweight and Ground-Optimized LOAM)，應用點云分割對地面點進行優(yōu)化處理并且濾除大量的噪聲點，將位姿分為兩類分別估計。然而，上述算法存在計算量過大，無法長時間運行的問題。

以視覺作為主要觀測手段的SLAM 算法通常被稱為視覺同步定位和繪圖VSLAM(Visual Simultaneous Localization and Mapping)，其發(fā)展類似于激光SLAM，經歷了卡爾曼濾波到粒子濾波再到圖優(yōu)化的過程。西班牙的RAUL等[7]提出的ORB-SLAM2 基于PTAM(Parallel Tracking and Mapping)進行改進，該算法基于ORB 特征點，將算法分為追蹤、局部建圖及回環(huán)檢測3部分。德國的ENGEL 等[8]提出的LSD-SLAM(Large-scale Direct SLAM)是一種在高梯度圖像區(qū)域最小化光度誤差的半感知直接算法。瑞士的FORSTER 等[9]提出介于直接法和特征法之間的半直接視覺測程SVO(Semi-direct Visual Odometry)。上述視覺SLAM 算法由于視覺捕獲信息時易受光照影響的缺陷而存在定位漂移的問題。

近年，融合激光雷達和單目相機的SLAM算法采用緊耦合的模式框架備受關注，ZHANG 等[10]提出一個融合激光雷達、相機和慣導的算法V-LOAM(Visual-Lidar Odometry and Mapping)，其各個模塊采用緊耦合的運作方式，并且運動模型采用等速度高斯過程，測程的精度極高。在V-LOAM 的基礎上，ZHANG 等[11]提出一個在線處理運動數(shù)據(jù)算法LVIO(Laser-Visual-Inertial Odometry)，該算法利用來自3D 激光雷達、相機和慣性測量單元的數(shù)據(jù)構建大范圍環(huán)境地圖。與使用卡爾曼濾波或因子圖優(yōu)化的傳統(tǒng)方法不同，該算法采用順序的、多層處理線程，從粗到細求解運動。然而，上述融合算法主要針對提升算法精確性進行設計，計算量巨大。本文參照LVIO 的算法框架并對其進行輕量化改進，提升在精確性、魯棒性和資源占用率方面的整體表現(xiàn)。

1 問題描述與建模

為描述SLAM 問題，首先，將車輛在一段連續(xù)時間的運動分解成離散時刻t=1,…,k時刻的運動，這些時刻的位置x1,x2,…,xk構成車輛的運動軌跡。然后，設地圖由許多特征點組成，每個時刻傳感器會測量到一部分特征點，得到它們的觀測數(shù)據(jù)，設特征點有n個，為y1,…,yn。不管是激光雷達還是相機，運動方程的數(shù)學模型為

式中：f(·)為運動方程函數(shù)；uk為運動傳感器的讀數(shù)；wk為噪聲。與運動方程相對應，觀測方程為

式中：h(·)為觀測方程函數(shù)；vk,j為觀測噪聲；觀測方程描述的是當車輛在xk位置上提取到第j個特征點yj，產生了一個觀測數(shù)據(jù)zk,j。

運動方程和觀測方程描述了最基本的SLAM問題，即知道運動測量的讀數(shù)u，以及傳感器的讀數(shù)z時，如何求解定位問題(估計x)和建圖問題(估計y)。此時，SLAM 問題就被建模成一個狀態(tài)估計問題。

考慮到車輛行駛所處的復雜環(huán)境，運動和觀測方程通常為非線性，噪聲分布通常為非高斯分布。面對復雜的非線性系統(tǒng)，主流SLAM一般使用以圖優(yōu)化(Graph Optimization)為代表的非線性優(yōu)化技術進行狀態(tài)估計，其中，光束平差法(Bundle Adjustment，BA)是一種性能優(yōu)良的非線性優(yōu)化方法，本文采用基于BA 的圖優(yōu)化方式估計系統(tǒng)位姿和特征。

本文算法用到的坐標系包括：激光雷達坐標系{ }L，相機坐標系{ }C和世界坐標系{ }W。首先，相機的內參矩陣和激光雷達相對于相機的位姿矩陣都可通過聯(lián)合標定獲得。然后，將激光雷達獲取的數(shù)據(jù)從{ }L投影到{ }C下，即可以{ }C代表車輛整體的坐標系{ }S，以車輛初始時刻的{ }S作為{W}。算法坐標系如圖1所示，其中，X，Y，Z軸的軸向分別為車輛的正左方，正上方和正前方。車輛的坐標系隨著車輛的移動而變化，因此，用Sk表示車輛在時刻k的坐標系。

圖1 算法坐標系示意Fig.1 Schematic diagram of algorithm coordinate system

2 算法求解

2.1 算法總體框架

結合算法的需求分析和相關理論知識，本文提出一種基于激光視覺融合的車輛自主定位建圖SLAM算法，分為前端里程計和后端非線性優(yōu)化兩部分。

對于前端里程計部分：首先，分別從預處理后的雷達和相機數(shù)據(jù)中提取ORB特征點和點云邊緣平面特征點，并分別對其進行幀間匹配；然后，利用對應的雷達幀的點云以三角面投影的方式獲取ORB特征點深度得到Space-ORB，再將其和雷達特征點以松耦合的方式輸入到位姿估計模塊，輸出初始位姿。

在后端非線性優(yōu)化部分：利用得到的初步估計位姿將特征點映射到世界坐標系下，并輸入到全局優(yōu)化模塊，通過關鍵幀和滑動窗口的平衡選取策略，以及分類優(yōu)化策略對初始位姿和特征點進一步優(yōu)化；同時，利用基于激光點云的ICP(Iterative Closest Point)修正法和基于ORB 特征點的視覺詞袋模型執(zhí)行回環(huán)檢測；最終，輸出優(yōu)化后的6 自由度位姿和點云地圖。算法整體框架如圖2所示。

圖2 算法整體框架Fig.2 Overall framework of algorithm

2.2 基于點云構建Space-ORB視覺特征點

ORB 特征點沒有深度信息將會導致位姿估計時存在尺度偏差，故基于ORB 的擴展提出Space-ORB。對連續(xù)幀中可以互相匹配的ORB 特征點，利用三角面投影的方法獲取該點的深度。將相機像素平面C上的ORB特征點pc和{ }S的Z軸正向的空間雷達點全部按照比例投影到{ }S的Z軸正向10 m處的平面K上，得到視覺映射點和雷達映射點pl。之后，在K上對每個通過k-d 樹搜索算法在雷達映射點中找到距離其最近的3 個點，再以這3個點所對應原始雷達點,構建一個三角形平面T，將投影到T上得到，則被估計為環(huán)境中ORB特征點pc的真實位置，獲取到該點的深度信息，即獲得了Space-ORB。視覺特征點獲取深度如圖3所示。

圖3 視覺特征點獲取深度示意Fig.3 Visual feature points to acquire depth schematic

2.3 基于松耦合的激光視覺融合位姿估計

傳感器在k與k-1 時刻獲取到的激光和視覺特征匹配點在前、后兩幀整體坐標系下的齊次坐標和前、后兩幀的位姿變換矩陣滿足關系，即

為保證所有匹配點在前、后幀中的總誤差最小，構造位姿優(yōu)化函數(shù)，求使前、后兩幀中各匹配點的位姿誤差對應的二范數(shù)平方總和最小的6維向量。

在構建無約束優(yōu)化模型并得到導數(shù)矩陣后，即可利用Levenberg-Marquardt(L-M)法進行目標優(yōu)化函數(shù)的迭代求解，在達到設定的迭代次數(shù)或者精度之后，停止迭代，得到初步估計的局部位姿。

式(4)的位姿優(yōu)化函數(shù)以誤差項的二范數(shù)平方總和最小為目標函數(shù)，但在SLAM算法中存在誤匹配特征點會使對應的誤差項嚴重偏大，進而導致其梯度也很大，即算法將花費大量的計算量用于調整一個錯誤的值。該問題的根源是二范數(shù)平方的增長速率過快造成的。為此，將式(4)中的匹配點誤差項代入Cauchy核函數(shù)，由于Cauchy核函數(shù)采用了對數(shù)操作，當個別誤匹配點引起誤差項過大時目標優(yōu)化函數(shù)值也不至于過大，可以實現(xiàn)異常值的抑制，即

為進一步降低誤匹配點造成的誤差項，在Cauchy 誤差項基礎上提出Cauchy-Huber 誤差度量函數(shù)。當匹配點的Cauchy 誤差項較小時，Huber函數(shù)仍保持原來的度量函數(shù)；當較大時，Huber 函數(shù)將原來的二次函數(shù)變?yōu)榫€性函數(shù)，可降低誤匹配點的影響，提升算法對異常值的泛化性和位姿優(yōu)化求導時的平滑性。

式中：δ為所有匹配點誤差項的標準差。

2.4 基于關鍵幀和滑動窗口的平衡選取策略

同時優(yōu)化過去所有位姿和數(shù)據(jù)點會導致計算量過大，并且誤差通常都是局部存在的，針對此問題，某些SLAM算法采用選取關鍵幀加滑動窗口的方式進行全局優(yōu)化。但是其選取關鍵幀的頻率和滑動窗口的長度是固定的，導致泛化性較差。本文對關鍵幀的位置和滑動窗口長度的選取提出一種平衡選取策略，如圖4所示。圖中，矩形框為滑動窗口，包含待優(yōu)化的數(shù)據(jù)幀。

圖4 基于關鍵幀和滑動窗口的位姿優(yōu)化Fig.4 Pose optimization based on keyframe and sliding window

如圖4中圓圈內的數(shù)字所示，關鍵幀的選擇策略是先將所有的數(shù)據(jù)幀分為3類：可以直接用于穩(wěn)定優(yōu)化的幀，無用信息過多不應使用的幀，可以使用但需要處理減少計算成本的幀。第1 類是會影響關鍵測量的幀，在一些快速變換的場景下，例如，大幅度轉彎處，此時特征匹配難度增加，需要增加關鍵幀的密度。第2 類是當車輛自身相對道路靜止時，周遭場景內可能會有其他的動態(tài)物體干擾車輛姿態(tài)估計，此時，需要設定動態(tài)特征點的數(shù)量閾值，在低于閾值時，不選取任何關鍵幀。第3 類是余下一般情況的幀。提取關鍵幀的頻率fk為

式中：n為可連續(xù)追蹤特征點的數(shù)量；nmax、nmin分別為最大值、最小值；v為車輛速度；vmax為最大車速。

為保證滿足優(yōu)化精度的同時計算量也盡可能小，提出平衡選取公式來決定優(yōu)化窗口的長度lw，即

當前關鍵幀和窗口最新的關鍵幀進行特征點的匹配計算，小于某個閾值時就把當前關鍵幀納入優(yōu)化窗口作為最新的關鍵幀，同時長度l本身有最大lmax和最小lmin的限制值。

2.5 基于特征點和位姿的分類優(yōu)化策略

為更有效率的后端優(yōu)化，特征點的選取也極為重要。特征點本身應當具有良好的觀測性，無異常值，并且在3 維空間中均勻分布，便于后續(xù)的建圖工作。本文設置閾值df，以激光雷達的最大探測范圍將車輛附近區(qū)域分成遠近2 個區(qū)域；將Space-ORB，邊緣激光點，平面激光點分為遠近2類，即激光和視覺特征點分為6類。

由于特征點距離車輛的遠、近對運動姿態(tài)估計中的平移誤差和旋轉誤差有不同的影響，近點對于平移估計影響較大，遠點對于旋轉估計影響較大。除此之外，在車輛的6自由度位姿估計中，X、Z 平面下的位置和偏航角β的精確度更為重要。因此，將位姿李代數(shù)分為ξX,Z,ξβ,ξY和ξα,γ4 類，令不同類的點分別對不同的位姿進行優(yōu)化，如圖5所示。

圖5 特征點和位姿分類的優(yōu)化策略Fig.5 Optimization strategy for feature points and pose classification

2.6 基于點云ICP算法的回環(huán)檢測模塊設計

回環(huán)檢測，主要解決位置估計隨時間漂移的問題。通過檢測機制，判斷車輛當前所處位置與之前到過的位置的場景相似性，在檢測出回環(huán)后，通過優(yōu)化手段，將偏移值消除，完成回環(huán)檢測。本文參考LeGO-LOAM[6]算法中的回環(huán)檢測模塊并對其加以改進，添加了一個位姿距離約束條件以提升其檢測率。

首先，要對是否存在回環(huán)進行判斷。設當前幀激光特征點云為Pinput，對半徑為r的圓形范圍內進行位姿距離檢測，這里要排除當前幀前t秒內的幀，將檢測到的與當前位姿距離小于r的目標幀點云投影到世界坐標系下作為待檢測點云Ptarget。然后對Pinput和Ptarget執(zhí)行ICP 算法，在滿足迭代次數(shù)并且噪聲分數(shù)較低的條件下，輸出位姿向量ft，此時，即認為檢測到了回環(huán)。

得到ft后，可算出被檢測到的位姿在歷史位姿幀中的位置kl。最后，對第ks幀和第kl幀之間的車輛位姿進行優(yōu)化修正，通過加上第ks幀和第kl幀位姿的額外約束，將其代入到全局優(yōu)化函數(shù)中。最終輸出修正后的位姿軌跡和點云地圖，即優(yōu)化幀。回環(huán)檢測流程如圖6所示。

圖6 回環(huán)檢測流程Fig.6 Loop detection flow chart

3 實驗與結果分析

3.1 算法魯棒性實驗

本文實驗選取KITTI[12]數(shù)據(jù)集的Odometry 場景合集作為測試對象。該數(shù)據(jù)集包含了標注真值和未標注真值的各11 個場景的真實數(shù)據(jù)序列，涵蓋郊區(qū)公路、高速公路、城市道路等各類典型道路場景，視覺測距序列涵蓋39.2 km的道路場景，每個場景數(shù)據(jù)包含：圖像、激光雷達點云及真實位姿的數(shù)據(jù)序列，是測試定位建圖算法通用的數(shù)據(jù)集。關于魯棒性的第1 個實驗是通過算法提取的特征點個數(shù)檢驗其對不同環(huán)境的適應能力。具體來說，分別檢測不同環(huán)境下本文融合算法所提取的視覺特征點和激光雷達特征點的數(shù)量，并將結果與激光算法A-LOAM 和視覺算法ORB-SLAM2(Mono)進行比較。為更直觀地比較3 種算法在不同場景下提取特征的差異，本文將每種算法在各個場景下的特征提取數(shù)占所有場景總特征提取數(shù)的百分比繪制成條狀對比圖，如圖7所示。

圖7 各算法提取的特征點個數(shù)占其總數(shù)的百分比Fig.7 Percentage of number of feature points extracted by each algorithm in total

由圖7可知，在以郊區(qū)為主要場景的環(huán)境(場景3、場景4)和高速公路(場景1)中，視覺特征點較少，激光雷達特征點較多，這將影響到單以視覺特征點作為運動先驗的ORB-SLAM2(Mono)算法后續(xù)位姿估計的計算。在以城市為主要場景的環(huán)境(場景0、場景5、場景6、場景7)中，激光雷達特征點較少，而視覺特征點較多，這將影響到單以激光雷達特征點作為運動先驗的A-LOAM算法后續(xù)位姿估計的計算。融合算法同時接受視覺和激光雷達特征點作為運動先驗，因此，從每個場景中提取的特征點總量相對平均，這也證明了該算法在不同環(huán)境下具有較好的魯棒性。

測試算法魯棒性的第2 個實驗是在各個場景下以每隔100 m 為節(jié)點，取前800 m 的平均平移誤差和旋轉誤差。由圖8(a)和圖8(b)可知，A-LOAM算法的平移誤差隨長度的增加而增大，這是因為該算法沒有回環(huán)檢測，導致路徑過長時漂移嚴重。相比之下，本文融合算法和ORB-SLAM2(Mono)的平移誤差和旋轉誤差隨長度的增加而較為平穩(wěn)的下降。此外，還測試了算法在不同車速下的誤差。由圖8(c)和圖8(d)可以看出，ORB-SLAM2(Mono)在車速變化時引起的誤差變化較大，而融合算法和A-LOAM的誤差變化較為穩(wěn)定。

圖8 所有場景的平均平移和旋轉誤差Fig.8 Average translation and rotation errors for all scenarios

綜上可知，融合算法在不同的環(huán)境下具有較好的適應能力，且穩(wěn)定性較強，面對不同的車速變化具有更強的抗干擾能力，算法魯棒性較高。

3.2 算法精確性實驗

本文對算法的精確性評估是通過計算位姿軌跡與地面真實軌跡之間的絕對誤差APE(Absolute Pose Error)和相對誤差RPE(Relative Pose Error)實現(xiàn)的。APE 指兩個軌跡上相應位姿之間的歐式距離，反映算法整體的精確性；RPE 指兩個軌跡上相應的相鄰位姿之差的歐式距離，反映算法局部的精確性。本文在Odometry00～Odometry10 共11 個場景下分別運行3個算法，將輸出位姿分別與真實軌跡比較計算APE 和RPE，并輸出標準差、均方根誤差及均值。各算法在11個場景下的輸出誤差值取平均值并構建條狀對比圖，如圖9所示。

由圖9 可知，相比于另外兩種算法，ORBSLAM2(Mono)算法在4 類誤差中表現(xiàn)較差。在絕對平移和旋轉誤差方面，融合算法的各項誤差值均略低于A-LOAM，均值達到了28.47 m 和0.03 rad。而在相對旋轉誤差方面，融合算法各項誤差值遠低于A-LOAM，均值達到了0.11 m 和0.002 rad。因此，與另外兩種算法相比，融合算法綜合精確度誤差最小。

圖9 各算法在所有場景下的平均誤差Fig.9 Average error of each algorithm in all scenarios

3.3 回環(huán)檢測實驗

本文將融合算法與Lego-loam[6]進行回環(huán)檢測模塊的實驗比對，以Odometry 中的Sequence05 作為測試場景，該場景擁有較多的回環(huán)部分。算法輸出的地圖主要為道路和兩側的邊緣特征，包括算法輸出的車輛運行軌跡?；丨h(huán)檢測效果如圖10所示。

圖10(a)為Lego-loam 輸出的軌跡和點云地圖中未能檢測出回環(huán)的路段截取，可以看出，道路的邊緣位置點云發(fā)生了重影現(xiàn)象，算法在所示路段未能很好檢測到回環(huán)導致其所構建的地圖在同一地方出現(xiàn)了2 條重合的街道。針對相同的路段，圖10(b)為本文融合算法的運行結果，街道及其邊緣特征點云分布清晰，不存在交叉模糊的情況，成功檢測到回環(huán)并進行了修正。

圖10 回環(huán)檢測效果對比Fig.10 Comparison of loop detection effect

因此，本文算法對Lego-loam 的回環(huán)檢測模塊進行改進后，檢測率得到提升，車輛軌跡和地圖點云漂移現(xiàn)象有明顯改善。

3.4 資源占用率測試實驗

自動駕駛系統(tǒng)除了定位模塊之外，還需配有目標檢測，路徑規(guī)劃等模塊。因此，在有限的計算資源下，應當盡可能地縮小算法的計算量，同時也要保證算法的性能。 本文對3 種算法在sequence00～sequence10 各個環(huán)境下運行時計算機的CPU(I7-7700HQ，2.80G HZ×4)和內存(16 G)平均占用率進行了測試和對比。測試方法是運行時每5 s 記錄1 次CPU 和內存的占用率，然后取平均值。算法CPU占用率如表1所示，算法內存占用率如表2所示。

表1 算法CPU占用率比較表Table 1 Comparison of CPU occupancy of algorithms

表2 算法內存占用率比較表Table 2 Comparison of memory occupancy of algorithms

表格的最后1列為3種算法在所有場景下的占用率數(shù)值再取平均值。從平均值中可以看出，本文融合算法的CPU 平均占用率為22.18%，介于其他兩種算法之間，但其內存平均占用率為21.50%，遠低于其他兩種算法。這是由于本文算法提取特征點總量較少，且后端的優(yōu)化策略減少了計算量。綜合前文的算法性能測試來看，本文提出的融合算法資源占用率更少，而準確度和魯棒性更高，適用于車輛自主駕駛系統(tǒng)的定位。

3.5 校園環(huán)境實車實驗

除了數(shù)據(jù)集外，本文還利用前文所述的硬件和軟件平臺進行實車實驗，車輛行駛環(huán)境為校園內部，多為教學樓和植被。通過布置在車上的激光雷達和相機，對車輛周圍的環(huán)境進行信息采集，同時，利用算法對車輛進行定位并對校園環(huán)境構建3 維點云地圖。

利用上述算法在北京交通大學主校區(qū)內大范圍的運行效果如圖11所示，其中，圖11(a)為主校區(qū)內的點云地圖以及車輛軌跡；圖11(b)為衛(wèi)星鳥瞰圖下的主校區(qū)。本次實驗全程路徑約3 km，局部存在部分小型回環(huán)，整體為大型回環(huán)。從圖11(a)中可以看出，車輛行駛路徑周圍環(huán)境的點云地圖構建較為清晰，在各個局部小回環(huán)和大型回環(huán)處均能成功檢出并修正漂移。將圖11(a)與圖11(b)進行比較可以看出，算法在大范圍場景下最終生成的地圖構建良好，車輛軌跡無明顯漂移現(xiàn)象。因此，算法在小范圍和大范圍場景下均有良好的表現(xiàn)，實車實驗較為成功。

圖11 北京交通大學主校區(qū)建圖與車輛軌跡Fig.11 Building map and vehicle track of main campus of Beijing Jiaotong University

4 結論

本文提出一種基于激光與視覺融合SLAM 的車輛自主定位算法。KITTI 數(shù)據(jù)集及校園實車實驗表明，本文算法在車輛不同行駛距離和不同車速下具有穩(wěn)定的定位精度，且在各場景下的特征點總數(shù)趨于穩(wěn)定，具有良好的魯棒性。與其他兩種經典算法相比，本文算法在精確性、魯棒性和資源占用率上均有良好表現(xiàn)。算法平均定位相對誤差為0.11 m 和0.002 rad，綜合精確度最高；CPU 平均占用率為22.18%，介于其他兩種算法之間；內存平均占用率為21.50%，遠低于其他兩種算法。