01SLAM在智能駕駛領(lǐng)域的應(yīng)用背景

隨著人工智能和自動駕駛技術(shù)的進步，車輛搭載的輔助駕駛系統(tǒng)可以實現(xiàn)越來越強的功能，如自適應(yīng)巡航、高級變道輔助及自動泊車等。為了在復(fù)雜環(huán)境下對這些高階功能實現(xiàn)支持，需要對車輛自身的位置以及車輛周圍的環(huán)境進行精確感知。同時，目前自動駕駛系統(tǒng)的研究逐漸從L2/L3向L4級別系統(tǒng)過渡，為了實現(xiàn)對算法的訓(xùn)練與測試，對位置與環(huán)境信息的精度也提出了新的要求。

傳統(tǒng)的車載導(dǎo)航與感知系統(tǒng)依托于高精度地圖與全球衛(wèi)星定位系統(tǒng)（Global Navigation Satellite System，GNSS）對位置與環(huán)境信息進行采集，雖然目前典型的GNSS如北斗等已經(jīng)可以實現(xiàn)米級以上精度，在車輛導(dǎo)航領(lǐng)域得到廣泛應(yīng)用，然而民用衛(wèi)星信號的定位精度越來越難以滿足高階駕駛輔助系統(tǒng)（Advanced Driver Assistance System，ADAS）的需求。同時，在市區(qū)和室內(nèi)環(huán)境下，衛(wèi)星信號易受復(fù)雜電磁環(huán)境干擾，并且遮蔽物較多，這些都會嚴重影響定位精度，嚴重時可能造成ADAS系統(tǒng)誤判，產(chǎn)生惡性事故。此外，在高精地圖的應(yīng)用上，也存在政策方面的限制與更新不及時的問題。

為了解決上述需求，原本用于解決自主機器人未知環(huán)境探索的同步建圖與定位（Simultaneous Localization And Mapping，SLAM）技術(shù)開始被應(yīng)用于開發(fā)新型車載數(shù)據(jù)采集系統(tǒng)，以實現(xiàn)對智能駕駛系統(tǒng)的定位、勢態(tài)感知和導(dǎo)航。SLAM技術(shù)主要解決的定位與建圖問題完全可以應(yīng)對車載數(shù)據(jù)采集系統(tǒng)的場景提取問題與高精地圖的更新問題，從而支持對高階智能駕駛系統(tǒng)的開發(fā)、訓(xùn)練與測試，因此成為了最近智能駕駛領(lǐng)域研究的熱點問題之一。

02SLAM原理概述

SLAM技術(shù)本質(zhì)上是一種利用概率論對自身傳感器獲取的信息進行融合優(yōu)化，以實現(xiàn)對自身位置及周圍環(huán)境進行高精度估計的技術(shù)。因此一個SLAM系統(tǒng)首先必須搭載有能感知外界信息的傳感器并且具備相應(yīng)的優(yōu)化算法，SLAM系統(tǒng)的整體結(jié)構(gòu)如圖1所示。

圖1 SLAM系統(tǒng)結(jié)構(gòu)

由圖1可見，一個典型的SLAM系統(tǒng)由前端處理、后端優(yōu)化、定位與建圖算法以及回環(huán)檢測算法四部分組成，接下來分別對這四部分進行簡要介紹。

在前端處理環(huán)節(jié)，主要是通過各種算法將傳感器讀取的信息轉(zhuǎn)化為與位姿和地圖坐標系變化有關(guān)的信息，如IMU預(yù)積分算法將IMU的連續(xù)讀數(shù)進行定積分，從而得到關(guān)于各時刻自主機器人位移與旋轉(zhuǎn)的離散變化量，使得進一步優(yōu)化的計算量大大降低。而激光雷達掃描得到的點云則可以通過線性插值算法結(jié)合IMU數(shù)據(jù)進行去畸變，并通過對環(huán)境中的特征點進行匹配得到移動體的位移與旋轉(zhuǎn)變化量。

在得到了初步的變化量之后，在后端優(yōu)化環(huán)節(jié)，可以通過建立殘差相對約束（因子圖優(yōu)化）或誤差協(xié)方差融合（卡爾曼濾波）的方法對前端中基于不同來源得到的變化量進行優(yōu)化以實現(xiàn)估計誤差最小化。

