高 策 褚端峰▲ 何書賢 賀 宜 吳超仲 陸麗萍

(1.武漢理工大學(xué)智能交通系統(tǒng)研究中心 武漢 430063；2.武漢理工大學(xué)水路公路交通安全控制與裝備教育部工程研究中心 武漢 430063；3.武漢理工大學(xué)計算機科學(xué)與技術(shù)學(xué)院 武漢 430063)

0 引 言

車輛衛(wèi)星定位能夠為車輛提供實時、準(zhǔn)確的位置信息，為車輛服務(wù)提供了基礎(chǔ)。隨著智能網(wǎng)聯(lián)汽車技術(shù)的發(fā)展，對車輛定位提出了更高的要求，車輛的精準(zhǔn)定位也成為了當(dāng)前研究的熱點。

目前，車輛衛(wèi)星定位的精度一般在10 m左右[1]。為了提高車輛衛(wèi)星定位的精度，卡爾曼濾波和粒子濾波是2種常用的定位濾波算法。早期的卡爾曼濾波只能適用于線性系統(tǒng)[2]，但衛(wèi)星定位系統(tǒng)是非線性的，這就導(dǎo)致了研究非線性卡爾曼濾波算法的必要性。一般使用卡爾曼濾波算法時，噪聲的統(tǒng)計特性是難以確定的，往往通過調(diào)參確定相對濾波效果較好的噪聲特性，但是不能保證濾波的穩(wěn)定性。為了解決噪聲的統(tǒng)計特性問題，趙欣[3]提出了基于噪聲統(tǒng)計特性自適應(yīng)估計的濾波算法。C.Zeng等[4]使用擴展卡爾曼濾波進行GPS信號的高動態(tài)跟蹤。田寵[5]針對卡爾曼濾波容易發(fā)散的問題，提出了帶衰減因子的自適應(yīng)濾波器技術(shù)，采用衰減的加權(quán)因子，使估計器逐漸忘記陳舊數(shù)據(jù)的作用，同時增加新數(shù)據(jù)的比重，避免濾波過程的發(fā)散。付心如[6]研究了一種觀測噪聲協(xié)方差和抗差自適應(yīng)相結(jié)合的卡爾曼濾波算法，有效的解決了觀測異常對組合定位的影響。H.Feng等[7]使用卡爾曼濾波器對車輛位置進行預(yù)測，并對卡爾曼濾波器的預(yù)測性能進行了分析。張園[8]針對定位中有色噪聲的情況，通過增加似然分布自適應(yīng)調(diào)整來改進粒子濾波達到目標(biāo)跟蹤的目的。Q Shen等[9]針對車載定位收到的不連續(xù)周期信號，應(yīng)用相對于參考衛(wèi)星形成差分定位算法進行定位優(yōu)化。王琰[10]針對歷元定位精度有時不如單一系統(tǒng)定位精度的問題，提出了衛(wèi)星分群的抗差卡爾曼濾波算法，根據(jù)衛(wèi)星類型分類構(gòu)建方差膨脹因子，從而改善定位精度。

對于車載衛(wèi)星定位系統(tǒng)，在開闊環(huán)境中，GNSS可以為車輛提供實時、精確、可靠的位置信息，目前的濾波算法也可以很好地對衛(wèi)星定位過程中的噪聲進行濾除，對于常規(guī)噪聲，一般采用卡爾曼濾波或者以卡爾曼為基礎(chǔ)的衍生算法進行濾波操作[11-13]；但是當(dāng)車輛處于復(fù)雜的行車環(huán)境中時，例如高大建筑物遮擋、城市隧道等，衛(wèi)星定位信號會受到嚴(yán)重的干擾甚至是丟失，無法提供精確、持續(xù)、可靠的車輛位置信息，嚴(yán)重影響到行車安全。

車輛在行駛過程中衛(wèi)星定位信號受到較大干擾時，由于噪聲的統(tǒng)計特性突變，造成了卡爾曼濾波等算法并不能起到很好的濾波作用，造成車輛定位誤差較大。針對上述問題，提出基于動態(tài)閾值的聯(lián)合濾波方法，對噪聲突變的車輛漂移位置進行識別，并使用高斯過程回歸進行真實位置預(yù)測，并結(jié)合觀測值構(gòu)建觀測補償量，使用含有動態(tài)補償量的卡爾曼濾波進行車輛衛(wèi)星定位的優(yōu)化，實現(xiàn)車輛精準(zhǔn)定位。

