慣性/視覺/激光雷達(dá)SLAM技術(shù)綜述

毛 軍，付 浩，褚超群，何曉峰，陳昶昊

(國防科技大學(xué)智能科學(xué)學(xué)院,長沙 410073)

0 引言

同時(shí)定位與建圖(Simultaneous Localization and Mapping，SLAM)技術(shù)是指移動機(jī)器人利用自身搭載的傳感器構(gòu)建環(huán)境地圖，同時(shí)利用環(huán)境信息進(jìn)行自主定位。SLAM因具有較好的自主性和精度而廣泛應(yīng)用于各類移動平臺中。

SLAM最早于20世紀(jì)80年代被提出，早期的SLAM系統(tǒng)主要是基于單線激光雷達(dá)傳感器構(gòu)建的，且僅適用于在二維平面中運(yùn)動的載體。隨著激光雷達(dá)技術(shù)的發(fā)展，基于激光雷達(dá)的3D SLAM系統(tǒng)得到迅速發(fā)展。目前，性能較好的激光雷達(dá)可以實(shí)現(xiàn)百米外厘米級的測距精度，支撐了一系列高精度SLAM應(yīng)用場景。然而，激光雷達(dá)SLAM仍面臨眾多難題，例如在自運(yùn)動較快時(shí)雷達(dá)數(shù)據(jù)會發(fā)生畸變，導(dǎo)致SLAM精度下降；此外，在隧道、高速路等結(jié)構(gòu)特征不明顯的區(qū)域，雷達(dá)幀間數(shù)據(jù)難以配準(zhǔn)，會造成SLAM系統(tǒng)退化甚至失效。

相較于激光雷達(dá)SLAM系統(tǒng)，視覺SLAM系統(tǒng)具有不主動發(fā)出信號的優(yōu)點(diǎn)。此外，視覺是對環(huán)境的稠密感知，可以利用視覺特征構(gòu)建出更符合人類感知的環(huán)境地圖。然而，視覺傳感器本質(zhì)上是一種角度測量傳感器，不能直接測量距離信息，因此需要從多視圖中重構(gòu)出特征的距離。而且，復(fù)雜變化和特征匱乏的視覺環(huán)境也會對視覺SLAM系統(tǒng)造成影響甚至使得系統(tǒng)失效。

激光雷達(dá)、相機(jī)和慣性測量單元(Inertial Meas-urement Unit, IMU)三者之間具有較好的互補(bǔ)性。IMU測量不受環(huán)境特征的影響，慣性導(dǎo)航系統(tǒng)僅基于載體運(yùn)動產(chǎn)生的慣性信息就可以對速度、位置和姿態(tài)進(jìn)行全參數(shù)估計(jì)。慣性導(dǎo)航估計(jì)的運(yùn)動參數(shù)可用于校正雷達(dá)數(shù)據(jù)的畸變、補(bǔ)償單目視覺缺失的尺度信息等；而激光雷達(dá)SLAM和視覺SLAM測量的載體運(yùn)動則可以校正慣性導(dǎo)航系統(tǒng)的累積誤差。近年來，慣性/視覺SLAM、慣性/激光雷達(dá)SLAM、視覺/激光雷達(dá)SLAM等基于多傳感器融合的SLAM系統(tǒng)發(fā)展迅速，并且展現(xiàn)出比基于單一傳感器更高的精度和更強(qiáng)的環(huán)境適應(yīng)性。

經(jīng)過30多年的發(fā)展，SLAM框架已日趨成熟。T.Bailey等對SLAM算法框架進(jìn)行了介紹；J.Fu-entes-Pacheco等則對視覺SLAM技術(shù)的主要方法進(jìn)行了綜述。2015年，C.Cadena等在Robotics: Science and System (RSS)會議上對SLAM技術(shù)的發(fā)展進(jìn)行了綜述，并對其未來發(fā)展趨勢進(jìn)行了展望，認(rèn)為魯棒的感知端是未來SLAM技術(shù)發(fā)展的重要方向。將IMU、相機(jī)和激光雷達(dá)進(jìn)行組合是提升SLAM系統(tǒng)精度和魯棒性的重要方式，相較于視覺SLAM和激光雷達(dá)SLAM算法，多傳感器SLAM技術(shù)涉及信息融合技術(shù)，因此有必要進(jìn)行詳細(xì)的分析。本文首先從SLAM的基本原理出發(fā)，分析其對多傳感器融合的需求；然后從多傳感器時(shí)空配準(zhǔn)和多傳感器信息融合兩方面對慣性/激光/視覺組合SLAM技術(shù)進(jìn)行介紹，并對基于多傳感器融合的SLAM技術(shù)進(jìn)行展望。

1 SLAM簡述

移動機(jī)器人在未知環(huán)境中運(yùn)動時(shí)，可以通過SLAM系統(tǒng)構(gòu)建環(huán)境地圖，同時(shí)利用構(gòu)建的地圖進(jìn)行自主定位，如圖1所示。本節(jié)首先介紹了SLAM的數(shù)學(xué)模型，然后簡要分析SLAM系統(tǒng)對多傳感器融合的需求。

圖1 SLAM原理示意圖[4]Fig.1 Schematic diagram of SLAM

1.1 SLAM模型簡介

設(shè)SLAM問題中待估計(jì)的機(jī)器人運(yùn)動參數(shù)為0:={,,…,}，其中表示時(shí)刻載體的位姿參數(shù)；設(shè)SLAM構(gòu)建的地圖可表示為一組地標(biāo)的集合={,,…,}，其中表示第個(gè)地標(biāo)在地圖中的位置。機(jī)器人在移動過程中可以通過傳感器對地標(biāo)進(jìn)行觀測，這些觀測的集合表示為0:={,,…,}；而驅(qū)使機(jī)器人從-1到轉(zhuǎn)移的控制量為，在移動過程中所有控制量的集合表示為0:={,,…,}。

