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

論文對(duì)應(yīng)圖流

1. 動(dòng)機(jī)

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

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

本文介紹了一種如何使用多類(lèi)車(chē)載傳感器進(jìn)行融合，快速可靠地生成點(diǎn)云地圖的方法。主要貢獻(xiàn)如下：

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

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

示例車(chē)輛與運(yùn)行環(huán)境的點(diǎn)云地圖

一個(gè)典型無(wú)人車(chē)輛點(diǎn)云地圖及其局部形狀

2.方法

建圖流程圖如下：

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

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

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

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

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

2.2RTK因子與RTK信息處理

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

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

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

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

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

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

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

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

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

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

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

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

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

2.4 退化檢測(cè)與修正

在廣場(chǎng)等空曠場(chǎng)景，激光匹配由于缺少特征，其運(yùn)動(dòng)估計(jì)會(huì)由于存在額外自由度而出現(xiàn)漂移、抖動(dòng)等情況。這種現(xiàn)象通常稱(chēng)為退化。在退化時(shí)，我們希望通過(guò)其他傳感器的信息來(lái)補(bǔ)充激光的估計(jì)。

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

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

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

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

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

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

3.對(duì)比實(shí)驗(yàn)

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

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

我們選擇了13個(gè)場(chǎng)景，按大小分為小型與大型兩類(lèi)，以絕對(duì)位置精度（Absolute Translation Error, ATE）作為評(píng)價(jià)指標(biāo)。作為對(duì)比，也給出了帶RTK輸入的Mapping位置。

整體而言，我們?cè)诖蟛糠謭?chǎng)景下比Lego-LOAM和Cartographer取得了更好的精度指標(biāo)。如果帶有RTK信息，那么我們?cè)诖蟛糠謭?chǎng)景下能取得10cm精度左右的軌跡精度。

一些代表性地圖

4.軟件的自動(dòng)化部署與運(yùn)營(yíng)

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

由于建圖是自動(dòng)觸發(fā)、運(yùn)營(yíng)的，我們將建圖結(jié)果統(tǒng)計(jì)成報(bào)告，自動(dòng)發(fā)送給運(yùn)營(yíng)人員，實(shí)現(xiàn)了整個(gè)建圖流程的24小時(shí)無(wú)人化管理。

責(zé)任編輯：lq6