1 卡爾曼-高斯聯(lián)合濾波模型

車載衛(wèi)星定位在不同情況下受到的信號干擾強度顯然是不同的，對于行車過程中衛(wèi)星信號受到干擾較大的情況下，卡爾曼濾波不能很好的解決這一問題[14]。為充分利用車輛的速度、加速度及航向角等信息，提高車輛定位系統(tǒng)的時間和空間覆蓋率以及增加定位系統(tǒng)的信息利用率，建立基于動態(tài)閾值的卡爾曼-高斯聯(lián)合濾波模型，該模型不僅設(shè)置了與車輛狀態(tài)信息相關(guān)的動態(tài)閾值，可有效地識別出行車過程中衛(wèi)星信號受到較大干擾造成的車輛定位漂移點，還對車輛的歷史軌跡信息進行有監(jiān)督式學(xué)習(xí)，從而可以根據(jù)車輛的歷史信息預(yù)測當(dāng)前時刻的車輛真實位置，并結(jié)合真實觀測值構(gòu)建卡爾曼濾波觀測方程補償量。通過聯(lián)合濾波模型可以濾除多種行車工況下的噪聲信號，對于比較惡劣的情況下產(chǎn)生的漂移現(xiàn)象，也會有很好的濾波作用?；诳柭?高斯聯(lián)合濾波模型的車載定位優(yōu)化算法見圖1。

首先，使用與車輛狀態(tài)信息相關(guān)聯(lián)的動態(tài)閾值進行車輛位置漂移點的識別；然后，利用車輛歷史軌跡信息進行高斯過程回歸，對當(dāng)前車輛真實位置信息進行預(yù)測；最后，使用預(yù)測值和真實觀測值構(gòu)建觀測補償量，通過在卡爾曼觀測方程加入動態(tài)補償量，進行車輛定位的聯(lián)合濾波，從而提高車輛定位精度。

1.1 動態(tài)閾值計算

圖1 車載定位優(yōu)化Fig.1 Vehicle positioning optimization

車輛行駛過程中的軌跡是連續(xù)的，車輛下一時刻的位置信息與車輛當(dāng)前時刻的位置及車輛狀態(tài)信息是密不可分的。車輛下一時刻的位置可以通過車輛當(dāng)前時刻的位置與車輛的狀態(tài)信息推算得出[15-16]，所以同樣可以使用當(dāng)前車輛位置信息與車輛的狀態(tài)信息判斷下一時刻車輛位置觀測值的準(zhǔn)確性。當(dāng)周期Δt足夠短時，認(rèn)為當(dāng)前時刻的位置與前一時刻的位置關(guān)系具體見式（1）。

式中，Px,t為t時刻在東北天坐標(biāo)系下東西方向上的位置，m；Py,t為t時刻在東北天坐標(biāo)系下南北方向上的位置，m；vt-1為t-1時刻的速度，m/s2；at-1為t-1時刻的加速度；jt-1為t-1時刻的航向角（以真北方為基準(zhǔn)），rad。

通過車輛位置信息和狀態(tài)信息判斷下一時刻的車輛位置信息的可靠性，從車輛衛(wèi)星定位信息觀測結(jié)果出發(fā)，設(shè)置與車輛速度及航向角等實時信息相關(guān)的動態(tài)閾值，使用動態(tài)閾值進行車輛衛(wèi)星定位漂移點的識別。

動態(tài)閾值的設(shè)置與車輛當(dāng)前的行駛狀態(tài)信息相關(guān)，使用車輛位置推算的增量作為基本閾值，綜合考慮信號干擾、速度以及加速度傳感器誤差的情況下，加入閾值增強因子l組成動態(tài)閾值，動態(tài)閾值公式見式（2）。

使用動態(tài)閾值可以判斷車輛衛(wèi)星定位信息是否可以信任，即在車輛衛(wèi)星定位過程中是否產(chǎn)生漂移現(xiàn)象，判斷依據(jù)見式（3）。

式中，Pt為t時刻車輛位置的實際觀測值；Pt-1為t-1時刻車輛位置的實際觀測值。

