什么是3D柵格地圖

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

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

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

SLAM與自動駕駛

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

傳統(tǒng)SLAM與自動駕駛SLAM的區(qū)別

傳統(tǒng) SLAM：

2D；

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

沒有 GPS 信息，很多室內(nèi)場景是收集不到 GPS 信息的；

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

自動駕駛SLAM：

3D，描繪的是一個三維環(huán)境；

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

GPS 信息，室外場景的優(yōu)勢就是可以接收 GPS 信息；

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

3D柵格地圖的挑戰(zhàn)

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

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

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

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

應(yīng)對挑戰(zhàn)：位姿圖 Pose Graph

位姿圖其實是一個優(yōu)化問題

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

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

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

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

位姿圖的挑戰(zhàn)

位姿圖整體的數(shù)學(xué)模型看似簡單，但是存在許多細節(jié)問題需要做優(yōu)化：

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

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

◆ 如何求解非線性優(yōu)化。圖建好之后如何去做非線性的優(yōu)化。

◆ 如何評估優(yōu)化效果。圖是否和真實的環(huán)境一致，以及如何評估結(jié)果滿足要求。

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

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

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

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

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

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

應(yīng)對挑戰(zhàn)：去除非靜態(tài)障礙物體

去除非靜態(tài)障礙體的方法大致有三類：

◆ 通過概率模型去除。

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

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

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