王鉉彬，李星星，廖健馳，馮紹權(quán)，李圣雨，周宇軒

武漢大學(xué)測繪學(xué)院，湖北 武漢 430079

同時定位與地圖構(gòu)建技術(shù)(simultaneous localization and mapping,SLAM)是智能化移動載體實現(xiàn)自我感知的一項關(guān)鍵技術(shù)，被廣泛應(yīng)用于移動機器人、無人機、自動駕駛等領(lǐng)域[1-4]。隨著智能化移動載體搭載的傳感器種類逐漸豐富，如何有效地融合不同類型的傳感器，為移動載體提供更高精度的即時定位與建圖是當(dāng)下需要解決的重要問題，也是SLAM技術(shù)發(fā)展的需要。

視覺慣性SLAM技術(shù)充分利用了視覺傳感器與慣性傳感器的互補特性[5]，即在視覺友好的環(huán)境下視覺SLAM可以實現(xiàn)高精度的定位與建圖，但在一些弱紋理、低光照、高動態(tài)的場景下表現(xiàn)欠佳，而慣性導(dǎo)航(inertial navigation system,INS)能夠在短時間內(nèi)實現(xiàn)高精度的航位推算，從而在視覺退化的情形下，融合視覺與慣性可以有效提升視覺SLAM的穩(wěn)健性。此外，視覺SLAM也可以幫助INS修正傳感器的累積誤差。從視覺與慣性的融合方式來區(qū)分，可以分為松耦合[6]和緊耦合[7-8]，其中緊耦合方案能夠充分發(fā)揮出兩種傳感器的優(yōu)勢，成為目前的主流方案。在已有的緊耦合視覺慣性SLAM算法中，從估計器的角度來區(qū)分，可以分為基于濾波的方法和基于優(yōu)化的方法?；跒V波的方法具有效率高、易拓展等優(yōu)勢，其中具有代表性的算法是文獻(xiàn)[9]提出的MSCKF算法，該方法對視覺路標(biāo)點進行了邊緣化處理，顯著提高了算法效率，算法中同時維護了多個相機狀態(tài)，通過多相機的共視關(guān)系構(gòu)成多狀態(tài)約束，有效抑制了里程計的誤差累積。相比于基于濾波的方法，通常認(rèn)為基于優(yōu)化的方法在足夠的計算資源下可以實現(xiàn)更高的估計精度[10]，主要原因是基于優(yōu)化的方法在每次迭代優(yōu)化時會重新線性化，從而減少了視覺量測模型中高度非線性帶來的誤差。文獻(xiàn)[11]提出了基于關(guān)鍵幀的視覺慣性SLAM方案OKVIS，該方法采用了滑動窗口的策略，較好地平衡了算法的精度和效率。文獻(xiàn)[12]開源了一套穩(wěn)健的、高精度單目視覺慣性SLAM系統(tǒng)VINS-Mono，該算法能夠?qū)崿F(xiàn)系統(tǒng)快速初始化，并具備重定位和回環(huán)的能力。

相比于視覺SLAM，單次測距精度高、測距不受光照條件影響等特性使得基于激光雷達(dá)的SLAM方案具有更好的穩(wěn)健性。但激光SLAM在應(yīng)用過程中依然存在一些問題:①基于勻速運動假設(shè)的點云畸變矯正模型不夠精確；②非結(jié)構(gòu)化場景中激光SLAM性能退化顯著；③激光點云缺乏紋理信息導(dǎo)致激光SLAM閉環(huán)檢測能力弱；④點云配準(zhǔn)算法的精度與時間較大程度依賴初值等。為了提升激光SLAM算法的可靠性與穩(wěn)定性，研究者將慣性傳感器(inertial measurement unit,IMU)與激光雷達(dá)融合用于解決激光SLAM的性能退化問題。文獻(xiàn)[13—14]利用IMU量測去除激光點云在運動過程中產(chǎn)生的畸變，減少了算法中基于勻速運動假設(shè)模型帶來的誤差。文獻(xiàn)[15]提出了一種激光點云特征與IMU預(yù)積分量測聯(lián)合優(yōu)化的緊耦合激光雷達(dá)慣性系統(tǒng)，該算法有效提升了單一激光SLAM系統(tǒng)在高動態(tài)場景下的估計精度。文獻(xiàn)[16]提出了一種基于因子圖的緊耦合激光雷達(dá)慣性里程計框架，該方法提出了一種高效的基于局部滑動窗口的激光雷達(dá)幀到圖匹配方法，同時支持閉環(huán)檢測，進一步提升了SLAM系統(tǒng)的長時間導(dǎo)航性能。在閉環(huán)優(yōu)化方面，文獻(xiàn)[14，16]均采用了基于里程計幾何關(guān)系的閉環(huán)檢測方法，當(dāng)里程計累計誤差較大時閉環(huán)檢測性能將會退化。而視覺特征相比于激光點云具有豐富的紋理特性，可用于設(shè)計基于外觀的回環(huán)檢測算法從而不受里程計發(fā)散的影響。

