蔡英鳳，陸子恒，李祎承，陳 龍，王 海

（1.江蘇大學汽車工程研究院，鎮(zhèn)江 212013；2.江蘇大學汽車與交通工程學院，鎮(zhèn)江 212013）

前言

同時建圖與定位（simultaneous localization and mapping，SLAM）在自動駕駛車輛中用于在未知環(huán)境下進行高精地圖構建及定位結果的輸出，為自動駕駛汽車的其他功能，例如導航規(guī)劃，感知等模塊提供精確的自車姿態(tài)和軌跡信息，保障其他模塊功能的完整性。近年來，隨著自動駕駛技術的快速發(fā)展，SLAM課題受到了越來越廣泛的研究。

根據采用傳感器方案的不同，SLAM技術有單一傳感器和多傳感融合等方案。對于單一傳感器，主要有攝像頭和激光雷達兩類環(huán)境感知策略。視覺SLAM的典型方案有ORB-SLAM等，攝像頭成本較低，易于安裝，常用于無人機或低成本自動駕駛車上，但是單目相機視場角小，對光照條件敏感，無法準確獲取環(huán)境深度信息和尺度信息，在高速運動場景下容易出現圖像模糊導致信息丟失，因此視覺方案通常用于小尺度場景的建圖與定位。

激光雷達不會受到環(huán)境光照的影響，能夠準確、動態(tài)地測量環(huán)境深度信息，而不用考慮額外計算所引入的誤差。其在水平方向擁有360°的視場角，能夠探測最遠200 m內的環(huán)境信息。近些年隨著激光雷達成本逐漸降低，越來越多的自動駕駛汽車平臺選擇將激光雷達作為系統(tǒng)的主要感知部件之一。但是激光雷達在高速運動過程中會產生畸變，對隧道、高速等具有重復特征的場景適應性較差。LOAM采用基于曲率的激光雷達點云提取特征方案提升了系統(tǒng)的精度和魯棒性，但是在高速等缺乏特征的場景下容易發(fā)散失效。

融合方案通常采用視覺-慣性測量單元（inertial measurement unit，IMU）或激光雷達-IMU的方案?；趹T性傳感器的緊耦合方案通常有兩種，一類是基于EKF（extended Kalman filter）或ESKF（error state Kalman filter）的 濾 波 方 案，比 如MSCKF、FAST-LIO等，另一類是基于優(yōu)化的方案，代表算法有VINS-Mono和LIO-SAM等。上述方案中IMU可用于矯正激光雷達點云運動畸變，為幀間匹配提供先驗運動估計。與IMU融合的方案使系統(tǒng)能夠應對短時間內的環(huán)境信息缺失問題，從而提升系統(tǒng)在長時間運行下的魯棒性和精度。

在慣性融合方案的基礎上，使用相機-激光雷達-IMU的多模態(tài)傳感器融合建圖與定位方案逐步發(fā)展，展示出更高的精度和魯棒性。LIMO中采用激光雷達點云投影輔助恢復視覺特征點的深度信息，解決了深度信息估計精度和穩(wěn)定性較低的問題。VIL-SLAM中在視覺檢測到回環(huán)信息并得到匹配結果后，利用點云匹配進一步優(yōu)化計算結果，以解決基于PnP（perspective-n-point）計算的視覺匹配方案精度低的問題。LVI-SAM采用因子圖優(yōu)化的融合方案，設計了激光雷達慣性里程計和視覺慣性里程計兩個子系統(tǒng)，在子系統(tǒng)中完成與IMU的融合后再輸出優(yōu)化的結果。

但是上述方案仍存在一定的問題。歸納如下：（1）利用原始點云投影到像素平面后再進行篩選和平面擬合，會導致在特征點密集的區(qū)域出現相同的點云塊被多次進行提取和擬合，降低了系統(tǒng)的運行效率；（2）多個子系統(tǒng)獨立優(yōu)化重復利用了IMU的信息，導致IMU偏置在不同子系統(tǒng)中吸收了不同的系統(tǒng)殘差，破壞了數據的一致性，影響了系統(tǒng)的穩(wěn)定性；（3）最后由于視覺感知范圍較小，單視覺回環(huán)檢測方案在路口或者寬車道行駛環(huán)境下無法正確檢測回環(huán)信息。

