摘要

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

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

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

關于高精定位

自動駕駛已經發展了很多年了。自動駕駛作為未來出行解決方案，很多大車廠和互聯網公司都在研究如何去開發安全舒適的自動駕駛車輛。現階段的自動駕駛系統框架有感知、定位、決策、路徑規劃、控制、高精地圖、云計算等核心內容。層級上來說，定位模塊和其他模塊有上下層級關系。感知屬于最底層，定位就是感知模塊的上一層。之后的控制、決策都要依賴定位模塊進行運算。

這里簡單分析一下定位和其他模塊的關系。

感知層是通過傳感器感知物理世界。傳感器把物理世界按照一定的數據編碼好，傳送給感知模塊。那么感知模塊就可以通過相關的算法提取物理世界模型。不僅如此，感知還可以提供detection，classification等功能。定位模塊就是利用感知模塊的傳感器來確定車輛在世界坐標系里面的位置。定位模塊用到的核心傳感器是RTK GPS, GNSS, IMU，LIDAR，Camera等。

定位模塊利用這些數據，通過定位算法輸出車輛在全球坐標系里面的位置。規劃決策模塊利用感知、定位、高精地圖等數據進行整合和決策。最終決策層的output會發送到控制層，然后通過控制層對車輛上的執行器進行drive by wire控制。云端也會實時的接收車輛上的所有數據。云端確定車輛位置以后，會利用云端上的計算能力和數據庫為車輛提供更好的駕駛能力。

理想條件下，定位模塊利用RTK GPS系統就可以對無人車進行導航及定位。GPS系統是通過計算電磁波的傳播時間來判斷車輛的位置。但是現實中電磁波的傳播會因為大氣層、高樓大廈、天氣等因素產生時間誤差。時間誤差就意味著計算的距離數據不夠準確。不夠準確的距離就導致了我們根本不能確定我們的無人車到底在哪個位置。無人車在城市道路中的定位要求是誤差小于50cm，超出這個數據，定位模塊的數據就不能用來做自動駕駛。為了解決這些噪聲問題和單個傳感器不夠精準的問題，研究人員就通過融合多個傳感器，最大限度的解決誤差問題。

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

百度Apollo高精定位解決方案

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

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

這里所有的傳感器大部分都可以用來做定位。比如激光雷達可以結合高精地圖實現特征匹配定位，攝像機可以結合高精地圖進行特征匹配從而實現定位，通過GPS實現定位，通過IMU實現定位，GPS+IMU實現定位，GPS+IMU+Lidar+Radar也可以實現定位。Apollo平臺是IMU+RTK GPS為主，其他定位方法為輔的定位模式。

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

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

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

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

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

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

▼下面對這些算法進行分析▼1基于激光雷達定位

激光雷達可以精確的獲取車輛和周邊環境的相對距離信息。同時激光雷達還可以輔助相關的算法可提供基于點運數據的物體識別。現在學術界普遍利用攝像機，激光雷達，IMU等傳感器進行融合。一般融合的輸出是車輛的位置和相應的姿態狀態。 Hanieh Deilamsalehy，Timothy C. Havens提出的理論就是利用IMU，攝像機和激光雷達對車輛或者無人機的三維姿態進行估算[2]。這里不使用車輛動力學方程，只是通過上述三個傳感器對車輛的姿態進行估算。這里涉及到的問題就是，怎么把2D和3D的數據融合在一起。還有Hyungjin Kim, Bingbing Liu, and Hyun Myung進行的研究是如何匹配點云地圖和點云特征來實現車輛定位[3]。具體流程是，事先采集點云信息和激光放射信息構建激光雷達地圖，然后通過車輛上的激光雷達傳感器實時接收的數據與激光雷達地圖比較，得到自身車輛的相對位置。

Fig 3亮度圖譜和事先標記的特征

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

