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

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

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

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

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

0 引言

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

SLAM最早于20世紀(jì)80年代被提出，早期的SLAM系統(tǒng)主要是基于單線激光雷達(dá)傳感器構(gòu)建的，且僅適用于在二維平面中運動的載體。隨著激光雷達(dá)技術(shù)的發(fā)展，基于激光雷達(dá)的3D SLAM系統(tǒng)得到迅速發(fā)展。目前，性能較好的激光雷達(dá)可以實現(xiàn)百米外厘米級的測距精度，支撐了一系列高精度SLAM應(yīng)用場景。然而，激光雷達(dá)SLAM仍面臨眾多難題，例如在自運動較快時雷達(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)點。此外，視覺是對環(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)僅基于載體運動產(chǎn)生的慣性信息就可以對速度、位置和姿態(tài)進(jìn)行全參數(shù)估計。慣性導(dǎo)航估計的運動參數(shù)可用于校正雷達(dá)數(shù)據(jù)的畸變、補(bǔ)償單目視覺缺失的尺度信息等；而激光雷達(dá)SLAM和視覺SLAM測量的載體運動則可以校正慣性導(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等[1]對SLAM算法框架進(jìn)行了介紹；J.Fu-entes-Pacheco等[2]則對視覺SLAM技術(shù)的主要方法進(jìn)行了綜述。2015年，C.Cadena等[3]在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ā)，分析其對多傳感器融合的需求；然后從多傳感器時空配準(zhǔn)和多傳感器信息融合兩方面對慣性/激光/視覺組合SLAM技術(shù)進(jìn)行介紹，并對基于多傳感器融合的SLAM技術(shù)進(jìn)行展望。

1 SLAM簡述

移動機(jī)器人在未知環(huán)境中運動時，可以通過SLAM系統(tǒng)構(gòu)建環(huán)境地圖，同時利用構(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ī)器人運動參數(shù)為X0:k={x0,x1,…,xk}，其中xk表示k時刻載體的位姿參數(shù)；設(shè)SLAM構(gòu)建的地圖可表示為一組地標(biāo)的集合M={m1,m2,…,mn}，其中mn表示第n個地標(biāo)在地圖中的位置。機(jī)器人在移動過程中可以通過傳感器對地標(biāo)進(jìn)行觀測，這些觀測的集合表示為Z0:k={z0,z1,…,zk}；而驅(qū)使機(jī)器人從xk-1到xk轉(zhuǎn)移的控制量為uk，在移動過程中所有控制量的集合表示為U0:k={u0,u1,…,uk}。

SLAM就是根據(jù)觀測Z0:k和控制輸入U0:k，估計運動軌跡X0:k和地標(biāo)位置M的過程。在SLAM算法框架中，對觀測Z0:k的提取部分通常被稱為SLAM的前端(front-end)；而利用Z0:k和U0:k對運動軌跡和地標(biāo)位置進(jìn)行估計的部分被稱為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中，通常直接采用三維點云描述環(huán)境；在視覺SLAM中，則可以采用像素的光度或視覺特征描述地標(biāo)。建立幀間觀測的關(guān)聯(lián)也是前端算法的重要組成部分，數(shù)據(jù)關(guān)聯(lián)算法與地標(biāo)描述方法緊密聯(lián)系。在激光雷達(dá)SLAM系統(tǒng)中，廣泛使用迭代最近點(Iterative Closest Point, ICP)[5]、正態(tài)分布變換(Normal Distributions Trans-form, NDT)[6]等方法進(jìn)行幀間點云配準(zhǔn)[7]。在視覺SLAM系統(tǒng)中,則主要采用光流跟蹤[8]、特征匹配[9]等方式建立幀間觀測關(guān)聯(lián)。

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

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

(1)

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

(2)

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

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

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

