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

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

1. 簡介

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

其次，定位是許多系統(tǒng)都需要的服務(wù)，而非一個單獨的系統(tǒng)。第三，我們回避了“導(dǎo)航”這個詞，是因為這只是定位算法的一個應(yīng)用場景。最后，“l(fā)ocation”這個概念比“positioning”的概念更準(zhǔn)確，因為如果不能保證地圖和周邊環(huán)境有同樣的絕對精度，一個所謂真實的經(jīng)緯度定位是沒有意義的。

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

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

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

輪速傳感器在ABS系統(tǒng)中的應(yīng)用；

數(shù)據(jù)庫如道路地圖信息用于地圖匹配等。

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

2. 未來的定位算法應(yīng)用

本節(jié)討論提高定位算法性能的需求，重點關(guān)注自動駕駛及地圖領(lǐng)域。

2.1 自動駕駛

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

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

2.2 地圖

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

3. 道路地圖和地圖匹配

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

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

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

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

慣性傳感器航跡推算

利用地圖匹配

動態(tài)濾波

4.1.航跡推算：里程計

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

這個模型有如下的形式

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

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

4.3. 航跡推算：地圖匹配

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

4.4. 航跡推算：動態(tài)濾波

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

時間更新：當(dāng)下一個測量到達(dá)時，利用運動模型來預(yù)測車輛位置

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

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

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

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

根據(jù)具體模型的不同，也可以將KF和PF組合應(yīng)用。這種思想是將狀態(tài)空間分成兩部分，如果一部分可以滿足高斯分布假設(shè)，則可以用KF來處理，而對于另一部分則使用PF。基于PF的濾波定位算法可以參考下表：

6. 慣性傳感器的應(yīng)用

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

6.1. 航跡推算IMU

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

6.2. GPS/IMU融合

對于下面的狀態(tài)量

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

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

7. 討論與結(jié)論

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