為了進一步提升SLAM系統(tǒng)的定位建圖性能，基于視覺、慣性及激光雷達(dá)的多源融合SLAM方法也得到了廣泛研究[17]。文獻(xiàn)[18]基于松耦合的方式融合了IMU、單目相機及激光雷達(dá)3種傳感器，該方法利用視覺里程計的位姿估計結(jié)果為激光雷達(dá)幀到圖的匹配提供初始位姿，有效提升了點云匹配效率與位姿估計精度。文獻(xiàn)[19]采用了緊耦合的視覺慣性里程計為激光雷達(dá)幀到圖匹配提供更精確的先驗位姿信息，同時該算法具有回環(huán)檢測能力，能夠?qū)崿F(xiàn)全局位姿圖優(yōu)化。但松耦合的方式?jīng)]有充分利用原始觀測信息，無法實現(xiàn)更高的狀態(tài)估計精度。文獻(xiàn)[20]基于MSCKF框架實現(xiàn)了一種緊耦合視覺/慣性/激光雷達(dá)里程計，該算法相比于視覺慣性里程計以及激光雷達(dá)里程計位姿估計性能大幅提升，但不具備閉環(huán)優(yōu)化的能力。文獻(xiàn)[21]基于因子圖優(yōu)化實現(xiàn)了一套基于視覺慣性系統(tǒng)(visual-inertial system,VIS)與激光雷達(dá)慣性系統(tǒng)(LiDAR-inertial system,LIS)的緊耦合視覺/慣性/激光雷達(dá)SLAM框架，該算法通過關(guān)聯(lián)視覺特征與激光點云信息，有效提升了VIS的估計性能，并進一步提升了激光雷達(dá)幀到圖匹配的效率和精度，但該方法同時維護了VIS與LIS兩個獨立的估計器，沒有在估計器層面進行原始數(shù)據(jù)的聯(lián)合優(yōu)化。

綜上，本文提出一種基于圖優(yōu)化的緊耦合雙目視覺/慣性/激光雷達(dá)SLAM方法(S-VIL SLAM)。該方法在原始觀測值層面融合了IMU量測、視覺特征及激光點云特征，基于滑動窗口實現(xiàn)了多源數(shù)據(jù)的聯(lián)合非線性優(yōu)化?？紤]到基于里程計的回環(huán)檢測方法隨著里程計誤差累積，閉環(huán)檢測性能將出現(xiàn)退化，本文提出視覺增強的激光雷達(dá)閉環(huán)優(yōu)化算法，基于視覺特征進行基于外觀的閉環(huán)檢測，利用激光點云精配準(zhǔn)實現(xiàn)精確的全局位姿圖優(yōu)化，進一步修正SLAM系統(tǒng)的累積誤差。為了驗證本文算法性能，基于自主搭建的多傳感器硬件采集平臺在室外環(huán)境中進行了車載試驗，并對試驗結(jié)果進行了分析。

1 本文方法

1.1 緊耦合雙目視覺/慣性/激光雷達(dá)SLAM框架