SLAM就是根據(jù)觀測0:和控制輸入0:，估計(jì)運(yùn)動軌跡0:和地標(biāo)位置的過程。在SLAM算法框架中，對觀測0:的提取部分通常被稱為SLAM的前端(front-end)；而利用0:和0:對運(yùn)動軌跡和地標(biāo)位置進(jìn)行估計(jì)的部分被稱為SLAM的后端(back-end)，如圖2所示。

前端算法一方面要從每一幀的傳感器數(shù)據(jù)中提取出地標(biāo)觀測信息，另一方面利用數(shù)據(jù)關(guān)聯(lián)方法判斷新一幀數(shù)據(jù)中的觀測地標(biāo)是首次觀測到的新地標(biāo)還是某一已經(jīng)被觀測到的舊地標(biāo)。由于不同的SLAM系統(tǒng)使用的傳感器不同，因而觀測信息的提取方式也有較大差異。例如，在激光雷達(dá)SLAM中，通常直接采用三維點(diǎn)云描述環(huán)境；在視覺SLAM中，則可以采用像素的光度或視覺特征描述地標(biāo)。建立幀間觀測的關(guān)聯(lián)也是前端算法的重要組成部分，數(shù)據(jù)關(guān)聯(lián)算法與地標(biāo)描述方法緊密聯(lián)系。在激光雷達(dá)SLAM系統(tǒng)中，廣泛使用迭代最近點(diǎn)(Iterative Closest Point, ICP)、正態(tài)分布變換(Normal Distributions Trans-form, NDT)等方法進(jìn)行幀間點(diǎn)云配準(zhǔn)。在視覺SLAM系統(tǒng)中,則主要采用光流跟蹤、特征匹配等方式建立幀間觀測關(guān)聯(lián)。

圖2 SLAM算法架構(gòu)圖[3]Fig.2 Scheme of the SLAM algorithm

經(jīng)過30多年的發(fā)展，SLAM后端算法模型已日趨成熟，大致分為基于最大后驗(yàn)概率(Maximum A Posteriori, MAP)優(yōu)化、拓展卡爾曼濾波(Extended Kalman Filter, EKF)和粒子濾波等三類后端處理算法。由于粒子濾波算法也可以被認(rèn)為是一種MAP問題的求解方式，因此本文主要介紹MAP和EKF兩種后端處理算法。MAP和EKF算法可以概括為兩個(gè)主要步驟：第一步是根據(jù)和-1對進(jìn)行預(yù)測；第二步是根據(jù)觀測對0:和進(jìn)行估計(jì)。MAP模型可以用式(1)表示

(1)

在多數(shù)SLAM系統(tǒng)中，采用高斯誤差模型和一階馬爾可夫模型近似后，式(1)的求解可以轉(zhuǎn)化為一個(gè)非線性最小二乘求解問題，即

(2)

其中，表示第個(gè)地標(biāo)；表示時(shí)刻對地標(biāo)的觀測；(·)和(·)分別表示運(yùn)動模型和觀測模型，和則表示相應(yīng)模型的信息矩陣。在EKF方法中，(·)和(·)分別表示狀態(tài)轉(zhuǎn)移函數(shù)和觀測函數(shù)。當(dāng)載體運(yùn)動時(shí)間長、觀測特征多時(shí)，SLAM中待估計(jì)的參數(shù)量也會增大。但通常情況下，某一個(gè)地標(biāo)點(diǎn)僅在少數(shù)幾個(gè)位置能被機(jī)器人觀測到，這在數(shù)學(xué)上就表現(xiàn)為(·)和(·)具有明顯的稀疏性，利用這種稀疏性可顯著提升SLAM的后端算法效率。目前，常用的MAP優(yōu)化工具箱有g(shù)2o、GTSAM和ceres等，而EKF后端算法中常用的有多狀態(tài)約束卡爾曼濾波(Multi-State Constraint Kalman Filter，MSCKF)方法。

1.2 多傳感器融合SLAM系統(tǒng)需求

由于傳感器的限制，激光雷達(dá)和視覺SLAM在載體動態(tài)性較強(qiáng)、環(huán)境復(fù)雜條件下都面臨諸多問題。將多種具有互補(bǔ)性的傳感器進(jìn)行融合是構(gòu)建高精度、高魯棒SLAM系統(tǒng)的重要途徑。

慣性導(dǎo)航信息可用于提升視覺和激光雷達(dá)SLAM的前端和后端算法性能。如圖3所示，由于激光雷達(dá)在掃描過程中載體是運(yùn)動的，當(dāng)動態(tài)性較強(qiáng)時(shí)，常用的勻速補(bǔ)償模型難以校正幀內(nèi)點(diǎn)云的畸變；而慣性導(dǎo)航系統(tǒng)能以較高的頻率測量激光雷達(dá)一個(gè)掃描周期內(nèi)的運(yùn)動信息，這些運(yùn)動信息可有效地校準(zhǔn)雷達(dá)點(diǎn)云數(shù)據(jù)。慣性導(dǎo)航的輸出結(jié)果對于幀間數(shù)據(jù)匹配也具有重要輔助作用，其可以作為幀間匹配算法的初值，而相對準(zhǔn)確的初值可以有效提升配準(zhǔn)算法的精度和收斂速度。慣性導(dǎo)航輸出信息還可用于補(bǔ)償運(yùn)動造成的圖像模糊，從而提升視覺SLAM的前端魯棒性。

