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

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

前言

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

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

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

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

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

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

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

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

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

1.1 系統(tǒng)概覽

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

圖2 系統(tǒng)架構(gòu)與數(shù)據(jù)處理流程圖

1.2 前端處理

本節(jié)將會(huì)介紹對(duì)視覺、激光雷達(dá)的測(cè)量數(shù)據(jù)進(jìn)行預(yù)處理的步驟。對(duì)于視覺圖像，會(huì)進(jìn)行視覺特征點(diǎn)的提取、跟蹤與深度信息恢復(fù)；對(duì)于激光雷達(dá)點(diǎn)云，會(huì)進(jìn)行特征提取與里程計(jì)的構(gòu)建。前端數(shù)據(jù)預(yù)處理流程圖如圖3所示。

圖3 前端數(shù)據(jù)預(yù)處理流程圖

1.2.1 視覺特征處理

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

1.2.2 激光雷達(dá)里程計(jì)

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

圖4 激光雷達(dá)里程計(jì)計(jì)算流程圖

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

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

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

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

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

1.2.4 尺度信息恢復(fù)

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

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

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

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

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

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

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

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

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

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

1.3.2 視覺因子

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

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

1.3.3 激光雷達(dá)因子

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

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

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

SLAM系統(tǒng)在長(zhǎng)時(shí)間或大場(chǎng)景的運(yùn)行下會(huì)不可避免地出現(xiàn)軌跡偏移問題，這會(huì)導(dǎo)致構(gòu)建的地圖出現(xiàn)全局一致性不好的問題?；丨h(huán)檢測(cè)可以有效修正系統(tǒng)的軌跡偏移，使地圖具有更好的一致性。

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

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

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

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

2 實(shí)驗(yàn)驗(yàn)證

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

2.1 公開數(shù)據(jù)集下的驗(yàn)證實(shí)驗(yàn)

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

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

表1 測(cè)試數(shù)據(jù)集信息

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

圖8 各組算法在數(shù)據(jù)序列0、5、6、8下測(cè)試得到的軌跡結(jié)果與真值的對(duì)比

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

圖9 數(shù)據(jù)集1下的相對(duì)平移誤差圖（RPE平移）和相對(duì)旋轉(zhuǎn)誤差圖（RPE旋轉(zhuǎn)）

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

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

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

表2 在KITTI多個(gè)場(chǎng)景下與主流方案的對(duì)比結(jié)果指標(biāo)

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

2.2 實(shí)車驗(yàn)證

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

圖11 自動(dòng)駕駛汽車實(shí)驗(yàn)平臺(tái)

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

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

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

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

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

3 結(jié)論

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