圖1為本文提出的緊耦合雙目視覺/慣性/激光雷達(dá)SLAM算法框架。整體框架主要由3部分構(gòu)成，分別為：①模塊Ⅰ多傳感器數(shù)據(jù)預(yù)處理；②模塊Ⅱ基于滑動窗口的多傳感器數(shù)據(jù)聯(lián)合非線性優(yōu)化；③模塊Ⅲ視覺增強的激光雷達(dá)閉環(huán)優(yōu)化。模塊Ⅰ主要是對不同傳感器獲取的原始數(shù)據(jù)進行預(yù)處理。對于輸入的圖像數(shù)據(jù)，SLAM前端首先進行視覺特征的提取與跟蹤，然后根據(jù)相機的視差變化，進行關(guān)鍵幀的篩選。對于激光點云數(shù)據(jù)，首先提取原始激光點云中的幾何特征(角點和面點)，然后利用IMU預(yù)報的位姿對載體運動過程中可能導(dǎo)致的點云畸變進行去除。對于IMU量測數(shù)據(jù)，本文對兩個相鄰關(guān)鍵幀之間的IMU數(shù)據(jù)進行預(yù)積分，構(gòu)建新的預(yù)積分觀測值用于后端優(yōu)化。模塊Ⅱ首先對不同傳感器的觀測數(shù)據(jù)構(gòu)建殘差項，包括視覺重投影殘差、IMU預(yù)積分殘差、激光點云匹配殘差及邊緣化殘差。然后利用非線性最小二乘進行求解，本文使用了Ceres solver[22]優(yōu)化庫。為了兼顧精度和效率，本文后端優(yōu)化采用了滑動窗口的方式，對于即將滑出窗口的狀態(tài)進行邊緣化操作，構(gòu)建先驗項參與后續(xù)優(yōu)化。模塊Ⅲ主要負(fù)責(zé)閉環(huán)優(yōu)化，其中閉環(huán)檢測部分采用了基于詞袋模型的視覺回環(huán)檢測方法，通過比較圖像間的相似性，判斷是否發(fā)生閉環(huán)。一旦檢測到閉環(huán)，則進行時空一致性檢核，進一步確保閉環(huán)的準(zhǔn)確性。確認(rèn)發(fā)生閉環(huán)后，使用迭代最近點算法(iterative closest point,ICP)對發(fā)生閉環(huán)的兩幀激光點云進行精配準(zhǔn)，得到精確的閉環(huán)約束信息，進行全局位姿圖優(yōu)化。

圖1 緊耦合雙目視覺/慣性/激光雷達(dá)SLAM算法Fig.1 The full pipeline of the proposed tightly-coupled stereo visual-inertial-LiDAR SLAM

1.2 多傳感器數(shù)據(jù)預(yù)處理

本文采用文獻(xiàn)[12]的方法對視覺和IMU數(shù)據(jù)進行預(yù)處理，得到用于后端優(yōu)化的視覺特征點和IMU預(yù)積分觀測值，這里不展開敘述。對于關(guān)鍵幀的篩選，本文主要基于以下準(zhǔn)則。

(1) 基于連續(xù)跟蹤的特征點判斷當(dāng)前幀與上一幀的平均視差，如果平均視差大于設(shè)定的閾值，則判斷當(dāng)前幀為關(guān)鍵幀，否則為非關(guān)鍵幀。計算平均視差的公式為

(1)

式中，N表示連續(xù)跟蹤特征點的個數(shù)；k表示特征標(biāo)識ID；duk=ujk-uik；dvk=vjk-vik；u和v表示像素坐標(biāo)；i和j分別表示上一幀和當(dāng)前幀序號。

(2) 如果當(dāng)前幀連續(xù)跟蹤的特征點數(shù)量低于設(shè)定的閾值，則設(shè)定當(dāng)前幀為關(guān)鍵幀，這個準(zhǔn)則主要是為了保證跟蹤的質(zhì)量，防止出現(xiàn)完全跟蹤失敗的情況。

對于獲得的激光點云數(shù)據(jù)，首先進行特征提取[13]。對于每個激光點云計算曲率大小，然后根據(jù)曲率值排序提取幾何特征(角點和面點)。機械式激光雷達(dá)將掃描一圈的點云數(shù)據(jù)作為單幀激光數(shù)據(jù)，當(dāng)載體靜止的時候，激光雷達(dá)掃描得到的每一幀數(shù)據(jù)都是相對于同一個激光雷達(dá)坐標(biāo)系，而當(dāng)載體在激光雷達(dá)掃描期間發(fā)生運動，則同一幀的點云數(shù)據(jù)會對應(yīng)不同時刻的激光雷達(dá)坐標(biāo)系，這會導(dǎo)致點云數(shù)據(jù)相對于實際環(huán)境中的真實物體產(chǎn)生位置偏差，這種位置偏差在點云匹配過程中會導(dǎo)致匹配錯誤，從而影響SLAM算法的整體精度。為了消除或者最大程度減小運動畸變的影響，本文利用IMU預(yù)報的高頻位姿結(jié)果輔助激光雷達(dá)矯正點云畸變。如圖2所示，假設(shè)tk時刻采集到單幀激光數(shù)據(jù)Pk，Lpi=[XiYiZi]為點集Pk內(nèi)的一點，i∈[tk,tk+1]，則Lpi在tk+1時刻對應(yīng)的坐標(biāo)為