在SLAM后端算法中，融合IMU信息對提升SLAM精度和魯棒性也具有重要意義。基于激光雷達(dá)和視覺的SLAM系統(tǒng)通常采用勻速模型來近似運(yùn)動模型(·)，而在動態(tài)性較強(qiáng)時(shí)，該近似方法會導(dǎo)致模型精度下降。慣性傳感器能以較高的頻率輸出載體的運(yùn)動信息，利用慣性導(dǎo)航輸出可構(gòu)建更為準(zhǔn)確的SLAM模型。在單目視覺SLAM系統(tǒng)中，慣性信息還可用于補(bǔ)償缺失的尺度因子。另一方面，視覺和激光雷達(dá)對地標(biāo)的觀測信息則可用于校正慣性導(dǎo)航的累積誤差，從而實(shí)現(xiàn)組合增強(qiáng)。

圖3 利用慣導(dǎo)校正激光雷達(dá)幀內(nèi)畸變的示意圖 (當(dāng)車體靜止時(shí)，雷達(dá)在地面上掃描的數(shù)據(jù)為一個(gè)圓形(如左圖所示)；當(dāng)車體向前運(yùn)動時(shí)，這個(gè)圓形會產(chǎn)生一定的畸變(如右圖所示)；在雷達(dá)采樣周期內(nèi)(通常為100ms)，可以接收到多個(gè)慣導(dǎo)數(shù)據(jù)，利用這些慣導(dǎo)數(shù)據(jù)可以對雷達(dá)數(shù)據(jù)進(jìn)行幀內(nèi)校正)Fig.3 Illustration of undistorting a Lidar scan with the aid of an inertial navigation system(Illustration of the lidar scans captured in a sampling duration. Left: when the agent is static and the scans form a circle. Right: when the agent is not static, the sampled data contains distortions. By using the motion parameters estimated by the inertial system, the lidar data distortion can be fixed)

相機(jī)與激光雷達(dá)之間具有顯著的互補(bǔ)性。激光雷達(dá)僅能測量地標(biāo)的距離信息，當(dāng)?shù)貥?biāo)特征受運(yùn)動物體遮擋時(shí)，僅利用點(diǎn)云的幾何信息難以與地圖進(jìn)行配準(zhǔn)。將相機(jī)與激光雷達(dá)配準(zhǔn)后，可以將相機(jī)感知的視覺特征信息賦予雷達(dá)點(diǎn)云，然后利用特征的相似性輔助點(diǎn)云配準(zhǔn)。相機(jī)本質(zhì)上是一種角度測量傳感器，在成像過程中地標(biāo)特征的距離信息丟失，因此在視覺SLAM系統(tǒng)中需要根據(jù)多幀或多目的觀測重構(gòu)地標(biāo)的三維位置信息，這給SLAM算法帶來了負(fù)擔(dān)。在光照、視角等發(fā)生強(qiáng)烈變化時(shí)，視覺SLAM前端也面臨失效風(fēng)險(xiǎn)。將激光雷達(dá)測量的距離信息賦予視覺特征可實(shí)現(xiàn)三維視覺功能，同時(shí)在視覺環(huán)境發(fā)生變化時(shí)，激光雷達(dá)測量的三維結(jié)構(gòu)仍能保證SLAM前端的正常運(yùn)行。

2 慣性/視覺/激光雷達(dá)標(biāo)定方法

對異源傳感器的精確標(biāo)定是有效使用傳感器測量信息的前提條件。通常，對單個(gè)傳感器的標(biāo)定稱為內(nèi)參標(biāo)定，而對不同傳感器之間的相互關(guān)系的標(biāo)定則稱為外參標(biāo)定。外參標(biāo)定一般包括時(shí)間同步和空間標(biāo)定兩個(gè)部分。本文主要關(guān)注IMU、相機(jī)和激光雷達(dá)之間的外參標(biāo)定方法，對于傳感器內(nèi)參標(biāo)定，讀者可查閱相關(guān)文獻(xiàn)。

2.1 時(shí)間同步

時(shí)間同步的含義是使不同傳感器數(shù)據(jù)具有統(tǒng)一的時(shí)間基準(zhǔn)，以方便后續(xù)的數(shù)據(jù)融合。為了實(shí)現(xiàn)相機(jī)圖像數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)具有較小的采集時(shí)間間隔，文獻(xiàn)[16]在旋轉(zhuǎn)的激光雷達(dá)下方安裝了一個(gè)簧片探針，當(dāng)激光雷達(dá)旋轉(zhuǎn)至車體正前方時(shí)，簧片探針產(chǎn)生一個(gè)外觸發(fā)信號，控制前視相機(jī)開始產(chǎn)生一幀圖像。

影響相機(jī)成像時(shí)間的因素還包括相機(jī)曝光時(shí)間和快門方式。目前，大多數(shù)相機(jī)都采用自動曝光時(shí)間方法，其優(yōu)點(diǎn)是能夠產(chǎn)生當(dāng)前場景下盡可能清晰的圖像，但缺點(diǎn)是曝光時(shí)間變得不可控。相機(jī)的快門方式分為全局快門(global shutter)和卷簾式快門(rolling shutter)。全局快門是指圖像的所有行同時(shí)進(jìn)行曝光，而卷簾式快門則是逐行進(jìn)行曝光，圖像頂部和底部的曝光時(shí)間差異可能高達(dá)幾十毫秒，若是不考慮這種時(shí)間上的差異，則會降低圖像幀間匹配的性能，從而影響融合定位結(jié)果。為了克服卷簾快門效應(yīng)，文獻(xiàn)[17]提出了采用B樣條插值方法擬合出任意時(shí)刻的相機(jī)位姿，從而實(shí)現(xiàn)對場景更精確的建模和位姿估計(jì)。