為了解決上述問題，充分利用多模態(tài)傳感器各自的優(yōu)勢，本文中提出了一個基于視覺-激光雷達-IMU緊耦合的SLAM系統(tǒng)，實現了一個基于非線性滑窗優(yōu)化的狀態(tài)估計方案。本文主要的創(chuàng)新工作在于：（1）采用加強平面特征點空間約束的特征提取方案對點云進行特征提取，在降低點云總數的同時提升可用于恢復視覺特征點深度信息的點云占比，進一步提升系統(tǒng)的實時性；（2）采用將激光雷達里程計直接作為幀間相對約束添加到視覺慣性框架中的聯(lián)合優(yōu)化方案，將3種約束構建在一個非線性方程中進行聯(lián)合優(yōu)化，減少系統(tǒng)的復雜程度；（3）設計了高頻視覺回環(huán)加低頻激光回環(huán)組成的視覺-激光雷達耦合的回環(huán)檢測框架，可以在降低系統(tǒng)資源消耗的情況下實現由粗到精的回環(huán)檢測與位姿匹配，進一步減少系統(tǒng)在大場景下長時間工作導致的累計誤差。本文中提出的方法在公開數據集上進行了測試，驗證了系統(tǒng)在各類場景下具有較好的魯棒性和精度，同時也在自主搭建的車載平臺上進行了實車測試，如圖1所示，證明所提方法同樣能夠勝任大場景實際環(huán)境的狀態(tài)估計任務。

圖1 本文方法重建的國內某高校校園環(huán)境

1 多傳感器融合SLAM系統(tǒng)設計

1.1 系統(tǒng)概覽

本文中提出的多傳感器融合SLAM系統(tǒng)主要由以下幾部分組成：（1）前端數據預處理部分，包括特征點提取，基于稀疏金字塔的光流追蹤，以及特征點深度估計，激光雷達點云特征提取，幀-局部地圖的里程計計算以及利用點云特征優(yōu)化視覺特征點深度信息；（2）后端狀態(tài)估計部分，利用前端提供的各類觀測數據構建相應的約束，如通過視覺特征點構建的視覺重投影誤差約束，IMU構建的預積分約束以及激光雷達里程計提供的關鍵幀之間相對姿態(tài)與先驗位置約束；（3）基于視覺-激光雷達耦合的回環(huán)檢測部分。系統(tǒng)的整體架構圖如圖2所示。

圖2 系統(tǒng)架構與數據處理流程圖

1.2 前端處理

本節(jié)將會介紹對視覺、激光雷達的測量數據進行預處理的步驟。對于視覺圖像，會進行視覺特征點的提取、跟蹤與深度信息恢復；對于激光雷達點云，會進行特征提取與里程計的構建。前端數據預處理流程圖如圖3所示。

圖3 前端數據預處理流程圖

1.2.1 視覺特征處理

在視覺前端接收到新的關鍵幀圖像后，首先會采用稀疏金字塔光流對上一幀圖像中提取的特征點進行跟蹤，并采用RANSAC（隨機采樣一致）篩選特征點用來計算基礎矩陣，從而判定每個特征點的跟蹤質量，保留跟蹤質量較高的特征點，不滿足基礎矩陣的點或已經丟失的特征點將會被剔除。追蹤到的特征點的追蹤次數會進行更新。維護的特征點會在后端中被用于構建視覺重投影約束，從而約束狀態(tài)量之間的相對姿態(tài)。為了保證維護的特征點數量滿足要求，還會基于跟蹤點數提取一定數量的新特征點。視覺前端根據特征點歷史深度信息和光流追蹤結果，通過PnP算法得到當前關鍵幀的姿態(tài)，姿態(tài)數據會與激光雷達里程計結果進行一致性判定，判定當前系統(tǒng)是否發(fā)生退化。對于觀測次數等于2、還沒有恢復深度信息的特征點，在利用激光雷達點云特征進行深度信息優(yōu)化前，需要通過當前幀的姿態(tài)進行特征點三角化恢復深度信息。

1.2.2 激光雷達里程計