Apollo的激光雷達定位方案跟文獻[3]很類似。通過事先采集的定位地圖，反射值地圖和高度值地圖，最大限度的搜集物理世界的特征。然后通過車輛上的傳感器實時匹配自身獲取的數據和來自云端的數據。在匹配過程中，所有數據都是數值，所以也可以看做是optimization問題。只要定義好cost function，那么讓cost function最小的點就是車輛最可能在的位置。

Fig 5 事先采集物理世界數據

Fig 6求Cost function最小的點

2基于攝像機定位

基于攝像機的定位方法和基于激光雷達定位的方法很像。但是激光雷達采集的本身就是3D數據，所以他可以對車輛直接進行3D位置定位。相對來說，攝像機的問題就是它本身采集的數據是2D數據，那么位置找到車輛的位置，就需要距離等信息。所以利用攝像機定位的研究一般是把2D的圖像通過幾何方法進行投影變化，之后再利用投影變換過后的3D數據獲得自身車輛與周邊物體的距離信息，從而實現自車定位。

所謂的深度攝像機也是通過算法補充攝像機只能提取2D數據的缺陷。不過這種方法往往因為2D到3D變化的過程中會損失不少位置信息，所以這種方法的準確性也有待提高。單純的利用攝像機對車輛進行高精定位是不合適的選擇。攝像機的主要功能還是在物體識別和物體追蹤上。不僅如此，攝像機還涉及到標定，校正，扭曲等步驟，每一個步驟都會導致相應的噪聲。然而因為攝像機的價格便宜的原因，預計會在一些場景中可以用來做定位。

3基于IMU和RTK GPS的組合導航定位

RTK GPS本身因為有基站的原因，可以為無人車提供相當高的定位精度。拋開價格和部署密度，還需要考慮的是RTK GPS的定位更新頻率和定位精度。如果說，RTK GPS可以以（假設）100hz的頻率更新，那么無人車則不需要額外的設備。但是實際上無人車的GPS只能以（假設）10hz的頻率更新。但是0.1s對于車輛來說太長了。采樣時間里0.1s內，無人車能知道的只有t-1時間的位置數據。對于無人車來說，他的定位是要每個時間點都要更新才能保證車輛安全。所以還需要IMU(慣性測量單元)。

IMU是利用陀螺儀和加速度計來測量車輛的姿態數據的。為什么需要姿態數據？因為我們可以通過卡爾曼濾波和粒子濾波器等融合算法對姿態數據進行轉換，最終可以以100hz的頻率估計出車輛的位置信息。也就是說時間t到時間t+0.1的時間里，IMU會通過算法提供車輛的實時位置信息。這種方式就叫做組合導航系統。它可以通過融合多個系統預測值，觀測值來獲取更精準，更快速的位置數據輸出。它的優點是超越了單個子系統的性能，并且彌補各個單一子系統的缺點。而且因為是多個系統共同輸出位置數據，所以當某個系統出現故障的時候，其他子系統也可以作為冗余備份系統進行定位。這種方式也提高了數據的魯棒性。

Fig 7 百度Apollo組合導航系統

定位模塊代碼分析

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

Fig 8 百度Apollo Github庫

Fig 9 Module里面的localization模塊

用到的坐標系是ECEF-WGS84 coordinate

RTK模塊有Status RTKLocalization::Start（）， RTKLocalization::Stop（）， RTKLocalization::OnTimer（）， RTKLocalization::InterpolateIMU（）， RTKLocalization::PrepareLocalizationMsg（）等等函數。這里就可以看出，百度是利用RTK+GPS的方式對車輛進行定位的

Localization的msf的common文件里可以看到有velodyne_ultility文件。且在common文件里面也有local文件夾。這就是百度通過激光雷達和本地高精地圖匹配來獲得定位數據的代碼。這里的函數包括2D矩形標注，timestamp，點云坐標變換，voxel相關的頭文件

Params里面提供了GNSS的參數，Velodyne激光雷達等設備的參數調校

Msf里面也提供了一些相關測試數據。

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

總結

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