圖2 激光點云畸變矯正Fig.2 Illustration of the LiDAR scan deskewing

(2)

1.3 基于滑動窗口的緊耦合雙目視覺/慣性/激光雷達(dá)里程計

1.3.1 系統(tǒng)狀態(tài)定義

首先給出本文算法的狀態(tài)定義

(3)

(4)

(5)

(6)

1.3.2 殘差方程構(gòu)建

(1)視覺殘差。以滑動窗口內(nèi)被連續(xù)觀測的特征點fl為例，介紹視覺殘差項的構(gòu)建。假設(shè)特征點fl分別被第i幀和第j幀圖像觀測到，并且第i幀為滑動窗口內(nèi)首次觀測到該點的關(guān)鍵幀?；陔p目視覺觀測，介紹本文算法構(gòu)建視覺殘差所使用的3種視覺重投影方程

(7)

(8)

(10)

(11)

式中，Dp表示像素噪聲方差。

(2) IMU預(yù)積分殘差。IMU預(yù)積分殘差項定義為[12]

(12)

t∈[k,k+1]

(13)

(3) 激光點云匹配殘差。在本文算法中，維護了世界坐標(biāo)系下的局部點云地圖(Submap)，通過當(dāng)前激光雷達(dá)幀與Submap的點云匹配構(gòu)建殘差項。首先將特征點從LiDAR坐標(biāo)系投影到世界坐標(biāo)系下，可以表示為

(14)

(15)

式中，分子的幾何含義是如圖3(a)所示虛線平行四邊形的面積，分母表示jk兩點的距離，所以由平行四邊形面積公式就可以得到角點i到直線jk的距離。對于面點，構(gòu)建點到面的匹配殘差。如圖3(b)所示,點到面的距離可以表示為

圖3 激光點云匹配殘差Fig.3 Illustration of the laser point cloud matching residuals

(16)

式中，wpj、wpk、wpm為與wpi最鄰近的不共線3點。dplanar分子表示的是四面體的體積，分母表示的是平面jmk的面積，所以點到面的距離可以通過四面體體積公式求得。激光點云的匹配殘差

1.3.3 滑動窗口模型

本文滑動窗口模型如圖4所示，窗口內(nèi)X0到X10表示載體的待估狀態(tài)。對于視覺、慣性及激光雷達(dá)3種不同觀測，本文采用“高頻+低頻”的量測更新方式，其中視覺量測進行高頻更新(10 Hz)，可以用于維護一定精度的連續(xù)狀態(tài)估計，激光雷達(dá)量測采用低頻更新(2 Hz)，利用幀到圖的匹配實現(xiàn)更高的估計精度，抑制里程計的發(fā)散。IMU數(shù)據(jù)除了用于構(gòu)建關(guān)鍵幀之間的預(yù)積分觀測值外，也用來進行全局狀態(tài)傳遞，為激光點云幀到圖匹配提供較好的初值。本文算法維護了世界坐標(biāo)系下的全局地圖，主要由激光點云特征組成，滑窗內(nèi)的子圖在每次進行幀到圖匹配前由全局地圖臨時生成，它可以看作是以載體為中心的隨著載體運動的3D體素，大小可以根據(jù)應(yīng)用場景的范圍進行調(diào)整。

圖4 滑動窗口模型Fig.4 The sliding window model

本文滑動窗口大小設(shè)置為10[26]，當(dāng)窗口滿時，需要對窗口內(nèi)的狀態(tài)節(jié)點進行滑動操作。如果最新節(jié)點為關(guān)鍵幀，則窗口滑出最老節(jié)點，否則，窗口滑出次新節(jié)點。為了維護全局地圖，當(dāng)被滑出的節(jié)點包含激光點云時，則將激光點云投影到世界坐標(biāo)系下，并補充到全局地圖中。最老節(jié)點的狀態(tài)經(jīng)過多次迭代優(yōu)化已經(jīng)達(dá)到了局部最優(yōu)并在滑出窗口后被固定，用窗口內(nèi)最老節(jié)點的位姿將點云投影到世界坐標(biāo)系下是可靠的。在滑窗過程中如果直接丟棄掉最老節(jié)點相關(guān)的狀態(tài)和觀測量會導(dǎo)致滑窗中原有的狀態(tài)節(jié)點間約束丟失，從而降低估計器的精度，在本文基于滑動窗口的優(yōu)化算法中，對滑窗中的最老節(jié)點進行了邊緣化處理[26]，主要目的是在減少算法計算復(fù)雜度的同時保留該節(jié)點對窗口內(nèi)其他節(jié)點的約束關(guān)系，將這種約束關(guān)系以先驗項的形式參與后續(xù)滑窗的優(yōu)化。

