高精定位模塊分析

北京大學(xué)自動(dòng)駕駛課程大作業(yè)

摘要

自動(dòng)駕駛變成了一個(gè)非常熱門的話題翩概。但實(shí)際上绢馍，在美國國防部組織的無人駕駛挑戰(zhàn)賽上美國的斯坦福大學(xué)獲得第一名的時(shí)候開始自動(dòng)駕駛的時(shí)代已經(jīng)到來了俗冻。自動(dòng)駕駛的系統(tǒng)，它分為很多部分。從傳感器的感知到之后的識(shí)別,定位,決策屋剑，再到之后的控制系統(tǒng)，涉及到很多的技術(shù)诗眨。其中高精定位是自動(dòng)駕駛非常重要的一個(gè)部分唉匾。自動(dòng)駕駛技術(shù)核心之一就是它的高精定位。因?yàn)檐囕v想在道路上好好的行駛需要對自身的位置有厘米級(jí)的定位精度。只有這樣才能達(dá)到無人駕駛的技術(shù)要求巍膘。高精定位的精度會(huì)受到傳感器的噪聲厂财，衛(wèi)星導(dǎo)航的多路徑傳播效應(yīng)，車輛自身定位問題峡懈，車輛動(dòng)力學(xué)問題等等的影響璃饱。每一個(gè)小問題都會(huì)導(dǎo)致之后誤差積累。誤差積累到一定程度,得到的最終結(jié)果就不可信肪康。就算所有傳感器都是最好的荚恶，但實(shí)際上也會(huì)受到很多影響。所以我們還需要用額外的算法去輔助車輛獲得高精度的位置信息磷支。這里面涉及到的就是卡爾曼濾波谒撼，擴(kuò)展卡爾曼濾波，無損卡爾曼濾波,還有粒子濾波器等等雾狈。本文通過對百度阿波羅的高精定位方案進(jìn)行分析廓潜，并通過查看百度阿波羅的github上的定位模塊代碼，分析阿波羅是如何達(dá)到L4級(jí)別的高精定位善榛。

介紹

自動(dòng)駕駛已經(jīng)發(fā)展了很多年了茉帅。自動(dòng)駕駛作為未來出行解決方案，很多大車廠和互聯(lián)網(wǎng)公司都在研究如何去開發(fā)安全舒適的自動(dòng)駕駛車輛《П祝現(xiàn)階段的自動(dòng)駕駛系統(tǒng)框架有感知堪澎，定位，決策味滞，路徑規(guī)劃樱蛤，控制，高精地圖剑鞍，云計(jì)算等核心內(nèi)容昨凡。層級(jí)上來說，定位模塊和其他模塊有上下層級(jí)關(guān)系蚁署。感知屬于最底層便脊，定位就是感知模塊的上一層。之后的控制光戈，決策都要依賴定位模塊進(jìn)行運(yùn)算哪痰。

這里簡單分析一下定位和其他模塊的關(guān)系。

感知層是通過傳感器感知物理世界久妆。傳感器把物理世界按照一定的數(shù)據(jù)編碼好晌杰，傳送給感知模塊。那么感知模塊就可以通過相關(guān)的算法提取物理世界模型筷弦。不僅如此肋演，感知還可以提供detection，classification等功能。定位模塊就是利用感知模塊的傳感器來確定車輛在世界坐標(biāo)系里面的位置爹殊。定位模塊用到的核心傳感器是RTK GPS, GNSS, IMU蜕乡，LIDAR，Camera等梗夸。定位模塊利用這些數(shù)據(jù)异希，通過定位算法輸出車輛在全球坐標(biāo)系里面的位置。規(guī)劃決策模塊利用感知绒瘦，定位称簿，高精地圖等數(shù)據(jù)進(jìn)行整合和決策。最終決策層的output會(huì)發(fā)送到控制層惰帽，然后通過控制層對車輛上的執(zhí)行器進(jìn)行drive by wire控制憨降。云端也會(huì)實(shí)時(shí)的接收車輛上的所有數(shù)據(jù)。云端確定車輛位置以后该酗，會(huì)利用云端上的計(jì)算能力和數(shù)據(jù)庫為車輛提供更好的駕駛能力授药。

