什么是3D柵格地圖

3D柵格地圖的定義認為一個真實的三維世界可以用一個概率模型來表示。

比如像今天這個會場，我們可以將它離散化成一個個立體方格，每個方格里會存儲一個概率，這個概率即方格空間不為空的概率。

上圖是一個 3D 柵格地圖，從數學的角度講，它并不是一個連續的描述，而是離散化后的地圖，只會保留一些概率值和屬性。這里展示的是一張厘米級精度的柵格地圖，我們可以很清楚看到周圍的環境，包括樹、建筑等等。

SLAM與自動駕駛

那么，如何制作高精地圖呢？傳統上有一種地圖制作方法叫SLAM（即時定位與地圖構建）。它指運動物體根據傳感器的信息，一邊計算自身位置，一邊構建環境地圖的過程。目前，SLAM的應用領域主要有機器人、虛擬現實和增強現實。其用途包括傳感器自身的定位，以及后續的路徑規劃、場景理解。

傳統SLAM與自動駕駛SLAM的區別

傳統 SLAM：

2D；

視覺傳感器，單靠攝像頭，在某個空間轉來轉去，最終通過算法來建模；

沒有 GPS 信息，很多室內場景是收集不到 GPS 信息的；

生成地圖和定位同時進行。

自動駕駛SLAM：

3D，描繪的是一個三維環境；

激光傳感器，需要達到比較高的精度；

GPS 信息，室外場景的優勢就是可以接收 GPS 信息；

離線處理，數據不需要在線處理，前面介紹過，如果選擇在線，某些場景算法無法得到良好的處理，所以需要抽取一些信息來做離線的驗證。

3D柵格地圖的挑戰

相比于傳統 SLAM，我們來看下有哪些問題是3D柵格地圖需要解決的：

全局 GPS 信息并不總是好的。上圖左下角是3D柵格地圖的底圖，里面包含一條當時車輛的行駛軌跡。你會發現依據 GPS 的這條行駛軌跡，這輛車已經開到馬路牙子（路肩）上了，但實際情況并非如此。因此，當拿到這樣一份地圖數據時，我們需要從算法層做出優化，并且做好細節。

不同時間段的數據如何align（校準）。一般來說，地圖的采集不可能一次完成，因此，從算法層面上看，3D柵格地圖需要處理把不同時間段的數據校準到一起的問題。

去除非靜態的物體。我們所建立的是一個概率的模型，3D柵格地圖只需要保留一些靜態的物體以輔助感知系統，許多非靜態的物體都需要從地圖中去除。

應對挑戰：位姿圖 Pose Graph

位姿圖其實是一個優化問題

整體上我們認為位姿圖（Pose Graph）是一個優化問題。假設將路線采集以時間去劃分——比如每隔4分鐘采一個點，最后能得到不同的位置的環境。舉個例子：在一個大廳里，我現在站在某點，一分鐘后我可能站在另一處，再繞一圈的話后站在門口，通過把整個GPS獲得的位置信息離散化之后，將我在每一點所看到的周圍的環境直接拼起來，就可能得到一個三維的地圖，因為每個點看到的角度都是不一樣的。

但因為原始位置可能跟真實位置有偏差，直接拼起來常常導致很多問題。所以我們把這些點離散化之后，希望能夠把所有點都對齊調整到準確的位置。那怎么確保最終的結果是正確的？

對整個圖來說， 除了頂點之外，還有邊。我們希望通過邊的約束將點調好，邊的約束你可以認為是個相對約束，假設我知道下個點的準確位置，那么便可倒推另一個點的準確位置。也就是說，在有準確的相對位置的基礎上，輔以一些比較少的絕對位置，就可以得到一個全局的準確位置。

總的來說，位姿圖的優化目標是把整個圖離散化到一個個點之后，通過建立一些邊的約束，最終通過優化某些點的位置來滿足邊的約束。位姿圖也就成為了一個優化問題。

位姿圖的挑戰

位姿圖整體的數學模型看似簡單，但是存在許多細節問題需要做優化：

◆ 如何選擇頂點。比如需要考慮是不是毫秒的點都要加進去，GPS 信號不好時的點要不要加進去。

◆ 如何保證邊的準確性。一個頂點到另一個頂點的相對位置，需要通過什么方式去建立。

◆ 如何求解非線性優化。圖建好之后如何去做非線性的優化。

◆ 如何評估優化效果。圖是否和真實的環境一致，以及如何評估結果滿足要求。

◆ 效率和資源。對于工程來講，數據量這么大，你需要考慮怎么設計工程算法以保證效率和資源。比如說，考慮是不是需要在算法基礎上進行定制優化，如何用 GPU 或者分布式的計算方式。

我想通過上圖右邊的例子解釋位姿圖——左側原始的圖展示的是有一個物體來回在球面上運動旋轉，通過在邊與邊之間建立相對約束，進行優化后，你會發現整個環境的重建成了右側一個比較理想、完整的球體，概括來說，我們的工作就是需要把左側的圖來變成右側的圖。

迭代最近點算法：保證邊的約束

我們前面提到位姿圖需要保證邊的約束，為了計算出這個約束，業界用的比較多的傳統經典算法叫做 ICP（Iterative Closest Point algorithm）——迭代最近點算法。

舉個例子：左邊是只紅色的兔子，右邊是只藍色的兔子，現實中這兩只兔子的位置是一樣的，但在圖片里兩只兔子位置不一樣，說明位置存在偏差。也就是說，輸入是兩組點云 A 和 B，輸出是兩組點云之間在空間上的旋轉和平移。

具體算法的思路大致是：找到點云集合 A 中的每個點在集合 B 中的對應點，通過求解最佳的剛體變換，不斷的迭代優化，最終得到一個收斂解。

應對挑戰：去除非靜態障礙物體

去除非靜態障礙體的方法大致有三類：

◆ 通過概率模型去除。

◆ 點云處理，單純的概率模型是不夠的，例如有時候小區路邊經常停著車，而某些時候車是被開走的。數據采集時，恰好車停在路邊，后續那就需要依賴離線點云處理。

◆ 機器學習，即利用機器學習算法去把非靜態的物體從地圖里面摳出來。

上圖是一個簡單的去除非靜態障礙物前后對比圖，處理之前，你會發現右側道路有一些車的輪廓，當從空間上去除之后，地圖變得清晰許多