2. 慣性導(dǎo)航

假設(shè)一輛汽車正以恒定速度直線行駛，如果我們知道汽車的初始位置、速度和行駛時長，那么我們可以很容易知道車輛在任意時刻的位置。同樣的，根據(jù)初始位置、速度和加速度，依然可以確定汽車的實時位置，這也是慣性導(dǎo)航的基本原理。

對于自動駕駛汽車，加速度可以用三軸加速度計來測量。但僅使用加速度計還不足以計算我們的位置和速度。加速度計是在車輛坐標(biāo)系記錄中進(jìn)行測量，我們需要知道如何將該測量值轉(zhuǎn)換到全局坐標(biāo)系。

這種轉(zhuǎn)換可以通過陀螺儀實現(xiàn)。三軸陀螺儀的三個外部平衡環(huán)一直在旋轉(zhuǎn)，但其旋轉(zhuǎn)軸始終固定在全局坐標(biāo)系中。車輛位置可以通過測量旋轉(zhuǎn)軸和三個外部平衡環(huán)的相對位置來計算。

加速度計和陀螺儀是慣性測量單元（或IMU）的主要組件。IMU的一個重要特征在于它以高頻率更新（其頻率可達(dá)1000Hz），所以IMU可以提供接近實時的位置信息。其缺點是，IMU的運動誤差會隨時間而增加，我們只能依靠它在很短的時間內(nèi)進(jìn)行定位。

一種更有效的方法是同時結(jié)合GPS和IMU來定位汽車。一方面IMU彌補了GPS更新頻率低的缺陷，另一方面GPS糾正了IMU的運動誤差。

但是，即使將二者結(jié)合使用，也不能完全解決自動駕駛的定位問題。比如，我們在山中或者地下隧道中行駛時，可能會長時間沒有GPS更新，這會讓整個定位系統(tǒng)面臨失效風(fēng)險。

3. LiDAR定位

利用LiDAR（激光雷達(dá)），我們可以通過點云匹配來對汽車進(jìn)行定位。該方法將來自激光雷達(dá)傳感器的檢測數(shù)據(jù)，與預(yù)先存在的高精度地圖連續(xù)匹配。通過這種比較，可以獲知車輛在高精度地圖上的全球位置和行駛方向。

匹配點云常用的一種方法是迭代最近點（或CIP）。假設(shè)想對兩次點云掃描進(jìn)行匹配，對于第一次掃描中的每個點，我們需要找到另一次掃描中最接近的匹配點，最終會收集到許多匹配點對。我們把沒對點之間的距離誤差相加，然后計算平均距離誤差，再通過點云的旋轉(zhuǎn)和平移來最大限度的降低這一平均距離誤差。一旦最大限度的降低了點云之間的誤差，就可以在傳感器掃描和地圖之間找到匹配。然后，就可以將LiDAR掃描到的車輛位置轉(zhuǎn)換到全局地圖上，并計算車在地圖上的精確位置。

濾波算法是另一種LiDAR定位方法，該方法可以消除冗余信息，并在地圖上找到最可能的車輛位置。Apollo使用了直方圖濾波算法，該方法也被稱為誤差平方和算法（或SSD）。

為了應(yīng)用直方圖濾波，我們將LiDAR掃描的點云滑過地圖上的每個位置。在每個位置，計算掃描的點與高精度地圖上的對應(yīng)點之間的誤差或距離。然后對誤差的平方求和，和值越小，掃描結(jié)果與地圖匹配的越好。

下圖中，紅色點表示匹配較好的點，藍(lán)色點則匹配的較差。

第三種LiDAR定位方法是卡爾曼濾波。卡爾曼濾波是一種算法，用于根據(jù)過去狀態(tài)和新的傳感器測量結(jié)果預(yù)測當(dāng)前的狀態(tài)。具體來說，卡爾曼濾波使用了預(yù)測—更新循環(huán)。

首先我們根據(jù)之前的狀態(tài)以及對移動距離和方向估計，來預(yù)測新的位置。由于運動估計可能存在偏差，所以需要通過所以LiDAR測量實際的位置并加以糾正。一旦我們測量了新的位置，便可以使用概率方法將傳感器測量結(jié)果與現(xiàn)在實際的位置預(yù)測結(jié)合起來，并不斷的重復(fù)這個預(yù)測-更新循環(huán)。

LiDAR定位的優(yōu)勢主要在于穩(wěn)健性，從高精度地圖開始，只要存在有效的傳感器，我們就能始終進(jìn)行定位。其缺點在于難以構(gòu)建高精度地圖，并且保持其更新。事實上，幾乎不可能使地圖完全保持最新，因為幾乎每個地圖均包含瞬態(tài)元素：汽車、行人等等。下次路過時，這些元素幾乎都會消失，地圖在不斷發(fā)生變化。

4. 視覺定位

對于自動駕駛汽車，圖像是收集起來最簡單的數(shù)據(jù)類型。攝像頭便宜且種類繁多，而且易于使用。

我們能夠使用圖像來定位汽車嗎？事實上，單純通過圖像來實現(xiàn)車輛精確定位非常困難。所以攝像頭圖像通常與高精度地圖、GPS的數(shù)據(jù)相結(jié)合，從而實現(xiàn)更好的定位效果。

假設(shè)一輛車正在路上行駛，它感知到右邊有一棵樹。但是地圖顯示許多位置的車道右側(cè)都有樹，我們?nèi)绾沃儡囕v看到的是哪棵樹呢？

我們可以用概率來解決這個問題。想象一下，我們位于道路上許多不同點中的任意一處。我們假設(shè)從一些點可以看到右側(cè)的樹，而另一些點則看不到。顯然，我們位于右側(cè)存在樹的地方，從而可以排除在地圖上車道右側(cè)沒有種樹的哪些點。

然后，讓車輛繼續(xù)向前開，并觀察周邊的世界，可以發(fā)現(xiàn)有一個位置在車輛右側(cè)有成排的兩棵樹，那么我們處于該位置的概率是最大的。

我們可以通過觀察結(jié)果、概率和地圖來確定最可能的位置。因為我們使用粒子或點來估計最可能的位置，因此該過程又被稱作粒子濾波。

樹木在許多道路上其實是比較稀少，于是一種更常用的方法是利用車道線進(jìn)行定位。首先使用粒子濾波原理對車道線進(jìn)行拍照，然后使用圖像來確定車輛在道路中的位置，再將道路圖像與地圖進(jìn)行比較，從而匹配出最可能的位置。

視覺定位的優(yōu)點在于圖像很容易獲取，且成本很低；缺點則是缺乏三維信息和對三維地圖的利用。

5. Apollo定位策略

百度Apollo使用基于GPS、IMU和LiDAR的多傳感器融合定位系統(tǒng)，利用了不同傳感器的互補優(yōu)勢，提高了定位的穩(wěn)定性和準(zhǔn)確性。

Apollo定位模塊依賴于IMU、GPS、LiDAR 、雷達(dá)和高精度地圖，這些傳感器同時支持GNSS定位和LiDAR定位。GNSS定位輸出位置和速度信息，LiDAR 定位輸出位置和行進(jìn)方向信息。融合框架通過卡爾曼濾波將這些輸出結(jié)合在一起。

卡爾曼濾波建立在兩步預(yù)測測量周期之上。在Apollo中，慣性導(dǎo)航解決方案用卡爾曼濾波的預(yù)測步驟，GNSS和LiDAR定位用于卡爾曼濾波的測量結(jié)果更新步驟。

6. 項目實例