考慮定位信息不確定性的多車協(xié)同定位算法*

魯光泉 潘日佩

(北京航空航天大學交通科學與工程學院 北京 100191)

0 引 言

隨著車聯(lián)網技術的發(fā)展和車輛智能化程度的提高，車輛定位技術作為各種車輛應用的核心支撐，受到了各國學者的高度重視。(global positioning system，GPS)是目前交通領域應用最成熟的定位技術，其廣泛應用于企業(yè)車輛管理、路網交通流信息(交通流量、車流密度、路網負荷)采集等方面，同時，GPS定位技術也為車輛安全應用提供了車輛經緯度、速度、加速度等車輛位置以及行駛狀態(tài)數據。

但GPS測得的位置信息精確度在車輛實際運行中往往不夠理想。由于城市環(huán)境下高樓林立造成衛(wèi)星信號被遮擋，原始GPS定位信息存在著很大的誤差，有時甚至不能夠定位。且車輛在行駛過程中在不斷地變換工作環(huán)境，這也就使得車輛獲取的定位信息具有很大的不確定性，定位精度低以及較差的定位可靠性限制了GPS系統(tǒng)在車輛安全、防碰撞檢測等關系生命安全領域的使用[1]。

針對該問題傳統(tǒng)的解決方法一般分為2種，第一種方法是基于通信的定位增強，包括差分GPS(differential GPS，DGPS)[2]，實時動態(tài)(real-time kinematic，RTK)定位[3]等；第二種方法是利用GPS與其他傳感器進行組合導航，其中應用范圍較廣的是GPS/INS組合導航系統(tǒng)，其利用INS系統(tǒng)所產生的導航信息連續(xù)性好且對外界干擾不敏感的特點同GPS相結合，一度成為學術界研究的熱點[4-6]。這些方法雖然對定位精度的提升均有所幫助，但是大多相對復雜且需要很多的外部設備進行輔助，短時間內無法對實際的定位精度產生顯著提升。而隨著通信技術和車車通信技術的愈加成熟，車輛與車輛之間可以通過無線通信設備將所有檢測到的車輛信息在一定范圍內進行交互，使得每輛車可以同時感知自車與周圍車輛的所有信息[7-8]。這為通過無線傳輸技術進行多車協(xié)同定位以提升自車的定位精度提供了新的可能[9-10]。Amini等提出了一種GPS和V2X相結合的集成定位算法,有效的提升定位精度[11]。Ansari等針對協(xié)同碰撞預警應用建立并優(yōu)化了基于GPS和DSRC的實時相對定位(RRP)系統(tǒng)[12]。Golestan等采用協(xié)同定位思想，融合擴展卡爾曼濾波方法和擴展粒子濾波算法循環(huán)在線評估車載自組織網絡中的車輛位置信息，使得定位精度得到顯著改善[13]。

但是車輛在實際運行過程中，隨著車輛運行環(huán)境的變化，GPS的測量不確定度在不斷的改變，而現階段各國學者研究的多車協(xié)同定位方法大多將GPS的定位不確定度當成確定值來進行信息融合，并沒有將定位信息的時變的不確定性考慮在內，且這些研究僅對每一時刻的獨立位置信息進行探究，沒有綜合考慮車輛在運動過程中位置的連續(xù)性，這在很大的程度上降低了協(xié)同定位的實用性。在現階段，車輛的前置距離傳感器在車輛上已得到廣泛的應用，因此本文通過車車通信技術，使用車載GPS傳感器和前置距離傳感器并結合定位信息不確定性，利用車輛間的相對位置關系以及周圍車輛連續(xù)傳來的位置信息進行信息融合來進行協(xié)同定位，以提升定位的準確性與可靠性。

1 車輛定位不確定度

為符合車輛ACC等系統(tǒng)的實際應用，假設本文所研究的的車聯(lián)網車輛均配備低成本的車載GPS接收機以及前置距離傳感器，車輛在運行時可在通信范圍內交換所有測量到的運動狀態(tài)信息以及位置信息。每一時刻，車輛將會估計周圍所有車輛的位置，同時也會利用從周圍車輛得到的多個觀測值進行信息融合。我們稱多車協(xié)同算法中進行信息融合的車輛為目標車輛，稱與目標車輛保持車車通信的周圍車輛為信息傳輸車輛。