本節(jié)在激光雷達點云中提取結構特征信息用于激光雷達里程計的計算。在系統(tǒng)正常運行后，以第1幀激光雷達坐標系為基準構建一個局部地圖坐標系w^，使第1幀激光雷達坐標系與w^系的轉換關系為單位矩陣，將w^稱為激光雷達里程計坐標系，后續(xù)的點云幀的最新姿態(tài)將會在w^系下進行表示。激光雷達點云特征提取的方式繼承自LOAM系列，基于每個點在其所屬環(huán)的曲率信息在一幀點云中提取平面點p和邊緣點p。曲率小于閾值的平坦點被視為平面點，曲率大于閾值的點被視為邊緣點。與LOAM不同的是，為了后續(xù)方便對視覺特征點進行深度信息優(yōu)化，平面特征的提取會考慮相鄰環(huán)之間的曲率信息，而不只考慮特征點所屬環(huán)的信息。若當前特征點相鄰環(huán)的鄰近點不屬于平面特征，當前特征點也不會被視為平面點。平面特征提取部分的改進有利于后續(xù)步驟中視覺特征深度信息的優(yōu)化。

圖4 激光雷達里程計計算流程圖

1.2.3 深度信息優(yōu)化

在單目視覺里程計中，由于單攝像頭不具有準確測量深度信息的能力，因此只能對特征點深度進行有尺度的估計，若對特征點的觀測次數較少或視差不足，則深度估計會帶有很大的噪聲。在多模態(tài)傳感器融合框架下，可以利用激光雷達點云優(yōu)化視覺特征點的深度信息，從而提升視覺慣性里程計的魯棒性和精度。本文中提出的方法與激光雷達里程計環(huán)節(jié)相耦合，無須對點云采取額外的特征提取或擬合操作，因此可以減少系統(tǒng)資源的占用，在保證系統(tǒng)實時性的基礎上優(yōu)化視覺特征點的深度信息。

在激光雷達里程計環(huán)節(jié)中（1.2.2節(jié)），激光雷達點云會進行特征提取，用于與局部地圖匹配輸出里程計信息。在點云特征的篩選中，本文中優(yōu)化了對平面特征點云的篩選，從而保證這部分點云具有足夠強的平面特性。與投影完整點云再進行篩選與擬合的方案相比，本文中提出的方法可以避免位于視覺特征點密集區(qū)處的點云塊被多次提取擬合。此外，具有強平面假設的點云塊可以進一步減少后續(xù)流程中對點云進行平面擬合的要求，從而提升系統(tǒng)的實時性。通過激光雷達點云對視覺特征點深度信息優(yōu)化的流程圖如圖5所示。

圖5 視覺特征點深度信息優(yōu)化的流程

在獲取最新的面特征點云p后，通過激光雷達與攝像頭的外參以及攝像頭的內參將p投影到像素坐標系下，僅保留落在像素平面內的點云p，為了便于檢索視覺特征點附近的激光點，將視覺特征點聯(lián)合p構建Kd-Tree（K-dimensional tree）。遍歷視覺特征點，對于滿足深度信息優(yōu)化條件的特征點，利用Kd-Tree尋找其最近的n（n＞3）個激光點，將這些點返回到三維坐標系下進行平面擬合。由于此處的點云塊具有強平面假設，因此可以簡化平面擬合的步驟。在這n個激光點中隨機抽取3個點構建一個最簡易的平面，可以得到平面的法向量和平面的中心點坐標，剩余點與平面中心點構建向量并與平面法向量計算向量點積，驗證該點是否共同構成平面。若三點共線或者向量點積誤差的和大于閾值，則需要重新篩選3個點，并重復上述步驟。若找尋到滿足閾值條件的平面后，平面中心點的深度信息可以提供給視覺特征點，與三角化得到的深度信息進行對比，若兩者誤差小于一定閾值，則認為深度信息優(yōu)化結束，并將優(yōu)化后的深度賦給該特征點。需要具體說明的是，為了降低系統(tǒng)的計算復雜度，對于觀測次數大于閾值的特征點，由于已經在后端優(yōu)化框架中多次迭代優(yōu)化，特征點深度信息已經達到比較高的精度，因此無須采用點云進行深度信息的優(yōu)化。

1.2.4 尺度信息恢復