待融合的多源傳感器信息可能會由連在同一局域網(wǎng)上的多臺計(jì)算機(jī)并行處理，為了保證不同信息源的時(shí)間一致，需要將不同計(jì)算機(jī)的系統(tǒng)時(shí)間設(shè)為一致。此時(shí)，既可以采用網(wǎng)絡(luò)時(shí)間協(xié)議(Network Time Protocol，NTP)進(jìn)行軟件上的時(shí)間同步，也可以安裝衛(wèi)星導(dǎo)航授時(shí)設(shè)備等對多系統(tǒng)時(shí)間基準(zhǔn)進(jìn)行標(biāo)校。

對于激光雷達(dá)來說，其數(shù)據(jù)通常都是通過用戶數(shù)據(jù)包協(xié)議(User Datagram Protocol，UDP)進(jìn)行廣播。每個(gè)數(shù)據(jù)包都包含一個(gè)時(shí)間戳，該時(shí)間戳通常來自于激光雷達(dá)的內(nèi)部時(shí)鐘，可以通過外接衛(wèi)星導(dǎo)航接收機(jī)對內(nèi)部時(shí)鐘進(jìn)行校準(zhǔn)。

2.2 相機(jī)/激光雷達(dá)標(biāo)定

相機(jī)和激光雷達(dá)都是直接對環(huán)境中的地標(biāo)進(jìn)行測量，其中激光雷達(dá)測量的是地標(biāo)距離傳感器的距離，而相機(jī)可以感知地標(biāo)的視覺紋理和相對相機(jī)坐標(biāo)系的角度，二者間的標(biāo)定是一個(gè)典型的手眼標(biāo)定問題，如圖4所示。

圖4 相機(jī)/激光雷達(dá)標(biāo)定示意圖[18-19]Fig.4 Illustration of camera-lidar calibration[18-19]

記視覺傳感器為，其內(nèi)參標(biāo)定矩陣為；激光雷達(dá)傳感器為，二者剛性固聯(lián)于平臺載體上。手眼標(biāo)定問題的實(shí)質(zhì)就是求解兩個(gè)傳感器坐標(biāo)系間的相對位姿變換_=[_,_]，該變換關(guān)系可以將雷達(dá)坐標(biāo)系中的一點(diǎn)變換至相機(jī)像素坐標(biāo)系

1.5 統(tǒng)計(jì)學(xué)處理 采用SPSS 19統(tǒng)計(jì)軟件分析數(shù)據(jù)，實(shí)驗(yàn)結(jié)果以表示，采用獨(dú)立樣本t檢驗(yàn)或Wilcoxon秩和檢驗(yàn)進(jìn)行組間比較，檢驗(yàn)水準(zhǔn)(α)為0.05。

=π(*_*)

(3)

其中，π表示相機(jī)投影映射。因此，相機(jī)-雷達(dá)的手眼標(biāo)定問題可以被數(shù)學(xué)描述為最小化式(4)中的殘差問題

(4)

求解以上問題的關(guān)鍵就是要獲得對應(yīng)點(diǎn)對{,}。為了獲得精確的對應(yīng)點(diǎn)對，很多算法都采取在場景中放置特定標(biāo)定物的形式來輔助尋找對應(yīng)點(diǎn)對。文獻(xiàn)[18]是最早進(jìn)行相機(jī)-雷達(dá)標(biāo)定的工作之一，該文嘗試將相機(jī)與單線雷達(dá)進(jìn)行標(biāo)定，通過在場景中放置棋盤格，提取棋盤格在雷達(dá)數(shù)據(jù)中形成的線狀特征，進(jìn)而求解相機(jī)-雷達(dá)外參；文獻(xiàn)[19]提出了一個(gè)包含人機(jī)交互界面的相機(jī)-雷達(dá)標(biāo)定軟件，可以在少量人工參與的情況下快速完成標(biāo)定工作；文獻(xiàn)[20]綜合利用棋盤格的邊緣信息和平面信息，僅需要將棋盤格放在某一固定位置處即可實(shí)現(xiàn)標(biāo)定；文獻(xiàn)[21]利用棋盤格的尺寸信息克服雷達(dá)數(shù)據(jù)的固有噪聲和多幀數(shù)據(jù)拼接時(shí)產(chǎn)生的誤差，通過對棋盤格進(jìn)行更精確的位置估計(jì)，從而得到更精確的標(biāo)定結(jié)果；文獻(xiàn)[22]設(shè)計(jì)了一種包含4個(gè)棋盤格和4個(gè)圓孔的標(biāo)定板，能夠標(biāo)定多種不同線束的雷達(dá)。

除了采用特點(diǎn)標(biāo)志物來輔助計(jì)算對應(yīng)點(diǎn)對外，近年來研究人員也提出了一些在自然場景中無需輔助標(biāo)定物的標(biāo)定方法。例如文獻(xiàn)[23]提出了一種利用圖像中的灰度邊緣信息和雷達(dá)點(diǎn)云中的深度邊緣信息進(jìn)行匹配標(biāo)定的方法，作者認(rèn)為深度邊緣往往對應(yīng)的就是圖像灰度邊緣。因此，可以先在雷達(dá)點(diǎn)云中計(jì)算出深度邊緣點(diǎn)，進(jìn)而將這些深度邊緣點(diǎn)投影至圖像中，計(jì)算這些投影點(diǎn)與最近圖像邊緣線的距離，優(yōu)化的目標(biāo)就是使得這些投影點(diǎn)到圖像邊緣線間的距離最短。類似的想法也出現(xiàn)在文獻(xiàn)[24]中，作者將雷達(dá)投影點(diǎn)到圖像邊緣線間的距離看成一種標(biāo)定性能評價(jià)指標(biāo)，利用該指標(biāo)可以判斷在某一時(shí)刻圖像和雷達(dá)是否因網(wǎng)絡(luò)阻塞或其他原因?qū)е露邿o法實(shí)現(xiàn)較好的匹配對應(yīng)。