理想條件下，定位模塊利用RTK GPS系統(tǒng)就可以對無人車進(jìn)行導(dǎo)航及定位呜魄。GPS系統(tǒng)是通過計(jì)算電磁波的傳播時(shí)間來判斷車輛的位置悔叽。但是現(xiàn)實(shí)中電磁波的傳播會(huì)因?yàn)榇髿鈱樱邩谴髲B爵嗅，天氣等因素產(chǎn)生時(shí)間誤差娇澎。時(shí)間誤差就意味著計(jì)算的距離數(shù)據(jù)不夠準(zhǔn)確。不夠準(zhǔn)確的距離就導(dǎo)致了我們根本不能確定我們的無人車到底在哪個(gè)位置睹晒。無人車在城市道路中的定位要求是誤差小于50cm趟庄，超出這個(gè)數(shù)據(jù)，定位模塊的數(shù)據(jù)就不能用來做自動(dòng)駕駛伪很。所以這些噪聲問題和單個(gè)傳感器不夠精準(zhǔn)的問題戚啥，研究人員就通過融合多個(gè)傳感器，最大限度的解決誤差問題锉试。下面分析一下百度阿波羅平臺(tái)的高精定位解決方案猫十。

百度阿波羅高精定位解決方案

首先分析一下百度阿波羅自動(dòng)駕駛平臺(tái)的硬件架構(gòu)。其中有激光雷達(dá)呆盖，毫米波雷達(dá)拖云，攝像機(jī),GPS系統(tǒng)，工業(yè)電腦絮短，IMU(慣性測量單元)江兢。

Fig 1 百度阿波羅自動(dòng)駕駛平臺(tái)硬件配置

這里所有的傳感器大部分都可以用來做定位。比如激光雷達(dá)可以結(jié)合高精地圖實(shí)現(xiàn)特征匹配定位丁频，攝像機(jī)可以結(jié)合高精地圖進(jìn)行特征匹配從而實(shí)現(xiàn)定位，通過GPS實(shí)現(xiàn)定位，通過IMU實(shí)現(xiàn)定位席里，GPS+IMU實(shí)現(xiàn)定位叔磷，GPS+IMU+Lidar+Radar也可以實(shí)現(xiàn)定位。阿波羅平臺(tái)是IMU+RTK GPS為主奖磁，其他定位方法為輔的定位模式改基。

其實(shí)除了這些交叉定位以外，還有一種定位方式是利用街景給車輛定位[1].Waymo利用谷歌強(qiáng)大的街景數(shù)據(jù)線下定義出路牌號(hào)碼咖为，商店標(biāo)志秕狰，紅路燈等等靜態(tài)數(shù)據(jù)來確定自身車輛所在的位置。大體的思路就是躁染，只要車輛本身知道攝像機(jī)識(shí)別到的商店牌匾鸣哀，紅綠燈和其他特征的全球坐標(biāo)系的話，通過透視原理或者相對距離就可以知道自身車輛的全球坐標(biāo)吞彤。這種方法簡單又直接我衬，但是涉及到的問題是巨大的數(shù)據(jù)量和人工標(biāo)注。這種方法的優(yōu)點(diǎn)是如果地圖都標(biāo)記好了饰恕，那么車輛本身有個(gè)攝像機(jī)就可以實(shí)現(xiàn)精準(zhǔn)的定位和導(dǎo)航挠羔。不過就算地圖標(biāo)注好了，但是有一些問題埋嵌。那就是基于攝像機(jī)的圖像識(shí)別問題破加。攝像機(jī)由于是通過采集光線的方式收集數(shù)據(jù)，所以會(huì)受到強(qiáng)光影響雹嗦。因此攝像機(jī)的識(shí)別精度會(huì)隨著光照強(qiáng)度的變化輸出不夠穩(wěn)定的識(shí)別結(jié)果拌喉。如果識(shí)別到的圖像不夠精確，穩(wěn)定俐银，那么基于這個(gè)識(shí)別結(jié)果導(dǎo)出的位置計(jì)算也是不夠精準(zhǔn)的尿背。且基于圖像的深度數(shù)據(jù)提取的能力也是值得深入考究的。畢竟從2D轉(zhuǎn)化到3D的過程中捶惜，很容易發(fā)生相關(guān)的數(shù)據(jù)噪聲和損失田藐。不僅如此，像在高速公路等場景吱七。汽久，周邊沒有特別的可識(shí)別的特征供定位。所以踊餐，waymo的這種方式或許可以提供一種新的思路景醇，但暫時(shí)無人車定位還是要依賴RTK GPS的幫助來完成精準(zhǔn)定位。

Fig 2 Waymo基于街景路牌識(shí)別的定位方式