車輛當(dāng)前位置實際觀測值與前一時刻的位置之差的絕對值大于動態(tài)閾值時，可以認(rèn)為車輛當(dāng)前時刻定位信號不可信任，需要使用高斯過程回歸進行車輛真實位置預(yù)測；該預(yù)測值結(jié)合觀測值構(gòu)建補償量，通過在卡爾曼濾波觀測方程中加入動態(tài)補償量，達到聯(lián)合濾波的目的。

考慮到車載衛(wèi)星定位系統(tǒng)為動態(tài)定位，根據(jù)實車實驗分析得出定位的精度跟車速密切相關(guān)，特別是當(dāng)車速較低時，車載衛(wèi)星定位精度較差，存在定位漂移現(xiàn)象，所以針對不同的車輛行駛狀態(tài)，設(shè)置相應(yīng)的閾值增強因子。根據(jù)反復(fù)的實車實驗得知，當(dāng)車速小于1 m/s時，λ取值為3；當(dāng)車速大于等于1 m/s時，λ取值為1.5，動態(tài)閾值能夠很好的識別車輛定位漂移點。

1.2 基于高斯過程回歸的車輛位置預(yù)測

高斯過程基于貝葉斯原理，屬于非參數(shù)概率模型，通過訓(xùn)練歷史數(shù)據(jù)將先驗分布轉(zhuǎn)化為后驗?zāi)Ｐ停ㄟ^后驗?zāi)Ｐ偷玫骄哂懈怕室饬x的預(yù)測結(jié)果[17]。高斯過程回歸是一種基于高斯隨機函數(shù)的機器學(xué)習(xí)方法，主要利用樣本進行學(xué)習(xí)來判斷整個值函數(shù)的分布，對處理小樣本、非線性等復(fù)雜問題具有很好的適應(yīng)性。

利用車輛的歷史軌跡信息作為學(xué)習(xí)樣本進行高斯過程回歸，樣本的選取采用滑動窗口模式，隨著時間的推移，窗口在學(xué)習(xí)樣本上滑動，每1個周期內(nèi)舊的樣本被丟棄，新的樣本被添加；為了減少計算量，更快地得到預(yù)測值，每次高斯過程回歸的學(xué)習(xí)樣本都是選取當(dāng)前時刻之前的3個周期作為學(xué)習(xí)樣本，進行核函數(shù)的參數(shù)優(yōu)化，然后預(yù)測當(dāng)前時刻的車輛位置。車輛位置預(yù)測過程見圖2。

圖2 車輛位置預(yù)測Fig.2 Vehicle position prediction

選取高斯核函數(shù)作為高斯過程回歸的協(xié)方差函數(shù)，見式（5）。

根據(jù)高斯過程的性質(zhì)，即學(xué)習(xí)樣本跟預(yù)測樣本符合同一分布的特點，可以得到學(xué)習(xí)樣本和預(yù)測樣本的聯(lián)合分布。根據(jù)上面的協(xié)方差函數(shù)可以得到觀測值與預(yù)測值的聯(lián)合分布，觀測值 y-3，y-2，y-1與預(yù)測值y的聯(lián)合分布見式（6）。

其中，y-3，y-2，y-1為前3個時刻的觀測值，y為當(dāng)前時刻的預(yù)測值。

根據(jù)所得到的學(xué)習(xí)樣本跟預(yù)測樣本的聯(lián)合分布可以得到預(yù)測樣本的條件分布，見式（7）。

使用預(yù)測樣本分布的均值作為當(dāng)前時刻的預(yù)測值為

筆者并未對所有時刻的車輛位置數(shù)據(jù)都基于高斯過程回歸進行預(yù)測，僅當(dāng)車輛衛(wèi)星定位數(shù)據(jù)不可以信任時，基于高斯過程回歸進行車輛位置預(yù)測。因為車輛在行駛過程中具有一定的趨勢性，當(dāng)前時刻的位置一定跟歷史軌跡相關(guān)[18]，所以使用車輛歷史軌跡信息作為學(xué)習(xí)樣本進行高斯過程回歸然后預(yù)測當(dāng)前時刻的車輛位置的方法具有一定的可靠性。

1.3 基于觀測值補償?shù)目柭鼮V波

