前面文章《自動駕駛運動規劃（Motion Planning）》中提到可以使用佔位圖（Occupancy Grid Map）表示自動駕駛行駛區域的哪些區域被障礙物（如靜止的車輛、路中間的石墩子、樹木、路肩等）佔用，Motion Planning模組會透過查詢佔位地圖避開這些道路障礙物，避免與它們碰撞，從而達到安全駕駛的目的。

1、佔位柵格地圖(Occupancy Grid Map)

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

如上圖所示，將車輛行駛道路環境用網格（Cell）切分，並且每個網格（Cell）用二值數值0和1填充，0表示該網格（Cell）被佔用，1表示該網格（Cell）沒有被佔用。

m^{i} \in \{0, 1\}

由此，可以得到如下所示的一張柵格佔位地圖。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

要製作理想的佔位柵格地圖必須滿足的以下幾個假設條件：

1）佔位柵格地圖是對道路行駛區域中的靜態環境（Static Environment）的描述。也就意味著，我們在製圖前必須將地面、動態物體（車輛、行人等）從感測器資料中移除掉；

2）每個網格（Cell）與其它的所有網格的狀態是相互獨立的，即它的狀態不受周圍其它網格狀態的影響；

3）在每個時刻，車輛的位置是精確的、已知的。

2、機率佔位柵格地圖(Probabilistic Occupancy Grid Map)

在實際的應用中，車輛感測器的資料測量是存在誤差的，車輛的定位結果也是存在誤差的，動態障礙物的識別也是存在誤差的，因此用機率表示一個網格（Cell）被佔用的可能性是一個更加可行的方案。每個網格儲存一個［0， 1］之間的機率值，這個值越大，表示網格被佔用的可能性越大；這個值越小，表示網格被佔用的可能性越小。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

3、機率佔位柵格圖(Probabilistic Occupancy Grid Map)製圖

柵格地圖的每個Cell的機率值計算公式如下：

bel_t(m^i) = p(m^i | (y, x)_{1:t})

其中

(y, x)_{1:t}

是1到t時刻的車輛位置和感測器測量結果，通過歷史資訊的累計，可以提升製作的地圖的準確性。

如何將1到t時刻的所有感測器測量結果融合起來呢？貝葉斯理論（Bayes Theorem）是一個不錯的選擇。

bel_t(m^i) = \eta p(y_t | m^i) bel_{t-1}(m^i)

其中

\eta

是歸一化引數，

p(y_t | m^i)

是感測器的測量模型。透過貝葉斯理論（Bayes Theorem）將多次感測器測量結果融合到同一個Cell中，從而獲得高可信度的網格佔用機率。

3.1 貝葉斯理論(Bayes Theorem)更新存在的問題

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

重複的浮點數乘法運算導致計算結果的數值變得很小而難以精確表達和運算。Logit函式可以把自變數從（0，1）連續單調地對映到正負無窮。logit函式的定義如下：

f(x) = log {\frac{x}{1 - x}}

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

所以我們使用Logit函式替代標準的Bayes更新過程。

3.2 貝葉斯更新過程的推導

貝葉斯理論（Bayes Theorem）更新網格（Cell）佔用機率的公式如下：