可以看到定位的方法有很多種吝岭。但是問題就是哪種方法可以達(dá)到很高的精度且魯棒性還高呢三痰。那么現(xiàn)在的技術(shù)框架下吧寺，主流的還是在用RTK GPS+ IMU的方式進(jìn)行精準(zhǔn)定位。為什么不用其他的散劫？因?yàn)闇?zhǔn)確度和魯棒性获搏。在半自動(dòng)駕駛L3,和完全自動(dòng)駕駛的L4階段常熙，如果車輛失去了位置信息或者得到了錯(cuò)誤的位置信息仿贬，那么車輛所做出的決策本身從根源就是錯(cuò)誤的诅蝶。自身位置都確定不了的情況下调炬，變道缰泡，跟隨前車棘钞，路徑規(guī)劃都只會(huì)得到錯(cuò)誤的結(jié)果宜猜。不僅如此姨拥，L4級(jí)別的車輛因?yàn)橛袑囕v的完全控制權(quán)，所以失去位置信息的車輛可能會(huì)導(dǎo)致嚴(yán)重的事故憨奸。所以對于完全自動(dòng)駕駛的無人車來說排宰，定位的精準(zhǔn)固然重要国觉，但是更重要的魯棒性要好痕寓。否則硬毕，無人車根本不能判斷現(xiàn)在的位置信息的置信度吐咳，從而也無法輸出正確的結(jié)果韭脊。

現(xiàn)在大部分的無人車定位技術(shù)框架是用RTK GPS+IMU的方法進(jìn)行主定位，其他方式輔助定位蔫饰。那么為什么是RTK GPS 而不是 普通的GPS呢篓吁？因?yàn)槠胀ǖ腉PS是通過計(jì)算和衛(wèi)星的三角距離來確定自己的位置的。這里最重要的就是波的傳播速度摘盆。只要波的傳播速度是理想狀態(tài)箱熬，那么用最基本的GPS也是沒什么問題的。問題就在于波從人造衛(wèi)星發(fā)出蚤认，會(huì)受到大氣層米苹，衛(wèi)星軌道，衛(wèi)星鐘差和多路徑等影響砰琢。所以單純的GPS不能提供準(zhǔn)確的位置信息蘸嘶。而RTK則是通過建立基站的方式先找出載波的偏差，然后GPS的處理器再通過這個(gè)偏差來修正接收的數(shù)據(jù)陪汽。但是目前為止训唱，這個(gè)方案的問題是需要建立大量的基站，才能覆蓋無人車的移動(dòng)范圍挚冤。不僅如此基站建立的設(shè)備况增，人力昂貴也是一大問題。不過训挡，暫時(shí)這個(gè)方案的魯棒性和準(zhǔn)確性很好澳骤，所以廣泛應(yīng)用在無人車的定位算法上。

但是就算基站建好了澜薄，覆蓋的范圍夠廣闊了为肮，但是還有會(huì)有一些場景不能達(dá)到定位精度要求。比如樹蔭下行駛表悬，隧道里行駛弥锄，極端環(huán)境下行駛都會(huì)讓RTK GPS失去精準(zhǔn)性。然而無人車卻不可避免的在這種環(huán)境下行駛蟆沫。所以我們需要額外的傳感器或者算法去輔助我們的無人車籽暇。阿波羅自動(dòng)駕駛系統(tǒng)所提供的解決方案有激光雷達(dá)定位，攝像機(jī)定位饭庞，多傳感器融合定位戒悠。下面分別對這些算法進(jìn)行分析。

基于激光雷達(dá)定位

激光雷達(dá)可以精確的獲取車輛和周邊環(huán)境的相對距離信息舟山。同時(shí)激光雷達(dá)還可以輔助相關(guān)的算法可提供基于點(diǎn)運(yùn)數(shù)據(jù)的物體識(shí)別〕窈現(xiàn)在學(xué)術(shù)界普遍利用攝像機(jī)，激光雷達(dá)累盗，IMU等傳感器進(jìn)行融合寒矿。一般融合的輸出是車輛的位置和相應(yīng)的姿態(tài)狀態(tài)。 Hanieh Deilamsalehy若债，Timothy C. Havens提出的理論就是利用IMU符相，攝像機(jī)和激光雷達(dá)對車輛或者無人機(jī)的三維姿態(tài)進(jìn)行估算[2]。這里不使用車輛動(dòng)力學(xué)方程，只是通過上述三個(gè)傳感器對車輛的姿態(tài)進(jìn)行估算啊终。這里涉及到的問題就是镜豹，怎么把2D和3D的數(shù)據(jù)融合在一起。還有Hyungjin Kim, Bingbing Liu, and Hyun Myung進(jìn)行的研究是如何匹配點(diǎn)云地圖和點(diǎn)云特征來實(shí)現(xiàn)車輛定位[3]蓝牲。具體流程是趟脂，事先采集點(diǎn)云信息和激光放射信息構(gòu)建激光雷達(dá)地圖，然后通過車輛上的激光雷達(dá)傳感器實(shí)時(shí)接收的數(shù)據(jù)與激光雷達(dá)地圖比較例衍，得到自身車輛的相對位置昔期。