圖3 利用慣導(dǎo)校正激光雷達(dá)幀內(nèi)畸變的示意圖
(當(dāng)車體靜止時，雷達(dá)在地面上掃描的數(shù)據(jù)為一個圓形
(如左圖所示)；當(dāng)車體向前運動時，這個圓形會產(chǎn)生一定的
畸變(如右圖所示)；在雷達(dá)采樣周期內(nèi)(通常為100ms)，
可以接收到多個慣導(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)特征受運動物體遮擋時，僅利用點云的幾何信息難以與地圖進(jìn)行配準(zhǔn)。將相機(jī)與激光雷達(dá)配準(zhǔn)后，可以將相機(jī)感知的視覺特征信息賦予雷達(dá)點云，然后利用特征的相似性輔助點云配準(zhǔn)。相機(jī)本質(zhì)上是一種角度測量傳感器，在成像過程中地標(biāo)特征的距離信息丟失，因此在視覺SLAM系統(tǒng)中需要根據(jù)多幀或多目的觀測重構(gòu)地標(biāo)的三維位置信息，這給SLAM算法帶來了負(fù)擔(dān)。在光照、視角等發(fā)生強(qiáng)烈變化時，視覺SLAM前端也面臨失效風(fēng)險。將激光雷達(dá)測量的距離信息賦予視覺特征可實現(xiàn)三維視覺功能，同時在視覺環(huán)境發(fā)生變化時，激光雷達(dá)測量的三維結(jié)構(gòu)仍能保證SLAM前端的正常運行。

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

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

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

2.1 時間同步

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

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

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

對于激光雷達(dá)來說，其數(shù)據(jù)通常都是通過用戶數(shù)據(jù)包協(xié)議(User Datagram Protocol，UDP)進(jìn)行廣播。每個數(shù)據(jù)包都包含一個時間戳，該時間戳通常來自于激光雷達(dá)的內(nèi)部時鐘，可以通過外接衛(wèi)星導(dǎo)航接收機(jī)對內(nèi)部時鐘進(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)定是一個典型的手眼標(biāo)定問題，如圖4所示。

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

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

cpi=π(K*Trc_l*lPi)

(3)

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

(4)

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

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

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

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

相較于相機(jī)與激光雷達(dá)都是對地標(biāo)特征的測量，IMU和相機(jī)的測量信息之間差異較大。為標(biāo)定IMU/相機(jī)之間的外參，J.Alves等[25]提出了將固聯(lián)的IMU和相機(jī)固定于一個單擺上，然后以測量得到的單擺運動作為參考進(jìn)行標(biāo)定。然而，該方法需要借助外部設(shè)備，對標(biāo)定設(shè)備具有一定的要求，且標(biāo)定效率不高。僅利用IMU/相機(jī)自身的測量信息進(jìn)行外參標(biāo)定，則擺脫了對外部測量設(shè)備的需求。F.M.Mirzaei等[26]和M.Fleps等[27]分別提出了一種基于EKF和非線性優(yōu)化的自標(biāo)定方法。這種方法僅使用了一個棋盤格標(biāo)定板，然后通過采集相機(jī)面對標(biāo)定板運動的圖像和慣性信息就可以對外參進(jìn)行準(zhǔn)確估計，如圖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)運動激勵才能保障算法的收斂性。先治文[28]提出了一種多位置靜態(tài)標(biāo)定方法，不需要采集運動信息就能對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ù)是時間對齊的。然而，不同的傳感器時間響應(yīng)不同，對IMU和相機(jī)數(shù)據(jù)之間的時間差異進(jìn)行標(biāo)定，對于SLAM算法的狀態(tài)估計一致性具有重要意義。相較于設(shè)計高精度的同步硬件[30]，利用軟件算法標(biāo)定時間差異具有不需要額外硬件開銷的優(yōu)點。P.Furgale等[31]提出了一種時空聯(lián)合標(biāo)定算法，可實現(xiàn)對相機(jī)/IMU外參和傳感器時間差異進(jìn)行聯(lián)合標(biāo)定，相關(guān)算法已形成開源工具箱，被SLAM研究者和工程師廣泛使用[32]。

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

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

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

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

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

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

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

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

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

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

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

Tab.1 Lidar-inertial SLAM systems

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

