2. 慣性導(dǎo)航

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

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

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

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

一種更有效的方法是同時(shí)結(jié)合GPS和IMU來(lái)定位汽車。一方面IMU彌補(bǔ)了GPS更新頻率低的缺陷，另一方面GPS糾正了IMU的運(yùn)動(dòng)誤差。

但是，即使將二者結(jié)合使用，也不能完全解決自動(dòng)駕駛的定位問題。比如，我們?cè)谏街谢蛘叩叵滤淼乐行旭倳r(shí)，可能會(huì)長(zhǎng)時(shí)間沒有GPS更新，這會(huì)讓整個(gè)定位系統(tǒng)面臨失效風(fēng)險(xiǎn)。

3. LiDAR定位

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

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

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

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

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

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

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

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

4. 視覺定位

對(duì)于自動(dòng)駕駛汽車，圖像是收集起來(lái)最簡(jiǎn)單的數(shù)據(jù)類型。攝像頭便宜且種類繁多，而且易于使用。

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

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

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

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

我們可以通過觀察結(jié)果、概率和地圖來(lái)確定最可能的位置。因?yàn)槲覀兪褂昧Ｗ踊螯c(diǎn)來(lái)估計(jì)最可能的位置，因此該過程又被稱作粒子濾波。

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

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

5. Apollo定位策略

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

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

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

6. 項(xiàng)目實(shí)例