由于視覺里程計對尺度信息不可觀，因此需要和IMU配合完成尺度的初值計算，而尺度恢復對運動激勵要求很高，勻速運動或快速的純旋轉等退化的運動形式都會導致初始化失敗。因此本文中提出了利用激光雷達里程計優(yōu)化尺度信息恢復的方法。首先在利用已知深度信息的特征點完成視覺姿態(tài)計算后，接收相應的激光雷達里程計結果，使用關鍵幀之間的激光雷達相對變化量以勻速模型計算載體在關鍵幀之間的相對運動速度v^和相對位移δp，并將v^和δp作為相對約束加入到后端狀態(tài)估計優(yōu)化中，用于限制由視覺求解出來的速度和位置狀態(tài)量。同時δp會與視覺里程計求解出來姿態(tài)進行對齊，從而恢復尺度信息，計算方式如下：

1.3 后端狀態(tài)估計

完成前端數據處理后，后端狀態(tài)估計模塊會接收前端數據并進行優(yōu)化求解。本文中優(yōu)化了后端的約束構建方式，在視覺慣性里程計的基礎上直接添加激光雷達里程計結果作為幀間約束來提升系統(tǒng)的魯棒性和精度，而非獨立處理視覺慣性里程計與激光慣性里程計兩個模塊后再對其結果進行位姿圖優(yōu)化，從而節(jié)省系統(tǒng)的計算資源。

后端狀態(tài)估計模塊采用了緊耦合的滑窗優(yōu)化算法，聯(lián)合優(yōu)化來自IMU、激光雷達與視覺的觀測約束。在后端中維護的狀態(tài)量如下：

式中x表示IMU在滑窗內下的第k個狀態(tài)向量，包含IMU坐標系即b系在世界坐標系w下的位置、速度、姿態(tài)以及IMU本身的偏置誤差b和b。同時后端還會同步優(yōu)化在視覺特征點的深度信息，此處的深度信息指的是此特征點在首次觀測到該特征點的圖像的相機坐標系下的深度距離。本文中采用視覺特征點深度距離的倒數，即逆深度對特征點信息進行參數化，λ即表示在滑窗內檢測到第j個視覺特征點的逆深度。m為滑窗內所有待優(yōu)化的特征點個數。圖6為后端狀態(tài)估計模型中各種傳感器觀測所構建的相互約束因子圖。傳感器觀測一共提供了3種類別的約束因子，分別是IMU預積分因子、視覺重投影約束因子和激光雷達里程計因子。它們分別對滑窗中待優(yōu)化的狀態(tài)量，即速度、位置、姿態(tài)和IMU偏置等進行約束。每種不同的因子可以視為相應的傳感器在系統(tǒng)中發(fā)揮的作用。

圖6 后端狀態(tài)估計約束因子圖

圖中T和M分別表示位置與姿態(tài)和速度與IMU自身的偏置信息，為系統(tǒng)中待優(yōu)化的狀態(tài)量，每一個關鍵幀擁有一組對應的狀態(tài)量信息。在相鄰關鍵幀之間，IMU觀測通過預積分操作構建獨立的預積分因子，約束了速度、位置、姿態(tài)和偏置狀態(tài)量（1.3.1節(jié)）。激光雷達里程計為關鍵幀提供了相對位姿的約束信息（1.3.3節(jié)）。同時，對于滑窗內觀測到的視覺特征點，若其已經完成了深度信息的恢復，那么觀測到該特征點的其他圖像，可以與首次觀測到該特征點的圖像構建視覺重投影約束，從而對該幀的位置和姿態(tài)構建相應的約束（1.3.2節(jié)）。

通過上述公式可以實現IMU位置、速度和姿態(tài)的更新，即由k時刻下的IMU相對于世界系的狀態(tài)量得到k+1時刻下狀態(tài)量的估計值。

為節(jié)約系統(tǒng)的計算資源，避免在優(yōu)化流程中對相關鍵幀內的IMU數據多次積分，本文中對IMU傳感器采用了預積分的處理方式，即單獨將相鄰關鍵幀內的所有IMU數據預先積分，得到一組單獨的約束條件，在后續(xù)的優(yōu)化流程中，這組約束條件僅和IMU的加速度和角速度偏置有關，從而與相關狀態(tài)量實現了解耦。將式（6）中的積分項進行替換，即可得到位置、速度和姿態(tài)的預積分量：

在完成IMU預積分操作后，可以構建相鄰狀態(tài)量之間的約束關系，用于后端的狀態(tài)估計環(huán)節(jié)。

1.3.2 視覺因子