從標(biāo)定精度來看，相機(jī)-激光雷達(dá)外參標(biāo)定本質(zhì)上還是一個(gè)最小二乘問題，因此如果能獲得更多更精確的匹配對應(yīng)關(guān)系，則標(biāo)定精度就會更高。相較于無需輔助標(biāo)定物的標(biāo)定方法而言，借助棋盤格等輔助標(biāo)定物的標(biāo)定方法由于可以對棋盤格的尺寸進(jìn)行精確測量，因此可能會得到更高的標(biāo)定精度。但無需輔助標(biāo)定物的標(biāo)定方法由于其標(biāo)定過程更加方便，因此更受研究人員青睞，也是目前的一個(gè)研究熱點(diǎn)。

2.3 慣性/相機(jī)標(biāo)定

相較于相機(jī)與激光雷達(dá)都是對地標(biāo)特征的測量，IMU和相機(jī)的測量信息之間差異較大。為標(biāo)定IMU/相機(jī)之間的外參，J.Alves等提出了將固聯(lián)的IMU和相機(jī)固定于一個(gè)單擺上，然后以測量得到的單擺運(yùn)動作為參考進(jìn)行標(biāo)定。然而，該方法需要借助外部設(shè)備，對標(biāo)定設(shè)備具有一定的要求，且標(biāo)定效率不高。僅利用IMU/相機(jī)自身的測量信息進(jìn)行外參標(biāo)定，則擺脫了對外部測量設(shè)備的需求。F.M.Mirzaei等和M.Fleps等分別提出了一種基于EKF和非線性優(yōu)化的自標(biāo)定方法。這種方法僅使用了一個(gè)棋盤格標(biāo)定板，然后通過采集相機(jī)面對標(biāo)定板運(yùn)動的圖像和慣性信息就可以對外參進(jìn)行準(zhǔn)確估計(jì)，如圖5所示。使用標(biāo)定板的標(biāo)定算法原理與1.1節(jié)所述的SLAM原理相似，視覺標(biāo)定板提供的已知地標(biāo)位置信息為IMU/相機(jī)外參聯(lián)合標(biāo)定提供了參考。通過對標(biāo)定算法的可觀性進(jìn)行分析還發(fā)現(xiàn)，采集的數(shù)據(jù)中需要包含足夠的旋轉(zhuǎn)運(yùn)動激勵(lì)才能保障算法的收斂性。先治文提出了一種多位置靜態(tài)標(biāo)定方法，不需要采集運(yùn)動信息就能對IMU和相機(jī)的外參進(jìn)行標(biāo)定，但這種標(biāo)定方法需要將視覺標(biāo)定板放置于高精度水平臺上。

圖5 慣性/視覺標(biāo)定示意圖[29]Fig.5 Illustration of visual-inertial calibration[29]

在以上介紹的標(biāo)定算法中，通常認(rèn)為IMU和相機(jī)之間的數(shù)據(jù)是時(shí)間對齊的。然而，不同的傳感器時(shí)間響應(yīng)不同，對IMU和相機(jī)數(shù)據(jù)之間的時(shí)間差異進(jìn)行標(biāo)定，對于SLAM算法的狀態(tài)估計(jì)一致性具有重要意義。相較于設(shè)計(jì)高精度的同步硬件，利用軟件算法標(biāo)定時(shí)間差異具有不需要額外硬件開銷的優(yōu)點(diǎn)。P.Furgale等提出了一種時(shí)空聯(lián)合標(biāo)定算法，可實(shí)現(xiàn)對相機(jī)/IMU外參和傳感器時(shí)間差異進(jìn)行聯(lián)合標(biāo)定，相關(guān)算法已形成開源工具箱，被SLAM研究者和工程師廣泛使用。

借助標(biāo)定板對IMU/相機(jī)時(shí)空參數(shù)進(jìn)行離線標(biāo)定的方法，難以應(yīng)對安裝基座形變等情況，并且一旦離線標(biāo)定參數(shù)精度不高，則將影響到多傳感器SLAM系統(tǒng)的融合效果。因此，研究利用自然特征的在線標(biāo)定方法具有重要的應(yīng)用價(jià)值。J.Kelly等提出的標(biāo)定算法既可以使用標(biāo)定板，也可以使用未知的地標(biāo)特征點(diǎn)進(jìn)行IMU/相機(jī)外參標(biāo)定。沈劭劼等將外參和時(shí)間參數(shù)加入到慣性/視覺組合SLAM系統(tǒng)中，實(shí)現(xiàn)了對運(yùn)動參數(shù)和標(biāo)定參數(shù)的在線聯(lián)合估計(jì)。

2.4 慣性/激光雷達(dá)標(biāo)定

相較于視覺-IMU 標(biāo)定方法，關(guān)于激光雷達(dá)-IMU 標(biāo)定方法的研究相對較少。目前，主要采用分步標(biāo)定策略。首先在慣性/視覺/激光雷達(dá)系統(tǒng)中標(biāo)定IMU/相機(jī)、激光雷達(dá)/相機(jī)之間的外參，然后再利用傳遞性獲得IMU與激光雷達(dá)之間的相互安裝關(guān)系。