相較于松組合系統(tǒng)，慣性/激光雷達(dá)緊組合SLAM系統(tǒng)在后端算法中直接利用點云觀測信息優(yōu)化定位與建圖結(jié)果；同時，在前端點云匹配中也會利用組合系統(tǒng)輸出的高頻運動信息進(jìn)行補(bǔ)償和初始化。MC2SLAM[41]利用慣性信息對運動軌跡進(jìn)行預(yù)測，并構(gòu)建幀間相對運動因子；在優(yōu)化過程中則采用因子圖對先驗殘差、雷達(dá)里程計殘差和幀間相對運動殘差進(jìn)行聯(lián)合優(yōu)化，其測試結(jié)果在KITTI數(shù)據(jù)集上展示出比LOAM算法更高的精度。LIOM[42]將定位與建圖設(shè)置為兩個并行的節(jié)點，在定位模塊中采用基于滑動窗口的因子圖優(yōu)化方法進(jìn)行信息融合。為提升組合算法運行效率，LIOM還采用了慣性預(yù)積分理論[52]以避免對慣性的重復(fù)積分運算。LINS[43]采用迭代卡爾曼濾波器對慣性/激光雷達(dá)進(jìn)行緊組合，雖然其精度與LIOM相當(dāng)，但LINS的運行速度卻明顯快于LIOM。在同一計算設(shè)備中的測試表明，LIOM處理一幀雷達(dá)數(shù)據(jù)的時間大于100ms，而LINS卻小于30ms。LIO-SAM[44]利用iSAM[53]平滑優(yōu)化工具箱，將慣性、激光雷達(dá)和全球定位系統(tǒng)(Global Positioning System，GPS)進(jìn)行融合，并且采用一種局部地圖匹配策略替換LOAM和LIOM中的全局地圖匹配策略，使得LIO-SAM具有更高的精度和更短的計算耗時。與多數(shù)慣性/激光雷達(dá)SLAM在前端算法中提取線、面等特征不同，F(xiàn)AST-LIO2[51]直接將激光雷達(dá)測得的點與地圖進(jìn)行匹配，并且采用K-D樹來保障對地圖的快速更新。針對新型的固態(tài)激光雷達(dá)，LiLi-OM[45]設(shè)計了一種從固態(tài)激光雷達(dá)中提取特征的方法，并設(shè)計了一種針對激光雷達(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)建里程計，然后通過松組合的方式將兩個模塊的輸出結(jié)果進(jìn)一步融合[54-57]。SuperOdometry[54]則以IMU為中心構(gòu)建了慣性/視覺里程計和慣性/激光雷達(dá)里程計分別對運動參數(shù)進(jìn)行估計，然后利用位姿圖優(yōu)化方法將兩個里程計輸出進(jìn)行融合(如圖6所示)。構(gòu)建兩個里程計模塊的松組合方法雖然降低了算法設(shè)計的復(fù)雜度，但未能發(fā)揮視覺和雷達(dá)在前端感知中的互補(bǔ)性。在VIL-SLAM[55]中，采用一個慣性/雙目視覺里程計進(jìn)行運動估計，并利用里程計輸出的運動信息和激光雷達(dá)點云信息進(jìn)行建圖；在閉環(huán)優(yōu)化中，首先采用視覺信息檢測閉環(huán)，然后利用雷達(dá)點云配準(zhǔn)解算相對運動信息，進(jìn)而對運動軌跡和環(huán)境地圖進(jìn)行優(yōu)化。在Zhang J.等提出的方法[58-59]中，包含視覺/慣性里程計和激光雷達(dá)里程計兩個模塊，在視覺里程計中激光雷達(dá)測量的深度信息和視覺特征進(jìn)行了配準(zhǔn)；而視覺里程計的輸出也用于輔助激光雷達(dá)點云的幀間匹配。然而，同時運行視覺和激光雷達(dá)兩個里程計算法模塊需要更多的計算資源，算法效率不高。

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

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

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

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

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

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

Tab.2 Inertial-visual-lidar SLAM systems

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

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

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

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

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

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

編輯：黃飛

?