如何使用多類車載傳感器快速可靠地生成點云地圖？

本文介紹了在智行者各產(chǎn)品線廣泛使用的點云地圖構(gòu)建方法，其論文發(fā)表在32nd IEEE Intelligent Vehicles Symposium, 2021會議上。

論文對應(yīng)圖流

1. 動機

高精地圖是L4級別自動駕駛車輛得以自動運行的關(guān)鍵技術(shù)。高精地圖為自動駕駛車輛提供車道級別的導(dǎo)航信息與拓撲信息，無論是乘用車還是低速車都需要這類信息來進行導(dǎo)航。通常，高精地圖是在點云地圖基礎(chǔ)上生成的。隨著自動駕駛車輛的不斷普及，如何快速、高效、可靠地為各類場地生成點云地圖，成為自動駕駛技術(shù)普及的關(guān)鍵。

然而，目前高精地圖方案主要針對城市高速道路。在實際運營自動駕駛車輛時，仍然存在諸多問題。比如，圖商提供的高精地圖方案成本過高，而建圖及時性與地圖覆蓋率明顯不足；乘用車方案則對弱GPS場景、非結(jié)構(gòu)化道路、半室內(nèi)場景支持不夠。另一方面，機器人端使用的在線建圖與定位方案，雖然能夠以很快速度建立地圖，但以柵格地圖為代表的2D地圖無法支持較復(fù)雜的交通規(guī)則，在室外場景也存在可靠性問題。

本文介紹了一種如何使用多類車載傳感器進行融合，快速可靠地生成點云地圖的方法。主要貢獻如下：

我們使用魯棒的因子圖模型來融合各類傳感器數(shù)據(jù)。我們討論了需要定義哪些因子，如何平衡各類因子對優(yōu)化問題的影響。

我們討論在現(xiàn)實場景中面臨的一些建圖問題，例如：如何處理RTK異常數(shù)據(jù)，如何處理激光退化，如何使用回環(huán)檢測保障地圖質(zhì)量，如何在半室內(nèi)或全室內(nèi)場景約束高度和全局姿態(tài)，等等。

示例車輛與運行環(huán)境的點云地圖

一個典型無人車輛點云地圖及其局部形狀

2.方法

建圖流程圖如下：

建圖算法框架整體由前端加后端組成。前端用IMU和輪速計組成航跡推算模塊（Dead Reckoning），然后計算激光里程計。后端由因子圖優(yōu)化以及一些后處理模塊組成。在這個框架中，我們面臨的問題主要來自后端優(yōu)化。下面給出一些數(shù)據(jù)來討論后端優(yōu)化面臨的問題與處理方式。

2.1 因子圖優(yōu)化建模

后端的一大挑戰(zhàn)是在各種傳感器輸入信息中，找到正確且一致的部分，這在弱GPS、非結(jié)構(gòu)化環(huán)境中顯得更加重要。為了能夠靈活處理傳感器的信息，我們使用因子圖優(yōu)化來建模整個問題。因子圖優(yōu)化可以很好地表達多種傳感器的自身特性，同時也允許我們靈活地控制優(yōu)化流程，便于分析哪些因子主導(dǎo)了優(yōu)化，哪些因子作為異常值被排除。

因子圖本身是定義在SE(3)流形上的最小二乘優(yōu)化問題：

基礎(chǔ)的因子包括：RTK因子、DR因子和LiDAR里程計因子。額外可選的因子包括：高度因子、全局旋轉(zhuǎn)因子；不同因子主要用來施加各種場景中的地圖形狀約束。

2.2RTK因子與RTK信息處理

RTK信號在固定解時標稱精度達到cm級別，但其他時刻精度隨自身狀態(tài)變化很大，在單點解時也可能達到10m級別精度。如果受到多徑效應(yīng)影響，RTK接收機算法還可能給出錯誤的狀態(tài)值，即使在固定解狀態(tài)下也可能有很大誤差。

由于我們使用了雙天線，因此將RTK因子視作6自由度的位姿約束：

下圖顯示了同一場景下的RTK狀態(tài)與測量值。

RTK信號不可靠的案例。上：RTK與優(yōu)化軌跡對比圖，藍色為RTK，紅色為優(yōu)化軌跡，背影顏色為RTK狀態(tài)；左下：3D視角對比軌跡；右下：點云地圖。

由圖可見，RTK即使在固定解狀態(tài)時，也可能有各種跳變、慢變情況。本身精度在高度方面明顯劣于水平方面。然而，在建圖時，我們一方面要求地圖與RTK對齊，一方面要處理RTK異常情況，這就要求算法能夠自動化處理RTK異常值。為此，我們設(shè)計了一種兩輪兩階段優(yōu)化的處理邏輯。流程如下：