借助高精度導(dǎo)航設(shè)備可以為IMU/激光雷達(dá)標(biāo)定提供參考信息，例如以GNSS/IMU組合導(dǎo)航系統(tǒng)輸出的運(yùn)動信息與激光雷達(dá)SLAM測量的運(yùn)動信息進(jìn)行配準(zhǔn)，從而標(biāo)定IMU與激光雷達(dá)的安裝關(guān)系。在進(jìn)行雷達(dá)標(biāo)定的過程中，一個(gè)不容忽視的因素就是雷達(dá)由于自身運(yùn)動所引起的幀內(nèi)畸變。雖然可利用GNSS/IMU組合導(dǎo)航系統(tǒng)輸出對幀內(nèi)運(yùn)動進(jìn)行插值擬合，得到連續(xù)的激光雷達(dá)SLAM輸出；然而，利用GNSS/IMU組合導(dǎo)航系統(tǒng)對激光雷達(dá)SLAM進(jìn)行插值的前提是需要已知標(biāo)定關(guān)系。為了解決上述問題，可以采用反復(fù)迭代優(yōu)化的方法或者限制雷達(dá)采集時(shí)刻車體是靜止的，這樣就避免了幀內(nèi)補(bǔ)償。

目前，市場上一些新型號激光雷達(dá)已集成了IMU，如大疆的Livox雷達(dá)等，這類雷達(dá)在出廠時(shí)就已經(jīng)標(biāo)定好了雷達(dá)與IMU 間的外參，可供開發(fā)人員直接使用。

多傳感器標(biāo)定技術(shù)雖然在數(shù)學(xué)原理上已經(jīng)基本成熟，但是該技術(shù)目前仍然是機(jī)器人領(lǐng)域的研究熱點(diǎn)，相關(guān)論文在每年的頂會頂刊上依然層出不窮。研究標(biāo)定過程更方便、標(biāo)定精度更高，甚至是無需人工參與的在線自標(biāo)定方法始終是學(xué)界和工業(yè)界共同追求的目標(biāo)。

3 基于慣性/視覺/激光雷達(dá)融合的SLAM技術(shù)

本節(jié)將介紹基于慣性/視覺/激光雷達(dá)融合的SLAM技術(shù)。IMU測量的是載體運(yùn)動的慣性信息，而相機(jī)和激光雷達(dá)都是對地標(biāo)的測量。如何將慣性信息與地標(biāo)信息進(jìn)行有效融合是組合SLAM系統(tǒng)需要解決的重要問題。本文將組合SLAM系統(tǒng)分為慣性/視覺、慣性/激光雷達(dá)和慣性/視覺/激光雷達(dá)三類系統(tǒng)?；趹T性/視覺組合的導(dǎo)航技術(shù)讀者可以參考作者前續(xù)發(fā)表的綜述文獻(xiàn)，在本文中不再復(fù)述。

3.1 慣性/激光雷達(dá)組合SLAM

根據(jù)組合方式的不同，可將慣性/激光雷達(dá)組合SLAM分為松組合和緊組合兩大類。在組合SLAM中，根據(jù)狀態(tài)估計(jì)算法又可分為基于濾波的狀態(tài)估計(jì)方法和基于非線性優(yōu)化的參數(shù)優(yōu)化方法，慣性/激光雷達(dá)組合SLAM部分文獻(xiàn)總結(jié)如表1所示。由于各算法的測試條件和評估方法不同，難以對算法精度進(jìn)行對比，因此表1中僅列出了各算法在特定測試條件下的定位精度指標(biāo)作為參考。

表1 慣性/激光雷達(dá)組合SLAM

LOAM是可實(shí)時(shí)化的激光雷達(dá)SLAM代表作之一，其在2014年被提出時(shí)即在KITTI數(shù)據(jù)集的精度測試榜上排名第一。相較于純激光雷達(dá)的LOAM算法，采用卡爾曼濾波器將激光雷達(dá)與IMU進(jìn)行松組合，并利用IMU輸出的高頻運(yùn)動信息對雷達(dá)點(diǎn)云進(jìn)行校正，可顯著提升LOAM算法精度。LeGO-LOAM則以LOAM為基礎(chǔ)，針對地面應(yīng)用對特征提取和狀態(tài)估計(jì)進(jìn)行了優(yōu)化，不僅提升了算法精度，還可在Nvidia Jetson TX2 計(jì)算模塊中實(shí)時(shí)運(yùn)行。IMU-Aided LIO算法則將IMU與輪式碼盤組合獲得高頻運(yùn)動信息，然后將該信息用于補(bǔ)償激光雷達(dá)掃描過程中產(chǎn)生的點(diǎn)云畸變。

相較于松組合系統(tǒng)，慣性/激光雷達(dá)緊組合SLAM系統(tǒng)在后端算法中直接利用點(diǎn)云觀測信息優(yōu)化定位與建圖結(jié)果；同時(shí)，在前端點(diǎn)云匹配中也會利用組合系統(tǒng)輸出的高頻運(yùn)動信息進(jìn)行補(bǔ)償和初始化。MC2SLAM利用慣性信息對運(yùn)動軌跡進(jìn)行預(yù)測，并構(gòu)建幀間相對運(yùn)動因子；在優(yōu)化過程中則采用因子圖對先驗(yàn)殘差、雷達(dá)里程計(jì)殘差和幀間相對運(yùn)動殘差進(jìn)行聯(lián)合優(yōu)化，其測試結(jié)果在KITTI數(shù)據(jù)集上展示出比LOAM算法更高的精度。LIOM將定位與建圖設(shè)置為兩個(gè)并行的節(jié)點(diǎn)，在定位模塊中采用基于滑動窗口的因子圖優(yōu)化方法進(jìn)行信息融合。為提升組合算法運(yùn)行效率，LIOM還采用了慣性預(yù)積分理論以避免對慣性的重復(fù)積分運(yùn)算。LINS采用迭代卡爾曼濾波器對慣性/激光雷達(dá)進(jìn)行緊組合，雖然其精度與LIOM相當(dāng)，但LINS的運(yùn)行速度卻明顯快于LIOM。在同一計(jì)算設(shè)備中的測試表明，LIOM處理一幀雷達(dá)數(shù)據(jù)的時(shí)間大于100ms，而LINS卻小于30ms。LIO-SAM利用iSAM平滑優(yōu)化工具箱，將慣性、激光雷達(dá)和全球定位系統(tǒng)(Global Positioning System，GPS)進(jìn)行融合，并且采用一種局部地圖匹配策略替換LOAM和LIOM中的全局地圖匹配策略，使得LIO-SAM具有更高的精度和更短的計(jì)算耗時(shí)。與多數(shù)慣性/激光雷達(dá)SLAM在前端算法中提取線、面等特征不同，F(xiàn)AST-LIO2直接將激光雷達(dá)測得的點(diǎn)與地圖進(jìn)行匹配，并且采用K-D樹來保障對地圖的快速更新。針對新型的固態(tài)激光雷達(dá)，LiLi-OM設(shè)計(jì)了一種從固態(tài)激光雷達(dá)中提取特征的方法，并設(shè)計(jì)了一種針對激光雷達(dá)的關(guān)鍵幀滑動窗口優(yōu)化策略。在與LIOM、LINS和LIO-SAM等算法的對比中，LiLi-OM展現(xiàn)出更好的定位精度。

