綜述 | 自動駕駛和SLAM中動態(tài)障礙物濾除方案綜述（1）

在單次激光SLAM或者定位的過程中，動態(tài)障礙物（如：移動的車輛、行人等）會對SLAM或者定位造成兩個方面的負(fù)面影響：點(diǎn)云配準(zhǔn) 和 地圖構(gòu)建

針對點(diǎn)云配準(zhǔn)，動態(tài)障礙物能造成最極端的情況是“當(dāng)一幀激光數(shù)據(jù)中的大部分（甚至100%）點(diǎn)云都是從動態(tài)障礙物上返回的數(shù)據(jù)，那么該幀點(diǎn)云則沒有能力在reference scan（或者reference map）中找到correspondent points，點(diǎn)云的配準(zhǔn)就會失效，但在實(shí)際情況下，點(diǎn)云配準(zhǔn)時的query scan數(shù)據(jù)中受到動態(tài)障礙物影響的點(diǎn)云個數(shù)是“有限的”，由此query scan中大多數(shù)的點(diǎn)云還是可以找到它們的correspondences，少數(shù)的異常量不會產(chǎn)生太大的干擾。另一方面，若是采用特征法進(jìn)行點(diǎn)云配準(zhǔn)，動態(tài)障礙物在預(yù)處理階段也會被剔除，由此基于特征的配準(zhǔn)方法受到動態(tài)障礙物的影響會更小。針對地圖構(gòu)建，動態(tài)障礙物對點(diǎn)云地圖的構(gòu)建的影響在于會在最終生成的地圖中產(chǎn)生“鬼影”，而若簡單使用在第一種情況中提到的特征點(diǎn)云構(gòu)建地圖的話，又會錯誤地濾除掉很多實(shí)際需要的物體信息，導(dǎo)致地圖特征稀疏，難以進(jìn)一步處理，具體的影響可見以下圖示：

最后，一言以蔽之：（1）在點(diǎn)云配準(zhǔn)層面動態(tài)障礙物過多會影響點(diǎn)云配準(zhǔn)的精度和魯棒性（導(dǎo)致配準(zhǔn)出現(xiàn)失效），（2）在地圖構(gòu)建層面時動態(tài)障礙物會在地圖中形成“鬼影”，影響后期地圖的進(jìn)一步處理（路徑規(guī)劃、特征提取、定位，etc）。這里也推薦「3D視覺工坊」新課程《面向自動駕駛領(lǐng)域的3D點(diǎn)云目標(biāo)檢測全棧學(xué)習(xí)路線！(單模態(tài)+多模態(tài)/數(shù)據(jù)+代碼)》。

首先，以移動機(jī)器人（AGV類）的使用場景為例，其基礎(chǔ)使用場景是二維室內(nèi)環(huán)境，所以使用的SLAM算法和機(jī)器人位姿估計器模型都是基于2D平面操作。那基于2D平面場景，SLAM中最常見、最經(jīng)典的算法是gmapping、cartographer或者karto，定位中常用的算法是EKF、AMCL和ICP的組合，導(dǎo)航中的路徑規(guī)劃和運(yùn)動控制就都是基于概率柵格地圖（pgm）的平面規(guī)劃了。由此可見，基于移動機(jī)器人（AGV）的SLAM、定位和導(dǎo)航都離不開概率柵格地圖（pgm），而在概率柵格地圖上最傳統(tǒng)和最有效的去除動態(tài)障礙物的方法就是利用“ray casting算法”的柵格地圖概率的“更新”，而“更新”的最終目的是為了在柵格地圖上不留下動態(tài)障礙物的hit點(diǎn)（此處的更新之所以打上引號，是因?yàn)楦赂怕蕱鸥竦哪Ｐ陀泻芏?，用引號表示泛指）?/p>

圖：失敗的“動態(tài)障礙物hit點(diǎn)去除”（上）和成功的“失敗的動態(tài)障礙物hit點(diǎn)去除” （下）

