GNSS能夠提供車輛的絕對位置、速度等信息，是現今大多數定位算法的主要信息源。該論文的作者認為在未來，定位的重點是車輛自身與環境的相對位置而非絕對位置，通過慣性傳感器和地圖信息的融合，獲得精確的定位結果。

如今大多數導航定位系統都依賴于全球導航衛星系統（GNSS），通過GNSS和里程計、慣性傳感器的組合定位可以獲得較好的精度和魯棒性。對于要實現自動駕駛的汽車，定位的精度和可靠性有著更高的要求。一般情況下車輛都是在路面上行駛的，考慮這種情況，我們將里程計選作第一輸入，而當輪速無法獲取時，則使用慣性傳感器的輸入。最后，采用粒子濾波和貝葉斯方法融合地圖信息，而不是標準的地圖匹配。大量試驗結果表明，無GNSS的定位系統性能和有GNSS的系統性能相差無幾。

1. 簡介

現階段的定位系統的目標還是為人服務的，是為導航系統提供當前位置，或者收集車輛狀態數據。我們特定討論“Localization Algorithm”這個具體的概念出于以下幾個原因。首先，“算法”指軟件開發，與僅依靠GNSS信息的定位系統相比，目前已經有足夠多的可用的傳感器或數據信息，定位性能有了很大的飛躍。

其次，定位是許多系統都需要的服務，而非一個單獨的系統。第三，我們回避了“導航”這個詞，是因為這只是定位算法的一個應用場景。最后，“location”這個概念比“positioning”的概念更準確，因為如果不能保證地圖和周邊環境有同樣的絕對精度，一個所謂真實的經緯度定位是沒有意義的。

上圖展示了未來車輛的數據流，一個顯著的趨勢便是車輛的自主化以及先進駕駛輔助系統(ADAS)的應用。因此，當務之急是提高定位及速度估計系統的性能。從汽車的發展來看，執行器的變化（如制動、轉向、發動機轉矩等）很小，而過去幾十年來傳感器卻在急劇增多，如：

發動機控制以及懸架傳感器中的慣性單元（IMU），用于車輛狀態估計；

視覺、夜視、雷達、聲吶等傳感器用于環境感知，保證車輛行駛在正確的車道并保證跟車距離等；

輪速傳感器在ABS系統中的應用；

數據庫如道路地圖信息用于地圖匹配等。

前面提到的所有場景都應用了傳感器融合技術，也有一些利用傳感器融合計算出無法直接測量或識別的狀態量或物體的實例，如基于攝像頭的障礙物、行人檢測；利用輪速傳感器進行胎壓和路面附著系數的估計等。我們的方法依賴信號數據處理技術、車輛運動學模型和傳感器。需要注意的是，道路地圖信息是非線性的，不能用線性高斯模型近似，因此粒子濾波（PF）架構要優于卡爾曼濾波（KF）算法。傳感器融合架構見下圖。

2. 未來的定位算法應用

本節討論提高定位算法性能的需求，重點關注自動駕駛及地圖領域。

2.1 自動駕駛

未來的自動駕駛對定位算法有著極高的依賴性。一方面，在DARPA的推動下，已有的智能車主要基于激光雷達進行定位。但這種方案價格昂貴，且安裝在車頂的激光雷達很影響車輛的設計，如空氣動力學特性、美觀等；另一方面，我們也看到利用差分GPS，包括通過多天線和攝像頭信息得到橫擺信息估計的無人駕駛車輛在有高精度地圖的區域表現出了極高的性能（如奧迪RS7在德國霍根海姆的自動駕駛展示）。

自動駕駛汽車對定位需要很高的可靠性。定位算法必須要能夠在隧道、停車、城市街道、峽谷等GNSS信號丟失或不準的時候保持正常工作。如果只是GNSS無法解算位置信息，但可以解算出偽距信息的話，利用多模型濾波和地圖信息限制進行定位也是一種解決方案。而對于一般的室內行駛、長隧道等情況，重點應在于無衛星信號的地圖輔助定位技術。

2.2 地圖

對于定位來說，先驗地圖信息是已知的，或者它也可以通過傳感器數據來得到。通常精確的向量地圖對定位都很有幫助，包括一些路標、限速標志等都可以從數據庫中獲取。

3. 道路地圖和地圖匹配

地圖匹配是一種提高定位性能的方法，由各類傳感器估計出的位置投影到最近的道路上。這種方法有兩個問題：首先，地圖沒有考慮實際的地形地勢信息，這就有可能導致定位點從一條路跳到另一條路上；其次，車輛的運動動力學和地圖信息的組合并不是最優的。本小節的目的是介紹幾種不同的方法，我們稱為動態地圖匹配（dynamic map matching）。這包括了將運動模型、傳感器模型、道路模型等在非線性濾波器中進行組合。難點在于將噪聲、干擾等不確定性干擾的軌跡與地圖相匹配。圖4描述了幾種原理，從中可以看出，僅通過里程計和地圖信息是可以得到較為精確的定位信息的。