3.2 慣性/視覺/激光雷達(dá)組合SLAM技術(shù)

雖然相機(jī)和激光雷達(dá)都可以感知地標(biāo)信息，但兩種傳感器測量的信息類型不同。一種簡單的融合思路是分別以視覺和激光雷達(dá)為基礎(chǔ)構(gòu)建里程計(jì)，然后通過松組合的方式將兩個(gè)模塊的輸出結(jié)果進(jìn)一步融合。SuperOdometry則以IMU為中心構(gòu)建了慣性/視覺里程計(jì)和慣性/激光雷達(dá)里程計(jì)分別對運(yùn)動參數(shù)進(jìn)行估計(jì)，然后利用位姿圖優(yōu)化方法將兩個(gè)里程計(jì)輸出進(jìn)行融合(如圖6所示)。構(gòu)建兩個(gè)里程計(jì)模塊的松組合方法雖然降低了算法設(shè)計(jì)的復(fù)雜度，但未能發(fā)揮視覺和雷達(dá)在前端感知中的互補(bǔ)性。在VIL-SLAM中，采用一個(gè)慣性/雙目視覺里程計(jì)進(jìn)行運(yùn)動估計(jì)，并利用里程計(jì)輸出的運(yùn)動信息和激光雷達(dá)點(diǎn)云信息進(jìn)行建圖；在閉環(huán)優(yōu)化中，首先采用視覺信息檢測閉環(huán)，然后利用雷達(dá)點(diǎn)云配準(zhǔn)解算相對運(yùn)動信息，進(jìn)而對運(yùn)動軌跡和環(huán)境地圖進(jìn)行優(yōu)化。在Zhang J.等提出的方法中，包含視覺/慣性里程計(jì)和激光雷達(dá)里程計(jì)兩個(gè)模塊，在視覺里程計(jì)中激光雷達(dá)測量的深度信息和視覺特征進(jìn)行了配準(zhǔn)；而視覺里程計(jì)的輸出也用于輔助激光雷達(dá)點(diǎn)云的幀間匹配。然而，同時(shí)運(yùn)行視覺和激光雷達(dá)兩個(gè)里程計(jì)算法模塊需要更多的計(jì)算資源，算法效率不高。

圖6 將慣性/視覺與慣性/激光雷達(dá)兩種里程計(jì)進(jìn)行融合示意圖[54]Fig.6 Schematic diagram of fusing visual-inertial odometry and lidar-inertial odometry[54]

僅在SLAM前端算法中進(jìn)行融合可以降低后端算法對計(jì)算資源的需求。Liang X.等提出的方法就以激光雷達(dá)SLAM算法為主，僅在前端算法中采用視覺特征輔助閉環(huán)檢測。Zuo X.等則利用先驗(yàn)的激光雷達(dá)點(diǎn)云地圖為慣性/雙目視覺里程計(jì)提供位置約束信息。LIMO則以視覺里程計(jì)為主，通過數(shù)據(jù)匹配將激光雷達(dá)測量的深度信息賦予視覺特征。將視覺圖像和雷達(dá)點(diǎn)云進(jìn)行配準(zhǔn)后，圖像信息中就包含了激光雷達(dá)測量的稀疏深度信息，基于融合后的感知數(shù)據(jù)構(gòu)建SLAM可以發(fā)揮兩種感知信息的優(yōu)勢。

