Sebastian帶你學(xué)習(xí)定位

上周我們發(fā)布了Apollo入門課堂第②講—高精地圖，收到了很多開發(fā)者的認(rèn)同和贊賞，也看到越來越多的開發(fā)者對(duì)學(xué)習(xí)自動(dòng)駕駛技術(shù)有了濃厚的興趣。希望有更多開發(fā)者能夠通過本門課程，學(xué)到更多的自動(dòng)駕駛知識(shí)。

本周我們將介紹定位模塊的知識(shí)，了解車輛如何以個(gè)位數(shù)厘米級(jí)別的精度進(jìn)行自定位。

定位是讓無人車知道自身確切位置的方法，這是一個(gè)美妙但是十分艱難的任務(wù)，同時(shí)也對(duì)無人駕駛車十分重要。定位不僅僅是找出自身的大概方位，而是要以10cm級(jí)別，將車感信息與高精地圖信息進(jìn)行比較來精確的位置尋找。

第三課，定位

Sebastian帶你學(xué)習(xí)定位

1

定位的概述

在日常生活中假設(shè)你手中有一張高精地圖，定位的任務(wù)是確定你的車輛在這張高精地圖上的位置。

大多數(shù)時(shí)候，我們使用GPS來進(jìn)行定位，但是GPS對(duì)于無人車來說還不夠精確。因此我們必須找到另一種方法來更準(zhǔn)確地確定車輛在地圖上的位置。最常用的方法是將汽車傳感器所看到的內(nèi)容與地圖上所顯示的內(nèi)容進(jìn)行比較。車輛傳感器可以測(cè)量車輛與靜態(tài)障礙物之間的距離。我們?cè)谲囎陨淼淖鴺?biāo)系中測(cè)量這些距離以及這些靜態(tài)障礙物的方向。在車的坐標(biāo)系中，汽車的前進(jìn)方向始終向前，坐標(biāo)系正方向始終與車頭保持一致，但不一定與地圖坐標(biāo)系保持一致。當(dāng)車輛傳感器測(cè)量到地圖上的物體，會(huì)將傳感器的地標(biāo)觀測(cè)值，與地標(biāo)在地圖上的位置匹配，轉(zhuǎn)換到地圖自帶坐標(biāo)系，反之亦然，從而達(dá)到地圖與車感數(shù)據(jù)的對(duì)比。

定位提供了許多可選擇的方法，但每種都有自己的優(yōu)劣。接下來將介紹主流的定位方法。

高精地圖最重要特征之一是精度，手機(jī)上的導(dǎo)航地圖只能達(dá)到米級(jí)精度，而高精地圖可以使車輛能夠達(dá)到厘米級(jí)的精度，這對(duì)確保無人車的安全性至關(guān)重要。

2

定位方法介紹

GNSS RTK

如果在野外迷路，假如你看到自己離一棵樹 75 米遠(yuǎn)，你只知道自己位于一個(gè)以樹為中心半徑為75米的圓上。但是如果看到一個(gè)離自己 64 米遠(yuǎn)的房子，你就會(huì)知道自己位于兩個(gè)圓的交點(diǎn)處，但仍不知道自己位于哪個(gè)交點(diǎn)上。假設(shè)你看到了第三個(gè)路標(biāo)，如路燈。經(jīng)過測(cè)量你發(fā)現(xiàn)自己離路燈55 米遠(yuǎn)，自己就知道了相對(duì)于這些地標(biāo)的確切位置。

首先。假設(shè)現(xiàn)在有一個(gè)世界地圖標(biāo)注了這三個(gè)地標(biāo)在世界上的確切位置，那么就可以知道自身的確切位置了，這就被稱為三角測(cè)量。

GPS就是這樣的工作原理，只是參照物并不是地標(biāo)而是衛(wèi)星。三顆衛(wèi)星再加上一顆用來定位高度的衛(wèi)星，4顆衛(wèi)星就可以知道自身的確切位置了。GPS這類系統(tǒng)的通用名稱為全球?qū)Ш叫l(wèi)星系統(tǒng)或GNSS，GPS 是其中使用最廣泛的GNSS 系統(tǒng)。

GPS分為三部分。第一部分是衛(wèi)星，在任何特定時(shí)間大約有 30 顆 GPS 衛(wèi)星在外層空間運(yùn)行，它們各自距離地球表面約 2 萬公里。第二部分由世界各地的控制站組成，控制站用于監(jiān)視和控制衛(wèi)星，其主要目的是讓系統(tǒng)保持運(yùn)行并驗(yàn)證 GPS 廣播信號(hào)的精確度。第三部分是 GPS 接收器，GPS 接收器存在于手機(jī)、電腦、汽車、船只以及許多其他設(shè)備中。

GPS接收器實(shí)際上并不直接探測(cè)你與衛(wèi)星之間的距離，它首先測(cè)量信號(hào)的飛行時(shí)間即TOF原理。通過將光速乘以這個(gè)飛行時(shí)間，來計(jì)算離衛(wèi)星的距離。由于光速的值很大，即使是少量的時(shí)間誤差也會(huì)在距離計(jì)算中造成巨大的誤差。

