摘要

自動駕駛變成了一個非常熱門的話題。但實(shí)際上，在美國國防部組織的無人駕駛挑戰(zhàn)賽上美國的斯坦福大學(xué)獲得第一名的時候開始自動駕駛的時代已經(jīng)到來了。自動駕駛的系統(tǒng)，它分為很多部分。從傳感器的感知到之后的識別、定位、決策，再到控制系統(tǒng)，涉及很多技術(shù)。其中高精定位是自動駕駛非常重要的一個部分。

自動駕駛技術(shù)核心之一就是高精定位。因?yàn)檐囕v想在道路上實(shí)現(xiàn)自動駕駛需要車輛本身的位置達(dá)到厘米級的定位精度。高精定位的精度會受到傳感器的噪聲、衛(wèi)星導(dǎo)航的多路徑傳播效應(yīng)、車輛自身定位問題和車輛動力學(xué)問題等因素的影響。每一個小問題都會導(dǎo)致之后的誤差積累。當(dāng)誤差積累到一定程度，得到的最終結(jié)果就不可信了。就算所用的傳感器都是最好的，但實(shí)際中也會受到很多影響。所以我們還需要用額外的算法去輔助車輛獲得高精度的位置信息。這里面涉及到的就是卡爾曼濾波、擴(kuò)展卡爾曼濾波、無損卡爾曼濾波，還有粒子濾波器等等。

本文通過對百度阿波羅的高精定位方案進(jìn)行分析，并通過查看百度Apollo的Github上的定位模塊代碼，分析Apollo是如何達(dá)到L4級別的高精定位。

關(guān)于高精定位

自動駕駛已經(jīng)發(fā)展了很多年了。自動駕駛作為未來出行解決方案，很多大車廠和互聯(lián)網(wǎng)公司都在研究如何去開發(fā)安全舒適的自動駕駛車輛。現(xiàn)階段的自動駕駛系統(tǒng)框架有感知、定位、決策、路徑規(guī)劃、控制、高精地圖、云計算等核心內(nèi)容。層級上來說，定位模塊和其他模塊有上下層級關(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會發(fā)送到控制層，然后通過控制層對車輛上的執(zhí)行器進(jìn)行drive by wire控制。云端也會實(shí)時的接收車輛上的所有數(shù)據(jù)。云端確定車輛位置以后，會利用云端上的計算能力和數(shù)據(jù)庫為車輛提供更好的駕駛能力。

理想條件下，定位模塊利用RTK GPS系統(tǒng)就可以對無人車進(jìn)行導(dǎo)航及定位。GPS系統(tǒng)是通過計算電磁波的傳播時間來判斷車輛的位置。但是現(xiàn)實(shí)中電磁波的傳播會因?yàn)榇髿鈱印⒏邩谴髲B、天氣等因素產(chǎn)生時間誤差。時間誤差就意味著計算的距離數(shù)據(jù)不夠準(zhǔn)確。不夠準(zhǔn)確的距離就導(dǎo)致了我們根本不能確定我們的無人車到底在哪個位置。無人車在城市道路中的定位要求是誤差小于50cm，超出這個數(shù)據(jù)，定位模塊的數(shù)據(jù)就不能用來做自動駕駛。為了解決這些噪聲問題和單個傳感器不夠精準(zhǔn)的問題，研究人員就通過融合多個傳感器，最大限度的解決誤差問題。

下面分析百度Apollo平臺的高精定位解決方案。

百度Apollo高精定位解決方案

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

Fig 1 百度Apollo自動駕駛平臺硬件配置

這里所有的傳感器大部分都可以用來做定位。比如激光雷達(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)定位。Apollo平臺是IMU+RTK GPS為主，其他定位方法為輔的定位模式。

其實(shí)除了這些交叉定位以外，還有一種定位方式是利用街景給車輛定位[1]。Waymo利用谷歌強(qiáng)大的街景數(shù)據(jù)線下定義出路牌號碼，商店標(biāo)志，紅綠燈等等靜態(tài)數(shù)據(jù)來確定自身車輛所在的位置。大體的思路就是，只要車輛本身知道攝像機(jī)識別到的商店牌匾，紅綠燈和其他特征的全球坐標(biāo)系的話，通過透視原理或者相對距離就可以知道自身車輛的全球坐標(biāo)。這種方法簡單又直接，但是涉及到的問題是巨大的數(shù)據(jù)量和人工標(biāo)注。這種方法的優(yōu)點(diǎn)是如果地圖都標(biāo)記好了，那么車輛本身有個攝像機(jī)就可以實(shí)現(xiàn)精準(zhǔn)的定位和導(dǎo)航。

不過就算地圖標(biāo)注好了，但是有一些問題。那就是基于攝像機(jī)的圖像識別問題。攝像機(jī)由于是通過采集光線的方式收集數(shù)據(jù)，所以會受到強(qiáng)光影響。因此攝像機(jī)的識別精度會隨著光照強(qiáng)度的變化輸出不夠穩(wěn)定的識別結(jié)果。如果識別到的圖像不夠精確、穩(wěn)定，那么基于這個識別結(jié)果導(dǎo)出的位置計算也是不夠精準(zhǔn)的。且基于圖像的深度數(shù)據(jù)提取的能力也是值得深入考究的。畢竟從2D轉(zhuǎn)化到3D的過程中，很容易發(fā)生相關(guān)的數(shù)據(jù)噪聲和損失。不僅如此，像在高速公路等場景，周邊沒有特別的可識別的特征供定位。所以，Waymo的這種方式或許可以提供一種新的思路，但暫時無人車定位還是要依賴RTK GPS的幫助來完成精準(zhǔn)定位。

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