1.4 視覺增強的激光雷達(dá)閉環(huán)優(yōu)化

隨著載體運動距離的增加，里程計不可避免地會發(fā)生漂移，而載體在運動過程中，軌跡中經(jīng)常會包含閉環(huán)，有效的閉環(huán)信息可以幫助SLAM系統(tǒng)建立當(dāng)前時刻和先前時刻的數(shù)據(jù)關(guān)聯(lián)，進而修正里程計的累積誤差，提高SLAM系統(tǒng)估計器在長時間下的正確性和穩(wěn)定性。本文算法通過里程計輸出的關(guān)鍵幀信息構(gòu)建全局位姿圖，利用軌跡中的閉環(huán)信息和關(guān)鍵幀之間的相對約束關(guān)系對整體軌跡進行優(yōu)化，用于得到更優(yōu)的定位和建圖效果。本文設(shè)計了一種視覺增強的激光雷達(dá)閉環(huán)優(yōu)化算法，即充分考慮視覺信息和激光點云的互補特性，利用視覺圖像豐富的紋理信息進行高效的閉環(huán)檢測，當(dāng)檢測出閉環(huán)信息后，對當(dāng)前關(guān)鍵幀與閉環(huán)幀進行基于ICP的點云精配準(zhǔn)，得到精確的相對位姿關(guān)系，然后用于位姿圖優(yōu)化，具體流程如圖1所示。

本文使用經(jīng)典的DBoW算法[27]來實現(xiàn)閉環(huán)檢測。對于輸入的每個關(guān)鍵幀圖像提取至少500個新的角點并計算BRIEF[28]描述子，然后利用已經(jīng)訓(xùn)練好的字典查詢得到當(dāng)前關(guān)鍵幀的向量描述，對于每幅圖像，只將其向量描述存入數(shù)據(jù)庫中用于閉環(huán)檢索。對于當(dāng)前關(guān)鍵幀，DBoW算法會對比其和數(shù)據(jù)庫中關(guān)鍵幀的相似性，如果檢測到閉環(huán)幀，則返回閉環(huán)幀的序列號，稱其為候選幀，但為了得到準(zhǔn)確的閉環(huán)幀，還需要進一步對閉環(huán)的兩幀做驗證。通常載體回到先前經(jīng)過的地方會發(fā)生時間上連續(xù)的閉環(huán)，相比于單次檢測到閉環(huán)，連續(xù)時間檢測到的閉環(huán)被認(rèn)為是更正確的閉環(huán)情況，除了上述時間一致性的檢測，還需要進行空間一致性檢測，本文利用發(fā)生閉環(huán)的兩幀相匹配的視覺特征，通過PnP[29]算法得到它們之間的位姿關(guān)系，并和里程計給出的相對位姿關(guān)系進行比較，進一步確定是否發(fā)生閉環(huán)。圖5為兩組大尺度場景下的視覺閉環(huán)檢測圖像相似度評分序列圖。當(dāng)后端每次收到新的關(guān)鍵幀，首先會在已有的數(shù)據(jù)庫內(nèi)檢索，然后返回評分最高的3幅圖像ID和對應(yīng)的相似度評分，得分越高表明兩幅圖像的相似度越高。圖5中出現(xiàn)明顯峰值的圖像序列與實際軌跡中發(fā)生閉環(huán)的區(qū)域一致。在設(shè)置閉環(huán)閾值的時候往往追求更高的準(zhǔn)確率，防止錯誤的閉環(huán)約束對系統(tǒng)造成嚴(yán)重的影響。在實際應(yīng)用過程中會先對應(yīng)用場景進行測試分析，然后設(shè)置較為合理的閾值。

圖5 圖像相似度評分Fig.5 Image similarity score