普通卡爾曼濾波不能很好解決車輛受到較大干擾產(chǎn)生的漂移現(xiàn)象，原因是觀測值中的噪聲產(chǎn)生了突變，造成卡爾曼濾波無法有效濾波統(tǒng)計特性突變的噪聲。針對車輛定位過程中噪聲統(tǒng)計特性突變的情況，建立基于觀測值補償?shù)目柭鼮V波模型，狀態(tài)量的選取見式（9）。

式中，e表示車輛在正東方向上的位置，m；n表示車輛在正北方向上的位置，m；v表示車輛的速度，m/s；h表示車輛的航向角（以正北方向為基準(zhǔn)），rad；a表示車輛的加速度，m/s2；w表示車輛航向角的變化率，rad/s。

基于觀測值補償?shù)目柭鼮V波模型的狀態(tài)方程跟觀測方程見式（10）。

式中：Wk+1為過程噪聲，該噪聲為白色高斯過程噪聲，均值為0，協(xié)方差為Q；Vk+1為觀測噪聲，該噪聲為白色高斯過程噪聲，均值為0，協(xié)方差為R；γ為觀測值補償量的系數(shù)，取值見式（11）。

ΔEk為觀測值補償量，見式（12）。

其中，ek，nk，vk，hk為車輛位置、速度和航向角的真實觀測值為使用高斯過程回歸進行的預(yù)測值。

基于觀測值補償?shù)目柭鼮V波可根據(jù)動態(tài)閾值的識別進行動態(tài)濾波。當(dāng)動態(tài)閾值識別出車輛位置漂移點時，進行高斯過程回歸，對車輛位置進行預(yù)測，通過預(yù)測值與真實觀測值的差構(gòu)建卡爾曼濾波觀測方程的補償量，從而應(yīng)對定位過程中噪聲統(tǒng)計特性突變的情況。

2 聯(lián)合濾波過程

整個聯(lián)合濾波過程均利用原始車輛數(shù)據(jù)及車輛狀態(tài)信息，通過構(gòu)建卡爾曼-高斯聯(lián)合濾波模型進行車輛位置的濾波。首先，根據(jù)實時車速和航向角信息設(shè)置動態(tài)閾值，根據(jù)動態(tài)閾值結(jié)合上一時刻的車輛位置信息判斷當(dāng)前車輛位置是否為漂移點；然后，對動態(tài)閾值識別出的車輛衛(wèi)星定位漂移點，進行高斯過程回歸，使用車輛的歷史軌跡信息作為學(xué)習(xí)樣本預(yù)測當(dāng)前時刻的車輛位置；最后，根據(jù)高斯過程回歸的預(yù)測值和真實觀測值構(gòu)建卡爾曼濾波觀測補償量，結(jié)合動態(tài)閾值識別位置漂移情況，對卡爾曼濾波觀測方程加入補償量，從而實現(xiàn)卡爾曼濾波觀測方程動態(tài)補償，對車輛位置跟蹤進行優(yōu)化。聯(lián)合濾波過程見圖3。

圖3 聯(lián)合濾波模型流程圖Fig.3 Flow chart of joint filter model

利用卡爾曼-高斯聯(lián)合濾波模型進行實時動態(tài)濾波具體流程如下。

步驟1。計算當(dāng)前時刻的動態(tài)閾值。通過當(dāng)前時刻的車速與航向角等信息，計算出動態(tài)閾值。

步驟2。判斷當(dāng)前時刻的車輛位置與前1時刻車輛位置之差，是否超出動態(tài)閾值。若超出，則執(zhí)行步驟3，否則直接執(zhí)行步驟4。

步驟3。利用前3個時刻的車輛歷史位置信息進行高斯過程回歸的學(xué)習(xí)，再預(yù)測當(dāng)前時刻的車輛位置信息，根據(jù)預(yù)測值和真實觀測值之差構(gòu)建觀測補償量。

步驟4。對卡爾曼濾波觀測方程加入動態(tài)補償，使用加入補償量的卡爾曼濾波對車輛位置信息進行濾波優(yōu)化。

步驟5。對每1時刻的車輛位置信息依次重復(fù)上述步驟。

3 實驗驗證與分析

3.1 實驗方案設(shè)計