目標車輛要對多個觀測值進行精確地篩選，就必須追求對測量值估計的精細化，盡量客觀的評估接收到的每個測量數據的質量。在現有的研究中，大多采用平均誤差來對定位數據的質量進行評價，但在實際的數據測量中，車輛位置的真值是不可知的，因此很難對真實的誤差進行量化評定。相對而言，測量不確定度則使用區(qū)間對測量真值可能的取值范圍進行表征，其表現了在一個特定工況下獲得的定位數據的可能取值范圍。與測量誤差相比，不確定度能夠更好的在車輛之間進行傳遞且能體現出該數據的可信任程度，因此定位信息的不確定性應以時變的定位不確定度來進行表示。本文定義車輛定位不確定度是與車輛定位最終結果相關聯(lián)的一個參數，用以表征合理地賦予定位結果的分散性，即車輛定位不確定度表示了對定位觀測成果是否可靠或對觀測成果是否有效的懷疑程度。

因此，若想更準確的車輛進行多車協(xié)同定位，需要實時確定每輛車傳輸來的位置信息的不確定度?，F階段，車輛定位和控制數據的獲取最成熟的方法是利用卡爾曼濾波來進行全球定位系統(tǒng)(GPS)和慣性導航系統(tǒng)(INS)相結合的組合導航，因此本文的定位信息不確定度將結合卡爾曼濾波進行提取。為了較準確的計算定位結果的實時不確定度，采用自適應濾波技術[14-15]。

圖1 自適應窗口大小確定方法Fig.1 The determination method of the adaptive window

(1)

2 車間相對位置模型

2.1 相對位置預測模型

在路網中車輛確定自車GPS不確定度之后，

信息傳輸車輛若想求得較精確的目標車輛位置估計，需考慮兩車的相對位置關系。針對本文所研究的車輛，在直接測量通信范圍內任意信息傳輸車輛與目標車輛的相對位置時，可用2種方法得到：①通過距離傳感器直接測量或通過多個距離傳感器累加；②直接利用兩車的GPS數據進行相對位置的求解。在實際使用中，根據試驗車所使用的DENSO MMW Radar的參數，設定前置距離傳感器的測量不確定度為0.5m，其數據質量高于車載GPS數據。但隨著間隔車輛數增多，由不確定度的傳播規(guī)律，多個距離傳感器測得的數據累加值的不確定度也會隨之增大，這時通過其測得的數據質量有時甚至低于直接使用GPS獲得的數據。且當信息傳輸車輛距離目標車輛距離較遠或車間存在障礙物時，距離傳感器會發(fā)生受到干擾而失效的情況，這時兩車相對位置只能通過GPS數據求得。

但是車輛實際運行中，GPS的測量數據質量相對不高，若距離傳感器不可用且兩車中存在觀測噪聲較大的車輛時，所測得的相對位置數據的不確定度將會很大，可信程度較低。為解決此問題以便提升車輛通信范圍內所有信息傳輸車輛與目標車輛的相對位置精度，建立車間相對位置模型。

對運動中的車輛進行建模需對靜態(tài)的位置模型結合動態(tài)信息，在所研究的兩車輛間建立矢量線確定車輛間的連接屬性，并用角度與長度2個特征值描述2車之間的相對位置關系以確保2車之間位置的連續(xù)性。如圖2(a)所示，在僅考慮2輛車時，對A,B2車進行分析。設定A車為目標車輛，B車為信息傳輸車輛，建立A與B之間的相對位置矢量線，其中LBA(i)是i時刻A與B的相對距離，αBA(i)為i時刻由B到A的方向角，每條矢量線由以上2個特征值確定。每隔時間間隔Δt，車輛間將更新1次矢量線，預測得到的下一時刻矢量線長度和方向角的計算公式為

LBA(i+1)=

(2)

(3)

式中：Δv(i)和Δa(i)分別為i時刻B車相對于A車的速度與加速度。通過矢量線模型，利用上一時刻距離傳感器以及GPS測得的車間矢量線可完成對2輛相對位置的預測。

2.2 相對位置測量值

在獲得2車相對位置的測量值時，若2輛車無法利用距離傳感器直接測量或相隔車輛較多，則需引入定位效果較好的第3輛參考車輛進行間接測量。此時為了保證3輛車之間的連接關系，在3輛車之間增加相應的幾何約束，以圖2(b)為例，在引入C車后，利用三角函數關系便可利用C來確定B和A的相對位置關系，推導過程不做贅述。