在視覺/慣性/激光雷達(dá)里程計算法中，由于慣性量測可以提供重力向量的方向，所以在世界坐標(biāo)系下，可以確定載體相對于水平面的絕對變化，即俯仰角和橫滾角是完全可觀的，在整個狀態(tài)估計期間能夠?qū)崿F(xiàn)較好的估計，而航向角和X、Y、Z4個自由度是相對估計，累積誤差也主要集中在這4個方向。所以本文在全局位姿圖優(yōu)化過程中對里程計得到的俯仰角和橫滾角進行固定，只對航向角、X、Y、Z4個自由度進行優(yōu)化。

圖6所示的位姿圖中，每一個關(guān)鍵幀都代表一個節(jié)點，不同的節(jié)點之間通過邊連接。位姿圖中共包含兩種類型的邊，一種邊稱為序列邊，表示不同關(guān)鍵幀之間的約束關(guān)系，由里程計估計的位姿得到，其相對關(guān)系可以表達(dá)為

圖6 全局位姿圖優(yōu)化Fig.6 Diagram of the global pose graph optimization

(17)

四自由度的位姿圖優(yōu)化殘差項可以表達(dá)為“計算值減去觀測值”

(18)

最終位姿圖優(yōu)化的目標(biāo)函數(shù)可以表達(dá)為

(19)

通過最小化序列邊和閉環(huán)邊的殘差，實現(xiàn)對參數(shù)的最優(yōu)估計。本文對于閉環(huán)邊使用了穩(wěn)健核函數(shù)，防止錯誤的閉環(huán)邊對優(yōu)化造成破壞。對于序列邊的殘差項沒有使用核函數(shù)，因為觀測值來自里程計本身，已經(jīng)經(jīng)過了粗差檢核，不會存在明顯異常結(jié)果。

2 車載試驗與結(jié)果分析

2.1 試驗描述

本文算法基于武漢大學(xué)測繪學(xué)院GREAT軟件[30]實現(xiàn)，為了驗證本文算法性能，利用自主搭建的集成多種傳感器的硬件采集平臺，在室外環(huán)境下進行了車載試驗。多傳感器集成硬件采集平臺如圖7所示，平臺搭載了多頻多系統(tǒng)GNSS接收機、測量型GNSS天線、MEMS-IMU、戰(zhàn)術(shù)級慣導(dǎo)、雙目相機及十六線激光雷達(dá)。設(shè)備的具體參數(shù)見表1。

圖7 多傳感器集成硬件采集平臺Fig.7 Multi-sensor integrated data acquisition platform

表1 傳感器設(shè)備型號與信息Tab.1 Sensor models and information

數(shù)據(jù)采集平臺采用硬件同步的方式將不同傳感器的時間統(tǒng)一到GPS時間系統(tǒng)下。該設(shè)備利用GNSS/INS組合導(dǎo)航板卡[31]發(fā)出的高頻PPS脈沖信號(10～20 Hz)來觸發(fā)相機和激光雷達(dá)工作，保證多源數(shù)據(jù)的采集時間同步。在完成多傳感器時間同步以后，需要進一步確定不同傳感器之間的空間關(guān)系，對于相機和IMU之間的外參，本文使用ETH ASL實驗室開源的Kalibr工具箱[32]對IMU和相機進行聯(lián)合標(biāo)定。對于LiDAR和相機之間外參數(shù)，本文使用Autoware工具箱[33]進行標(biāo)定，LiDAR和IMU的空間關(guān)系可以通過相機和IMU的外參進一步轉(zhuǎn)換得到。本文使用戰(zhàn)術(shù)級慣導(dǎo)和多頻多系統(tǒng)GNSS RTK緊耦合雙向平滑結(jié)果作為參考真值，使用MEMS-IMU數(shù)據(jù)進行融合算法驗證。

本文在武漢大學(xué)校園內(nèi)進行了兩組車載測試，試驗軌跡如圖8所示。試驗過程中，車輛進行了靜止、加減速、轉(zhuǎn)彎等豐富的機動，其運動速度、姿態(tài)及高程變化如圖9所示。測試1全長2090 m，軌跡中包含狹長林蔭道，行駛道路兩旁有高大的樹木，測試路段存在明顯的高程起伏變化。測試2整段軌跡全長約2760 m，在車輛行駛期間，有光線明暗的變化，部分區(qū)域還存在過度曝光，測試2場景相比于測試1更具挑戰(zhàn)性，包含了多個連續(xù)上下坡的路段，高程起伏變化最大超過10 m。

圖8 車載試驗軌跡Fig.8 The test trajectories of vehicle-borne experiment