本文基于卡爾曼-高斯聯(lián)合濾波模型采取實車數(shù)據(jù)驗證，實驗采用比亞迪e5電動車搭載某款OBU（on-board unit），板載的車載定位模塊為u-blox M8模塊，并搭載千尋RTK。實驗路線選取武漢市太子湖附近的普通城市道路，車輛順時針沿著車城北路、太子湖路、神龍大道、車城東路行駛，具體路線見圖4。

圖4 實驗路線Fig.4 Route of experiment

為了更好地驗證聯(lián)合濾波對車輛衛(wèi)星定位精度及可靠性的改進，實驗中OBU與RTK設(shè)備同步采集車輛的衛(wèi)星定位信息，周期都設(shè)置為0.2 s。RTK設(shè)備具有0.02 m的高定位精度，因此作為參考系統(tǒng)，其輸出結(jié)果可近似作為汽車的實際位置信息即真值。采取對比實驗驗證，比較原始車輛衛(wèi)星定位數(shù)據(jù)、普通卡爾曼濾波、基于動態(tài)閾值的卡爾曼-高斯聯(lián)合濾波進行的車輛定位精度及可靠性。

3.2 算法驗證與分析

通過實驗采集的數(shù)據(jù)分析得知，車輛在行駛過程中，通過速度傳感器得到的車速信息是可信的，車輛的航向角信息比較可靠。如圖5～6所示，車輛在行駛過程中，車速與航向角信息隨著時間比較平穩(wěn)的變化，并沒有發(fā)生較大的突變情況，所以采用車速信息與航向角信息作為動態(tài)閾值的設(shè)定依據(jù)比較可靠。

圖5 車速信息Fig.5 Vehicle speed

圖6 航向角信息Fig.6 Vehicle heading angle

對于車輛在行駛過程中產(chǎn)生的衛(wèi)星定位漂移現(xiàn)象，由于噪聲統(tǒng)計特性的突變，導(dǎo)致常規(guī)的卡爾曼濾波不能很好地解決，濾波效果變差?；趧討B(tài)閾值的卡爾曼-高斯聯(lián)合濾波正是針對車輛衛(wèi)星定位信號中噪聲統(tǒng)計特性突變的情況，對于統(tǒng)計特性突變的噪聲進行識別，然后進行聯(lián)合濾波。普通卡爾曼濾波與聯(lián)合濾波效果見圖7。

圖7 聯(lián)合濾波與卡爾曼濾波對比Fig.7 Comparison between joint filter and kalman filter

由圖7可見，普通卡爾曼濾波對于衛(wèi)星定位過程中噪聲統(tǒng)計特性發(fā)生突變造成的車輛定位漂移現(xiàn)象的濾波效果不明顯，而聯(lián)合濾波是基于動態(tài)閾值識別車輛衛(wèi)星定位漂移位置，并進行高斯過程回歸處理，結(jié)合真實觀測值設(shè)置動態(tài)的觀測補償量。對于易產(chǎn)生漂移現(xiàn)象的車輛定位，聯(lián)合濾波效果明顯好于普通卡爾曼濾波。

為了進一步驗證算法的可靠性，選取了一段普通的城市道路，整個實驗過程持續(xù)大約400 s，采樣周期為0.2 s，全程共1 985條衛(wèi)星定位數(shù)據(jù)，同時采集RTK數(shù)據(jù)作為車輛實際行駛軌跡的基準(zhǔn)數(shù)據(jù)（即真值）。

圖8 局部1聯(lián)合濾波結(jié)果Fig.8 Part 1 of joint filter result

圖9 局部2聯(lián)合濾波結(jié)果Fig.9 Part 2 of joint filter result

整個實驗過程中，選取了2處比較明顯的車輛衛(wèi)星定位漂移點，相對于RTK基準(zhǔn)位置數(shù)據(jù)出現(xiàn)了較大的偏移。圖8～9為2處存在衛(wèi)星定位漂移點的車輛位置信息，圖中黑色直線表示近似車輛實際行駛軌跡，三角形為車輛衛(wèi)星定位原始數(shù)據(jù)?？梢钥吹剑艿捷^大干擾時，車輛衛(wèi)星定位出現(xiàn)漂移現(xiàn)象，普通黑點為聯(lián)合濾波之后的結(jié)果，可以從圖8～9中看出，聯(lián)合濾波對于定位噪聲產(chǎn)生突變的車輛位置具有良好的濾波效果。