可以看到定位的方法有很多種。但是問題就是哪種方法可以達(dá)到很高的精度且魯棒性還高呢。那么現(xiàn)在的技術(shù)框架下，主流的還是在用RTK GPS+ IMU的方式進(jìn)行精準(zhǔn)定位。為什么不用其他的？因?yàn)闇?zhǔn)確度和魯棒性。在半自動駕駛L3和完全自動駕駛的L4階段，如果車輛失去了位置信息或者得到了錯誤的位置信息，那么車輛所做出的決策本身從根源就是錯誤的。自身位置都確定不了的情況下，變道、跟隨前車、路徑規(guī)劃都只會得到錯誤的結(jié)果。不僅如此，L4級別的車輛因?yàn)橛袑囕v的完全控制權(quán)，所以失去位置信息的車輛可能會導(dǎo)致嚴(yán)重的事故。所以對于完全自動駕駛的無人車來說，定位的精準(zhǔn)固然重要，但是更重要的魯棒性要好。否則，無人車根本不能判斷現(xiàn)在的位置信息的置信度，從而也無法輸出正確的結(jié)果。

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

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

▼下面對這些算法進(jìn)行分析▼1基于激光雷達(dá)定位

激光雷達(dá)可以精確的獲取車輛和周邊環(huán)境的相對距離信息。同時激光雷達(dá)還可以輔助相關(guān)的算法可提供基于點(diǎn)運(yùn)數(shù)據(jù)的物體識別。現(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]。這里不使用車輛動力學(xué)方程，只是通過上述三個傳感器對車輛的姿態(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ù)據(jù)與激光雷達(dá)地圖比較，得到自身車輛的相對位置。

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

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

Apollo的激光雷達(dá)定位方案跟文獻(xiàn)[3]很類似。通過事先采集的定位地圖，反射值地圖和高度值地圖，最大限度的搜集物理世界的特征。然后通過車輛上的傳感器實(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)

2基于攝像機(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變化的過程中會損失不少位置信息，所以這種方法的準(zhǔn)確性也有待提高。單純的利用攝像機(jī)對車輛進(jìn)行高精定位是不合適的選擇。攝像機(jī)的主要功能還是在物體識別和物體追蹤上。不僅如此，攝像機(jī)還涉及到標(biāo)定，校正，扭曲等步驟，每一個步驟都會導(dǎo)致相應(yīng)的噪聲。然而因?yàn)閿z像機(jī)的價格便宜的原因，預(yù)計會在一些場景中可以用來做定位。

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

RTK GPS本身因?yàn)橛谢镜脑颍梢詾闊o人車提供相當(dāng)高的定位精度。拋開價格和部署密度，還需要考慮的是RTK GPS的定位更新頻率和定位精度。如果說，RTK GPS可以以（假設(shè)）100hz的頻率更新，那么無人車則不需要額外的設(shè)備。但是實(shí)際上無人車的GPS只能以（假設(shè)）10hz的頻率更新。但是0.1s對于車輛來說太長了。采樣時間里0.1s內(nèi)，無人車能知道的只有t-1時間的位置數(shù)據(jù)。對于無人車來說，他的定位是要每個時間點(diǎn)都要更新才能保證車輛安全。所以還需要IMU(慣性測量單元)。

IMU是利用陀螺儀和加速度計來測量車輛的姿態(tài)數(shù)據(jù)的。為什么需要姿態(tài)數(shù)據(jù)？因?yàn)槲覀兛梢酝ㄟ^卡爾曼濾波和粒子濾波器等融合算法對姿態(tài)數(shù)據(jù)進(jìn)行轉(zhuǎn)換，最終可以以100hz的頻率估計出車輛的位置信息。也就是說時間t到時間t+0.1的時間里，IMU會通過算法提供車輛的實(shí)時位置信息。這種方式就叫做組合導(dǎo)航系統(tǒng)。它可以通過融合多個系統(tǒng)預(yù)測值，觀測值來獲取更精準(zhǔn)，更快速的位置數(shù)據(jù)輸出。它的優(yōu)點(diǎn)是超越了單個子系統(tǒng)的性能，并且彌補(bǔ)各個單一子系統(tǒng)的缺點(diǎn)。而且因?yàn)槭嵌鄠€系統(tǒng)共同輸出位置數(shù)據(jù)，所以當(dāng)某個系統(tǒng)出現(xiàn)故障的時候，其他子系統(tǒng)也可以作為冗余備份系統(tǒng)進(jìn)行定位。這種方式也提高了數(shù)據(jù)的魯棒性。

Fig 7 百度Apollo組合導(dǎo)航系統(tǒng)

定位模塊代碼分析

這里分析一下，Apollo定位模塊的代碼。Apollo在Github代碼如下圖。分析定位之前Calibration等模塊我們都是要用到的。但是這里先撇開其他所有東西，我們只分析一下定位相關(guān)的的代碼結(jié)構(gòu)。

Fig 8 百度Apollo 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ù)。

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

總結(jié)

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