p\left(m^{i} | y_{1: t}\right)=\frac{p\left(y_{t} | y_{1: t-1}, m^{i}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(y_{t} | y_{1: t-1}\right)} \tag{1}

根據一階馬爾科夫（Markov Assumption）假設，t時刻的狀態只與t-1時刻的狀態有關，因此公式（1）可寫為如下形式：

p\left(m^{i} | y_{1: t}\right)=\frac{p\left(y_{t} | m^{i}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(y_{t} | y_{1: t-1}\right)} \tag{2}

對測量模型應用貝葉斯（Bayes Theorem）更新過程：

p\left(y_{t} | m^{i}\right)=\frac{p\left(m^{i} | y_{t}\right) p\left(y_{t}\right)}{p\left(m^{i}\right)} \tag{3}

將公式3）代入公式2），可得：

p\left(m^{i} | y_{1: t}\right)=\frac{p\left(m^{i} | y_{t}\right) p\left(y_{t}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(m^{i}\right) p\left(y_{t} | y_{1: t-1}\right)} \tag{4}

然後計算1-p的值：

p\left(\neg m^{i} | y_{1: t}\right)=1-p\left(m^{i} | y_{1: t}\right)=\frac{p\left(\neg m^{i} | y_{t}\right) p\left(y_{t}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(\neg m^{i}\right) p\left(y_{t} | y_{1: t-1}\right)} \tag{5}

將p和1-p代入logit函式：

\operatorname{logit}(p)=\log \left(\frac{p}{1-p}\right)

\begin{aligned} \quad \frac{p\left(m^{i} | y_{1: t}\right)}{p\left(\neg m^{i} | y_{1: t}\right)} & =\frac{\frac{p\left(m^{i} | y_{t}\right) p\left(y_{t}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(m^{i}\right) p\left(y_{t}\right) p\left(m^{i} | y_{1: t-1}\right)}}{\frac{p\left(\neg m^{i} | y_{t}\right) p\left(y_{t}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(\neg m^{i}\right) p\left(y_{t} | y_{1: t-1}\right)}} \\ &=\frac{p\left(m^{i} | y_{t}\right) p\left(\neg m^{i}\right) p\left(m^{i} | y_{1: t-1}\right)}{p\left(\neg m^{i} | y_{t}\right) p\left(m^{i}\right) p\left(\neg m^{i} | y_{1: t-1}\right)} \\ &=\frac{p\left(m^{i} | y_{t}\right)\left(1-p\left(m^{i}\right)\right) p\left(m^{i} | y_{1: t-1}\right)}{\left(1-p\left(m^{i} | y_{t}\right)\right) p\left(m^{i}\right)\left(1-p\left(m^{i} | y_{1: t-1}\right)\right)} \end{aligned} \tag{6}

對公式6）等號兩側取log，進行整理後，得到：

\operatorname{logit}\left(p\left(m^{i} | y_{1: t}\right)\right)=\operatorname{logit}\left(p\left(m^{i} | y_{t}\right)\right)+\operatorname{logit}\left(p\left(m^{i} | y_{1: t-1}\right)\right)-\operatorname{logit}\left(p\left(m^{i}\right)\right)

於是得到

Bayes更新遞推公式

：

l_{t, i}=\operatorname{logit}\left(p\left(m^{i} | y_{t}\right)\right)+l_{t-1, i}-l_{0, i}

其中：

\operatorname{logit}\left(p\left(m^{i} | y_{t}\right)\right)

是Inverse Measurement Model，

l_{t-1, i}

是網格i在t-1時刻的置信度（belif），

l_{0,i}

是Initial belief。

可以看到，該遞推公式應用的關鍵是Inverse Measurement Model：

p\left(m^{i} | y_{t}\right))

，如何計算該值呢？

3.3 Inverse Measurement Model

佔位柵格地圖的感測器測量模型為：

p(y_t | m^{i})

，表示基於已有的地圖Cell機率，疊加感測器測量結果，得到新的佔位機率值。

而現在我們要求解的是：

p(m^{i} | y_t)

，這也是為什麼該公式被成為Inverse Measurement Model的原因。

下面來看看Inverse Measurement Model如何計算？下面以二維鐳射雷達掃描模型來說明（注意：實際應用的鐳射雷達是3D的，這裡用2D Lidar是為了簡化模型，所用理論可以很好推廣到3D模型）。

2D Lidar模型

它在2D平面上進行掃描，包含兩個引數：Scanner bearing和Scanner rangers。Scanner bearing均勻的分佈在［

-{\phi_{max}}^s, {\phi_{max}}^s

］之間，一般的我們可以認為它們均勻分佈在360度的各個方向上。Scanner rangers是從Lidar中心到障礙物的距離，Lidar發出鐳射、接收回波，從而計算出到周圍障礙物的距離；為了簡化期間，我們也假設Lidar傳送鐳射後立即收到回波，不存在時間延遲。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

Map座標系&Vehicle座標系&感測器座標系

數學模型構建過程中

座標系

是不可或缺的。這裡主要涉及到三個座標系：Map座標系、Vehicle座標系以及感測器座標系。2D Lidar的測量結果都是相對於自身感測器中心的，即以2D Lidar中心為座標原點；所有的測量結果最終都要轉換到Map座標系，完成地圖製作的計算。

假設2D Lidar在Map座標系中的姿態為

(x_{1,t}, x_{2,t}, x_{3,t})

，其中

x_{1,t}

和

x_{2,t}

是x和y座標，

x_{3,t}

是感測器朝向。透過該姿態，可以將2D Lidar測量結果轉換到Map座標系。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

Lidar測量結果與Map Cell關聯匹配

如何將2D Lidar模型與Map Cell關聯起來呢？如下圖所示，第i個Map Cell用

(r^{i}, {\phi}^i)

表示。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

然後透過2D Lidar bearing與Map Cell相對於感測器的方位進行最小誤差匹配，得到影響當前Map Cell的鐳射束。

k = argmin|({\phi}_i - {\phi}_i^s)|

匹配的過程如下：首先定義兩個值

\alpha

和

\beta

，各個網格Cell的機率計算如下：

1）如果

r^i > {r_{max}}^s

或者

|\phi^i - \phi_k^s| > \beta /2

， 表示為探測區域，沒有資訊，這些區域的機率值一般為0。5，表示不確定是否被佔用。

2） 如果

r_k^s < r_{max}^s

並且

r^i - r_k^s < \alpha / 2

，表示該區域大機率被佔用，因此要賦予一個大於0。5的機率值。

3） 如果|

r^i - r_k^s| > \alpha / 2

，這些網格被佔用的機率較低，因此要賦予一個小於0。5的機率值。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

如下圖所示，紅色區域為高機率被佔用區域，灰色區域為未知區域，其餘區域為低機率被佔用區域。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

至此，有了Inverse Measurement Model，Bayes更新的過程可以正常進行了。

更高效的Inverse Measurement Model計算方法

採用光線跟蹤（Ray Tracing）的Bresenham‘s Line Algorithm可以大大減少複雜的浮點數計算，提升計算效率。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

4、移除Lidar中地面和動態物體

實際應用中的鐳射雷達（Lidar）是3D的，會掃描到大量的地面點，這些地面點如果不被移除，按照計算匹配模型，會被當做障礙物處理。所以需要將地面點點雲資料從鐳射雷達點雲中移除掉。如何移除呢？一種可行的方法是，透過自動化識別演算法從Lidar點雲中將地面識別並剔除。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

地面識別的難度是比較高的，因為很多道路路面內外的界限在點雲中是不明確的，自動化識別演算法會誤把道路邊界外的區域識別為道路路面，從而導致錯誤的地圖資訊等。透過視覺分割演算法輔助點雲識別可以提升路面的識別率。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

動態物體（行人、車輛等）也需要從點雲資料中移除，這依賴於基於點雲和影象的感知技術。但同樣也存在很多技術難題，比如如何提升識別的準確率，如何將靜止的車輛識別出來等等。

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

注：本文首發於微信公眾號，轉載請註明出處，謝謝！

自動駕駛Mapping-佔位柵格圖(Occupancy Grid Map)

公眾號：半杯茶的小酒杯

個人部落格地址：

參考連結

本文主要整理自Coursera自動駕駛課程：Motion Planning for Self-Driving Cars第二週課程的學習筆記。

相關文章