為了更加直觀方便地觀測聯(lián)合濾波算法的效果，在衛(wèi)星地圖上繪制車輛原始位置信息以及濾波之后的軌跡，如圖10所示，地標(biāo)標(biāo)記點為原始車輛位置數(shù)據(jù)，可以看到車輛位置受到干擾，位置產(chǎn)生漂移的車輛經(jīng)緯度坐標(biāo)，普通黑點標(biāo)記為聯(lián)合濾波之后的車輛經(jīng)緯度坐標(biāo)。根據(jù)聯(lián)合濾波結(jié)果可以得出，聯(lián)合濾波算法不僅對常規(guī)噪聲有一定的濾波作用，對于車輛行駛過程中產(chǎn)生的較大的車輛漂移位置，可以很好地濾除。

圖10 漂移點的濾波Fig.10 Filter of Drift Points

為了更好地證明聯(lián)合濾波模型的有效性，對車輛的原始位置信息和聯(lián)合濾波之后的位置信息做了數(shù)據(jù)對比分析。選取了圖4實驗中的一段數(shù)據(jù)作數(shù)據(jù)分析，其中包含了圖8～9中的2段漂移信息，數(shù)據(jù)一共500個位置點信息。

表1 濾波結(jié)果對比Tab.1 Comparison of filter results

表1為對比結(jié)果，從表1可以看出，卡爾曼濾波和聯(lián)合濾波都對原始車輛衛(wèi)星定位數(shù)據(jù)的精度有所改進。其中，平均誤差反映定位精度，可以看出聯(lián)合濾波的平均誤差相對于普通卡爾曼濾波降低了大約0.12 m，定位精度提高了30%左右；聯(lián)合濾波最突出的優(yōu)點是可以有效濾除噪聲突變引發(fā)的漂移點，最大誤差由原始數(shù)據(jù)的9 m降低到0.8 m，可以反映聯(lián)合濾波對漂移點的濾波效果；標(biāo)準(zhǔn)差也可以看出聯(lián)合濾波對車輛定位位置漂移數(shù)據(jù)有很好的濾除效果。因此，聯(lián)合濾波在提高定位精度的同時，可以有效識別并濾除車輛衛(wèi)星定位漂移點，為車輛提供更加準(zhǔn)確的位置信息，增強車輛定位的抗干擾能力。

相對于傳統(tǒng)的車輛定位濾波方法，本文研究的聯(lián)合濾波模型優(yōu)點為：可以有效地識別并濾除衛(wèi)星信號受到較大干擾時產(chǎn)生的車輛位置漂移點，在提高濾波精度的同時，也提高了車輛衛(wèi)星定位的抗干擾性。

4 結(jié)束語

針對行車過程中衛(wèi)星定位信號受到干擾產(chǎn)生的車輛位置漂移現(xiàn)象，建立了卡爾曼-高斯聯(lián)合濾波模型，通過對車輛位置信息的實時濾波，提高了車輛位置信息的可靠性，增強了車載定位的抗干擾性。該模型從觀測結(jié)果出發(fā)，通過設(shè)置與車速、航向角相關(guān)的動態(tài)閾值，識別車輛位置漂移現(xiàn)象，通過對車輛歷史軌跡信息進行監(jiān)督式學(xué)習(xí)，預(yù)測車輛位置，然后結(jié)合車輛位置真實觀測值構(gòu)建動態(tài)補償量，通過對卡爾曼濾波觀測方程中加入觀測補償量實現(xiàn)動態(tài)聯(lián)合濾波?；诳柭?高斯的聯(lián)合濾波模型，可以有效識別出車輛定位漂移點，對于車輛衛(wèi)星定位起到了很好的濾波作用。在現(xiàn)有低成本車載定位裝置的情況下，有效提高車輛衛(wèi)星定位的精度及可靠性。

研究尚缺少復(fù)雜路況對算法的驗證，特別是存在城市隧道、高架橋等復(fù)雜且衛(wèi)星信號干擾較大的路況；該算法主要利用車輛的歷史軌跡數(shù)據(jù)進行有監(jiān)督式的機器學(xué)習(xí)，還應(yīng)結(jié)合道路信息，以此作為約束能更精確地確定車輛位置。