未來車輛定位算法的需求,與如今依賴GNSS的定位算法相比

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

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

1. 簡(jiǎn)介

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

其次，定位是許多系統(tǒng)都需要的服務(wù)，而非一個(gè)單獨(dú)的系統(tǒng)。第三，我們回避了“導(dǎo)航”這個(gè)詞，是因?yàn)檫@只是定位算法的一個(gè)應(yīng)用場(chǎng)景。最后，“l(fā)ocation”這個(gè)概念比“positioning”的概念更準(zhǔn)確，因?yàn)槿绻荒鼙ＷC地圖和周邊環(huán)境有同樣的絕對(duì)精度，一個(gè)所謂真實(shí)的經(jīng)緯度定位是沒有意義的。

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

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

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

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

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

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

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

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

2.1 自動(dòng)駕駛

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

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

2.2 地圖

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

3. 道路地圖和地圖匹配

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

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

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

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

慣性傳感器航跡推算

利用地圖匹配

動(dòng)態(tài)濾波

4.1.航跡推算：里程計(jì)

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

這個(gè)模型有如下的形式

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

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

4.3. 航跡推算：地圖匹配

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

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

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

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

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

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

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

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

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

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

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

6.1. 航跡推算IMU

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

6.2. GPS/IMU融合

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

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

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

7. 討論與結(jié)論

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