視覺約束由重投影約束構成。狀態(tài)估計后端接收來自前端預處理的數據，在接收到最新幀的圖像后，預處理部分會對圖像進行光流跟蹤，并保留跟蹤質量較高特征點。隨后，跟蹤次數大于閾值的點會使用三角化進行深度估計，并根據激光雷達數據對深度值進行進一步優(yōu)化。在完成上述操作后，帶有深度值的特征點會在后端狀態(tài)估計中構建視覺重投影殘差約束。

式中x表示與當前視覺重投影約束相關的狀態(tài)量。若視覺與IMU外參固定，則需要求解的雅克比分別有殘差相對于i和j時刻IMU姿態(tài)的雅克比以及殘差相對特征點逆深度的雅克比。

1.3.3 激光雷達因子

數據校驗合格后，激光雷達里程計可以構建起相鄰關鍵幀之間的約束項。為了方便殘差和雅克比的求解，本文將姿態(tài)從特殊歐式群T∈SE（3）分解為特殊正交群R∈SO（3）和平移向量t∈R。此時殘差計算方法如下：

式中：J(φ)為特殊正交群SO（3）下在φ處的右雅可比；J(φ)為特殊正交群SO（3）下在φ處的左雅可比；I為單位矩陣。

1.4 回環(huán)檢測優(yōu)化

SLAM系統(tǒng)在長時間或大場景的運行下會不可避免地出現軌跡偏移問題，這會導致構建的地圖出現全局一致性不好的問題?；丨h(huán)檢測可以有效修正系統(tǒng)的軌跡偏移，使地圖具有更好的一致性。

廣泛應用的視覺回環(huán)檢測方式是計算每幀圖像下視覺特征點的描述子并進行存儲，因此可以較為方便地采用詞袋模型存儲和檢索歷史特征信息，但是由于圖像視角較窄，無法正確識別回環(huán)的場景。視覺回環(huán)檢測失效示意圖如圖7所示，其中藍色線為車輛行駛軌跡，灰色線為車輛行駛方向。在KITTI測試場景中，雖然車輛經過了同一個路口，但是由于視角場的問題，在十字路口或對向行駛的場景下，單視覺無法正確檢測回環(huán)信息。機械式激光雷達在水平方向上擁有360°視角，但是幾何方式的特征存儲很難實現高效的回環(huán)信息檢測。為解決回環(huán)問題，提出了一種激光雷達和視覺耦合的回環(huán)檢測系統(tǒng)，利用兩種傳感器各自的優(yōu)勢條件，使系統(tǒng)能以較低算力消耗情況下實現較為準確的回環(huán)信息判定。

圖7 視覺回環(huán)檢測失效示意圖

本文中采用Dbow2詞袋模型作為視覺回環(huán)檢測的基礎。首先，將最新關鍵幀中通過光流追蹤上的特征點計算相應的描述子，并在歷史幀中進行匹配，搜尋最符合要求的若干幀，并剔除和當前幀處于時間或者距離閾值外的歷史幀。接著獲取剩余歷史幀數據的姿態(tài)數據，通過PnP（perspective-npoint）算法優(yōu)化得到當前幀與歷史幀之間的位置關系，由于視覺特征點深度信息精度較低，此時的回環(huán)是一個低精度的約束信息，為了提升回環(huán)約束的精度，將視覺回環(huán)的當前幀相對于歷史幀的姿態(tài)信息作為初值發(fā)送給激光雷達里程計當中。激光雷達里程計在存儲的歷史關鍵幀中根據最小距離原則尋找最優(yōu)匹配，找到后根據視覺回環(huán)提供的初值進行匹配，從而將姿態(tài)約束進一步優(yōu)化，最終實現高精度的回環(huán)檢測結果。

除由粗到精的視覺回環(huán)檢測優(yōu)化外，為解決視覺回環(huán)檢測的視角缺陷問題，在視覺回環(huán)檢測的基礎上添加了一個低消耗的激光雷達回環(huán)檢測機制，激光雷達里程計會實時存儲歷史關鍵幀的位姿，當檢測到最新幀點云的位置信息與歷史軌跡較為接近后，回環(huán)檢測機制就會搜索歷史信息并進行匹配。通常情況下，在沒有視覺回環(huán)檢測的情況時，此類激光雷達回環(huán)檢測方法可能會由于場景較大而失效，但本文中采用兩類回環(huán)方法的耦合，通過視覺回環(huán)應對普通場景下的回環(huán)檢測，降低系統(tǒng)長時間工作下的軌跡誤差，此時就可以較大程度地提升激光雷達通過空間距離回環(huán)檢測的準確性，從而彌補視覺視角問題導致的回環(huán)失效。