因此每顆衛(wèi)星都配備了高精確度的原子鐘。為進(jìn)一步減小誤差，使用實(shí)時(shí)運(yùn)動(dòng)定位（RTK）。RTK 涉及在地面上建立幾個(gè)基站，每個(gè)基站都知道自己精確的“地面實(shí)況”位置，但是每個(gè)基站也通過 GPS 測(cè)量自己的位置。已知的“地面實(shí)況”位置與通過 GPS 測(cè)量的位置之間的偏差，為 GPS 測(cè)量結(jié)果中的誤差。然后將這個(gè)誤差傳遞給其他 GPS 接收器以供其調(diào)整自身的位置計(jì)算。在 RTK 的幫助下，GPS 可以將定位誤差限制在 10 厘米以內(nèi)。

但是仍存在高樓和其他障礙物可能阻擋 GPS 信號(hào)的問題，這使定位變得困難或根本無法定位。GPS 的更新頻率很低，大約為 10 赫茲。由于無人駕駛車在快速移動(dòng)，可能需要更頻繁地更新位置。

慣性導(dǎo)航

假設(shè)一輛汽車正以恒定速度直線行駛，如果提供了汽車的初始位置、速度、行駛時(shí)長(zhǎng)，那么汽車的位置即從初始位置開始，然后速度乘以時(shí)間。

假如給予一輛汽車的初始速度，并給予加速度和駕駛時(shí)間，就可以使用加速度、初始速度、初始位置來計(jì)算汽車在任何時(shí)間點(diǎn)的車速和位置。

加速度需要三軸加速度計(jì)的傳感器來測(cè)量，有三種不同類型的三軸加速度計(jì)，它們采用不同的方法，但共同的目標(biāo)是精確測(cè)量加速度。加速度計(jì)根據(jù)車輛的坐標(biāo)系記錄測(cè)量結(jié)果，還需要陀螺儀傳感器將這些測(cè)量值轉(zhuǎn)換為全局坐標(biāo)系測(cè)量值。

三軸陀螺儀的三個(gè)外部平衡環(huán)一直在旋轉(zhuǎn)，但三軸陀螺儀中的旋轉(zhuǎn)軸始終固定在世界坐標(biāo)系中。在坐標(biāo)系中的位置是通過測(cè)量旋轉(zhuǎn)軸和三個(gè)外部平衡環(huán)的相對(duì)位置來計(jì)算的。

加速度計(jì)和陀螺儀是慣性測(cè)量單元（IMU）的主要組件，IMU 可以以高頻率更新，可達(dá) 1000 赫茲，所以 IMU 可以提供接近實(shí)時(shí)的位置信息。但缺點(diǎn)在于其運(yùn)動(dòng)誤差隨時(shí)間增加而增加，我們只能依靠慣性測(cè)量單元在很短的時(shí)間范圍內(nèi)進(jìn)行定位。但是我們可以結(jié)合 GPS 和 IMU 來定位汽車，一方面 IMU 彌補(bǔ)了 GPS 更新頻率較低的缺陷。另一方面 GPS 糾正了 IMU 的運(yùn)動(dòng)誤差。但是即使將 GPS 和 IMU 系統(tǒng)相結(jié)合也不能完全解決定位問題，比如我們?cè)谏介g行駛或城市峽谷中或在地下隧道中行駛，那么可能長(zhǎng)時(shí)間沒有 GPS 更新。

激光雷達(dá)定位

利用激光雷達(dá)，可以通過點(diǎn)云匹配來對(duì)汽車進(jìn)行定位。該方法將來自激光雷達(dá)傳感器的檢測(cè)數(shù)據(jù)與預(yù)先存在的高精地圖之間匹配，通過這種比較可獲知汽車在高精度地圖上的全球位置和行駛方向。匹配點(diǎn)云有多種方法，比如迭代最近點(diǎn)（ICP）就是一種方法。對(duì)于第一次掃描中的每個(gè)點(diǎn)，需要找到另一次掃描中最接近的匹配點(diǎn)。最終會(huì)收到許多匹配點(diǎn)對(duì)， 把每個(gè)點(diǎn)的距離誤差相加，然后計(jì)算平均距離誤差。假設(shè)我們對(duì)兩次點(diǎn)云掃描進(jìn)行匹配。我們的目標(biāo)是通過點(diǎn)云旋轉(zhuǎn)和平移來最大限度地降低這一平均距離誤差，就可以在傳感器掃描和地圖之間找到匹配，將通過傳感器掃描到的車輛位置轉(zhuǎn)換為全球地圖上的位置并計(jì)算出在地圖上的精確位置。