通常而言，基于卡爾曼濾波的后端優(yōu)化方法具有計算速度較快、實時性較高的優(yōu)點，但由于優(yōu)化只涉及兩個時刻的信息，因此全局精度欠佳。而基于圖優(yōu)化進行的后端優(yōu)化則具有更良好的估計精度，然而由于傳統(tǒng)的圖優(yōu)化算法是對全局進行優(yōu)化，因此在計算負擔上較高。

在實際使用中會根據(jù)系統(tǒng)的具體需求選擇相應(yīng)的算法，并且兩種方法都在向改善自身缺陷的方向進行進一步發(fā)展，目前來看因子圖優(yōu)化的SLAM技術(shù)是未來的發(fā)展方向，因此本文主要以圖優(yōu)化SLAM為核心來進行介紹。

傳統(tǒng)的建圖與定位基本上是通過雷達的點云或雙目攝像機建立的深度圖像進行建圖，并基于優(yōu)化后的系統(tǒng)位姿進行定位的，不過在一些目前較先進的多傳感器緊耦合算法會結(jié)合不同傳感器的優(yōu)勢進行建圖，以實現(xiàn)更好的效果，如LVI-SAM[1]使用激光雷達的點云特征同視覺傳感器的特征點進行匹配，以賦予視覺傳感器深度信息，實現(xiàn)單目攝像頭立體建圖。R3LIVE[2]則將視覺傳感器的畫面建立為體素信息賦予激光雷達的點云地圖，以實現(xiàn)三維彩色高精度建圖。

最后的回環(huán)檢測環(huán)節(jié)則是一種用于校正傳感器退化及漂移的手段，通過對已經(jīng)過的路段再次進行匹配來實現(xiàn)對長距離運行時逐漸發(fā)生的漂移誤差進行消除。

03幾種典型的SLAM算法

1、LOAM

作為激光SLAM的開山之作，LOAM[3]算法設(shè)計了經(jīng)典的激光雷達點云特征匹配算法。這種方法靠特征點與周圍相鄰點之間的弧度將特征點分為角點與面點，如式（1）所示：

公式（1）

其中，c為特征點弧度，S為當前點云集合，X為各點的位置信息。之后通過Kd-Tree與上一幀的點云進行匹配，并根據(jù)匹配后的誤差建立如式（2）、（3）所示目標函數(shù)：

公式（2）

公式（3）

將系統(tǒng)中所有節(jié)點的目標函數(shù)相加并通過最小二乘法進行最小化，即可實現(xiàn)對系統(tǒng)位姿的估計。這種方法為之后基于激光的SLAM算法奠定了基礎(chǔ)，并被大多數(shù)后續(xù)研究所采用。

2、LIO-SAM

在傳統(tǒng)的基于圖優(yōu)化的SLAM算法中，為了降低系統(tǒng)的計算負擔通常使用滑窗優(yōu)化來減少需要優(yōu)化的節(jié)點。然而，由于回環(huán)檢測涉及到對過去不相鄰的節(jié)點進行優(yōu)化，如果使用傳統(tǒng)的滑窗圖優(yōu)化方法會導(dǎo)致優(yōu)化矩陣的大面積重構(gòu)，大幅增加系統(tǒng)的計算負擔。

圖2 回環(huán)檢測導(dǎo)致的因子圖重構(gòu)

LIO-SAM[4]的貢獻在于引入了iSAM2優(yōu)化算法，將因子圖重構(gòu)為增量貝葉斯樹，使得在引入新的回環(huán)因子時不必對因子圖整體進行重構(gòu)，僅需要將與回環(huán)因子直接相連的節(jié)點刪除并重構(gòu)即可。這使得回環(huán)檢測得以被引入快速因子圖優(yōu)化SLAM系統(tǒng)，大大提升了系統(tǒng)在長時間運行時的穩(wěn)定性與精度。

3、LVI-SAM

在LIO-SAM的基礎(chǔ)上，Shan等人將視覺傳感器引入系統(tǒng)，提出了LVI-SAM。由于視覺傳感器提供的地圖紋理，這套系統(tǒng)相對于過去的LIO-SAM在建圖方面存在巨大的提升。

通常環(huán)境下系統(tǒng)運行在視覺傳感器為基礎(chǔ)的算法上，并通過構(gòu)造超球面的方法將雷達點云與視覺特征點進行匹配，實現(xiàn)對視覺紋理賦予深度信息。但視覺傳感器出現(xiàn)較大退化時，則切換到雷達系統(tǒng)進行建圖，使得這種方法在精度下限上不亞于過去的LIO-SAM算法。

本算法同樣采用iSAM2進行后端優(yōu)化，使得其可以很方便的引入回環(huán)檢測機制而不增加額外的計算負擔。