2 實驗驗證

為了評估系統(tǒng)的精度和魯棒性，分別在公開數據集和自主搭建的實車平臺上對本文所提的算法進行了測試與驗證。同時選取視覺慣性融合算法VINS-Mono與激光慣性融合算法LIO-SAM進行對比，用來驗證多模態(tài)傳感器融合具有更好的精度和環(huán)境適應能力。此外，還選取了視覺-激光雷達融合SLAM算法LIMO對比驗證視覺特征點深度信息優(yōu)化策略。最后在高校校園內采集了6 km實車數據對系統(tǒng)在實際大場景下長時間工作的魯棒性進行測試。

2.1 公開數據集下的驗證實驗

公開數據集選取了測試場景較為豐富的KITTI數據集，KITTI數據集是目前應用范圍最廣、最流行的公開數據集之一，能夠提供包括視覺圖像、64線激光雷達點云和IMU數據在內的各類傳感器數據，廣泛用于視覺、激光里程計等算法的驗證。本文中選取了KITTI里程計數據集中的多組序列進行實驗測試，包含有高速、城鎮(zhèn)、郊區(qū)等多個測試場景，能夠有效驗證本文中提出的多傳感器緊耦合SLAM系統(tǒng)的定位精度與環(huán)境適應能力。

首先本文在序列0、5、8和序列6這4組數據集上測試了本文所提方法的軌跡誤差。如表1所示。前3組數據集行駛軌跡較長，環(huán)境具有一定變化，同時軌跡中會有多次閉環(huán)，能夠更好地驗證SLAM算法在長時間運行下系統(tǒng)的漂移程度以及系統(tǒng)的回環(huán)檢測性能。序列6具有在雙向車道下對向行駛的觀測數據，用于驗證本文中提出的視覺與激光雷達耦和回環(huán)檢測與視覺回環(huán)檢測的性能對比。在這4組數據集上，本文與VINS-Mono和LIMO在相同環(huán)境下進行對比測試。所有方法均是在同一配置和環(huán)境下運行，使用的運算設備為Intel i9-9900KF，僅使用CPU參與運算，所有的算法均運行在實時速度下。

表1 測試數據集信息

測試數據與真值軌跡的對比結果如圖8所示。從圖中可以看到，本文中提出的方法相比于其他方法在軌跡一致性和端到端的誤差上有較好的提升。如圖8（a）所示，本文所提出的方法在各路段下與軌跡真值的一致性都優(yōu)于LIMO或VINS-Mono。圖8（b）中展示出相同的效果，同時由于其中的場景變化較多，VINS-Mono在該數據集上無法準確進行初始化，導致結果發(fā)散。同樣的問題出現在圖8（d）中，由于室外場景較大，特征點的深度信息估算較為困難，VINS-Mono在初始結果無法正確估計尺度信息，導致軌跡與真實數據相比尺度偏大。而本文所提的方法由于采用了激光雷達對視覺慣性初始化進行了輔助，可以將視覺尺度信息和載體速度約束到一定范圍內，保證系統(tǒng)不會因為異常值而崩潰。圖8（c）中只有本文中提出視覺與激光耦合的回環(huán)檢測算法在位于雙向車道上的采集數據中正確檢測出了回環(huán)信息，LIMO與VINS-Mono采用的視覺回環(huán)檢測策略由于視覺視場角較小，無法有效記錄大范圍的特征信息，導致回環(huán)檢測失敗，軌跡與真值相比出現了明顯的偏移。由此可見，利用視覺特征點在大場景或高速運動的環(huán)境下進行狀態(tài)估計，很容易出現特征點深度信息、尺度信息估計出現錯誤的問題，從而導致軌跡發(fā)散，而本文所提的方法可以優(yōu)化特征點的深度信息，融合的激光里程計能夠進一步提升對環(huán)境尺度信息估計的精度。

圖8 各組算法在數據序列0、5、6、8下測試得到的軌跡結果與真值的對比