圖2 車間相對位置模型Fig.2 Relative position modelling

而對于同一信息傳輸車輛來說，引入不同的參考車輛會產生不同的相對位置，因此，需要對所有測量值進行融合得到測量值的最優(yōu)估計。綜合考慮2輛車的定位數據和相對位置測量值的不確定度，信息傳輸車輛將通過對目標車輛位置的初步估計間接計算2輛車相對位置的測量值。假設一共引入g輛參考車輛來進行2輛車相對位置的估計，對選取車輛i為參考車輛時的測量值分配權重ωi，此時目標車輛位置的測量值為

(4)

多項式中的每1項代表1種確定目標車輛位置的方式。其中，Pm1和ωm1分別為通過多個車輛距離傳感器間接測量得到的目標位置及其權重，該值若無法測量則忽略；Pm2和ωm2分別為通過2車GPS數據測量得到的目標位置和其權重；Pe和ωe分別為選取參考車輛i時測量的目標位置及其權重。

為了更準確的分配權重，需要考慮影響相對位置誤差的變量有2個，分別為方向角不確定度σrA和相對距離不確定度σrD。在上述方式1,2中，由于測量原理的不同，通過雷達測量得到的方向角和相對距離較為獨立，而通過GPS測量得到的兩者則均來源于GPS坐標。利用正態(tài)分布“3σ準則”，考慮變量x的可能取值范圍為(x-3σ,x+3σ)，則信息傳輸車輛確定的目標車輛位置可能分布在如圖3(a)或(b)所示的區(qū)域中，計算該區(qū)域面積作為權重度量。則單獨利用雷達和GPS得到的權重度量分別為εm1、εm2為

SR=18dσrAσrD

(5)

SG=9σGDxσGDy

(6)

(7)

εm2=SG

(8)

式中：SR和SG分別為信息傳輸車輛通過距離傳感器和GPS確定目標車輛可能存在區(qū)域的面積，d為距離傳感器測得的相對距離，σGDx為2輛車GPS橫向不確定度的和，σGDy為2輛車GPS縱向不確定度的和。N為距離傳感器累計使用的次數。

圖3 不同方式確定的目標車輛可能存在區(qū)域Fig.3 Possible position of the target vehicle in different ways

利用參考車輛求解相對位置時，則主要由參考車輛i傳來的信息而確定，因此由其得到的目標位置對應的權重度量εi由2個面積相加得到，其求解方法將由2段相對位置的測量方式決定，分別由式(5)～(6)求得。對可計算的L個誤差度量進行歸一化即可求得上述所有方式對應的權重ωm。

求得權重后帶入式(4)，每輛信息傳輸車輛將會求得目標車輛位置的初步測量，從而計算出信息傳輸車輛和目標車輛的相對位置測量值，其不確定度可由不確定度傳播公式得到。相對位置信息將會同信息傳輸車輛的運動狀態(tài)信息和位置信息共同打包傳至目標車輛。

3 多車數據融合方法

局最優(yōu)估計和相應的協(xié)方差矩陣。

圖4 協(xié)同定位聯(lián)邦卡爾曼濾波算法原理圖Fig.4 Federal Kalman filter of cooperativevehicular positioning

3.1 聯(lián)邦卡爾曼濾波系統(tǒng)狀態(tài)方程的建立

取全局最優(yōu)估計狀態(tài)向量為

Xg=[xt,yt,vt,at,θt]T

(9)

(10)

系統(tǒng)噪聲Wg中變量均可看做均值為零的白噪聲，其對應的協(xié)方差矩陣Qg由各變量不確定度組成，可從公共參考系得到。

3.2 局部濾波器設計

各子系統(tǒng)利用信息傳輸車輛傳來的信息進行自車位置的最優(yōu)估計，局部濾波器將自車的位置信息和相對位置信息作為濾波目標，由式(2)、(3)可知，相對位置的狀態(tài)方程非線性關系較為復雜，很難在卡爾曼濾波中直接實現，因此選取中間變量和，兩者為各局部濾波器的專有狀態(tài)變量，分別代表信息傳輸車輛i與自車的相對橫向和縱向距離，其狀態(tài)更新方程為

(11)

