高精地圖之3D柵格地圖的應(yīng)用

什么是3D柵格地圖

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

比如像今天這個(gè)會(huì)場(chǎng)，我們可以將它離散化成一個(gè)個(gè)立體方格，每個(gè)方格里會(huì)存儲(chǔ)一個(gè)概率，這個(gè)概率即方格空間不為空的概率。

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

SLAM與自動(dòng)駕駛

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

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

傳統(tǒng) SLAM：

2D；

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

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

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

自動(dòng)駕駛SLAM：

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

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

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

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

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

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

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

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

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

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

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

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

但因?yàn)樵嘉恢每赡芨鎸?shí)位置有偏差，直接拼起來常常導(dǎo)致很多問題。所以我們把這些點(diǎn)離散化之后，希望能夠把所有點(diǎn)都對(duì)齊調(diào)整到準(zhǔn)確的位置。那怎么確保最終的結(jié)果是正確的？

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

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

位姿圖的挑戰(zhàn)

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

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

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

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

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

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

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

迭代最近點(diǎn)算法：保證邊的約束

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

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

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

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

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

◆ 通過概率模型去除。

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

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

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