什么是3D柵格地圖

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

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

上圖是一個 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 信息的；

生成地圖和定位同時進(jìn)行。

自動駕駛SLAM：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

位姿圖的挑戰(zhàn)

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

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

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

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

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

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

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

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

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

舉個例子：左邊是只紅色的兔子，右邊是只藍(lán)色的兔子，現(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é)習(xí)，即利用機器學(xué)習(xí)算法去把非靜態(tài)的物體從地圖里面摳出來。

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