Fig 3亮度圖譜和事先標(biāo)記的特征

Fig 4 實(shí)時(shí)匹配特征，從而事先對自車定位

百度的激光雷達(dá)定位方案跟文獻(xiàn)[3]很類似肄渗。通過事先采集的定位地圖镇眷，反射值地圖和高度值地圖咬最，最大限度的搜集物理世界的特征翎嫡。然后通過車輛上的傳感器實(shí)時(shí)匹配自身獲取的數(shù)據(jù)和來自云端的數(shù)據(jù)。在匹配過程中永乌，所有數(shù)據(jù)都是數(shù)值惑申，所以也可以看做是optimization問題。只要定義好cost function翅雏，那么讓cost function最小的點(diǎn)就是車輛最可能在的位置圈驼。

Fig 5 事先采集物理世界數(shù)據(jù)

Fig 6求Cost function最小的點(diǎn)

基于攝像機(jī)定位

基于攝像機(jī)的定位方法和基于激光雷達(dá)定位的方法很像。但是激光雷達(dá)采集的本身就是3D數(shù)據(jù)望几，所以他可以對車輛直接進(jìn)行3D位置定位绩脆。相對來說，攝像機(jī)的問題就是它本身采集的數(shù)據(jù)是2D數(shù)據(jù)橄抹，那么位置找到車輛的位置靴迫，就需要距離等信息。所以利用攝像機(jī)定位的研究一般是把2D的圖像通過幾何方法進(jìn)行投影變化楼誓，之后再利用投影變換過后的3D數(shù)據(jù)獲得自身車輛與周邊物體的距離信息玉锌，從而實(shí)現(xiàn)自車定位。所謂的深度攝像機(jī)也是通過算法補(bǔ)充攝像機(jī)只能提取2D數(shù)據(jù)的缺陷疟羹。不過這種方法往往因?yàn)?D到3D變化的過程中會(huì)損失不少位置信息主守，所以這種方法的準(zhǔn)確性也有待提高。單純的利用攝像機(jī)對車輛進(jìn)行高精定位是不合適的選擇榄融。攝像機(jī)的主要功能還是在物體識(shí)別和物體追蹤上参淫。不僅如此，攝像機(jī)還涉及到標(biāo)定愧杯，校正涎才，扭曲等步驟，每一個(gè)步驟都會(huì)導(dǎo)致相應(yīng)的噪聲民效。然而因?yàn)閿z像機(jī)的價(jià)格便宜的原因憔维，預(yù)計(jì)會(huì)在一些場景中可以用來做定位涛救。

基于IMU和RTK GPS的組合導(dǎo)航定位

RTK GPS本身因?yàn)橛谢镜脑颍梢詾闊o人車提供相當(dāng)高的定位精度业扒。拋開價(jià)格和部署密度检吆，還需要考慮的是RTK GPS的定位更新頻率和定位精度。如果說程储，RTK GPS可以以（假設(shè)）100hz的頻率更新蹭沛，那么無人車則不需要額外的設(shè)備。但是實(shí)際上無人車的GPS只能以（假設(shè)）10hz的頻率更新章鲤。但是0.1s對于車輛來說太長了摊灭。采樣時(shí)間里0.1s里，無人車能知道的只有t-1時(shí)間的位置數(shù)據(jù)败徊。對于無人車來說帚呼，他的定位是要每個(gè)時(shí)間點(diǎn)都要更新才能保證車輛安全。所以還需要IMU(慣性測量單元)皱蹦。IMU是利用陀螺儀和加速度計(jì)來測量車輛的姿態(tài)數(shù)據(jù)的煤杀。為什么需要姿態(tài)數(shù)據(jù)？因?yàn)槲覀兛梢酝ㄟ^卡爾曼濾波和粒子濾波器等融合算法對姿態(tài)數(shù)據(jù)進(jìn)行轉(zhuǎn)換沪哺，最終可以以100hz的頻率估計(jì)出車輛的位置信息沈自。也就是說時(shí)間t到時(shí)間t+0.1的時(shí)間里，IMU會(huì)通過算法提供車輛的實(shí)時(shí)位置信息辜妓。這種方式就叫做組合導(dǎo)航系統(tǒng)枯途。他可以通過融合多個(gè)系統(tǒng)預(yù)測值，觀測值來獲取更精準(zhǔn)籍滴，更快速的位置數(shù)據(jù)輸出酪夷。他的優(yōu)點(diǎn)是超越了單個(gè)子系統(tǒng)的性能，并且彌補(bǔ)各個(gè)單一子系統(tǒng)的缺點(diǎn)异逐。而且因?yàn)槭嵌鄠€(gè)系統(tǒng)共同輸出位置數(shù)據(jù)捶索，所以當(dāng)某個(gè)系統(tǒng)出現(xiàn)故障的時(shí)候，其他子系統(tǒng)也可以作為冗余備份系統(tǒng)進(jìn)行定位灰瞻。這種方式也提高了數(shù)據(jù)的魯棒性酝润。

