解讀多傳感器融合SLAM技術的發(fā)展趨勢

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

(國防科技大學智能科學學院，長沙 410073)

摘 要：同時定位與建圖(SLAM)技術已廣泛應用于各類自主移動平臺中，其中視覺SLAM和激光雷達SLAM是兩種主要的SLAM技術方案。然而，視覺SLAM系統(tǒng)易受視覺環(huán)境變化的影響，而激光雷達SLAM系統(tǒng)則在結構單一等環(huán)境中會出現(xiàn)精度退化甚至失效的情況。隨著智能移動平臺應用場景的不斷拓展，對SLAM系統(tǒng)的精度和魯棒性等提出了更高要求，將多種具有互補性的傳感器進行融合是提升SLAM系統(tǒng)性能的有效途徑。據(jù)此，聚焦慣性/視覺/激光雷達多傳感器融合SLAM技術，從多傳感器標定和多源數(shù)據(jù)融合兩個主要方面進行綜述，最后對多傳感器融合SLAM技術的發(fā)展趨勢進行了展望。

作者簡介：毛軍(1989-)，男，博士，講師，主要從事視覺導航、組合導航、仿生導航方面的研究。

通信作者：付浩(1986-)，男，博士，副研究員，主要從事地面無人平臺環(huán)境感知方面的研究。

0 引言

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

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

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

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

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

1 SLAM簡述

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

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

1.1 SLAM模型簡介

設SLAM問題中待估計的機器人運動參數(shù)為X0:k={x0,x1,…,xk}，其中xk表示k時刻載體的位姿參數(shù)；設SLAM構建的地圖可表示為一組地標的集合M={m1,m2,…,mn}，其中mn表示第n個地標在地圖中的位置。機器人在移動過程中可以通過傳感器對地標進行觀測，這些觀測的集合表示為Z0:k={z0,z1,…,zk}；而驅使機器人從xk-1到xk轉移的控制量為uk，在移動過程中所有控制量的集合表示為U0:k={u0,u1,…,uk}。

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

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

圖2 SLAM算法架構圖[3]
Fig.2 Scheme of the SLAM algorithm

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

(1)

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

(2)

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

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

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

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

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

圖3 利用慣導校正激光雷達幀內(nèi)畸變的示意圖
(當車體靜止時，雷達在地面上掃描的數(shù)據(jù)為一個圓形
(如左圖所示)；當車體向前運動時，這個圓形會產(chǎn)生一定的
畸變(如右圖所示)；在雷達采樣周期內(nèi)(通常為100ms)，
可以接收到多個慣導數(shù)據(jù)，利用這些慣導數(shù)據(jù)可以對雷達
數(shù)據(jù)進行幀內(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)

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

綜上所述，IMU、相機和激光雷達之間具有較強的互補性，將慣性/視覺/激光雷達進行融合是提升SLAM系統(tǒng)精度和魯棒性的重要途徑。然而，將這三種不同的傳感器進行融合首先需要對傳感器進行標定，然后還需要對SLAM算法進行設計，以有效融合三種異源信息。

2 慣性/視覺/激光雷達標定方法

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

2.1 時間同步

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

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

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

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

2.2 相機/激光雷達標定

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

圖4 相機/激光雷達標定示意圖[18-19]
Fig.4 Illustration of camera-lidar calibration[18-19]

記視覺傳感器為c，其內(nèi)參標定矩陣為K；激光雷達傳感器為l，二者剛性固聯(lián)于平臺載體上。手眼標定問題的實質(zhì)就是求解兩個傳感器坐標系間的相對位姿變換Trc_l=[Rc_l,tc_l]，該變換關系可以將雷達坐標系l中的一點lPi變換至相機像素坐標系cpi

cpi=π(K*Trc_l*lPi)

(3)

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

(4)

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

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

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

2.3 慣性/相機標定

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

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

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

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

2.4 慣性/激光雷達標定

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

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

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

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

3 基于慣性/視覺/激光雷達融合的SLAM技術

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

3.1 慣性/激光雷達組合SLAM

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

表1 慣性/激光雷達組合SLAM

Tab.1 Lidar-inertial SLAM systems

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

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

3.2 慣性/視覺/激光雷達組合SLAM技術

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

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

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

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

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

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

表2 慣性/視覺/激光雷達組合SLAM

Tab.2 Inertial-visual-lidar SLAM systems

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

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

1)對單個導航源置信度的準確估計。多源信息融合的基礎是能夠對每個導航源的置信度進行準確估計。在復雜場景下，視覺或激光雷達則會因周圍環(huán)境特征的缺失而變得不可信，如何在線對視覺和激光雷達測量信息的置信度進行準確估計是目前的一個研究熱點[76-78]。

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

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

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

編輯：黃飛

?