圖9 車輛運動狀態(tài)時間序列Fig.9 Vehicle motion during the vehicle-borne experiment

2.2 試驗結(jié)果與分析

利用本文S-VIL SLAM算法對上述復(fù)雜場景中采集的多傳感器數(shù)據(jù)進行離線解算，并與緊耦合雙目視覺慣性里程計(S-VIO)[34]、激光雷達(dá)里程計(LOAM)[13]及開源緊耦合激光雷達(dá)/視覺/慣性SLAM算法LVI-SAM[21]作了對比。本文主要從絕對軌跡誤差(absolute trajectory error,ATE)、相對位姿誤差(relative pose error,RPE)及建圖一致性3個方面進行分析和評估。

本文算法結(jié)果與參考真值有著不同的參考坐標(biāo)系，在進行絕對軌跡誤差分析之前，需要利用一段軌跡數(shù)據(jù)對估計軌跡和真值軌跡進行對齊。本文采用文獻(xiàn)[35]的方法，求得兩段軌跡的轉(zhuǎn)換關(guān)系(旋轉(zhuǎn)和平移)，并將該轉(zhuǎn)換參數(shù)用于估計結(jié)果的整段軌跡，將其轉(zhuǎn)到真值軌跡坐標(biāo)系下，然后求解估計軌跡相對于真值軌跡的偏差。圖10為不同方法的平面軌跡對比圖。其中紅色五角星代表軌跡的起點，三角形代表軌跡的終點，“w/o loop”表示里程計結(jié)果，“w/loop”表示施加閉環(huán)優(yōu)化的結(jié)果?？梢园l(fā)現(xiàn)在兩組試驗中，LOAM與S-VIO的軌跡與Ground Truth都發(fā)生明顯的分離；而S-VIL(w/o loop)與LVI-SAM(w/o loop)軌跡在大部分區(qū)域都能夠與Ground Truth吻合得較好，隨著行駛距離的增加，在軌跡后半段逐漸出現(xiàn)航向偏差；而經(jīng)過全局閉環(huán)優(yōu)化的S-VIL(w/loop)與LVI-SAM(w/loop)軌跡幾乎與Ground Truth重合。

圖10 絕對軌跡對比Fig.10 Absolute trajectory comparison

進一步對上述結(jié)果進行量化分析，統(tǒng)計結(jié)果見表2。本文方法S-VIL(w/o loop)估計結(jié)果相比于S-VIO三維位置精度提升66.2%～78.5%，姿態(tài)精度提升16.2%～48.9%；相比于LOAM三維位置精度提升64.7%～65.2%，姿態(tài)精度提升26.9%～36.7%。此外，對比S-VIO與LOAM方法在X、Y、Z3個方向上的估計結(jié)果，可以看出兩組測試中，激光雷達(dá)里程計相比于視覺慣性里程計平面估計精度較高，但高程估計較差，而本文基于原始觀測值的融合方法則實現(xiàn)了多源信息的優(yōu)勢互補，在X、Y、Z3個方向的估計性能都實現(xiàn)了提升。在第1組試驗中，S-VIL(w/o loop)與LVI-SAM(w/o loop)實現(xiàn)了相當(dāng)?shù)钠矫婀烙嬀群秃较蚪枪烙嬀龋疚姆椒ǖ母叱坦烙嬓阅軆?yōu)于LVI-SAM。第2組試驗中，隨著試驗場景更加挑戰(zhàn)，尤其是存在明顯的高程起伏變化，LVI-SAM里程計出現(xiàn)了較為明顯的退化，Z方向誤差RMS為25.50 m，而本文方法僅為2.71 m。

表2 絕對軌跡誤差RMS統(tǒng)計表Tab.2 RMS of absolute trajectory error

本文提出的閉環(huán)優(yōu)化算法在大尺度試驗場景下有效探測出了軌跡中的閉環(huán)信息，并利用軌跡閉環(huán)信息實現(xiàn)了全局位姿圖優(yōu)化。對比最終位姿圖優(yōu)化結(jié)果，本文算法在測試1中，X、Y、Z3個方向的誤差RMS分別為0.49、1.59、0.69 m，航向誤差RMS為0.23°；測試2中X、Y、Z3個方向的誤差RMS分別為1.12、1.13、0.56 m，航向誤差RMS為0.7°。與LVI-SAM(w/loop)相比，兩種方法實現(xiàn)了較為相當(dāng)?shù)钠矫婀烙嬀群秃较蚬烙嬀?，但本文方法高程估計精度更高，在兩組測試中均實現(xiàn)了分米級精度，而LVI-SAM算法的高程估計精度為2～3 m。

