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

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

0 引言

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

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

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

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

1 問題描述與建模

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

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

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

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

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

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

圖1 算法坐標(biāo)系示意Fig.1 Schematic diagram of algorithm coordinate system

2 算法求解

2.1 算法總體框架

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

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

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

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

2.2 基于點(diǎn)云構(gòu)建Space-ORB視覺特征點(diǎn)

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

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

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

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

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

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

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

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

式中：δ為所有匹配點(diǎn)誤差項(xiàng)的標(biāo)準(zhǔn)差。

2.4 基于關(guān)鍵幀和滑動窗口的平衡選取策略

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

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

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

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

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

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

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

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

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

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

2.6 基于點(diǎn)云ICP算法的回環(huán)檢測模塊設(shè)計(jì)

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

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

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

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

3 實(shí)驗(yàn)與結(jié)果分析

3.1 算法魯棒性實(shí)驗(yàn)

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

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

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

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

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

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

3.2 算法精確性實(shí)驗(yàn)

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

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

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

3.3 回環(huán)檢測實(shí)驗(yàn)

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

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

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

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

3.4 資源占用率測試實(shí)驗(yàn)

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

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

表2 算法內(nèi)存占用率比較表Table 2 Comparison of memory occupancy of algorithms

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

3.5 校園環(huán)境實(shí)車實(shí)驗(yàn)

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

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

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

4 結(jié)論

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