式中：vi，ai，θi分別為信息傳輸車輛i的速度、加速度和航向角。信息傳輸車輛的運動狀態(tài)信息在各自的組合導航里進行更新，不需要在各個局部濾波器中重復計算，因此其屬于控制變量。子系統(tǒng)的狀態(tài)方程記為

(12)

(13)

(14)

式中：Ui為控制信號，Mi為局部濾波器的系統(tǒng)噪聲，其由主濾波器的系統(tǒng)噪聲和2車相對位置的噪聲構成。

(15)

取各子系統(tǒng)測量到的自車位置估計和2車相對位置關系為測量值，建立局部濾波器量測方程。子系統(tǒng)的直接測量值為信息傳輸車輛的位置估計xi和yi以及相對距離r和方向角α，后2個參數更新方程為非線性，因此根據擴展卡爾曼濾波，局部濾波器的觀測向量Zi和觀測矩陣Hi分別為

(16)

Hk=

(17)

各觀測變量的噪聲同樣可看做均值為零的白噪聲，量測噪聲Vi對應的協(xié)方差矩陣Ri為

(18)

式中：σ(xi)和σ(yi)分別為信息傳輸車輛i的經緯度測量值的不確定度。

3.3 信息分配與融合

當周圍車輛的觀測值傳至局部濾波器時，因其不確定度在不斷地變化且存在不確定度較大的信息，容易造成濾波的發(fā)散。因此首先對數據進行篩選，設定閾值W，當數據不確定度大于W時，拒絕采用該數據，之后局部濾波器進行獨立的時間和量測更新。主濾波器隨后進行信息融合，計算最優(yōu)估計值和對應的協(xié)方差矩陣。

當主濾波器的權重自適應變化時，一般采用無重置結構設計，即不反饋全局最優(yōu)估計去重置各個局部濾波器，這樣做雖然能夠避免濾波的發(fā)散且具有良好的容錯性，但是由于沒有結合全局估計值，局部濾波器的濾波精度將和單獨使用自車濾波器時近乎相同，且收斂速度將會較慢，這對多車協(xié)同算法有很大的影響。針對此問題，添加周期性重置模塊，模塊每隔M個周期將主濾波器得到的全局狀態(tài)信息反饋到各個局部濾波器中，作為下一次濾波計算的初始值。采用該結構能夠起到對局部濾波器反饋矯正的作用，并有效的提高了聯(lián)邦卡爾曼濾波的精度和收斂速度。因多車協(xié)同聯(lián)邦卡爾曼濾波的各子濾波器主要針對經緯度信息進行計算，反饋的分配系數將以個子濾波器傳來的經緯度信息的不確定度作為計算依據，而周期性重置也同樣只針對位置信息，其他運動狀態(tài)信息保持不變。周期性重置方法如下。

(19)

(20)

改進后的自適應聯(lián)邦卡爾曼濾波能夠有效調整各局部濾波器的分配系數，最大程度的利用新信息進行數據融合，在得到相對最優(yōu)全局融合解的同時并保證了濾波器的穩(wěn)定性。

4 數值仿真與結果分析

在車輛實際行駛中，因位置在不斷地變化，使得車輛實時獲得的GPS數據質量也在不斷地改變，對于一個復雜的車流環(huán)境，車輛在不同區(qū)域可獲得的GPS定位信息質量差距較大，且在GPS信號極差路段，GPS數據漂移現象明顯，無論對單車的組合導航算法進行何種優(yōu)化，都很難得到較好的定位效果。因此在車車通信的條件下，本文提出的算法可以利用周邊車輛的定位數據進行協(xié)同定位，有效地彌補了單車定位的劣勢，增強了定位精度與穩(wěn)定性。

為了研究本算法效果，利用Matlab進行模擬仿真。構建一個交通仿真網絡，路網由4個路段共同形成一個閉環(huán)區(qū)域，每條路段為雙向4車道，車道寬3.75 m，長500 m。初始在每條路段各隨機分布10輛車，每輛車以v=10 m/s的速度運行，假設車輛通過基于IEEE 802.11b標準的無線局域網進行通信，設定有效通信距離為200 m。如圖5所示，為了模擬實際路況，首先設定一條路段的車輛均處于同一種定位工況，給改路段上車輛生成的每個準確GPS數據添加一個固定不確定度的測量噪聲使得用來體現GPS定位的隨機性；而在不同路段將會添加變化的不確定度來代表不同的定位工況，以此來體現車輛位置帶來的定位差異性。因此假設路網中路段2的GPS定位數據質量較好，其不確定度為2 m(1σ)；路段4的GPS定位數據質量較差，其不確定度為8 m(1σ)；路段1和路段3的GPS定位數據質量一般，其不確定度為4 m(1σ)。稱車輛從路網中1點出發(fā)再回到該點為1個工作周期并設定仿真時長為500 s，以保證所有車輛可以在路網中能夠完成至少2個工作周期。GPS采樣頻率f=10 Hz，距離傳感器測量的相對距離和方向角的不確定度分別為0.5 m和0.1 rad，觀察仿真效果。