Fig 7 百度阿波羅自動(dòng)駕駛系統(tǒng)組合導(dǎo)航系統(tǒng)

百度阿波羅自動(dòng)駕駛定位模塊代碼分析

這里分析一下，阿波羅的定位模塊的代碼。阿波羅的github代碼如下圖。分析定位之前calibration等模塊我們都是要用到的。但是這里先撇開其他所有東西，我們只分析一下定位相關(guān)的的代碼結(jié)構(gòu)谢谦。因?yàn)楹诵拇a模塊好像是不開放的欠气。

Fig 8 百度阿波羅github庫

Fig 9 Module里面的localization模塊

用到的坐標(biāo)系是ECEF-WGS84 coordinate

RTK模塊有Status RTKLocalization::Start（）遮怜， RTKLocalization::Stop（）， RTKLocalization::OnTimer（）， RTKLocalization::InterpolateIMU（）， RTKLocalization::PrepareLocalizationMsg（）等等函數(shù)耕蝉。這里就可以看出爪膊，百度是利用RTK+GPS的方式對車輛進(jìn)行定位的

Localization的msf的common文件里可以看到有velodyne_ultility文件。且在common文件里面也有l(wèi)ocal文件夾谦铃。這就是百度通過激光雷達(dá)和本地高精地圖匹配來獲得定位數(shù)據(jù)的代碼耘成。這里的函數(shù)包括2D矩形標(biāo)注，timestamp驹闰，點(diǎn)云坐標(biāo)變換瘪菌，voxel相關(guān)的頭文件

Params里面提供了GNSS的參數(shù)，velodyne激光雷達(dá)等設(shè)備的參數(shù)調(diào)校

Msf里面也提供了一些相關(guān)測試數(shù)據(jù)嘹朗。

百度阿波羅的github里面并沒有提供如何融合這些傳感器的代碼师妙，所以看不到更核心的內(nèi)容。不過作為核心代碼不開放也是合理的屹培。

總結(jié)

定位本身是比較難的問題默穴。因?yàn)槎ㄎ贿€涉及到很多特征提取問題，所以定位和感知是綁定在一起的褪秀。感知做的不好蓄诽，那么定位的精準(zhǔn)度也不會(huì)好。同時(shí)定位還有很多噪聲影響媒吗，比如GPS的鐘差問題仑氛，多路徑問題，遮擋問題闸英，比如IMU的誤差積累問題锯岖。基于激光雷達(dá)和攝像機(jī)的定位又嚴(yán)重依賴線下制作的高精地圖自阱。高精地圖的制作和RTK基站的部署又很耗時(shí)耗力耗財(cái)嚎莉，這些都為自動(dòng)駕駛的普及帶來了很多麻煩。阿波羅系統(tǒng)現(xiàn)在是RTK GPS + IMU為主沛豌，其他定位方式為輔助的方式進(jìn)行定位趋箩。當(dāng)然還要依賴高精地圖≡叨睿現(xiàn)階段的解決方案還不適合普及。不過相信隨著技術(shù)的發(fā)展叫确，阿波羅平臺(tái)可以提供更好的定位算法和可執(zhí)行的解決方案跳芳。

參考文獻(xiàn)

[1]Sacha Arnoud, Director of Engineering, Waymo, https://www.youtube.com/watch?v=LSX3qdy0dFg

[2] Hanieh Deilamsalehy, Timothy C. Havens, Sensor Fused Three-dimensional Localization Using IMU, Camera and LiDAR

[3] Hyungjin Kim, Bingbing Liu, and Hyun Myung, Senior Member, IEEE齿椅，Road-Feature Extraction using Point Cloud and 3D LiDAR Sensor for Vehicle Localization

謝謝支持苹粟，各位看官的關(guān)注就是持續(xù)更新的動(dòng)力~

看完就別吝嗇點(diǎn)贊加關(guān)注啦~

同時(shí)也希望朋友往咱們專欄投稿，讓我們在無人車算法的造詣上不停的成長~融击！