濾波算法是另一種LiDAR 定位方法。將通過傳感器掃描到的車輛位置轉(zhuǎn)換為全球地圖上的位置并計(jì)算出在地圖上的精確位置，濾波算法是另一種LiDAR 定位方法。濾波算法可消除冗余信息并在地圖上找到最可能的車輛位置。Apollo 使用了直方圖濾波算法，該方法有時(shí)也被稱為誤差平方和算法（ SSD）。為了應(yīng)用直方圖濾波，將通過傳感器掃描的點(diǎn)云滑過地圖上的每個(gè)位置，在每個(gè)位置上計(jì)算掃描的點(diǎn)與高精度地圖上的對(duì)應(yīng)點(diǎn)之間的誤差或距離然后對(duì)誤差的平方求和，求得的和越小掃描結(jié)果與地圖之間的匹配越好。在事例中，藍(lán)色表示較好，紅色較差，綠色表示一般。

卡爾曼濾波是另一種LiDAR 定位方法，卡爾曼濾波用于根據(jù)我們?cè)谶^去的狀態(tài)和新的傳感器測(cè)量結(jié)果預(yù)測(cè)我們當(dāng)前的狀態(tài)。具體來說，卡爾曼濾波使用了預(yù)測(cè)更新周期，首先我們根據(jù)之前的狀態(tài)以及對(duì)移動(dòng)距離和方向的來估計(jì)我們的新位置。當(dāng)然運(yùn)動(dòng)估計(jì)并不完美，所以需要通過使用傳感器測(cè)量我們的位置并加以糾正。一旦傳感器測(cè)量了我們的新位置，我們便可以使用概率規(guī)則將不完美的測(cè)量結(jié)果與現(xiàn)有位置匹配起來。我們會(huì)永遠(yuǎn)遵循這個(gè)預(yù)測(cè)更新周期，只要我們需要對(duì)車輛進(jìn)行定位，先預(yù)測(cè)我們的新位置，然后用傳感器測(cè)量我們的位置。LiDAR定位的主要優(yōu)勢(shì)在于穩(wěn)健性，只要從高精地圖開始并且存在有效的傳感器，就始終能夠進(jìn)行定位。主要缺點(diǎn)在于難以構(gòu)建高精地圖并使其保持最新，事實(shí)上幾乎不可能讓地圖完全保持最新，因?yàn)閹缀趺總€(gè)地圖均包含瞬態(tài)元素，汽車、行人、停放的汽車、垃圾等。

視覺定位

圖像是要收集的最簡(jiǎn)單的數(shù)據(jù)類型，攝像頭便宜且種類繁多、易于使用。通過圖像實(shí)現(xiàn)精確定位卻非常困難，實(shí)際上攝像頭圖像通常與來自其他傳感器的數(shù)據(jù)相結(jié)合以準(zhǔn)確定位車輛，將攝像頭數(shù)據(jù)與地圖和 GPS 數(shù)據(jù)相結(jié)合，比單獨(dú)使用攝像頭圖像進(jìn)行定位的效果更好。假設(shè)一輛車正在路上行駛，感知到右邊有樹，但是地圖顯示道路右側(cè)有幾棵樹有很多不同的點(diǎn)位置。如何知道車輛現(xiàn)在看到哪棵樹？我們可以使用概率來確定哪個(gè)點(diǎn)最可能代表我們的實(shí)際位置。有些點(diǎn)是無法看到右邊有樹，可以先排除。

繼續(xù)開車可以發(fā)現(xiàn)，有些點(diǎn)右邊只有一棵樹，也可以排除。

該過程稱為粒子濾波。當(dāng)然樹木在許多道路上比較稀少，但是車道線在許多道路上卻很常見，可以使用相同的粒子濾波原理對(duì)車道線進(jìn)行拍照，然后使用拍攝的圖像來確定車輛在道路中的位置，可以將道路攝像頭圖像與地圖進(jìn)行比較。如圖所示，藍(lán)色代表地圖上兩個(gè)不同位置的車道線，紅色代表車輛攝像頭觀察到的車道線，紅線與右側(cè)藍(lán)線的匹配度要比與左側(cè)藍(lán)線的匹配度高得多，更有可能位于右側(cè)圖像位置上。

視覺定位的優(yōu)點(diǎn)在于圖像數(shù)據(jù)很容易獲得，缺點(diǎn)在于缺乏三維信息和對(duì)三維地圖的依賴。

高精地圖不僅可以減少計(jì)算需求，還可以通過提供有關(guān)駕駛環(huán)境的詳細(xì)信息，來確保無人車的安全。保持這些地圖的更新是一項(xiàng)重大任務(wù)，測(cè)試車隊(duì)需要不斷地對(duì)高精度地圖進(jìn)行驗(yàn)證和更新。此外，這些地圖可能達(dá)到幾厘米的精度，這是水準(zhǔn)最高的制圖精度。

Apollo定位

Apollo使用基于GPS、IMU、激光雷達(dá)的多傳感器融合定位系統(tǒng)，這種方法利用了不同傳感器的互補(bǔ)優(yōu)勢(shì)，也提高了穩(wěn)定性和準(zhǔn)確性。

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

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