第一輪優(yōu)化目標是得到基本正確的RTK-DR-Lidar軌跡，并且處理RTK異常值；第二輪目標是通過回環(huán)檢測處理地圖重復(fù)區(qū)域，減少重影的發(fā)生概率。

上圖軌跡的誤差平方直方圖與正常值/異常值分布情況

實際操作當中，RTK異常值閾值可以存在一定的選擇范圍，只需要合理即可。我們既可以選得嚴格一些，把一些誤差較大的讀數(shù)剔除，也可以選的寬松一些，保留一些中間的約束。

2.3 回環(huán)檢測與多機/多軌跡協(xié)同

在第二輪優(yōu)化中，我們利用回環(huán)檢測機制來檢查地圖重復(fù)區(qū)域的約束。

回環(huán)檢測可以有效抑制地圖重影。左：不帶回環(huán)檢測的全局地圖和局部地圖；右：帶上回環(huán)檢測之后的地圖。整體上看，全局點云沒有太大的變化，但局部點云確實被回環(huán)檢測修正了。

同樣，如果地圖由多臺機器采集，或者地圖由多段軌跡拼接而成，也可以利用回環(huán)檢測來修正重復(fù)區(qū)域。

多段軌跡案例。左上：位姿圖；右上：多段軌跡的形狀；下方：拼合后地圖，地圖顏色由軌跡ID著色。

2.4 退化檢測與修正

在廣場等空曠場景，激光匹配由于缺少特征，其運動估計會由于存在額外自由度而出現(xiàn)漂移、抖動等情況。這種現(xiàn)象通常稱為退化。在退化時，我們希望通過其他傳感器的信息來補充激光的估計。

退化案例：上圖是一個廣場案例，廣場中央存在一個雕塑。從廣場邊緣走向中央時，激光出現(xiàn)明顯退化現(xiàn)象。中間：退化分值的分布情況；下方：退化修正前/修正后的軌跡。

在前端檢測到激光退化時，我們在后端主動縮小激光的信息矩陣，從而使DR起到該段軌跡的主導(dǎo)作用，局部地修正此處的激光結(jié)構(gòu)。

2.5 高度約束與全局姿態(tài)約束

對于室內(nèi)的地圖，由于缺少RTK約束，激光里程計在高度估計上可能存在累計誤差。在這種場景中，我們引入場地與車輛的假設(shè)：場地整體為平地，車輛整體姿態(tài)向上。這種約束由高度因子與全局旋轉(zhuǎn)因子實現(xiàn)。

同時，為了允許車輛存在一定程度上的高度變動和姿態(tài)抖動，我們給這兩種約束增加死區(qū)設(shè)定。

高度約束施加后的軌跡與點云（側(cè)視圖）變化

3.對比實驗

我們在采集的數(shù)據(jù)集上對比了一些經(jīng)典的開源激光建圖算法。實驗主要傳感器為：RTK服務(wù)（千尋FindCM）、6軸IMU（美泰SI3200 MEMS）、輪速計、頂部16線雷達（Velodyne-16）。由于現(xiàn)實世界的軌跡真值難以獲取，我們挑選了一部分RTK較好的場景，然后將建圖算法選擇出的RTK正常值作為真值進行比較。

我們主要對比的模塊為：DR、Lego-LOAM、Cartographer、mapping（無RTK）、mapping（有RTK）。其中Lego-LOAM使用了DR作為預(yù)測值，而Cartographer則直接使用IMU讀取。由于Cartographer不支持輪速計輸入，因此沒有給Cartographer輸入輪速。我們對Cartographer使用了默認參數(shù)，沒有針對性的調(diào)參。另外，由于我們將估計軌跡與RTK對比，因此在各個算法中都不輸入RTK數(shù)據(jù)，以保證公平性。

我們選擇了13個場景，按大小分為小型與大型兩類，以絕對位置精度（Absolute Translation Error, ATE）作為評價指標。作為對比，也給出了帶RTK輸入的Mapping位置。

整體而言，我們在大部分場景下比Lego-LOAM和Cartographer取得了更好的精度指標。如果帶有RTK信息，那么我們在大部分場景下能取得10cm精度左右的軌跡精度。

一些代表性地圖

4.軟件的自動化部署與運營

最后，為了保證建圖算法的高效運營，我們將每個建圖步驟抽象為流水線模型，并在云服務(wù)器端部署。流水線模型的每一步可以獨立啟動或中斷，且各個建圖任務(wù)彼此不發(fā)生沖突，保障了多個建圖任務(wù)可以獨立運行，互不干擾。

由于建圖是自動觸發(fā)、運營的，我們將建圖結(jié)果統(tǒng)計成報告，自動發(fā)送給運營人員，實現(xiàn)了整個建圖流程的24小時無人化管理。

責(zé)任編輯：lq6