圖5 仿真路網結構示意圖Fig.5 The schematic diagram of the simulation map

如圖6(a)所示，車輛利用本算法得到定位精度優(yōu)于基于傳統(tǒng)卡爾曼濾波和自適應卡爾曼濾波的自車組合導航，且算法收斂速度較快，能夠更好的調節(jié)濾波參數并適應車輛的實際運行工況。當路網中所有車輛均為車輛網車輛時，算法穩(wěn)定后的平均定位誤差為0.85 m，與改進后的自車組合導航相比，本算法在仿真時長內的平均定位精度提升了35.2%。如圖6(b)所示，當車輛運行工況較差時，本算法對定位精度的提升效果更加明顯，平均定位精度提升了39.8%。

圖6 車輛平均定位誤差Fig.6 Average position errors of vehicles

選取以圖5中A點為起點，車輛運行2個周期時長的定位數據，統(tǒng)計所有車輛在不同路段的定位信息不確定度的平均值。本文利用95%置信水平下對應的定位數據的不確定度(2σ)作為定位結果可靠性的衡量標準，如圖7所示，本算法最終獲得的定位信息不確定度與實際GPS測量不確定度相比明顯減小，有效的提高了測量結果的可靠性。與自車組合導航相比，可靠性平均提升了42.6%。

圖7 95%置信水平下車輛在不同工況下對應的的定位不確定度(2)Fig.7 The uncertainties under different working conditions and 95% confidence

當路網中存在非車聯(lián)網車輛時，觀察不同滲透率下多車協(xié)同定位算法的定位效果。如圖8所示，隨著滲透率的增大，多車協(xié)同定位精度不斷增高，但在滲透率低于20%和高于80%時，本算法的定位精度提升效果對滲透率的敏感性較低。當滲透率達到100%時，將有80%的車輛定位誤差小于1 m。

圖8 不同車聯(lián)網滲透率下的定位誤差CDFFig.8 Impact of the ratio of vehicles in V2V on position errors

算法驗證以仿真數據為基礎，與實車定位數據相比仍存在以下因素難以模擬，可能會對仿真結果的可靠程度產生影響：仿真過程中，車輛之間設定為實時通信，而在實際使用中，通信延遲和掉包可能會使得目標車輛獲得的信息產生遲滯性，從而使得定位效果變差；盡管利用添加特定不確定度噪聲的方法模擬定位信息的隨機性，但仍然難以模擬GPS信號瞬間被遮擋而變弱隨后恢復的現象。若車輛行駛所經過的路段的GPS不確定度變化極為頻繁，則改進后的自適應濾波的響應程度也會受到一定的影響，使得自車的定位不確定度計算不準，對最終結果有一定的影響。

5 結 論

筆者提出了一個車車通信環(huán)境下考慮定位信息不確定性的多車協(xié)同定位算法，該算法以自車組合導航為基礎，利用車載GPS傳感器和前置距離傳感器對車輛初始定位信息以及車間相對定位信息進行采集。之后通過車車通信的方式，結合車輛運動的連續(xù)性和定位信息的不確定性，利用周圍車輛傳來的對目標車輛的觀測值進行數據融合，達到了車輛間協(xié)同定位的目的。仿真結果表明與傳統(tǒng)的自車組合導航相比，本算法的平均定位精度提升了35.2%，可靠性提升了42.6%，有效的提升定位的準確性與可靠性。但因實驗條件所限，現階段對協(xié)同定位的研究大多采用數值仿真的方法進行驗證，難以將實際使用中的所有工況考慮在內，這對算法的整體可靠程度存在一定的影響。在后續(xù)研究中，將會以實車數據為基礎，對算法做出進一步的完善。