另一個會使用到SLAM和Localization的工程場景就是低速無人駕駛（或自動駕駛）領(lǐng)域了，不管是使用高精地圖的“傳統(tǒng)”自動駕駛方案，還是“重感知，輕地圖”的自動駕駛策略，都還是或多或少地需要使用到“地圖”（此處的引號代表一系列的泛指，pgm地圖、點(diǎn)云地圖、語義地圖等），高精地圖的各個圖層中（語義圖層也好，矢量圖層也罷）都要求提取出精細(xì)的車道線、箭頭、路標(biāo)等特征并且肯定不會再包含動態(tài)障礙物的信息，定位所使用的點(diǎn)云圖層也是要求盡可能地保證“干凈”無動態(tài)障礙物點(diǎn)云干擾。在自動駕駛領(lǐng)域中的動態(tài)障礙物濾除思量上，一方面區(qū)別于AGV只針對2D的場景的操作，其需要濾除的障礙物是3D物體，另一方面除了地圖上不能留有動態(tài)障礙物信息外，定位過程中的實(shí)時點(diǎn)云也需要盡可能地要求不受其動態(tài)障礙物的影響且其定位過程中的動態(tài)障礙物的濾除最好是在線的、實(shí)時的、高效地，以保證自動駕駛的精度和魯棒性。

至此，不難得出一個結(jié)論：（1）AGV導(dǎo)航中動態(tài)障礙物去除可以使用基于柵格地圖概率更新方式，本質(zhì)上是ray-casting，同時濾除過程可以容忍一定程度的“延遲”（概率更新需要多次、反復(fù)進(jìn)進(jìn)行以達(dá)到濾除的效果），（2）自動駕駛過程中點(diǎn)云數(shù)據(jù)去除動態(tài)障礙物是高精度地圖構(gòu)建的基礎(chǔ)任務(wù)，同時保證在線地、實(shí)時地動態(tài)障礙物濾除能在一定程度上提升定位過程中精度和保證定位的魯棒性。

基于第一節(jié)中的闡述，相信大家都能體會到在SLAM和定位的工程應(yīng)用中動態(tài)障礙物的過濾是一件對SLAM & Localization位姿輸出精度的提升效果有限，但是能極大地提升“用戶體驗(yàn)”的一個模塊，更重要的是動態(tài)障礙物濾除模塊是一個完備導(dǎo)航系統(tǒng)的重要組成成分。

基于第二節(jié)中的描述，也可以發(fā)現(xiàn)在不同的工程應(yīng)用場景下，動態(tài)障礙物濾除的具體要求也不相同，其一，工程師需要根據(jù)具體使用的場景和后續(xù)導(dǎo)航模塊需要的“地圖”來確認(rèn)動態(tài)障礙物的濾除算法，比如2D場景下可以基于2D的概率柵格地圖，利用ray-casting的算法實(shí)現(xiàn)濾除，3D場景下需要基于3D點(diǎn)云地圖，利用特征提取或者聚類的方式去除點(diǎn)云地圖中的動態(tài)障礙物點(diǎn)云；其二，根據(jù)使用場景的不同，動態(tài)障礙物濾除的時機(jī)和時效也存在差異，例如2D場景下基于2D的概率柵格地圖的“更新”方式在濾除動態(tài)障礙物信息的時候可以容忍一定程度的“延遲”，且2D定位時query scan中也不一定需要進(jìn)行動態(tài)障礙物點(diǎn)云的過濾處理，但在3D場景下，更具體些，在自動駕駛的使用場景下，定位過程就已經(jīng)需要盡可能地保證能在線實(shí)時地濾除動態(tài)障礙物信息，在3D地圖的構(gòu)建過程中會有更多精細(xì)的處理以保證“地圖”不被動態(tài)障礙物的信息污染。

至此，考慮“動態(tài)障礙物濾除”算法應(yīng)用的層面，動態(tài)障礙物去除算法模塊的使用分為兩個時機(jī)：（1）在線實(shí)時處理和（2）離線后處理。針對第一個“在線實(shí)時處理”，顧名思義就是在接收到每一幀（或者每一個batch）點(diǎn)云數(shù)據(jù)時，通過相鄰“單位點(diǎn)云數(shù)據(jù)”幀間的對比以分離動態(tài)障礙物點(diǎn)云，然后再直接將“單位點(diǎn)云數(shù)據(jù)”中的動態(tài)障礙物點(diǎn)云濾除，針對第二個時機(jī)：離線處理，此處的“離線”指的是區(qū)分“地圖構(gòu)建”與“位姿估計”這兩個階段，因?yàn)樵陔x線處理的前提是需要點(diǎn)云數(shù)據(jù)及其對應(yīng)的全局位姿信息，故而“離線地圖構(gòu)建”的階段需要在“位姿估計”階段后進(jìn)行，通常是在SLAM過程記錄關(guān)鍵幀的位姿及其對應(yīng)的點(diǎn)云數(shù)據(jù)，輸出給到“離線地圖構(gòu)建”的階段并在此階段內(nèi)對每一幀點(diǎn)云進(jìn)行動態(tài)障礙物點(diǎn)云的去除。