本文采用文獻(xiàn)[36]的方法對SLAM算法的相對位姿精度進行評估。相對位姿誤差可以表達(dá)為

(20)

式中，Qi表示參考真值的位姿；Pi表示估計結(jié)果的位姿；Δ表示軌跡中固定的距離間隔。相對位姿誤差統(tǒng)計結(jié)果如圖11、圖12所示。統(tǒng)計結(jié)果表明，S-VIL(w/o loop)相比于S-VIO和LOAM方法相對位姿估計性能顯著提升，其中航向估計提升最為明顯。隨著統(tǒng)計結(jié)果的間隔距離增加，本文方法的相對誤差發(fā)散更加緩慢。與LVI-SAM(w/o loop)相比，本文算法在兩組試驗中表現(xiàn)出更加穩(wěn)定的相對位姿估計性能，相對平移估計也更優(yōu)。兩組試驗中經(jīng)過閉環(huán)優(yōu)化的結(jié)果均實現(xiàn)了最高的相對位姿估計精度。

圖11 測試1相對位姿誤差Fig.11 The relative pose errors in test 1

通過上文的對比分析，本文算法在兩組試驗中均實現(xiàn)了最高的絕對位姿估計精度，本文利用全局位姿圖優(yōu)化結(jié)果S-VIL(w/ loop)將LiDAR特征點云投影到世界坐標(biāo)系下用于定性分析本文SLAM算法建圖效果，用視覺/慣性/激光雷達(dá)里程計S-VIL(w/o loop)投影得到的點云地圖作為對照。圖13為第1組試驗的全局地圖，車輛行駛過程中3次經(jīng)過白色方框所在路段，道路中間綠化帶能夠幫助驗證前后點云地圖的一致性，可以看出閉環(huán)優(yōu)化后的點云地圖綠化帶無明顯的重影，分辨率更高，而通過里程計結(jié)果投影得到的點云地圖有明顯的錯位現(xiàn)象。圖14為第2組試驗的全局地圖，通過放大閉環(huán)區(qū)域的點云地圖可以看出，圖14(b)里程計投影得到的全局地圖出現(xiàn)了明顯的位移偏差，而通過位姿圖優(yōu)化結(jié)果得到的點云地圖具有更好的前后一致性，全局地圖的分辨率也更高。在測試1與測試2試驗場景下，本文視覺增強的激光雷達(dá)閉環(huán)優(yōu)化算法可以有效探測出了軌跡中存在的閉環(huán)，并基于激光點云的ICP精配準(zhǔn)實現(xiàn)了高精度的位姿圖優(yōu)化結(jié)果，進一步提升了SLAM算法的整體位姿估計結(jié)果，構(gòu)建了具有良好全局一致性的點云地圖。

圖13 測試1全局點云地圖Fig.13 The global point cloud map in test 1

圖14 測試2全局點云地圖Fig.14 The global point cloud map in test 2

3 結(jié) 論

本文提出了一種基于圖優(yōu)化的緊耦合雙目視覺/慣性/激光雷達(dá)SLAM方法，通過室外大尺度場景下的車載試驗驗證了本文算法的正確性和有效性。試驗結(jié)果表明：本文緊耦合視覺/慣性/激光雷達(dá)里程計相比于緊耦合視覺慣性里程計和激光雷達(dá)里程計位姿估計精度均實現(xiàn)了顯著提升，視覺增強的激光雷達(dá)閉環(huán)優(yōu)化算法可以有效探測出大尺度場景下的軌跡閉環(huán)信息，在實現(xiàn)全局高精度位姿估計的同時能夠提供具有良好全局一致性和高分辨率的點云地圖。本文算法能夠為移動載體提供高精度的局部導(dǎo)航定位，幫助移動載體在室外GNSS信號不佳或者失鎖場景下獲得連續(xù)高精度導(dǎo)航定位能力。未來工作中，將針對多源融合SLAM算法中存在的傳感器退化問題展開研究，進一步提升多源融合SLAM算法在復(fù)雜環(huán)境下的穩(wěn)健性。

致謝：本文的數(shù)值計算得到了武漢大學(xué)超級計算中心的計算支持和幫助。