4. 航跡推算(Dead-reckoning)原理

航跡推算最基本的原理是對速度或加速度信號進行積分，可以基于車輛模型，也可以不基于車輛模型。比如，它可以利用IMU數據或者輪速數據。我們從以下幾個方面分析航跡推算：

里程計，或基于輪速傳感器的航跡推算

慣性傳感器航跡推算

利用地圖匹配

動態濾波

4.1.航跡推算：里程計

基于位置X,Y及航向角ψ的運動學模型，將速度觀測和角速度作為輸入信號，我們有如下的帶有噪聲的動態模型

這個模型有如下的形式

4.2. 航跡推算：慣性傳感器

通過慣性傳感器可以測量得到速度、加速度等信息，理論上這些值都可以通過積分得到位置、速度及航向。使用基于慣性傳感器的航跡推算與地圖匹配相結合可以修正傳感器的一些誤差并得到較為精確的定位。

4.3. 航跡推算：地圖匹配

如前所述，地圖匹配可以通過將估計值與道路最近點進行正交匹配來實現。利用貝葉斯濾波可以實現較好的地圖輔助式定位。

4.4. 航跡推算：動態濾波

一般用于定位的非線性濾波包含以下主要步驟：

時間更新：當下一個測量到達時，利用運動模型來預測車輛位置

測量更新：利用當前測量和傳感器模型來更新當前位置信息

5. 基于貝葉斯濾波的融合定位

非線性濾波是統計信號處理的一個分支，它關注的是根據基于時間的一組測量來估計狀態的問題。它解決的最一般的問題是計算貝葉斯條件后驗密度。計算后驗密度有幾種不同的算法。KF解決的是模型為線性，噪聲為高斯噪聲的一類濾波問題。對于非線性問題，可以將模型先線性化后再使用KF濾波，也就是擴展卡爾曼濾波(EKF)。如果高斯近似假設是成立的，也有不需要對非線性模型線性化的處理方法，如無跡卡爾曼濾波（UKF）在每一步都假設后驗符合高斯概率密度分布。這些方法的一大問題是只有后驗密度很符合高斯分布時才能達到很好的效果。

道路的約束實際上會帶來多峰后驗密度分布（定位點有可能在這條路或地圖上附近的路上等等）。因此，概率密度函數的高斯分布假設是不合適的。另一種完全不同的非線性濾波方法是粒子濾波，它通過尋找一組在狀態空間傳播的隨機樣本對概率密度函數進行近似，以樣本均值代替積分運算，從而獲得狀態最小方差分布。盡管算法中的概率分布只是真實分布的一種近似，但由于非參數化的特點，它擺脫了解決非線性濾波問題時隨機量必須滿足高斯分布的制約，能表達比高斯模型更廣泛的分布，也對變量參數的非線性特性有更強的建模能力。

根據具體模型的不同，也可以將KF和PF組合應用。這種思想是將狀態空間分成兩部分，如果一部分可以滿足高斯分布假設，則可以用KF來處理，而對于另一部分則使用PF?；赑F的濾波定位算法可以參考下表：

6. 慣性傳感器的應用

本小節介紹慣性傳感器如加速度計、陀螺儀在航跡推算、傳感器融合中的應用。

6.1. 航跡推算IMU

純積分或者說航跡推算對傳感器精度的要求很高，而對于量產車來說無法接受如此昂貴的慣性傳感器。而低成本傳感器的問題在于其傳感器的實時零偏和漂移無法獲知并且無法去除。例如，在車輛沿縱軸方向，傳感器誤差和道路坡度都會反映到縱向加速度上，可以認為是時變的未知偏差，積分兩次之后得到的位置會產生很大的位置偏差。對于不基于GPS的純慣性導航定位來說這是很難解決的一件事。

6.2. GPS/IMU融合

對于下面的狀態量

為四元數向量，為位置向量，為車身坐標系下的速度向量，為車身坐標系下的加速度向量。假定為車身坐標系轉化到慣性坐標系下的旋轉矩陣，則對于離散時刻可以得到慣性坐標系下的位置。

下圖展示了基于EKF濾波融合由谷歌手機采集的IMU和GPS數據后的結果。

7. 討論與結論

本文討論了未來車輛定位算法的需求，指出了與如今依賴GNSS的定位算法相比，未來的車輛定位需要更高的精確性和可靠性。為了達到這個目的，我們提出了基于多傳感器融合和統計信號處理方法的定位算法設計路線。由于車輛定位關心的是自身與環境的相對位置，因此位置估計與周邊路網的融合就尤為重要，而精確的道路地圖是實現精確定位的關鍵信息源。