此外，在圖9中展示了本文所提方法在數據序列0下運行結果在1 m的尺度下與真值相比較得到的相對位置誤差（RPE平移）和相對旋轉誤差（RPE旋轉），可以看到相對平移誤差大部分都在0.2 m以內，只有極少數時間誤差超過0.4 m。同時相對旋轉誤差平均值為0.11°，標準差為0.18°，可見里程計的精度十分穩(wěn)定。

圖9 數據集1下的相對平移誤差圖（RPE平移）和相對旋轉誤差圖（RPE旋轉）

此外，為了評估所提方法的絕對性能，本文中還引入了與真值的絕對軌跡誤差（ATE），用于衡量系統(tǒng)在每個軌跡點處與真值的絕對位置誤差。絕對軌跡誤差在軌跡上的投影如圖10所示，可以看到最大軌跡誤差為13.047 m，系統(tǒng)運行的總軌跡長度為3.6 km，端到端漂移誤差平均為5.1‰。

圖10 絕對軌跡誤差（APE，單位：m）在軌跡上的投影

此外，為了充分驗證本文所提方法與主流融合方案之間的性能對比以及在不同環(huán)境下的魯棒性，本文方案與LIO-SAM、VINS-Mono、LIMO和LOAM在KITTI里程計數據集的多組序列上進行更加充分的測試，測試結果如表2所示。在表2中采用了與KITTI類似的軌跡評價方案，對于每個數據序列，分別測試相應方法在該序列上每100、200、300和400 m下的相對平移誤差（RPE平移，單位：%）和相對旋轉誤差（RPE旋轉，單位：（°）/100 m）。這部分測試涵蓋了多種環(huán)境，比如城鎮(zhèn)、郊區(qū)以及高速環(huán)境，除了LOAM采用原始方案外，其余方法均默認開啟回環(huán)檢測。

表2 在KITTI多個場景下與主流方案的對比結果指標

實驗證明，本文中提出的視覺特征點深度信息優(yōu)化的方法能夠較好提升視覺特征點的精度，將激光雷達里程計結果作為幀間約束的后端聯(lián)合優(yōu)化策略進一步提升了SLAM系統(tǒng)的魯棒性，同時能夠獲取更好的精度。

2.2 實車驗證

在實車驗證部分，本文中使用了如圖11所示的自動駕駛平臺，包括一個80線機械旋轉式激光雷達、視覺傳感器、慣性測量單元和一套高精度慣性組合導航（INS/RTK）（僅用于為SLAM系統(tǒng)提供真值）。

圖11 自動駕駛汽車實驗平臺

本文中選取了校園環(huán)境內的場景，在長約6 km左右的閉環(huán)區(qū)域進行了數據錄制與系統(tǒng)驗證。采用的校園環(huán)境較為復雜，包含有坡度較大的上下坡，更加密集的建筑群和各類動態(tài)障礙物。

與其他方法的測試結果匯總在表3中，采用的指標為相對位置誤差（RPE平移）和絕對位置誤差（APE）的均方根值（RMSE）?？梢?，在校園此類低速園區(qū)中，本文所提的方法相比較主流方案能夠進一步提升系統(tǒng)的精度和大場景下穩(wěn)定運行的能力。

表3 校園環(huán)境下各種方法的定量誤差

圖12展示了在線構建的點云地圖效果，本文所提的多傳感器緊耦合SLAM系統(tǒng)能夠很好地適應實車測試，在大場景下長時間運行依然能夠保持良好的精度，構建出來的高精度地圖具有較高的一致性，局部地圖下的墻體、樹干等特征沒有出現重影模糊的問題。通過GNSS數據將構建出來的高精地圖投影到WGS84坐標系上能夠與衛(wèi)星地圖進行正確匹配。

圖12 基于本文方法構建的高精地圖在衛(wèi)星地圖上的投影及局部效果

3 結論

本文中提出了一種基于多傳感器融合的SLAM方案，融合了包括激光雷達、攝像頭和IMU在內的多源傳感器數據，構建了一套魯棒性更好、精度更高的實時建圖與定位系統(tǒng)。同時在數據融合的基礎上，利用激光雷達為視覺特征提供更有效和準確的深度信息，實現了更加魯棒的初始化設計和激光雷達里程計與視覺慣性里程計的緊耦合滑窗優(yōu)化。最后，本文所提出的算法經過了公開數據集和實車實驗的驗證，證明了本方法具有更好的里程計精度并且對不同的場景也有較強的適應能力。