圖3 深度點與視覺特征匹配

4、R2LIVE

針對圖優(yōu)化和卡爾曼濾波在SLAM領(lǐng)域的固有缺陷和優(yōu)點，香港大學(xué)的團隊另辟蹊徑將兩種方法結(jié)合在一起，提出了R2LIVE算法[5]。

這種方法采用計算速度較快的卡爾曼濾波對刷新頻率較高的機器人本身位姿進行后端優(yōu)化，實現(xiàn)系統(tǒng)位姿的實時更新。

同時利用精度較高的因子圖優(yōu)化方法對變化較小的地圖特征點位置進行優(yōu)化，從而實現(xiàn)高精度建圖。最終兩者結(jié)合實現(xiàn)了對移動機器人高精度實時定位的目的。

5、R3LIVE

作為R2LIVE的后續(xù)，香港大學(xué)的團隊提出了R3LIVE。這套方法重點在于實時三維建圖，因此舍棄了計算速度較慢的圖優(yōu)化方法，完全采用卡爾曼濾波進行后端優(yōu)化。其主要貢獻在于將視覺紋理轉(zhuǎn)換為體素點，并將其顏色渲染賦予激光系統(tǒng)構(gòu)建的三維點云地圖，最終得到了具有彩色紋理的高精實時三維地圖。根據(jù)其實驗驗證，最終建圖精度高于LVI-SAM算法。

上文介紹的幾種算法各有優(yōu)劣，如LOAM算法作為ICP點云匹配的經(jīng)典之作，至今仍然被視為激光雷達SLAM領(lǐng)域最優(yōu)秀的算法之一，LIO-SAM創(chuàng)造性的將iSAM2融入優(yōu)化算法以實現(xiàn)對回環(huán)檢測的快速處理，港大的R-live系列算法則專注于對彩色3D建圖的研究，具有極強的制圖功能且用途多樣，LVI-SAM則在LIO-SAM的基礎(chǔ)上引入了另一套視覺系統(tǒng)，在定位精度上得到了提升且具備了彩色建圖功能。

在文獻[6]中對現(xiàn)有的一些經(jīng)典算法在同一數(shù)據(jù)集下的性能進行了評估，評估顯示在未經(jīng)過特殊優(yōu)化及適配的條件下，傳統(tǒng)的LOAM算法反而具有最好的定位精度，這可能是由于傳感器時間戳與算法本身的設(shè)置不匹配所導(dǎo)致的信息跳變造成的。然而，這同時也顯示了傳統(tǒng)的LOAM由于其較強的魯棒性使其仍然具有實用價值。

圖4 M2DGR數(shù)據(jù)集下各SLAM算法定位精度[6]

04SLAM技術(shù)在智能駕駛中應(yīng)用的未來構(gòu)想

目前已經(jīng)存在諸多成熟的SLAM方案，涵蓋了激光、視覺、慣性導(dǎo)航與地圖匹配等各種傳感器類型。然而這些SLAM算法主要用于解決未知環(huán)境下移動機器人自主探索所提出，雖然其針對的問題同智能駕駛領(lǐng)域所面臨的定位與建圖問題存在重合，但是仍然需要從智能駕駛的實際需求出發(fā)，在現(xiàn)有成熟SLAM算法的基礎(chǔ)上進行改進。

例如，在傳統(tǒng)SLAM算法中廣泛采用回環(huán)檢測技術(shù)用于消除傳感器退化與漂移誤差，然而在汽車駕駛領(lǐng)域，很少會存在回環(huán)行駛的狀況，使得回環(huán)檢測難以被觸發(fā)。

相對的，通常情況下智能駕駛主要面對的難題在于車流與人流量較大，突然情況較多的鬧市環(huán)境，這種環(huán)境通常具有完備的地圖可以進行匹配，因此在自主機器人探索SLAM領(lǐng)域較少使用的地圖匹配算法（如GR-SLAM[7]算法）反而可以在智能駕駛SLAM領(lǐng)域大放異彩。

此外，得益于汽車較大的承載力，其所搭載的計算系統(tǒng)通常要強于無人機與自主機器人所搭載的單片機或嵌入式系統(tǒng)，因此具備充足的計算資源以在前端環(huán)節(jié)融入深度學(xué)習(xí)算法，改善視覺系統(tǒng)的紋理分辨率或?qū)す庀到y(tǒng)的畸變進行補償以替代目前常用的線性插值方法。