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

論文對應圖流

1. 動機

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

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

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

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

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

示例車輛與運行環境的點云地圖

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

2.方法

建圖流程圖如下：

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

2.1 因子圖優化建模

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

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

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

2.2RTK因子與RTK信息處理

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

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

下圖顯示了同一場景下的RTK狀態與測量值。

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

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

第一輪優化目標是得到基本正確的RTK-DR-Lidar軌跡，并且處理RTK異常值；第二輪目標是通過回環檢測處理地圖重復區域，減少重影的發生概率。

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

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

2.3 回環檢測與多機/多軌跡協同

在第二輪優化中，我們利用回環檢測機制來檢查地圖重復區域的約束。

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

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

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

2.4 退化檢測與修正

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

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

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

2.5 高度約束與全局姿態約束

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

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

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

3.對比實驗

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

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

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

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

一些代表性地圖

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

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

由于建圖是自動觸發、運營的，我們將建圖結果統計成報告，自動發送給運營人員，實現了整個建圖流程的24小時無人化管理。

責任編輯：lq6