將多傳感器進(jìn)行緊組合可以更有效地利用測量信息提升系統(tǒng)的魯棒性和精度，近年來基于緊組合的慣性/視覺/激光雷達(dá)系統(tǒng)也受到了研究者的關(guān)注。Yang Y.等基于MSCKF將慣性信息、視覺點(diǎn)特征信息和激光雷達(dá)中提取的平面特征信息進(jìn)行融合。同樣基于MSCKF框架，LIC-Fusion算法則從激光雷達(dá)中提取邊緣特征和平面特征進(jìn)行融合。在后續(xù)的LIC2.0算法中,還可以對視覺/慣性和激光雷達(dá)/慣性的外參進(jìn)行在線估計(jì)。PL-LOAM算法則以視覺點(diǎn)、線特征為感知特征，構(gòu)建單目視覺里程計(jì)；激光雷達(dá)則用于恢復(fù)視覺特征的深度信息，以及校正里程計(jì)尺度漂移。視覺信息和激光雷達(dá)測量的深度信息通過全局的集束優(yōu)化(Bundle Adjustment)進(jìn)行融合。D.Wisth等基于因子圖融合了慣性信息、視覺特征和激光雷達(dá)線特征信息，還使用了預(yù)積分、視覺特征深度信息配準(zhǔn)和激光雷達(dá)數(shù)據(jù)糾偏等模塊，如圖7所示。Zhu Y.等基于小型化的Livox固態(tài)激光雷達(dá)，設(shè)計(jì)了視覺/激光雷達(dá)融合SLAM算法，并實(shí)現(xiàn)了室外的高精度定位與建圖。Shan T.等提出的LVI-SAM中包括慣性視覺里程計(jì)和激光雷達(dá)里程計(jì)兩個(gè)模塊，并且在前端進(jìn)行了感知信息的融合，其中激光雷達(dá)用于恢復(fù)視覺特征深度，而視覺/慣性里程計(jì)輸出則用于輔助點(diǎn)云特征配準(zhǔn)，最后采用因子圖對慣性預(yù)積分信息和兩個(gè)里程計(jì)模塊輸出進(jìn)行了融合。Lin J.等提出的R2LIVE算法則結(jié)合EKF和因子圖進(jìn)行多傳感器融合，其中EKF將慣性信息、視覺角點(diǎn)特征和激光雷達(dá)平面特征點(diǎn)等觀測信息進(jìn)行緊組合，產(chǎn)生高頻的運(yùn)動估計(jì)輸出；而因子圖則以較低的頻率對視覺地圖進(jìn)行校正。在后續(xù)改進(jìn)的R3LIVE算法中，融合SLAM系統(tǒng)的實(shí)時(shí)性進(jìn)一步提升，并且在構(gòu)圖模塊中結(jié)合視覺和激光雷達(dá)信息，可以實(shí)時(shí)構(gòu)建稠密的、帶有顏色的三維地圖。

從算法實(shí)時(shí)性來說，目前提出的組合SLAM方法基本都能夠達(dá)到實(shí)時(shí)性要求(算法耗時(shí)小于激光雷達(dá)的數(shù)據(jù)采集間隔)。一般來說，SLAM算法前端的計(jì)算量要小于后端的計(jì)算量。因此，目前常見的SLAM系統(tǒng)通常將前端和后端分兩個(gè)線程實(shí)現(xiàn)。前端在負(fù)責(zé)特征點(diǎn)提取和數(shù)據(jù)關(guān)聯(lián)的基礎(chǔ)上，也會進(jìn)行雷達(dá)幀間的匹配或圖像特征點(diǎn)與局部子圖的粗匹配，而后端則負(fù)責(zé)某一時(shí)間滑窗內(nèi)的整體位姿優(yōu)化和地圖優(yōu)化。前端雖然會產(chǎn)生一定的位姿漂移，但在后端優(yōu)化線程中則會對漂移進(jìn)行校正，這樣既保證了位姿估計(jì)精度，也提升了算法的實(shí)時(shí)性。部分基于慣性/視覺/激光雷達(dá)融合的SLAM算法總結(jié)如表2所示。

圖7 慣性/視覺/激光雷達(dá)緊組合示意圖[70]Fig.7 Schematic diagram of a tightly integrated inertial-visual-lidar SLAM system[70]

表2 慣性/視覺/激光雷達(dá)組合SLAM

4 基于多傳感器融合的SLAM技術(shù)發(fā)展趨勢

基于多源傳感器融合的SLAM技術(shù)在精度和魯棒性上都已經(jīng)表現(xiàn)出比單個(gè)傳感器SLAM方法更優(yōu)的性能，但仍有以下重點(diǎn)問題值得關(guān)注：

1)對單個(gè)導(dǎo)航源置信度的準(zhǔn)確估計(jì)。多源信息融合的基礎(chǔ)是能夠?qū)γ總€(gè)導(dǎo)航源的置信度進(jìn)行準(zhǔn)確估計(jì)。在復(fù)雜場景下，視覺或激光雷達(dá)則會因周圍環(huán)境特征的缺失而變得不可信，如何在線對視覺和激光雷達(dá)測量信息的置信度進(jìn)行準(zhǔn)確估計(jì)是目前的一個(gè)研究熱點(diǎn)。

2)更加魯棒的感知前端。SLAM的前端是實(shí)現(xiàn)定位與建圖的基礎(chǔ)。一方面可以通過融合更多互補(bǔ)的傳感器，例如事件相機(jī)、固態(tài)激光雷達(dá)及毫米波雷達(dá)等，提升前端感知能力；另一方面，可以利用更為魯棒的特征信息構(gòu)建SLAM系統(tǒng)，例如基于語義特征的SLAM方法近年來就得到了廣泛的關(guān)注。

3)適應(yīng)性更強(qiáng)的后端優(yōu)化方法。現(xiàn)有基于卡爾曼濾波或因子圖的后端融合方法對測量信息的噪聲特征等都有一定的限定條件。在復(fù)雜環(huán)境和復(fù)雜動態(tài)條件下，測量信息中包含未知模型噪聲和大量野值點(diǎn)(outlier)時(shí)，優(yōu)化方法如何適應(yīng)環(huán)境和動態(tài)性變化，保持系統(tǒng)的有效性和精度是值得關(guān)注的問題。目前，基于學(xué)習(xí)和學(xué)習(xí)+模型的信息融合與標(biāo)定方法有望解決這類難題。

4)基于SLAM的無人系統(tǒng)環(huán)境感知技術(shù)。對于無人系統(tǒng)來說，SLAM是一種技術(shù)手段而不是終極目的。如何利用好SLAM提供的幀間相對位姿，實(shí)現(xiàn)基于多幀序貫數(shù)據(jù)融合的環(huán)境感知，以及如何從SLAM系統(tǒng)構(gòu)建出的環(huán)境高精度地圖中挖掘出物體、語義、關(guān)系等高層特征以輔助對場景的實(shí)時(shí)理解，是無人平臺環(huán)境感知系統(tǒng)亟需突破的關(guān)鍵核心技術(shù)。