由此，雖然在線實(shí)時的濾除方案可以保證時效性，但往往在線濾除的方法都存在動態(tài)障礙物信息濾除不夠干凈的問題，而離線后處理的方法雖然犧牲了實(shí)時性，但卻可以獲取SLAM周期內(nèi)的完整的軌跡信息作為參考信息并用以來識別動態(tài)障礙物點(diǎn)云，以追求“極致的”動態(tài)點(diǎn)云濾除的準(zhǔn)確性和充分性。

動態(tài)障礙物濾除的算法既可以根據(jù)上文中基于時機(jī)來完成分類，也可以基于原理來進(jìn)行細(xì)分，大體上主要分為三類：

• （1）基于點(diǎn)云分割的濾除算法（segmentation-based method）：基于分割的方法中當(dāng)前比較主流的是基于深度學(xué)習(xí)的語義分割方法，通過前期的點(diǎn)云特征訓(xùn)練，在使用過程中直接標(biāo)注出點(diǎn)云中的動態(tài)障礙物信息部分，在定位和建圖算法中根據(jù)點(diǎn)云中的標(biāo)注直接過濾即可，雖然濾除的內(nèi)容僅限于前期訓(xùn)練得到的模型且容易有假陽性和假陰性的問題，但其方法勝在簡單高效。另一類傳統(tǒng)的點(diǎn)云分割的方法則基于聚類的模式，如快速點(diǎn)特征直方圖 ( fast point feature histograms， FPFH )、基于確定視角下的特征分布直方圖VFH(viewpoint feature histogram)，用以在靜態(tài)聚類中區(qū)分出動態(tài)聚類，此類方法能夠改善計算成本提高實(shí)時性從而得到廣泛研究，再者就是基于區(qū)域（逆）生長以分割混雜著動態(tài)和靜態(tài)點(diǎn)云的目標(biāo)，識別出動態(tài)點(diǎn)云目標(biāo)以便于后期濾除，等。

• （2）基于可見性的濾除算法（visibility-based method）：該類算法中是基于一個物理前提：“光沿直線傳播，如果在同一條光路上存在遠(yuǎn)、近兩個點(diǎn)云，則近側(cè)的點(diǎn)云肯定就是動態(tài)點(diǎn)云”，基于可見性的方法在當(dāng)前的動態(tài)障礙物識別和濾除的研究中十分火熱，且研究的重點(diǎn)除了提升算法效率外，就是研究怎么解決該方案中的局限性：“誤殺問題”和“靜態(tài)點(diǎn)不可見”的問題。其一，誤殺問題包括但不限于點(diǎn)云自干擾、平行點(diǎn)差異導(dǎo)致的假陽性、遮擋點(diǎn)誤判、接觸點(diǎn)假陽性等一系列由于雷達(dá)點(diǎn)云的物理特性引發(fā)的誤殺問題。其二，靜態(tài)點(diǎn)不可見的問題相較于誤殺問題會少見一些，但更加難以解決，例如當(dāng)存在動態(tài)障礙物一直遮擋了部分或者全部的激光雷達(dá)的視線，則雷達(dá)沒有機(jī)會看到這些動態(tài)障礙物后方的靜態(tài)物體信息以至于動態(tài)障礙物點(diǎn)云永遠(yuǎn)不會被識別和濾除掉。

圖：基于可見性的濾除算法的動態(tài)點(diǎn)云識別

• （3）基于柵格的濾除算法（voxel-based or ray-casting based method）：該方案是最為經(jīng)典和傳統(tǒng)的，其方案背后的物理前提與基于可見性的濾除方案一致，不同點(diǎn)在于其具體實(shí)現(xiàn)的時候需要結(jié)合柵格地圖（例如概率占據(jù)柵格、八叉樹柵格，etc），識別與區(qū)分靜態(tài)和動態(tài)點(diǎn)的基本原理：激光點(diǎn)云所落在的柵格處hits計數(shù)+1或者占據(jù)概率上升，激光光束穿過的所有柵格misses計數(shù)+1或者空閑概率上升，通過統(tǒng)計同一柵格中hits和misses數(shù)量差值或者占據(jù)概率，以判斷柵格中包含的點(diǎn)云是否需要濾除。

圖：基于柵格的濾除算法的動態(tài)點(diǎn)云識別